Home > @robot > rne_mdh.m

rne_mdh

PURPOSE ^

RNE_MDH Compute inverse dynamics via recursive Newton-Euler formulation

SYNOPSIS ^

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

DESCRIPTION ^

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.

CROSS-REFERENCE INFORMATION ^

This function calls: This function is called by:

SOURCE CODE ^

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

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