Home > . > accel.m

accel

PURPOSE ^

ACCEL Compute manipulator forward dynamics

SYNOPSIS ^

function qdd = accel(robot, Q, qd, torque)

DESCRIPTION ^

ACCEL Compute manipulator forward dynamics

    QDD = ACCEL(ROBOT, Q, QD, TORQUE)
    QDD = ACCEL(ROBOT, [Q QD TORQUE])

 Returns a vector of joint accelerations that result from applying the 
 actuator TORQUE to the manipulator ROBOT in state Q and QD.

 Uses the method 1 of Walker and Orin to compute the forward dynamics.
 This form is useful for simulation of manipulator dynamics, in
 conjunction with a numerical integration function.

 See also: RNE, ROBOT, ODE45.

CROSS-REFERENCE INFORMATION ^

This function calls: This function is called by:

SOURCE CODE ^

0001 %ACCEL Compute manipulator forward dynamics
0002 %
0003 %    QDD = ACCEL(ROBOT, Q, QD, TORQUE)
0004 %    QDD = ACCEL(ROBOT, [Q QD TORQUE])
0005 %
0006 % Returns a vector of joint accelerations that result from applying the
0007 % actuator TORQUE to the manipulator ROBOT in state Q and QD.
0008 %
0009 % Uses the method 1 of Walker and Orin to compute the forward dynamics.
0010 % This form is useful for simulation of manipulator dynamics, in
0011 % conjunction with a numerical integration function.
0012 %
0013 % See also: RNE, ROBOT, ODE45.
0014 
0015 % MOD HISTORY
0016 % 4/99 add object support
0017 % 1/02 copy rne code from inertia.m to here for speed
0018 % $Log: not supported by cvs2svn $
0019 % $Revision: 1.3 $
0020 
0021 % Copyright (C) 1999-2008, by Peter I. Corke
0022 %
0023 % This file is part of The Robotics Toolbox for Matlab (RTB).
0024 %
0025 % RTB is free software: you can redistribute it and/or modify
0026 % it under the terms of the GNU Lesser General Public License as published by
0027 % the Free Software Foundation, either version 3 of the License, or
0028 % (at your option) any later version.
0029 %
0030 % RTB is distributed in the hope that it will be useful,
0031 % but WITHOUT ANY WARRANTY; without even the implied warranty of
0032 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
0033 % GNU Lesser General Public License for more details.
0034 %
0035 % You should have received a copy of the GNU Leser General Public License
0036 % along with RTB.  If not, see <http://www.gnu.org/licenses/>.
0037 
0038 
0039 function qdd = accel(robot, Q, qd, torque)
0040     n = robot.n;
0041 
0042     if nargin == 2,
0043             q = Q(1:n);
0044         qd = Q(n+1:2*n);
0045         torque = Q(2*n+1:3*n);
0046     else
0047         q = Q;
0048         if length(q) == robot.n,
0049             q = q(:);
0050             qd = qd(:);
0051         end
0052     end
0053 
0054     % compute current manipulator inertia
0055     %   torques resulting from unit acceleration of each joint with
0056     %   no gravity.
0057     M = rne(robot, ones(n,1)*q', zeros(n,n), eye(n), [0;0;0]);
0058 
0059     % compute gravity and coriolis torque
0060     %    torques resulting from zero acceleration at given velocity &
0061     %    with gravity acting.
0062     tau = rne(robot, q', qd', zeros(1,n));    
0063 
0064     qdd = inv(M) * (torque(:) - tau');
0065

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