%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% poly = spline(tacel, thetaini, thetafinal, velini, velfinal, acelini) Computes the coefficients a b c d e of the polynomial defined as theta(t) = a + bt + ct^2 + dt^3 + et^4 Returns: poly = [a b c d e] Inputs: tacel: total time to perform movement thetaini, thetafinal: initial and final joint coordinates. velini, velfinal: initial and final joint speeds. acelini: initial acceleration of the joints. Author: Arturo Gil. Universidad Miguel Hernández de Elche. email: arturo.gil@umh.es date: 26/06/2012 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
0001 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0002 % poly = spline(tacel, thetaini, thetafinal, velini, velfinal, acelini) 0003 % 0004 % Computes the coefficients a b c d e of the polynomial defined as 0005 % theta(t) = a + bt + ct^2 + dt^3 + et^4 0006 % Returns: poly = [a b c d e] 0007 % Inputs: 0008 % tacel: total time to perform movement 0009 % thetaini, thetafinal: initial and final joint coordinates. 0010 % velini, velfinal: initial and final joint speeds. 0011 % acelini: initial acceleration of the joints. 0012 % 0013 % Author: Arturo Gil. Universidad Miguel Hernández de Elche. 0014 % email: arturo.gil@umh.es date: 26/06/2012 0015 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 0016 0017 % Copyright (C) 2012, by Arturo Gil Aparicio 0018 % 0019 % This file is part of ARTE (A Robotics Toolbox for Education). 0020 % 0021 % ARTE is free software: you can redistribute it and/or modify 0022 % it under the terms of the GNU Lesser General Public License as published by 0023 % the Free Software Foundation, either version 3 of the License, or 0024 % (at your option) any later version. 0025 % 0026 % ARTE is distributed in the hope that it will be useful, 0027 % but WITHOUT ANY WARRANTY; without even the implied warranty of 0028 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 0029 % GNU Lesser General Public License for more details. 0030 % 0031 % You should have received a copy of the GNU Leser General Public License 0032 % along with ARTE. If not, see <http://www.gnu.org/licenses/>. 0033 function [theta, thetap, thetapp, poly] = single_joint_spline(thetaini, thetafinal, velini, velfinal, time_vector) 0034 0035 tfinal = time_vector(end); 0036 0037 A=[1 0 0 0; 0038 1 tfinal tfinal^2 tfinal^3; 0039 0 1 0 0; 0040 0 1 2*tfinal 3*tfinal^2]; 0041 0042 b = [thetaini thetafinal velini velfinal]'; 0043 0044 x = inv(A)*b; 0045 0046 poly = x; 0047 0048 t=time_vector(:); %0:0.001:tacel; 0049 0050 theta = x(1) + x(2)*t + x(3)*t.^2+ x(4)*t.^3; 0051 0052 thetap = x(2)+2*x(3)*t+3*x(4)*t.^2; 0053 0054 thetapp = 2*x(3)+6*x(4)*t; 0055 0056 %close all 0057 % figure, plot(t, theta) 0058 % figure, plot(t, thetap) 0059 % figure, plot(t, thetapp)