Home > @robot > rne_dh.m

rne_dh

PURPOSE ^

RNE_DH Compute inverse dynamics via recursive Newton-Euler formulation

SYNOPSIS ^

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

DESCRIPTION ^

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.

CROSS-REFERENCE INFORMATION ^

This function calls: This function is called by:

SOURCE CODE ^

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

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