FDYN Integrate forward dynamics [T Q QD] = FDYN(ROBOT, T0, T1) [T Q QD] = FDYN(ROBOT, T0, T1, TORQFUN) [T Q QD] = FDYN(ROBOT, T0, T1, TORQFUN, Q0, QD0) [T Q QD] = FDYN(ROBOT, T0, T1, TORQFUN, Q0, QD0, ARG1, ARG2, ...) Integrates the dynamics of manipulator ROBOT dynamics over the time interval T0 to T1 and returns vectors of joint position and velocity. ROBOT is a robot object and describes the manipulator dynamics and kinematics, and Q is an n element vector of joint state. A control torque may be specified by a user specified function TAU = TORQFUN(T, Q, QD, ARG1, ARG2, ...) where Q and QD are the manipulator joint coordinate and velocity state respectively], and T is the current time. Optional arguments passed to FDYN will be passed through to the user function. If TORQFUN is not specified, or is given as 0, then zero torque is applied to the manipulator joints. See also: ACCEL, NOFRICTION, RNE, ROBOT, ODE45.
0001 %FDYN Integrate forward dynamics 0002 % 0003 % [T Q QD] = FDYN(ROBOT, T0, T1) 0004 % [T Q QD] = FDYN(ROBOT, T0, T1, TORQFUN) 0005 % [T Q QD] = FDYN(ROBOT, T0, T1, TORQFUN, Q0, QD0) 0006 % [T Q QD] = FDYN(ROBOT, T0, T1, TORQFUN, Q0, QD0, ARG1, ARG2, ...) 0007 % 0008 % Integrates the dynamics of manipulator ROBOT dynamics over the time 0009 % interval T0 to T1 and returns vectors of joint position and velocity. 0010 % ROBOT is a robot object and describes the manipulator dynamics and 0011 % kinematics, and Q is an n element vector of joint state. 0012 % 0013 % A control torque may be specified by a user specified function 0014 % 0015 % TAU = TORQFUN(T, Q, QD, ARG1, ARG2, ...) 0016 % 0017 % where Q and QD are the manipulator joint coordinate and velocity state 0018 % respectively], and T is the current time. Optional arguments passed to FDYN 0019 % will be passed through to the user function. 0020 % 0021 % If TORQFUN is not specified, or is given as 0, then zero torque is 0022 % applied to the manipulator joints. 0023 % 0024 % See also: ACCEL, NOFRICTION, RNE, ROBOT, ODE45. 0025 0026 % Copyright (C) 1993-2008 Peter Corke 0027 % 0028 % This file is part of The Robotics Toolbox for Matlab (RTB). 0029 % 0030 % RTB is free software: you can redistribute it and/or modify 0031 % it under the terms of the GNU Lesser General Public License as published by 0032 % the Free Software Foundation, either version 3 of the License, or 0033 % (at your option) any later version. 0034 % 0035 % RTB is distributed in the hope that it will be useful, 0036 % but WITHOUT ANY WARRANTY; without even the implied warranty of 0037 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 0038 % GNU Lesser General Public License for more details. 0039 % 0040 % You should have received a copy of the GNU Leser General Public License 0041 % along with RTB. If not, see <http://www.gnu.org/licenses/>. 0042 0043 function [t, q, qd] = fdyn(robot, t0, t1, torqfun, q0, qd0, varargin) 0044 0045 % check the Matlab version, since ode45 syntax has changed 0046 v = ver; 0047 if str2num(v(1).Version)<6, 0048 %error('fdyn now requires Matlab version >= 6'); 0049 end 0050 0051 n = robot.n; 0052 if nargin == 3, 0053 torqfun = 0; 0054 x0 = zeros(2*n,1); 0055 elseif nargin == 4, 0056 x0 = zeros(2*n, 1); 0057 elseif nargin >= 6, 0058 x0 = [q0(:); qd0(:)]; 0059 end 0060 0061 [t,y] = ode45('fdyn2', [t0 t1], x0, [], robot, torqfun, varargin{:}); 0062 q = y(:,1:n); 0063 qd = y(:,n+1:2*n); 0064