www.gusucode.com > 机器人工具箱 - robot源码程序 > robot\rne.m
%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. % % verified against MAPLE code, which is verified by examples % % Copyright (C) 1992-2008, by Peter I. Corke % % This file is part of The Robotics Toolbox for Matlab (RTB). % % RTB is free software: you can redistribute it and/or modify % it under the terms of the GNU Lesser General Public License as published by % the Free Software Foundation, either version 3 of the License, or % (at your option) any later version. % % RTB is distributed in the hope that it will be useful, % but WITHOUT ANY WARRANTY; without even the implied warranty of % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the % GNU Lesser General Public License for more details. % % You should have received a copy of the GNU Leser General Public License % along with RTB. If not, see <http://www.gnu.org/licenses/>. function tau = rne(robot, a1, a2, a3, a4, a5) if robot.mdh ~= 0, error('rne only valid for standard D&H parameters') end 888 z0 = [0;0;1]; grav = robot.gravity; % default gravity from the object fext = zeros(6, 1); n = robot.n; if numcols(a1) == 3*n, Q = a1(:,1:n); Qd = a1(:,n+1:2*n); Qdd = a1(:,2*n+1:3*n); np = numrows(Q); if nargin >= 3, grav = a2; end if nargin == 4, fext = a3; end else np = numrows(a1); Q = a1; Qd = a2; Qdd = a3; if numcols(a1) ~= n | numcols(Qd) ~= n | numcols(Qdd) ~= n | ... numrows(Qd) ~= np | numrows(Qdd) ~= np, error('bad data'); end if nargin >= 5, grav = a4; end if nargin == 6, fext = a5; end end tau = zeros(np,n); for p=1:np, % for all points on path q = Q(p,:)'; qd = Qd(p,:)'; qdd = Qdd(p,:)'; Fm = []; Nm = []; pstarm = []; Rm = []; w = zeros(3,1); wd = zeros(3,1); v = zeros(3,1); vd = grav; % % init some variables, compute the link rotation matrices % for j=1:n, link = robot.link{j}; Tj = link(q(j)); if link.RP == 'R', D = link.D; else D = q(j); end alpha = link.alpha; pstarm(:,j) = [link.A; D*sin(alpha); D*cos(alpha)]; if j == 1, robot.base %pstarm(:,j) = t2r(robot.base) * pstar(:,j); Tj = robot.base * Tj; end Rm{j} = t2r(Tj); end % % the forward recursion % for j=1:n, link = robot.link{j}; R = Rm{j}'; pstar = pstarm(:,j); r = link.r; % % statement order is important here % if link.RP == 'R', % revolute axis wd = R*(wd + z0*qdd(j) + ... cross(w,z0*qd(j))); w = R*(w + z0*qd(j)); %v = cross(w,pstar) + R*v; vd = cross(wd,pstar) + ... cross(w, cross(w,pstar)) +R*vd; else % prismatic axis w = R*w; wd = R*wd; vd = R*(z0*qdd(j)+vd) + ... cross(wd,pstar) + ... 2*cross(w,R*z0*qd(j)) +... cross(w, cross(w,pstar)); end vhat = cross(wd,r) + ... cross(w,cross(w,r)) + vd; F = link.m*vhat; N = link.I*wd + cross(w,link.I*w); Fm = [Fm F]; Nm = [Nm N]; end % % the backward recursion % f = fext(1:3); % force/moments on end of arm nn = fext(4:6); for j=n:-1:1, link = robot.link{j}; pstar = pstarm(:,j); % % order of these statements is important, since both % nn and f are functions of previous f. % if j == n, R = eye(3,3); else R = Rm{j+1}; end r = link.r; nn = R*(nn + cross(R'*pstar,f)) + ... cross(pstar+r,Fm(:,j)) + ... Nm(:,j); f = R*f + Fm(:,j); R = Rm{j}; if link.RP == 'R', % revolute tau(p,j) = nn'*(R'*z0) + ... link.G^2 * ( link.Jm*qdd(j) + ... friction(link, qd(j)) ... ); else % prismatic tau(p,j) = f'*(R'*z0) + ... link.G^2 * ( link.Jm*qdd(j) + ... friction(link, qd(j)) ... ); end end end