RNE_DH Compute inverse dynamics via recursive Newton-Euler formulation TAU = RNE(ROBOT, Q, QD, QDD) TAU = RNE(ROBOT, [Q QD QDD]) Returns the joint torque required to achieve the specified joint position, velocity and acceleration state. Gravity vector is an attribute of the robot object but this may be overriden by providing a gravity acceleration vector [gx gy gz]. TAU = RNE(ROBOT, Q, QD, QDD, GRAV) TAU = RNE(ROBOT, [Q QD QDD], GRAV) An external force/moment acting on the end of the manipulator may also be specified by a 6-element vector [Fx Fy Fz Mx My Mz]. TAU = RNE(ROBOT, Q, QD, QDD, GRAV, FEXT) TAU = RNE(ROBOT, [Q QD QDD], GRAV, FEXT) where Q, QD and QDD are row vectors of the manipulator state; pos, vel, and accel. The torque computed also contains a contribution due to armature inertia. RNE can be either an M-file or a MEX-file. See the manual for details on how to configure the MEX-file. The M-file is a wrapper which calls either RNE_DH or RNE_MDH depending on the kinematic conventions used by the robot object. See also: ROBOT, ACCEL, GRAVLOAD, INERTIA.
0001 %RNE_DH Compute inverse dynamics via recursive Newton-Euler formulation 0002 % 0003 % TAU = RNE(ROBOT, Q, QD, QDD) 0004 % TAU = RNE(ROBOT, [Q QD QDD]) 0005 % 0006 % Returns the joint torque required to achieve the specified joint position, 0007 % velocity and acceleration state. 0008 % 0009 % Gravity vector is an attribute of the robot object but this may be 0010 % overriden by providing a gravity acceleration vector [gx gy gz]. 0011 % 0012 % TAU = RNE(ROBOT, Q, QD, QDD, GRAV) 0013 % TAU = RNE(ROBOT, [Q QD QDD], GRAV) 0014 % 0015 % An external force/moment acting on the end of the manipulator may also be 0016 % specified by a 6-element vector [Fx Fy Fz Mx My Mz]. 0017 % 0018 % TAU = RNE(ROBOT, Q, QD, QDD, GRAV, FEXT) 0019 % TAU = RNE(ROBOT, [Q QD QDD], GRAV, FEXT) 0020 % 0021 % where Q, QD and QDD are row vectors of the manipulator state; pos, vel, 0022 % and accel. 0023 % 0024 % The torque computed also contains a contribution due to armature 0025 % inertia. 0026 % 0027 % RNE can be either an M-file or a MEX-file. See the manual for details on 0028 % how to configure the MEX-file. The M-file is a wrapper which calls either 0029 % RNE_DH or RNE_MDH depending on the kinematic conventions used by the robot 0030 % object. 0031 % 0032 % See also: ROBOT, ACCEL, GRAVLOAD, INERTIA. 0033 0034 % 0035 % verified against MAPLE code, which is verified by examples 0036 % 0037 0038 % Copyright (C) 1992-2008, by Peter I. Corke 0039 % 0040 % This file is part of The Robotics Toolbox for Matlab (RTB). 0041 % 0042 % RTB is free software: you can redistribute it and/or modify 0043 % it under the terms of the GNU Lesser General Public License as published by 0044 % the Free Software Foundation, either version 3 of the License, or 0045 % (at your option) any later version. 0046 % 0047 % RTB is distributed in the hope that it will be useful, 0048 % but WITHOUT ANY WARRANTY; without even the implied warranty of 0049 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 0050 % GNU Lesser General Public License for more details. 0051 % 0052 % You should have received a copy of the GNU Leser General Public License 0053 % along with RTB. If not, see <http://www.gnu.org/licenses/>. 0054 function tau = rne_dh(robot, a1, a2, a3, a4, a5) 0055 0056 z0 = [0;0;1]; 0057 grav = robot.gravity; % default gravity from the object 0058 fext = zeros(6, 1); 0059 0060 % Set debug to: 0061 % 0 no messages 0062 % 1 display results of forward and backward recursions 0063 % 2 display print R and p* 0064 debug = 0; 0065 0066 n = robot.n; 0067 if numcols(a1) == 3*n, 0068 Q = a1(:,1:n); 0069 Qd = a1(:,n+1:2*n); 0070 Qdd = a1(:,2*n+1:3*n); 0071 np = numrows(Q); 0072 if nargin >= 3, 0073 grav = a2(:); 0074 end 0075 if nargin == 4, 0076 fext = a3; 0077 end 0078 else 0079 np = numrows(a1); 0080 Q = a1; 0081 Qd = a2; 0082 Qdd = a3; 0083 if numcols(a1) ~= n | numcols(Qd) ~= n | numcols(Qdd) ~= n | ... 0084 numrows(Qd) ~= np | numrows(Qdd) ~= np, 0085 error('bad data'); 0086 end 0087 if nargin >= 5, 0088 grav = a4(:); 0089 end 0090 if nargin == 6, 0091 fext = a5; 0092 end 0093 end 0094 0095 tau = zeros(np,n); 0096 0097 for p=1:np, 0098 q = Q(p,:)'; 0099 qd = Qd(p,:)'; 0100 qdd = Qdd(p,:)'; 0101 0102 Fm = []; 0103 Nm = []; 0104 pstarm = []; 0105 Rm = []; 0106 w = zeros(3,1); 0107 wd = zeros(3,1); 0108 v = zeros(3,1); 0109 vd = grav(:); 0110 0111 % 0112 % init some variables, compute the link rotation matrices 0113 % 0114 for j=1:n, 0115 link = robot.link{j}; 0116 Tj = link(q(j)); 0117 if link.RP == 'R', 0118 D = link.D; 0119 else 0120 D = q(j); 0121 end 0122 alpha = link.alpha; 0123 pstar = [link.A; D*sin(alpha); D*cos(alpha)]; 0124 if j == 1, 0125 pstar = t2r(robot.base) * pstar; 0126 Tj = robot.base * Tj; 0127 end 0128 pstarm(:,j) = pstar; 0129 Rm{j} = t2r(Tj); 0130 if debug>1, 0131 Rm{j} 0132 Pstarm(:,j)' 0133 end 0134 end 0135 0136 % 0137 % the forward recursion 0138 % 0139 for j=1:n, 0140 link = robot.link{j}; 0141 0142 Rt = Rm{j}'; % transpose!! 0143 pstar = pstarm(:,j); 0144 r = link.r; 0145 0146 % 0147 % statement order is important here 0148 % 0149 if link.RP == 'R', 0150 % revolute axis 0151 wd = Rt*(wd + z0*qdd(j) + ... 0152 cross(w,z0*qd(j))); 0153 w = Rt*(w + z0*qd(j)); 0154 %v = cross(w,pstar) + Rt*v; 0155 vd = cross(wd,pstar) + ... 0156 cross(w, cross(w,pstar)) +Rt*vd; 0157 0158 else 0159 % prismatic axis 0160 w = Rt*w; 0161 wd = Rt*wd; 0162 vd = Rt*(z0*qdd(j)+vd) + ... 0163 cross(wd,pstar) + ... 0164 2*cross(w,Rt*z0*qd(j)) +... 0165 cross(w, cross(w,pstar)); 0166 end 0167 0168 vhat = cross(wd,r) + ... 0169 cross(w,cross(w,r)) + vd; 0170 F = link.m*vhat; 0171 N = link.I*wd + cross(w,link.I*w); 0172 Fm = [Fm F]; 0173 Nm = [Nm N]; 0174 0175 if debug, 0176 fprintf('w: '); fprintf('%.3f ', w) 0177 fprintf('\nwd: '); fprintf('%.3f ', wd) 0178 fprintf('\nvd: '); fprintf('%.3f ', vd) 0179 fprintf('\nvdbar: '); fprintf('%.3f ', vhat) 0180 fprintf('\n'); 0181 end 0182 end 0183 0184 % 0185 % the backward recursion 0186 % 0187 0188 fext = fext(:); 0189 f = fext(1:3); % force/moments on end of arm 0190 nn = fext(4:6); 0191 0192 for j=n:-1:1, 0193 link = robot.link{j}; 0194 pstar = pstarm(:,j); 0195 0196 % 0197 % order of these statements is important, since both 0198 % nn and f are functions of previous f. 0199 % 0200 if j == n, 0201 R = eye(3,3); 0202 else 0203 R = Rm{j+1}; 0204 end 0205 r = link.r; 0206 nn = R*(nn + cross(R'*pstar,f)) + ... 0207 cross(pstar+r,Fm(:,j)) + ... 0208 Nm(:,j); 0209 f = R*f + Fm(:,j); 0210 if debug, 0211 fprintf('f: '); fprintf('%.3f ', f) 0212 fprintf('\nn: '); fprintf('%.3f ', nn) 0213 fprintf('\n'); 0214 end 0215 0216 R = Rm{j}; 0217 if link.RP == 'R', 0218 % revolute 0219 tau(p,j) = nn'*(R'*z0) + ... 0220 link.G^2 * link.Jm*qdd(j) + ... 0221 link.G * friction(link, qd(j)); 0222 else 0223 % prismatic 0224 tau(p,j) = f'*(R'*z0) + ... 0225 link.G^2 * link.Jm*qdd(j) + ... 0226 link.G * friction(link, qd(j)); 0227 end 0228 end 0229 end