%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
TAU= INVERSEDYNAMIC: Compute inverse dynamics via recursive Newton-Euler
formulation. TAU = INVERSEDYNAMIC(Q, QD, QDD, GRAV, FEXT) computes the
required torques (N�m) and forces (N) required to instantaneally bring
the arm to the specified state given by positions Q, speed QD,
acceleration QDD. The gravity is expressed in the base coordinate
system, typically defined as GRAV = [0 0 9.81]'. In addition, FEXT
is a 6-vector [Fx Fy Fz Mx My Mz] defining the forces and moments
externally applied to the end-effector (in coordinates of the end effector's n-th DH
system).
Example of use:
q=[0 0 0 0 0 0]
qd=[0 0 0 0 0 0]
qdd = [0 0 0 0 0 0]
g=[0 0 9.81]';
fext = [0 0 0 0 0 0]';
puma560=load_robot('puma', '560');
tau = inversedynamic(puma560, q, qd, qdd, g, fext)
where: q is the position of the arm, qd the joint velocities and qdd
the accelerations. The vector g defines the gravity in the base reference
system, whereas the vector fext = [fx fy fz Mx My Mz] defines the
forces and moments acting on the end effector's reference system.
Author: Arturo Gil Aparicio, arturo.gil@umh.es
Bibliography: The algorithm has been implemented as is "ROBOT ANALYSIS. The mechanics of Serial and Parallel
manipulators". Lung Weng Tsai. John Wiley and Sons, inc. ISBN:
0-471-32593-7. pages: 386--391.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%0001 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0002 % TAU= INVERSEDYNAMIC: Compute inverse dynamics via recursive Newton-Euler 0003 % formulation. TAU = INVERSEDYNAMIC(Q, QD, QDD, GRAV, FEXT) computes the 0004 % required torques (N�m) and forces (N) required to instantaneally bring 0005 % the arm to the specified state given by positions Q, speed QD, 0006 % acceleration QDD. The gravity is expressed in the base coordinate 0007 % system, typically defined as GRAV = [0 0 9.81]'. In addition, FEXT 0008 % is a 6-vector [Fx Fy Fz Mx My Mz] defining the forces and moments 0009 % externally applied to the end-effector (in coordinates of the end effector's n-th DH 0010 % system). 0011 % 0012 % Example of use: 0013 % q=[0 0 0 0 0 0] 0014 % qd=[0 0 0 0 0 0] 0015 % qdd = [0 0 0 0 0 0] 0016 % g=[0 0 9.81]'; 0017 % fext = [0 0 0 0 0 0]'; 0018 % puma560=load_robot('puma', '560'); 0019 % tau = inversedynamic(puma560, q, qd, qdd, g, fext) 0020 % where: q is the position of the arm, qd the joint velocities and qdd 0021 % the accelerations. The vector g defines the gravity in the base reference 0022 % system, whereas the vector fext = [fx fy fz Mx My Mz] defines the 0023 % forces and moments acting on the end effector's reference system. 0024 % 0025 % Author: Arturo Gil Aparicio, arturo.gil@umh.es 0026 % 0027 % Bibliography: The algorithm has been implemented as is "ROBOT ANALYSIS. The mechanics of Serial and Parallel 0028 % manipulators". Lung Weng Tsai. John Wiley and Sons, inc. ISBN: 0029 % 0-471-32593-7. pages: 386--391. 0030 % 0031 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0032 0033 % Copyright (C) 2012, by Arturo Gil Aparicio 0034 % 0035 % This file is part of ARTE (A Robotics Toolbox for Education). 0036 % 0037 % ARTE is free software: you can redistribute it and/or modify 0038 % it under the terms of the GNU Lesser General Public License as published by 0039 % the Free Software Foundation, either version 3 of the License, or 0040 % (at your option) any later version. 0041 % 0042 % ARTE is distributed in the hope that it will be useful, 0043 % but WITHOUT ANY WARRANTY; without even the implied warranty of 0044 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 0045 % GNU Lesser General Public License for more details. 0046 % 0047 % You should have received a copy of the GNU Leser General Public License 0048 % along with ARTE. If not, see <http://www.gnu.org/licenses/>. 0049 function tau = inversedynamic(robot, q, qd, qdd, grav, fext) 0050 tau = zeros(1,robot.DOF); 0051 0052 if robot.debug 0053 fprintf('\nComputing inverse dinamics for the %s robot', robot.name); 0054 end 0055 0056 %if the robot has not dynamic parameters, exit 0057 if ~robot.has_dynamics 0058 fprintf('\nRobot %s has not dynamic parameters set. Exiting', robot.name); 0059 return; 0060 end 0061 0062 z0 = [0;0;1]; 0063 0064 %evaluate robot parameter DH table for current variable q 0065 theta = eval(robot.DH.theta); 0066 d = eval(robot.DH.d); 0067 a = eval(robot.DH.a); 0068 alfa = eval(robot.DH.alpha); 0069 n=length(theta); 0070 0071 m = robot.dynamics.masses; % column vector with the mass of every link 0072 r_com = robot.dynamics.r_com; % position of the COM of each link, one row per link 0073 0074 vdi = grav; %linear acceleration, initially, consider only the gravity 0075 wi = zeros(3,1); %angluar speed 0076 wdi = zeros(3,1); %angular acceleration 0077 %vi = zeros(3,1); %linear velocity 0078 0079 %Store forces and moments at the COM of each link 0080 F_com = []; 0081 N_com = []; 0082 0083 %First compute motion forward from robot base 0084 % to the last link 0085 for j=1:n, 0086 A=dh(theta(j), d(j), a(j), alfa(j)); 0087 0088 R = A(1:3,1:3)'; %Note: compute the inverse by transpose 0089 %position vector of the origin of the ith link frame with respect to 0090 %the (i-i)th link frame 0091 ri = [a(j); d(j)*sin(alfa(j)); d(j)*cos(alfa(j))]; 0092 0093 if robot.kind(j) == 'R', %rotational axis 0094 wdi = R*(wdi + z0*qdd(j) + cross(wi,z0*qd(j))); 0095 wi = R*(wi + z0*qd(j)); 0096 vdi = R*vdi + cross(wdi,ri) + cross(wi, cross(wi,ri)); 0097 else% prismatic axis 0098 wi = R*wi; 0099 wdi = R*wdi; 0100 vdi = R*(vdi + z0*qdd(j)) + cross(wdi,ri) + cross(wi, cross(wi,ri)) + 2*cross(wi,R*z0*qd(j)); 0101 end 0102 0103 %Inertia matrix 0104 J = [robot.dynamics.Inertia(j,1) robot.dynamics.Inertia(j,4) robot.dynamics.Inertia(j,6); 0105 robot.dynamics.Inertia(j,4) robot.dynamics.Inertia(j,2) robot.dynamics.Inertia(j,5); 0106 robot.dynamics.Inertia(j,6) robot.dynamics.Inertia(j,5) robot.dynamics.Inertia(j,3) ]; 0107 %acceleration of the center of mass of link i 0108 v_comi = vdi + cross(wdi,r_com(j,:)') + cross(wi,cross(wi,r_com(j,:)')); 0109 % Force as Newton's law F=m*a of COM 0110 F = m(j)*v_comi; 0111 N = J*wdi + cross(wi,J*wi); 0112 F_com = [F_com F]; 0113 N_com = [N_com N]; 0114 end 0115 0116 % backward computations 0117 fi = fext(1:3); % forces expressed in the last reference system 0118 ni = fext(4:6); %moments at the last reference system. 0119 0120 for j=n:-1:1, 0121 ri = [a(j); d(j)*sin(alfa(j)); d(j)*cos(alfa(j))]; 0122 0123 if j == n 0124 R=eye(3,3); 0125 else 0126 A=dh(theta(j+1), d(j+1), a(j+1), alfa(j+1)); 0127 R=A(1:3,1:3); 0128 end 0129 ni = R*(ni + cross(R'*ri,fi)) + cross(ri+r_com(j,:)',F_com(:,j)) + N_com(:,j); 0130 fi = R*fi + F_com(:,j); 0131 0132 A=dh(theta(j), d(j), a(j), alfa(j)); 0133 R = A(1:3,1:3); 0134 0135 if robot.kind(j) == 'R', %rotational joint 0136 tau(1,j) = ni'*(R'*z0) + robot.motors.G(j)^2*robot.motors.Inertia(j)*qdd(j); 0137 else % prismatic joint 0138 tau(1,j) = fi'*(R'*z0) + robot.motors.G(j)^2*robot.motors.Inertia(j)*qdd(j); 0139 end 0140 %add viscous friction in this case 0141 if robot.dynamics.friction 0142 %NOte: we account for two types of friction: viscous and coulomb 0143 tau(1,j) = tau(1,j) + friction(robot, qd, j); 0144 end 0145 end