inversedynamics_puma560

 SCRIPT TO TEST THE DYNAMICS OF THE PUMA 560 ROBOT

 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 %
0002 % SCRIPT TO TEST THE DYNAMICS OF THE PUMA 560 ROBOT
0003 %
0004 % Copyright (C) 2012, by Arturo Gil Aparicio
0005 %
0006 % This file is part of ARTE (A Robotics Toolbox for Education).
0007 %
0008 % ARTE is free software: you can redistribute it and/or modify
0009 % it under the terms of the GNU Lesser General Public License as published by
0010 % the Free Software Foundation, either version 3 of the License, or
0011 % (at your option) any later version.
0012 %
0013 % ARTE is distributed in the hope that it will be useful,
0014 % but WITHOUT ANY WARRANTY; without even the implied warranty of
0015 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
0016 % GNU Lesser General Public License for more details.
0017 %
0018 % You should have received a copy of the GNU Leser General Public License
0019 % along with ARTE.  If not, see <http://www.gnu.org/licenses/>.
0020 
0021 close all
0022 %general call to inverse dynamic function
0023 %TAU = inversedynamic(robot, Q, QD, QDD, GRAV, FEXT)
0024 
0025 % uncomment the following lines to simulate the stanford arm
0026 % other arms can be simulated similarly
0027 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0028 % three different poses, please find which one is the worst
0029 % pose for the first joint, the second joint etc.
0030 % q1 = [0 0 0 0 0 0];
0031 % q2 = [0 pi/2 0.5 0 0 0];
0032 % q3 = [0 0 -0.5 0 0 0];
0033 %
0034 % %load robot parameters
0035 % robot=load_robot('stanford', '');
0036 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0037 
0038 
0039 % three different poses, please find which one is the worst
0040 % pose for the first joint, the second joint etc.
0041 q1 = [0 0 0 0 0 0];    
0042 q2 = [0 pi/2 -pi/2 0 0 0];    
0043 q3 = [0 0 -pi/2 0 0 0];
0044 
0045 
0046 %load robot parameters
0047 robot=load_robot('unimate', 'puma560');
0048 
0049 adjust_view(robot)
0050 robot.dynamics.friction=1;
0051 
0052 %Now compute torques at each pose
0053 fprintf('\nTorques at each joint given position q1, zero speed and acceleration, standard gravity acting on Z0')
0054 fprintf('\nComputing static torques at position q1 due to gravity [0 0 9.81]')
0055 tau = inversedynamic(robot, q1, [0 0 0 0 0 0], [0 0 0 0 0 0], [0  0 9.81]', [0 0 0 0 0 0]')
0056 drawrobot3d(robot,q1)
0057 disp('press any key to continue')
0058 pause
0059 
0060 
0061 fprintf('\nTorques at each joint given position q2, zero speed and acceleration, standard gravity acting on Z0')
0062 fprintf('\nComputing static torques at position q2 due to gravity [0 0 9.81]');
0063 fprintf('\nPLEASE note the differences with respect to torque tau_2 at q1');
0064 tau = inversedynamic(robot, q2, [0 0 0 0 0 0], [0 0 0 0 0 0], [0  0 9.81]', [0 0 0 0 0 0]')
0065 drawrobot3d(robot,q2)
0066 disp('press any key to continue')
0067 pause
0068 
0069 
0070 fprintf('\nTorques at each joint given position q3, zero speed and acceleration, standard gravity acting on Z0')
0071 fprintf('\nComputing static torques at position q3 due to gravity [0 0 9.81]');
0072 fprintf('\nPLEASE note the differences with respect to torque tau_2 at q1');
0073 tau = inversedynamic(robot, q3, [0 0 0 0 0 0], [0 0 0 0 0 0], [0  0 9.81]', [0 0 0 0 0 0]')
0074 drawrobot3d(robot,q3)
0075 disp('press any key to continue')
0076 pause
0077 
0078 
0079 fprintf('\nTorques at each joint given position q3, zero speed and acceleration, standard gravity acting on Z0')
0080 fprintf('\nComputing static torques at position q3 due to a 5 Kg load at end effector');
0081 fprintf('\nPLEASE note the differences with respect to torque tau_2 at q1');
0082 M = 5 %Kg
0083 tau = inversedynamic(robot, q3, [0 0 0 0 0 0], [0 0 0 0 0 0], [0  0 M*9.81]', [0 0 0 0 0 0]')
0084 drawrobot3d(robot,q3)
0085 disp('press any key to continue')
0086 pause
0087 
0088 
0089 %NOW TEST THE DYNAMICS WITH A FORCE APPLIED AT THE END EFFECTOR
0090 fprintf('\nTorques at each joint given position q3, zero speed and acceleration, standard gravity acting on Z0')
0091 fprintf('\nComputing static torques at position q3 due to gravity and 1 Newton applied at Y6');
0092 fprintf('\nPLEASE compare the result with the previous case');
0093 tau = inversedynamic(robot, q3, [0 0 0 0 0 0], [0 0 0 0 0 0], [0  0 9.81]', [0 1 0 0 0 0]')
0094 disp('press any key to continue')
0095 pause
0096 
0097 
0098 %NOW TEST THE DYNAMICS WITH A GENERAL CASE
0099 fprintf('\nTorques at each joint given position q3, general movement')
0100 fprintf('\nPLEASE compare the result with the previous cases');
0101 tau = inversedynamic(robot, q3, [1 1 1 1 1 1], [1 1 1 1 1 1], [0 0 9.81]', [1 1 1 1 1 1]')
0102 disp('press any key to continue')
0103 pause
0104 
0105 
0106 %FINALLY, CONSIDER THE CASE IN WHICH THE MANIPULATOR IS HANGING FROM THE CEILING
0107 fprintf('\nTorques at each joint given position q3, general movement')
0108 fprintf('\nPLEASE compare the result with the previous cases');
0109 tau = inversedynamic(robot, q3, [0 0 0 0 0 0], [0 0 0 0 0 0], [0  0 -9.81]', [0 0 0 0 0 0]')
0110 
0111 %the robot is now hanging, Z0 pointing downwards
0112 robot.T0=[-1 0 0 0;
0113           0 1 0 0;
0114           0 0 -1 1;
0115           0 0 0 1];
0116 
0117 drawrobot3d(robot,q3)
0118 disp('press any key to continue')
0119 pause