FDYN2 private function called by FDYN XDD = FDYN2(T, X, FLAG, ROBOT, TORQUEFUN) Called by FDYN to evaluate the robot velocity and acceleration for forward dynamics. T is the current time, X = [Q QD] is the state vector, ROBOT is the object being integrated, and TORQUEFUN is the string name of the function to compute joint torques and called as TAU = TORQUEFUN(T, X) if not given zero joint torques are assumed. The result is XDD = [QD QDD]. See also: FDYN
0001 %FDYN2 private function called by FDYN 0002 % 0003 % XDD = FDYN2(T, X, FLAG, ROBOT, TORQUEFUN) 0004 % 0005 % Called by FDYN to evaluate the robot velocity and acceleration for 0006 % forward dynamics. T is the current time, X = [Q QD] is the state vector, 0007 % ROBOT is the object being integrated, and TORQUEFUN is the string name of 0008 % the function to compute joint torques and called as 0009 % 0010 % TAU = TORQUEFUN(T, X) 0011 % 0012 % if not given zero joint torques are assumed. 0013 % 0014 % The result is XDD = [QD QDD]. 0015 % 0016 % See also: FDYN 0017 0018 % Copyright (C) 1999-2008, by Peter I. Corke 0019 % 0020 % This file is part of The Robotics Toolbox for Matlab (RTB). 0021 % 0022 % RTB is free software: you can redistribute it and/or modify 0023 % it under the terms of the GNU Lesser General Public License as published by 0024 % the Free Software Foundation, either version 3 of the License, or 0025 % (at your option) any later version. 0026 % 0027 % RTB is distributed in the hope that it will be useful, 0028 % but WITHOUT ANY WARRANTY; without even the implied warranty of 0029 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 0030 % GNU Lesser General Public License for more details. 0031 % 0032 % You should have received a copy of the GNU Leser General Public License 0033 % along with RTB. If not, see <http://www.gnu.org/licenses/>. 0034 0035 function xd = fdyn2(t, x, flag, robot, torqfun, varargin) 0036 0037 n = robot.n; 0038 0039 q = x(1:n); 0040 qd = x(n+1:2*n); 0041 0042 % evaluate the torque function if one is given 0043 if isstr(torqfun) 0044 tau = feval(torqfun, t, q, qd, varargin{:}); 0045 else 0046 tau = zeros(n,1); 0047 end 0048 0049 qdd = accel(robot, x(1:n,1), x(n+1:2*n,1), tau); 0050 xd = [x(n+1:2*n,1); qdd];