RNE 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. See also ROBOT, FROBOT, ACCEL, GRAVLOAD, INERTIA. Should be a MEX file.
0001 %RNE 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, and accel. 0022 % 0023 % The torque computed also contains a contribution due to armature 0024 % inertia. 0025 % 0026 % See also ROBOT, FROBOT, ACCEL, GRAVLOAD, INERTIA. 0027 % 0028 % Should be a MEX file. 0029 0030 % 0031 % verified against MAPLE code, which is verified by examples 0032 % 0033 0034 % Copyright (C) 1992-2008, by Peter I. Corke 0035 % 0036 % This file is part of The Robotics Toolbox for Matlab (RTB). 0037 % 0038 % RTB is free software: you can redistribute it and/or modify 0039 % it under the terms of the GNU Lesser General Public License as published by 0040 % the Free Software Foundation, either version 3 of the License, or 0041 % (at your option) any later version. 0042 % 0043 % RTB is distributed in the hope that it will be useful, 0044 % but WITHOUT ANY WARRANTY; without even the implied warranty of 0045 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 0046 % GNU Lesser General Public License for more details. 0047 % 0048 % You should have received a copy of the GNU Leser General Public License 0049 % along with RTB. If not, see <http://www.gnu.org/licenses/>. 0050 0051 function tau = rne(robot, a1, a2, a3, a4, a5) 0052 if robot.mdh ~= 0, 0053 error('rne only valid for standard D&H parameters') 0054 end 0055 888 0056 z0 = [0;0;1]; 0057 grav = robot.gravity; % default gravity from the object 0058 fext = zeros(6, 1); 0059 0060 n = robot.n; 0061 if numcols(a1) == 3*n, 0062 Q = a1(:,1:n); 0063 Qd = a1(:,n+1:2*n); 0064 Qdd = a1(:,2*n+1:3*n); 0065 np = numrows(Q); 0066 if nargin >= 3, 0067 grav = a2; 0068 end 0069 if nargin == 4, 0070 fext = a3; 0071 end 0072 else 0073 np = numrows(a1); 0074 Q = a1; 0075 Qd = a2; 0076 Qdd = a3; 0077 if numcols(a1) ~= n | numcols(Qd) ~= n | numcols(Qdd) ~= n | ... 0078 numrows(Qd) ~= np | numrows(Qdd) ~= np, 0079 error('bad data'); 0080 end 0081 if nargin >= 5, 0082 grav = a4; 0083 end 0084 if nargin == 6, 0085 fext = a5; 0086 end 0087 end 0088 0089 tau = zeros(np,n); 0090 0091 for p=1:np, % for all points on path 0092 q = Q(p,:)'; 0093 qd = Qd(p,:)'; 0094 qdd = Qdd(p,:)'; 0095 0096 Fm = []; 0097 Nm = []; 0098 pstarm = []; 0099 Rm = []; 0100 w = zeros(3,1); 0101 wd = zeros(3,1); 0102 v = zeros(3,1); 0103 vd = grav; 0104 0105 % 0106 % init some variables, compute the link rotation matrices 0107 % 0108 for j=1:n, 0109 link = robot.link{j}; 0110 Tj = link(q(j)); 0111 if link.RP == 'R', 0112 D = link.D; 0113 else 0114 D = q(j); 0115 end 0116 alpha = link.alpha; 0117 pstarm(:,j) = [link.A; D*sin(alpha); D*cos(alpha)]; 0118 if j == 1, 0119 robot.base 0120 %pstarm(:,j) = t2r(robot.base) * pstar(:,j); 0121 Tj = robot.base * Tj; 0122 end 0123 Rm{j} = t2r(Tj); 0124 end 0125 0126 % 0127 % the forward recursion 0128 % 0129 for j=1:n, 0130 link = robot.link{j}; 0131 0132 R = Rm{j}'; 0133 pstar = pstarm(:,j); 0134 r = link.r; 0135 0136 % 0137 % statement order is important here 0138 % 0139 if link.RP == 'R', 0140 % revolute axis 0141 wd = R*(wd + z0*qdd(j) + ... 0142 cross(w,z0*qd(j))); 0143 w = R*(w + z0*qd(j)); 0144 %v = cross(w,pstar) + R*v; 0145 vd = cross(wd,pstar) + ... 0146 cross(w, cross(w,pstar)) +R*vd; 0147 0148 else 0149 % prismatic axis 0150 w = R*w; 0151 wd = R*wd; 0152 vd = R*(z0*qdd(j)+vd) + ... 0153 cross(wd,pstar) + ... 0154 2*cross(w,R*z0*qd(j)) +... 0155 cross(w, cross(w,pstar)); 0156 end 0157 0158 vhat = cross(wd,r) + ... 0159 cross(w,cross(w,r)) + vd; 0160 F = link.m*vhat; 0161 N = link.I*wd + cross(w,link.I*w); 0162 Fm = [Fm F]; 0163 Nm = [Nm N]; 0164 end 0165 0166 % 0167 % the backward recursion 0168 % 0169 0170 f = fext(1:3); % force/moments on end of arm 0171 nn = fext(4:6); 0172 0173 for j=n:-1:1, 0174 link = robot.link{j}; 0175 pstar = pstarm(:,j); 0176 0177 % 0178 % order of these statements is important, since both 0179 % nn and f are functions of previous f. 0180 % 0181 if j == n, 0182 R = eye(3,3); 0183 else 0184 R = Rm{j+1}; 0185 end 0186 r = link.r; 0187 nn = R*(nn + cross(R'*pstar,f)) + ... 0188 cross(pstar+r,Fm(:,j)) + ... 0189 Nm(:,j); 0190 f = R*f + Fm(:,j); 0191 R = Rm{j}; 0192 if link.RP == 'R', 0193 % revolute 0194 tau(p,j) = nn'*(R'*z0) + ... 0195 link.G^2 * ( link.Jm*qdd(j) + ... 0196 friction(link, qd(j)) ... 0197 ); 0198 else 0199 % prismatic 0200 tau(p,j) = f'*(R'*z0) + ... 0201 link.G^2 * ( link.Jm*qdd(j) + ... 0202 friction(link, qd(j)) ... 0203 ); 0204 end 0205 end 0206 end