call_direct_dynamics

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  qdd = call_direct_dynamics(input)
  Auxiliar function for the simulink model SIMULATE_ROBOT_AND_CONTROLLER.
  Rearranges the inputs coming from the simulink model and calls the
  function accel.
  As a result the instantaneous acceleration at each joint is returned.

  See also ACCEL.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

0001 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0002 %  qdd = call_direct_dynamics(input)
0003 %  Auxiliar function for the simulink model SIMULATE_ROBOT_AND_CONTROLLER.
0004 %  Rearranges the inputs coming from the simulink model and calls the
0005 %  function accel.
0006 %  As a result the instantaneous acceleration at each joint is returned.
0007 %
0008 %  See also ACCEL.
0009 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0010 
0011 % Copyright (C) 2012, by Arturo Gil Aparicio
0012 %
0013 % This file is part of ARTE (A Robotics Toolbox for Education).
0014 %
0015 % ARTE is free software: you can redistribute it and/or modify
0016 % it under the terms of the GNU Lesser General Public License as published by
0017 % the Free Software Foundation, either version 3 of the License, or
0018 % (at your option) any later version.
0019 %
0020 % ARTE is distributed in the hope that it will be useful,
0021 % but WITHOUT ANY WARRANTY; without even the implied warranty of
0022 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
0023 % GNU Lesser General Public License for more details.
0024 %
0025 % You should have received a copy of the GNU Lesser General Public License
0026 % along with ARTE.  If not, see <http://www.gnu.org/licenses/>.
0027 function qdd = call_direct_dynamics(input)
0028 
0029 global robot
0030 
0031 %set friction to zero
0032 %robot.friction = 0;
0033 
0034 torque = input(1:6);   % Input torque at each joint
0035 q   = input(7:12);       % Joint positions
0036 qd  = input(13:18);       % Joint speeds
0037 
0038 
0039 % Compute acceleration
0040 qdd = accel(robot, q, qd, torque);
0041