SCRIPT TEST FOR THE 2DOF arm
0001 % SCRIPT TEST FOR THE 2DOF arm 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 q1 = [0 0]; 0023 q2 = [0 pi/2]; 0024 0025 0026 0027 %general call to inverse dynamic function 0028 %TAU = inversedynamic(param, Q, QD, QDD, GRAV, FEXT) 0029 0030 %load example arm 0031 planar=load_robot('example', '2dofplanar'); 0032 0033 0034 %Tau is cero at any joint at the presented position. 0035 fprintf('\nTorques at each joint given position = [0 0], and velocity=[0 0] and standard gravity acting on Z0') 0036 tau = inversedynamic(planar, q1, [0 0], [0 0], [0 0 9.81]', [0 0 0 0 0 0]') 0037 0038 drawrobot3d(planar,q1) 0039 disp('press any key to continue') 0040 pause 0041 0042 0043 %now, change the direction of gravity, which is equivalent to change the 0044 %mounting position of the arm. All torques appear as a consequence of a 0045 %gravitational load acting on the COM of each link 0046 fprintf('\nTorques at each joint given position = [0 0], and velocity=[0 0] and standard gravity acting on Y0') 0047 tau = inversedynamic(planar, q1, [0 0], [0 0], [ 0 -9.81 0]', [0 0 0 0 0 0]') 0048 disp('press any key to continue') 0049 pause 0050 0051 0052 %Now, apply forces expressed at the X2 Y2 Z2 reference system. 0053 %Please note that forces acting on X2, Z2 and moments acting on My2 and Mx2 0054 %do not contribute to the torques at each joint. Please add forces in Y2 0055 %direction and moments acting on Z2 direction 0056 fprintf('\nTorques at each joint given position = [0 0], and velocity=[0 0] and standard gravity acting on Z0, forces expressed on X2Y2Z2 [1 0 1 1 1 0]') 0057 tau = inversedynamic(planar, q1, [0 0], [ 0 0], [0 0 9.81]', [1 0 1 1 1 0]') 0058 disp('press any key to continue') 0059 pause 0060 0061 0062 0063 %Now, compute torques necessary to bring the arm to a general kinetic state 0064 fprintf('\nTorques at each joint given position = [0 0], and velocity=[0 0] and standard gravity acting on Z0, forces expressed on X2Y2Z2 [0 1 0 0 0 1]') 0065 tau = inversedynamic(planar, q2, [1 1], [1 1], [0 0 9.81]', [0 1 0 0 0 1]') 0066 0067 drawrobot3d(planar,q2) 0068 disp('press any key to continue') 0069 pause 0070 0071