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.
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