SCRIPT TEST FOR THE 3 DOF planar manipulator
0001 % SCRIPT TEST FOR THE 3 DOF planar manipulator 0002 0003 % Copyright (C) 2012, by Arturo Gil Aparicio 0004 % 0005 % This file is part of ARTE (A Robotics Toolbox for Education). 0006 % 0007 % ARTE is free software: you can redistribute it and/or modify 0008 % it under the terms of the GNU Lesser General Public License as published by 0009 % the Free Software Foundation, either version 3 of the License, or 0010 % (at your option) any later version. 0011 % 0012 % ARTE is distributed in the hope that it will be useful, 0013 % but WITHOUT ANY WARRANTY; without even the implied warranty of 0014 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 0015 % GNU Lesser General Public License for more details. 0016 % 0017 % You should have received a copy of the GNU Leser General Public License 0018 % along with ARTE. If not, see <http://www.gnu.org/licenses/>. 0019 0020 close all 0021 0022 %general call to inverse dynamic function 0023 %TAU = inversedynamic(param, Q, QD, QDD, GRAV, FEXT) 0024 0025 %three different poses 0026 q1 = [0, 0, 0]; 0027 q2 = [0, pi/2, 0]; 0028 0029 0030 %load 3dof planar robot 0031 %the parameters are loaded from robots/example/3dofplanar/parameters.m 0032 planar=load_robot('example', '3dofplanar'); 0033 0034 fprintf('\nTorques at each joint given position = [0 0 0], and velocity=[0 0 0] and standard gravity acting on Z0') 0035 %Please note that the forces and moments are specified with respect to the 0036 %X3 Y3 Z3 reference system. 0037 %In this mechanism, forces acting on Fx3 or Fz3 do not account for tau, nor 0038 %moments acting on My3 or Mx3 0039 tau = inversedynamic(planar, q1, [0 0 0], [0 0 0], [0 0 9.81]', [1 0 1 1 1 0]') 0040 0041 %draw the robot at this position 0042 drawrobot3d(planar,q1) 0043 disp('press any key to continue') 0044 pause 0045 0046 %Now, add a force on Fy, and observe the results on tau 0047 tau = inversedynamic(planar, q1, [0 0 0], [0 0 0], [0 0 9.81]', [0 1 0 0 0 0]') 0048 %draw the robot at this position 0049 drawrobot3d(planar,q1) 0050 disp('press any key to continue') 0051 pause 0052 0053 0054 %torques necessary to instantaneously bring the arm to the specified state 0055 fprintf('\nTorques at each joint given position = [0 pi/2 0], and velocity=[1 1 1] and acceleration = [1 1 1] and gravity acting on Z0') 0056 tau = inversedynamic(planar, q2, [1 1 1], [1 1 1], [0 0 9.81]', [0 0 0 0 0 0]') 0057 0058 %draw the robot 0059 drawrobot3d(planar,q2) 0060 disp('press any key to continue') 0061 pause 0062 0063 0064 %In this case, all torques are caused by gravity acting on the Center Of 0065 %Mass of each link. Note that we have changed the direction of the gravity 0066 %vector g 0067 fprintf('\nTorques at each joint given position = [0 0 0], and velocity=[0 0 0] and acceleration = [0 0 0] and gravity acting on Y0') 0068 tau = inversedynamic(planar, q1, [0 0 0], [0 0 0], [0 9.81 0]', [0 0 0 0 0 0]') 0069 0070 %draw the robot 0071 drawrobot3d(planar,q1) 0072 0073