spline

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
  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 poly = spline(tacel, thetaini, thetafinal, velini, velfinal, acelini)
0034 
0035 A=[1 0 0 0 0;
0036    1 tacel tacel^2 tacel^3 tacel^4;
0037    0 1 0 0 0;
0038    0 1 2*tacel 3*tacel^2 4*tacel^3;
0039    0 0 2 0 0];
0040 
0041 b = [thetaini thetafinal velini velfinal acelini]';
0042 
0043 x = inv(A)*b
0044 
0045 poly = x;
0046 
0047 
0048 t=0:0.001:tacel;
0049 
0050 theta = x(1) + x(2)*t + x(3)*t.^2+ x(4)*t.^3+x(5)*t.^4;
0051 
0052 thetap = x(2)+2*x(3)*t+3*x(4)*t.^2+4*x(5)*t.^3;
0053 
0054 thetapp = 2*x(3)+6*x(4)*t+12*x(5)*t.^2;
0055 
0056 figure, plot(t, theta)
0057 figure, plot(t, thetap)
0058 figure, plot(t, thetapp)