Home > . > rne.m

rne

PURPOSE ^

RNE Compute inverse dynamics via recursive Newton-Euler formulation

SYNOPSIS ^

function tau = rne(robot, a1, a2, a3, a4, a5)

DESCRIPTION ^

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.

CROSS-REFERENCE INFORMATION ^

This function calls: This function is called by:

SOURCE CODE ^

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

Generated on Sun 15-Feb-2009 18:09:29 by m2html © 2003