motor_selection

   SCRIPT TO FIND THE TORQUES AT EACH JOINT FOR DIFFERENT MOTION STATES OF
   THE ARM.
   The script uses the inverse dynamic model of the robot to simulate
   different motion states and compute the torques for each situation.
   The torques at each joint, as well as the torques at each motor are computed
   (reduced by the gear ratio). A trapezoidal speed profile can be
   selected at the parameters section of this file. This trapezoidal
   profile should meet the desired features of the robot, such as maximum
   joint speed, maximum acceleration/deceleration.
   Please note that the movement of the robot is not simulated. The reader
   should imagine that the manipulator is fixed at a given initial
   position and the inverse dynamic function returns the torques at each
   joint necessary to bring the arm to that motion state (defined by position q,
   speed qd and acceleration qdd).
   
   As a reference, below you can find the call to the inverse dynamics
   function.
 
   TAU = inversedynamic(robot, Q, QD, QDD, GRAV, FEXT)

 Copyright (C) 2012, by Arturo Gil Aparicio

 This file is part of ARTE (A Robotics Toolbox for Education).
 
 ARTE is free software: you can redistribute it and/or modify
 it under the terms of the GNU Lesser General Public License as published by
 the Free Software Foundation, either version 3 of the License, or
 (at your option) any later version.
 
 ARTE is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 GNU Lesser General Public License for more details.
 
 You should have received a copy of the GNU Leser General Public License
 along with ARTE.  If not, see <http://www.gnu.org/licenses/>.

