RNE_MDH 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_MDH 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 % Copyright (C) 1995-2008, by Peter I. Corke 0036 % 0037 % This file is part of The Robotics Toolbox for Matlab (RTB). 0038 % 0039 % RTB is free software: you can redistribute it and/or modify 0040 % it under the terms of the GNU Lesser General Public License as published by 0041 % the Free Software Foundation, either version 3 of the License, or 0042 % (at your option) any later version. 0043 % 0044 % RTB is distributed in the hope that it will be useful, 0045 % but WITHOUT ANY WARRANTY; without even the implied warranty of 0046 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 0047 % GNU Lesser General Public License for more details. 0048 % 0049 % You should have received a copy of the GNU Leser General Public License 0050 % along with RTB. If not, see <http://www.gnu.org/licenses/>. 0051 0052 function tau = rne_mdh(robot, a1, a2, a3, a4, a5) 0053 0054 z0 = [0;0;1]; 0055 grav = robot.gravity; % default gravity from the object 0056 fext = zeros(6, 1); 0057 0058 % Set debug to: 0059 % 0 no messages 0060 % 1 display results of forward and backward recursions 0061 % 2 display print R and p* 0062 debug = 0; 0063 0064 n = robot.n; 0065 if numcols(a1) == 3*n, 0066 Q = a1(:,1:n); 0067 Qd = a1(:,n+1:2*n); 0068 Qdd = a1(:,2*n+1:3*n); 0069 np = numrows(Q); 0070 if nargin >= 3, 0071 grav = a2(:); 0072 end 0073 if nargin == 4, 0074 fext = a3; 0075 end 0076 else 0077 np = numrows(a1); 0078 Q = a1; 0079 Qd = a2; 0080 Qdd = a3; 0081 if numcols(a1) ~= n | numcols(Qd) ~= n | numcols(Qdd) ~= n | ... 0082 numrows(Qd) ~= np | numrows(Qdd) ~= np, 0083 error('bad data'); 0084 end 0085 if nargin >= 5, 0086 grav = a4(:); 0087 end 0088 if nargin == 6, 0089 fext = a5; 0090 end 0091 end 0092 0093 tau = zeros(np,n); 0094 0095 for p=1:np, 0096 q = Q(p,:)'; 0097 qd = Qd(p,:)'; 0098 qdd = Qdd(p,:)'; 0099 0100 Fm = []; 0101 Nm = []; 0102 pstarm = []; 0103 Rm = []; 0104 w = zeros(3,1); 0105 wd = zeros(3,1); 0106 v = zeros(3,1); 0107 vd = grav(:); 0108 0109 % 0110 % init some variables, compute the link rotation matrices 0111 % 0112 for j=1:n, 0113 link = robot.link{j}; 0114 Tj = link(q(j)); 0115 if link.RP == 'R', 0116 D = link.D; 0117 else 0118 D = q(j); 0119 end 0120 alpha = link.alpha; 0121 pm = [link.A; -D*sin(alpha); D*cos(alpha)]; % (i-1) P i 0122 if j == 1, 0123 pm = t2r(robot.base) * pm; 0124 Tj = robot.base * Tj; 0125 end 0126 Pm(:,j) = pm; 0127 Rm{j} = t2r(Tj); 0128 if debug>1, 0129 Rm{j} 0130 Pm(:,j)' 0131 end 0132 end 0133 0134 % 0135 % the forward recursion 0136 % 0137 for j=1:n, 0138 link = robot.link{j}; 0139 0140 R = Rm{j}'; % transpose!! 0141 P = Pm(:,j); 0142 Pc = link.r; 0143 0144 % 0145 % trailing underscore means new value 0146 % 0147 if link.RP == 'R', 0148 % revolute axis 0149 w_ = R*w + z0*qd(j); 0150 wd_ = R*wd + cross(R*w,z0*qd(j)) + z0*qdd(j); 0151 %v = cross(w,P) + R*v; 0152 vd_ = R * (cross(wd,P) + ... 0153 cross(w, cross(w,P)) + vd); 0154 0155 else 0156 % prismatic axis 0157 w_ = R*w; 0158 wd_ = R*wd; 0159 %v = R*(z0*qd(j) + v) + cross(w,P); 0160 vd_ = R*(cross(wd,P) + ... 0161 cross(w, cross(w,P)) + vd ... 0162 ) + 2*cross(R*w,z0*qd(j)) + z0*qdd(j); 0163 end 0164 % update variables 0165 w = w_; 0166 wd = wd_; 0167 vd = vd_; 0168 0169 vdC = cross(wd,Pc) + ... 0170 cross(w,cross(w,Pc)) + vd; 0171 F = link.m*vdC; 0172 N = link.I*wd + cross(w,link.I*w); 0173 Fm = [Fm F]; 0174 Nm = [Nm N]; 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 ', vdC) 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 0194 % 0195 % order of these statements is important, since both 0196 % nn and f are functions of previous f. 0197 % 0198 link = robot.link{j}; 0199 0200 if j == n, 0201 R = eye(3,3); 0202 P = [0;0;0]; 0203 else 0204 R = Rm{j+1}; 0205 P = Pm(:,j+1); % i/P/(i+1) 0206 end 0207 Pc = link.r; 0208 0209 f_ = R*f + Fm(:,j); 0210 nn_ = Nm(:,j) + R*nn + cross(Pc,Fm(:,j)) + ... 0211 cross(P,R*f); 0212 0213 f = f_; 0214 nn = nn_; 0215 0216 if debug, 0217 fprintf('f: '); fprintf('%.3f ', f) 0218 fprintf('\nn: '); fprintf('%.3f ', nn) 0219 fprintf('\n'); 0220 end 0221 if link.RP == 'R', 0222 % revolute 0223 tau(p,j) = nn'*z0 + ... 0224 link.G^2 * link.Jm*qdd(j) + ... 0225 link.G * friction(link, qd(j)); 0226 else 0227 % prismatic 0228 tau(p,j) = f'*z0 + ... 0229 link.G^2 * link.Jm*qdd(j) + ... 0230 link.G * friction(link, qd(j)); 0231 end 0232 end 0233 end