%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% [Qdd]= ACCEL(ROBOT, Q, Qd, TORQUE) Computes the instantaneous acceleration Qdd at each joint for the robot ROBOT asuming that the robot is at a state defined by joint position Q0 and velocity Qd0 and a TORQUE is applied. Returns the a vector os instantaneous accelerations at each joint. Author: Arturo Gil. Universidad Miguel Hernández de Elche. email: arturo.gil@umh.es date: 26/04/2012 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0001 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0002 % [Qdd]= ACCEL(ROBOT, Q, Qd, TORQUE) 0003 % 0004 % Computes the instantaneous acceleration Qdd at each joint for the robot ROBOT 0005 % asuming that the robot is at a state defined by joint position Q0 and velocity 0006 % Qd0 and a TORQUE is applied. 0007 % 0008 % Returns the a vector os instantaneous accelerations at each joint. 0009 % 0010 % 0011 % Author: Arturo Gil. Universidad Miguel Hernández de Elche. 0012 % email: arturo.gil@umh.es date: 26/04/2012 0013 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0014 0015 % Copyright (C) 2012, by Arturo Gil Aparicio 0016 % 0017 % This file is part of ARTE (A Robotics Toolbox for Education). 0018 % 0019 % ARTE is free software: you can redistribute it and/or modify 0020 % it under the terms of the GNU Lesser General Public License as published by 0021 % the Free Software Foundation, either version 3 of the License, or 0022 % (at your option) any later version. 0023 % 0024 % ARTE is distributed in the hope that it will be useful, 0025 % but WITHOUT ANY WARRANTY; without even the implied warranty of 0026 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 0027 % GNU Lesser General Public License for more details. 0028 % 0029 % You should have received a copy of the GNU Leser General Public License 0030 % along with ARTE. If not, see <http://www.gnu.org/licenses/>. 0031 function qdd = accel(robot, q, qd, torque, g) 0032 0033 if ~exist('g','var') 0034 g=[0 0 9.81]'; %default gravity 0035 end 0036 0037 0038 n = robot.DOF; 0039 0040 % compute the manipulator intertia matrix 0041 % to do so, compute the torques resulting from unit acceleration of 0042 % each joint without gravity 0043 M=[]; 0044 for i=1:robot.DOF, 0045 %cirshift([1 0 0 0 0 0], i-1) results in acceleration varying from 0046 % [1 0 0 0 0 0], [0 1 0 0 0 0], [0 0 1 0 0 0] etc 0047 t = inversedynamic(robot, q', zeros(1,n), circshift([1 0 0 0 0 0]',i-1), [0 0 0]', [0 0 0 0 0 0]'); 0048 M = [M; t]; 0049 end 0050 0051 % use inverse dynamic model to compute torques 0052 % in the state defined by position q and velocity qd under 0053 % the action of gravity. 0054 tau = inversedynamic(robot, q', qd', zeros(1,n), g, [0 0 0 0 0 0]'); 0055 0056 0057 qdd = inv(M)*(torque(:) - tau');