0001 %   SCRIPT TO FIND THE TORQUES AT EACH JOINT FOR DIFFERENT MOTION STATES OF
0002 %   THE ARM.
0003 %   The script uses the inverse dynamic model of the robot to simulate
0004 %   different motion states and compute the torques for each situation.
0005 %   The torques at each joint, as well as the torques at each motor are computed
0006 %   (reduced by the gear ratio). A trapezoidal speed profile can be
0007 %   selected at the parameters section of this file. This trapezoidal
0008 %   profile should meet the desired features of the robot, such as maximum
0009 %   joint speed, maximum acceleration/deceleration.
0010 %   Please note that the movement of the robot is not simulated. The reader
0011 %   should imagine that the manipulator is fixed at a given initial
0012 %   position and the inverse dynamic function returns the torques at each
0013 %   joint necessary to bring the arm to that motion state (defined by position q,
0014 %   speed qd and acceleration qdd).
0015 %
0016 %   As a reference, below you can find the call to the inverse dynamics
0017 %   function.
0018 %
0019 %   TAU = inversedynamic(robot, Q, QD, QDD, GRAV, FEXT)
0020 %
0021 % Copyright (C) 2012, by Arturo Gil Aparicio
0022 %
0023 % This file is part of ARTE (A Robotics Toolbox for Education).
0024 %
0025 % ARTE is free software: you can redistribute it and/or modify
0026 % it under the terms of the GNU Lesser General Public License as published by
0027 % the Free Software Foundation, either version 3 of the License, or
0028 % (at your option) any later version.
0029 %
0030 % ARTE is distributed in the hope that it will be useful,
0031 % but WITHOUT ANY WARRANTY; without even the implied warranty of
0032 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
0033 % GNU Lesser General Public License for more details.
0034 %
0035 % You should have received a copy of the GNU Leser General Public License
0036 % along with ARTE.  If not, see <http://www.gnu.org/licenses/>.
0037 
0038 function motor_selection
0039 
0040 close all
0041 global robot
0042 
0043 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0044 % PARAMETERS SECTION
0045 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0046 
0047 % robot pose: experiment by changing the pose while observing the different
0048 %             torques at each joint
0049 q=[0 0 -pi/2 0 0 0]; %rad
0050 
0051 %maximum speeds for joint 1, 2, 3, 4, 5 and 6
0052 %maximum_speeds=[3 4 5 5.5 6 6.3]; %rad/second
0053 maximum_speeds=[3 3 4 5 5 5];%rad/second
0054 %maximum acceleration/deceleration for each joint
0055 %maximum_accels=[2 3 3.5 4 5 6]; %rad/second^2
0056 maximum_accels=[5 5 6 7 8 9]; %rad/second^2
0057 
0058 % time of the trapezoidal profile that the joint moves at maximum speed
0059 time_at_constant_speed=0.4; %seconds
0060 
0061 
0062 %load robot parameters
0063 robot=load_robot('unimate', 'puma560');
0064 drawrobot3d(robot, q)
0065 
0066 
0067 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0068 %  FIRST, COMPUTE TRAPEZOIDAL PROFILES
0069 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0070 %compute acceleration plus deceleration times for every joint
0071 time_acc = 2*maximum_speeds./maximum_accels+time_at_constant_speed;
0072 
0073 %compute the total time for the slowest joint
0074 total_time=max(time_acc);
0075 
0076 % Trapezoidal speed profiles for each joint
0077 [input_speeds, input_accels, time]=build_trapezoidal_speed_profile(maximum_speeds, maximum_accels, total_time);
0078 
0079 
0080 
0081 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0082 % FINALLY, COMPUTE TORQUES FOR EACH MOTION STATE.
0083 % Please note that we consider that the robot is placed at a fixed position and consider
0084 % different motion situations when we change the acceleration and speed at
0085 % each joint. For each motion state, the inverse dynamic model returns the
0086 % torques at each joint that would bring the robot to that motion.
0087 %
0088 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0089 compute_inverse_dynamics(q, input_speeds, input_accels, time);
0090 
0091 
0092 fprintf('\n\nOBSERVE THE PLOTS AND NOTE DOWN THE PEAK TORQUE, NOMINAL TORQUE AND MOTOR SPEEDS')
0093 fprintf('\nNOW COMPUTE THE TORQUES FOR 5 DIFFERENT SELECTED MOTIONS STATES')
0094 fprintf('\nPRESS ANY KEY TO CONTINUE...')
0095 
0096 pause
0097 
0098 % NOW COMPUTE THE WORST CASE CONSIDERING ONLY THE SPEED AND ACCELERATION AT
0099 % 5 MOTIONS STATES
0100 input_speeds = [zeros(6,1) maximum_speeds' maximum_speeds' maximum_speeds' zeros(6,1) ];
0101 input_accels = [maximum_accels' maximum_accels' zeros(6,1) -maximum_accels' -maximum_accels' ];
0102 
0103 compute_inverse_dynamics(q, input_speeds, input_accels, [1:5]);
0104 
0105 
0106 
0107 
0108 %Computes a trapezoidal speed profile for every joint given maximum
0109 %permitted accelerations and maximum joint speeds
0110 function [input_speeds, input_accelerations, time]=build_trapezoidal_speed_profile(maximum_speeds, maximum_accels, total_time)
0111 
0112 delta_time=0.01;
0113 
0114 %build time vector: twice acceleration time plus time at constant speed
0115 time = 0:delta_time:total_time;
0116 
0117 input_speeds=[];
0118 input_accelerations=[];
0119 
0120 for j=1:length(maximum_speeds), 
0121     vel_row=[];
0122     acc_row=[];
0123     for i=1:length(time), 
0124         [vel acc] = compute_values(time(i), maximum_speeds(j), maximum_accels(j), total_time);
0125         vel_row = [vel_row vel];
0126         acc_row = [acc_row acc];        
0127     end
0128     input_speeds = [input_speeds; vel_row];
0129     input_accelerations = [input_accelerations; acc_row];    
0130 end
0131 
0132 
0133 
0134 
0135 %returns the values of velocity and speed corresponding to a given time
0136 function [vel acc]=compute_values(time_i, vel_max, acc_max, total_time)
0137 
0138 tacc = vel_max/acc_max;
0139 tdec = total_time-tacc;
0140 
0141 if time_i < tacc
0142     vel = time_i.*acc_max;
0143     acc = acc_max;
0144     return;
0145 elseif (time_i >= tacc) & (time_i < tdec)
0146     vel = vel_max;
0147     acc = 0;
0148     return;
0149 else % time_i> tdec
0150     vel = vel_max-(time_i-tdec)*acc_max;
0151     acc = -acc_max;    
0152 end
0153 
0154 
0155 
0156 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0157 %   COMPUTE THE INVERSE DYNAMICS FOR EACH MOTION STATE
0158 %
0159 %
0160 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0161 function compute_inverse_dynamics(q, input_speeds, input_accels, time)
0162 global robot
0163 
0164 %adjust_view(robot)
0165 torques=[];
0166 for j=1:length(time), 
0167     fprintf('\nComputing time %d out of %d', j, length(time));
0168     % compute the torque to bring the robot instantaneously to this motion
0169     % state. change M=1  to add the effects of a 1kg mass load at the end effector
0170     M=1;
0171     %please note that the force due to the load acts on the z axis of
0172     tau=inversedynamic(robot, q, input_speeds(:,j), input_accels(:,j), [0  0 -9.81]', [M*9.81 0 0 0 0 0]');
0173     torques=[torques tau'];
0174 end
0175 
0176 
0177 %plot trapezoidal profiles
0178 figure, hold, xlabel('time (s)'), ylabel('Input reference speeds (rad/s)')
0179 plot(time, input_speeds(1,:), time, input_speeds(2,:), time, input_speeds(3,:),...
0180         time, input_speeds(4,:), time, input_speeds(5,:), time, input_speeds(6,:));
0181 legend('Speed for joint 1 (qd1)','Speed for joint 2 (qd2)','Speed for joint 3 (qd3)',... 
0182    'Speed for joint 4 (qd4)','Speed for joint 5 (qd5)','Speed for joint 6 (qd6)' )
0183 %plot trapezoidal profiles, acceleration
0184 figure, hold, xlabel('time (s)'), ylabel('Input reference acceleration (rad/s)')
0185 plot(time, input_accels(1,:), time, input_accels(2,:), time, input_accels(3,:),...
0186         time, input_accels(4,:), time, input_accels(5,:), time, input_accels(6,:));
0187 legend('Acceleration for joint 1 (qd1)','Acceleration for joint 2 (qd2)','Acceleration for joint 3 (qd3)',... 
0188    'Acceleration for joint 4 (qd4)','Acceleration for joint 5 (qd5)','Acceleration for joint 6 (qd6)' )
0189 
0190 
0191 % plot results. First, torques at each joint
0192 figure, hold, xlabel('time (s)'), ylabel('Join Torques (N m)')
0193 plot(time, torques(1,:), time, torques(2,:), time, torques(3,:),...
0194         time, torques(4,:), time, torques(5,:), time, torques(6,:));
0195 legend('Torque for joint 1 ','Torque  for joint 2 ','Torque  for joint 3 ',... 
0196    'Torque  for joint 4','Torque  for joint 5 ','Torque  for joint 6 ' )
0197 
0198 %plot torques at each motor
0199 figure, hold, xlabel('time (s)'), ylabel('Motor Torques (N m)')
0200 plot(time, torques(1,:)/robot.motors.G(1), time, torques(2,:)/robot.motors.G(2), time, torques(3,:)/robot.motors.G(3),...
0201         time, torques(4,:)/robot.motors.G(4), time, torques(5,:)/robot.motors.G(5), time, torques(6,:)/robot.motors.G(6));
0202 legend('Torque at motor 1 ','Torque at motor 2 ','Torque at motor 3 ',... 
0203    'Torque at motor 4 ','Torque at motor 5 ','Torque at motor 6 ' )
0204 
0205 %plot power needed by the motor at each time step, without considering the
0206 %losses at the gears
0207 figure, hold, xlabel('time (s)'), ylabel('Power needed by each motor (W)')
0208 plot(time, torques(1,:).*input_speeds(1,:), time, torques(2,:).*input_speeds(2,:), time, torques(3,:).*input_speeds(3,:),...
0209         time, torques(4,:).*input_speeds(4,:), time, torques(5,:).*input_speeds(5,:), time, torques(6,:).*input_speeds(6,:));
0210 legend('Power: motor 1','Power: motor 2','Power: motor 3',... 
0211    'Power: motor 4','Power: motor 5','Power: motor 6' )
0212 
0213 %plot motor speed in rpm for each motor
0214 figure, hold, xlabel('time (s)'), ylabel('Speed in r.p.m of every motor (rev/min)')
0215 plot(time, robot.motors.G(1)*input_speeds(1,:)*30/pi, time, robot.motors.G(2)*input_speeds(2,:)*30/pi, time, robot.motors.G(3)*input_speeds(3,:)*30/pi,...
0216         time, robot.motors.G(4)*input_speeds(4,:)*30/pi, time, robot.motors.G(5)*input_speeds(5,:)*30/pi, time, robot.motors.G(6)*input_speeds(6,:)*30/pi);
0217 legend('Speed at motor 1 (qd1*G)','Speed at motor 2 (qd2*G)','Speed at motor 3 (qd3*G)',... 
0218    'Speed at motor 4 (qd4*G)','Speed at motor 5 (qd5*G)','Speed at motor 6 (qd6*G)' )
0219 
0220 
0221 
0222 %Now present results:
0223 fprintf('\nMAIN RESULTS (referred to each motor): ')
0224 fprintf('\n------------------------------------------------------------------------------------ ')
0225 fprintf('\n                         Joint 1 - Joint 2 - Joint 3 - Joint 4  - Joint 5 - Joint 6: ')
0226 fprintf('\nPeak Torque (N·m):        %.3f     %.3f     %.3f     %.3f      %.3f    %.3f ', max(abs(torques(1,:)/robot.motors.G(1))), max(abs(torques(2,:)/robot.motors.G(2))) , max(abs(torques(3,:)/robot.motors.G(3))) , max(abs(torques(4,:)/robot.motors.G(4))) , max(abs(torques(5,:)/robot.motors.G(5))) , max(abs(torques(6,:)/robot.motors.G(6))))
0227 fprintf('\nNominal Torque (N·m):     %.3f     %.3f     %.3f     %.3f      %.3f    %.3f  ', abs(torques(1,round(length(torques)/2))/robot.motors.G(1)), abs(torques(2,round(length(torques)/2))/robot.motors.G(2)), abs(torques(3,round(length(torques)/2))/robot.motors.G(3))...
0228     , abs(torques(4,round(length(torques)/2))/robot.motors.G(4)), abs(torques(5,round(length(torques)/2))/robot.motors.G(5)), abs(torques(6,round(length(torques)/2))/robot.motors.G(6)))
0229 fprintf('\nMax motor speed (r.p.m.): %.1f    %.1f    %.1f    %.1f     %.1f   %.1f  ', max(abs(robot.motors.G(1)*input_speeds(1,:))*30/pi), max(abs(robot.motors.G(2)*input_speeds(2,:))*30/pi), max(abs(robot.motors.G(3)*input_speeds(3,:))*30/pi)...
0230     ,max(abs(robot.motors.G(4)*input_speeds(4,:))*30/pi), max(abs(robot.motors.G(5)*input_speeds(5,:))*30/pi), max(abs(robot.motors.G(6)*input_speeds(6,:))*30/pi))
0231 fprintf('\n------------------------------------------------------------------------------------ ')
0232 
0233