Home > . > fdyn.m

fdyn

PURPOSE ^

FDYN Integrate forward dynamics

SYNOPSIS ^

function [t, q, qd] = fdyn(robot, t0, t1, torqfun, q0, qd0, varargin)

DESCRIPTION ^

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.

CROSS-REFERENCE INFORMATION ^

This function calls: This function is called by:

SOURCE CODE ^

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

Generated on Sun 15-Feb-2009 18:09:29 by m2html © 2003