Home > . > jtraj.m

jtraj

PURPOSE ^

JTRAJ Compute a joint space trajectory between two points

SYNOPSIS ^

function [qt,qdt,qddt] = jtraj(q0, q1, tv, qd0, qd1)

DESCRIPTION ^

JTRAJ Compute a joint space trajectory between two points

    [Q QD QDD] = JTRAJ(Q0, Q1, N)
    [Q QD QDD] = JTRAJ(Q0, Q1, N, QD0, QD1)
    [Q QD QDD] = JTRAJ(Q0, Q1, T)
    [Q QD QDD] = JTRAJ(Q0, Q1, T, QD0, QD1)

 Returns a joint space trajectory Q from state Q0 to Q1.  The number
 of points is N or the length of the given time vector T.  A 7th
 order polynomial is used with default zero boundary conditions for
 velocity and acceleration.  Non-zero boundary velocities can be
 optionally specified as QD0 and QD1.

 The function can optionally return a velocity and acceleration
 trajectories as QD and QDD.

 Each trajectory is an mxn matrix, with one row per time step, and
 one column per joint parameter.

 See also: CTRAJ.

CROSS-REFERENCE INFORMATION ^

This function calls: This function is called by:

SOURCE CODE ^

0001 %JTRAJ Compute a joint space trajectory between two points
0002 %
0003 %    [Q QD QDD] = JTRAJ(Q0, Q1, N)
0004 %    [Q QD QDD] = JTRAJ(Q0, Q1, N, QD0, QD1)
0005 %    [Q QD QDD] = JTRAJ(Q0, Q1, T)
0006 %    [Q QD QDD] = JTRAJ(Q0, Q1, T, QD0, QD1)
0007 %
0008 % Returns a joint space trajectory Q from state Q0 to Q1.  The number
0009 % of points is N or the length of the given time vector T.  A 7th
0010 % order polynomial is used with default zero boundary conditions for
0011 % velocity and acceleration.  Non-zero boundary velocities can be
0012 % optionally specified as QD0 and QD1.
0013 %
0014 % The function can optionally return a velocity and acceleration
0015 % trajectories as QD and QDD.
0016 %
0017 % Each trajectory is an mxn matrix, with one row per time step, and
0018 % one column per joint parameter.
0019 %
0020 % See also: CTRAJ.
0021 
0022 % Copyright (C) 1993-2008, by Peter I. Corke
0023 %
0024 % This file is part of The Robotics Toolbox for Matlab (RTB).
0025 %
0026 % RTB is free software: you can redistribute it and/or modify
0027 % it under the terms of the GNU Lesser General Public License as published by
0028 % the Free Software Foundation, either version 3 of the License, or
0029 % (at your option) any later version.
0030 %
0031 % RTB is distributed in the hope that it will be useful,
0032 % but WITHOUT ANY WARRANTY; without even the implied warranty of
0033 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
0034 % GNU Lesser General Public License for more details.
0035 %
0036 % You should have received a copy of the GNU Leser General Public License
0037 % along with RTB.  If not, see <http://www.gnu.org/licenses/>.
0038 
0039 function [qt,qdt,qddt] = jtraj(q0, q1, tv, qd0, qd1)
0040     if length(tv) > 1,
0041         tscal = max(tv);
0042         t = tv(:)/tscal;
0043     else
0044         tscal = 1;
0045         t = [0:(tv-1)]'/(tv-1);    % normalized time from 0 -> 1
0046     end
0047 
0048     q0 = q0(:);
0049     q1 = q1(:);
0050 
0051     if nargin == 3,
0052         qd0 = zeros(size(q0));
0053         qd1 = qd0;
0054     elseif nargin == 5,
0055         qd0 = qd0(:);
0056         qd1 = qd1(:);
0057     else
0058         error('incorrect number of arguments')
0059     end
0060 
0061     % compute the polynomial coefficients
0062     A = 6*(q1 - q0) - 3*(qd1+qd0)*tscal;
0063     B = -15*(q1 - q0) + (8*qd0 + 7*qd1)*tscal;
0064     C = 10*(q1 - q0) - (6*qd0 + 4*qd1)*tscal;
0065     E = qd0*tscal; % as the t vector has been normalized
0066     F = q0;
0067 
0068     tt = [t.^5 t.^4 t.^3 t.^2 t ones(size(t))];
0069     c = [A B C zeros(size(A)) E F]';
0070     
0071     qt = tt*c;
0072 
0073     % compute optional velocity
0074     if nargout >= 2,
0075         c = [ zeros(size(A)) 5*A 4*B 3*C  zeros(size(A)) E ]';
0076         qdt = tt*c/tscal;
0077     end
0078 
0079     % compute optional acceleration
0080     if nargout == 3,
0081         c = [ zeros(size(A))  zeros(size(A)) 20*A 12*B 6*C  zeros(size(A))]';
0082         qddt = tt*c/tscal^2;
0083     end

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