%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% [qt, qdt, qddt] = compute_joint_trajectory(q_ini, q_final, time_vector, qd_ini, qd_final) Computes an smooth trajectory between the joint coordinates Q_ini and Q_final using a 5th degree polynomial. A polynomial for each joint is computed and evaluated for the proposed time vectord taking into account the inputs: Q_ini, Q_final: The initial a final joint coordinates q_ini and q_final time_vector: The time vector when the movement is to be planned. Qd_ini, Qd_final: The initial and final speeds of each joint. Outputs: Qt: the smooth joint trajectory computed by the function at each time step. Qdt: the joint speed at each time step. Qddt: the joint acceleration at each time step. Author: Arturo Gil. Universidad Miguel Hernández de Elche Date: 05/05/2012 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0001 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0002 % [qt, qdt, qddt] = compute_joint_trajectory(q_ini, q_final, time_vector, qd_ini, qd_final) 0003 % 0004 % Computes an smooth trajectory between the joint coordinates Q_ini and 0005 % Q_final using a 5th degree polynomial. 0006 % A polynomial for each joint is computed and evaluated for the proposed 0007 % time vectord taking into account the inputs: 0008 % 0009 % Q_ini, Q_final: The initial a final joint coordinates q_ini and q_final 0010 % time_vector: The time vector when the movement is to be planned. 0011 % Qd_ini, Qd_final: The initial and final speeds of each joint. 0012 % 0013 % Outputs: 0014 % Qt: the smooth joint trajectory computed by the function at each time 0015 % step. 0016 % Qdt: the joint speed at each time step. 0017 % Qddt: the joint acceleration at each time step. 0018 % 0019 % Author: Arturo Gil. Universidad Miguel Hernández de Elche 0020 % Date: 05/05/2012 0021 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0022 0023 % Copyright (C) 2012, by Arturo Gil Aparicio 0024 % 0025 % This file is part of ARTE (A Robotics Toolbox for Education). 0026 % 0027 % ARTE is free software: you can redistribute it and/or modify 0028 % it under the terms of the GNU Lesser General Public License as published by 0029 % the Free Software Foundation, either version 3 of the License, or 0030 % (at your option) any later version. 0031 % 0032 % ARTE is distributed in the hope that it will be useful, 0033 % but WITHOUT ANY WARRANTY; without even the implied warranty of 0034 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 0035 % GNU Lesser General Public License for more details. 0036 % 0037 % You should have received a copy of the GNU Leser General Public License 0038 % along with ARTE. If not, see <http://www.gnu.org/licenses/>. 0039 function [qt, qdt, qddt] = compute_joint_trajectory_indep(robot, q_ini, q_final, qd_ini, qd_final, time_vector) 0040 0041 %n=robot.DOF; 0042 0043 qt=zeros(1,length(time_vector));%[]; 0044 qdt=zeros(1,length(time_vector)); 0045 qddt=zeros(1,length(time_vector)); 0046 0047 for i=1:robot.DOF, 0048 [qi, qdi, qddi, poly]=single_joint_spline(q_ini(i), q_final(i), qd_ini(i), qd_final(i), time_vector); 0049 qt(i,:)=qi; 0050 qdt(i,:)=qdi; 0051 qddt(i,:)=qddi; 0052 end