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.
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