%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% [T Q QD]= FORWARDDYNAMIC(ROBOT, TIME_END, Q0, Qd0, TAU, torqfun, varargin) Compute forwarddynamics for the robot ROBOT for a period of TIME_END seconds. The initial state is defined by joint position Q0 and joint velocity Qd0. A constant vector of torques TAU is specified. Returns a time vector T, position Q and velocity QD when applying a torque TAU during a time period time_end Author: Arturo Gil. Universidad Miguel Hernández de Elche. email: arturo.gil@umh.es date: 26/04/2012 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0001 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0002 % [T Q QD]= FORWARDDYNAMIC(ROBOT, TIME_END, Q0, Qd0, TAU, torqfun, varargin) 0003 % 0004 % Compute forwarddynamics for the robot ROBOT for a period of TIME_END 0005 % seconds. The initial state is defined by joint position Q0 and joint 0006 % velocity Qd0. A constant vector of torques TAU is specified. 0007 % 0008 % Returns a time vector T, position Q and velocity QD when applying a 0009 % torque TAU during a time period time_end 0010 % 0011 % 0012 % Author: Arturo Gil. Universidad Miguel Hernández de Elche. 0013 % email: arturo.gil@umh.es date: 26/04/2012 0014 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0015 0016 % Copyright (C) 2012, by Arturo Gil Aparicio 0017 % 0018 % This file is part of ARTE (A Robotics Toolbox for Education). 0019 % 0020 % ARTE is free software: you can redistribute it and/or modify 0021 % it under the terms of the GNU Lesser General Public License as published by 0022 % the Free Software Foundation, either version 3 of the License, or 0023 % (at your option) any later version. 0024 % 0025 % ARTE is distributed in the hope that it will be useful, 0026 % but WITHOUT ANY WARRANTY; without even the implied warranty of 0027 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 0028 % GNU Lesser General Public License for more details. 0029 % 0030 % You should have received a copy of the GNU Leser General Public License 0031 % along with ARTE. If not, see <http://www.gnu.org/licenses/>. 0032 function [t, q, qd] = forwarddynamic(robot, time_end, q0, qd0, tau, g, torqfun, varargin) 0033 0034 n = robot.DOF; 0035 0036 % concatenate q and qd into the initial state vector 0037 q0 = [q0(:); qd0(:)]; 0038 0039 [t,y] = ode45(@fdyn_private, [0 time_end], q0, [], tau, g, robot, torqfun, varargin{:}); 0040 0041 q = y(:,1:n)'; 0042 qd = y(:,n+1:2*n)'; 0043 0044 end 0045 0046 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0047 % FDYN_PRIVATE private function called by FORWARDDYNAMIC 0048 % 0049 % XDD = FDYN_PRIVATE(T, X, TAU, ROBOT, TORQUEFUN) 0050 % 0051 % FORWARDDYNAMIC calls this function to evaluate the velocity and 0052 % acceleration. 0053 % TIME is the current time. 0054 % X = [Q QD] is the state vector 0055 % TAU is a vector of contant torques applied at each joint 0056 % 0057 % TORQUEFUN is the string name of the function to compute joint torques and called as 0058 % 0059 % TAU = TORQUEFUN(T, X) 0060 % 0061 % if not given zero joint torques are assumed. 0062 % 0063 % The function returns XDD = [QD QDD]. 0064 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0065 function xd = fdyn_private(time, x, tau, g, robot, torqfun, varargin) 0066 %time 0067 n = robot.DOF; 0068 0069 q = x(1:n)'; 0070 qd = x(n+1:2*n)'; 0071 0072 0073 qdd = accel(robot, x(1:n,1), x(n+1:2*n,1), tau, g); 0074 xd = [x(n+1:2*n,1); qdd]; 0075 end