Trajectories via Matlab

M-file:

%% Example of trajectory generation via cubic and quintic polynomials,
%  linear segment parabolic blend (LSPB), and sinusoids.
%
% Filename:  makeTrajectories.m
%
 
clear all; close all; clc; % clear memory, figures, and command window
 
%% ** Cubic Polynomial Trajectories **
% initial time, value and velocity
to = 0; qo = 0.5;  qdoto = 0;
% final time, value and velocity
tf = 8; qf = -0.7; qdotf = 0; 
% Coefficient matrix for cubic trajectory and its derivative
% at initial and final values.
A = [1,  to,  to^2, to^3;   ...
     0,  1,   2*to, 3*to^2; ...
     1,  tf,  tf^2, tf^3;   ...
     0,  1,   2*tf, 3*tf^2];
% Vector of intial and final positions and velocities
b = [qo; qdoto; qf; qdotf];
% Compute coefficients of trajectory polynomial using
% notion of a = inv(A)*b, but using Gaussian Elimination
a = A\b;
% Evaluate cubic polynomial at times for plotting
t = linspace(to, tf, 501);
q     = a(1) + a(2)*t + a(3)*t.^2 + a(4)*t.^3;
qdot  = a(2) + 2*a(3)*t + 3*a(4)*t.^2;
qddot = 2*a(3) + 6*a(4)*t;
% Plot trajectories
figure(1);
subplot(2,2,1);
plot(t,q,'b-',t,qdot,'g--',t,qddot,'r-.','LineWidth',2);
legend('pos','vel','acc');
xlabel('time (sec)'); ylabel('Trajectory');
title('Trajectory using Cubic Polynomial');
grid;
 
%% ** Quintic Polynomial Trajectories **
clear all;
% initial time, value, velocity and acceleration
to = 0; qo = 0.5; qdoto = 0; qddoto = 0;
% final time, value, velocity and acceleration
tf = 8; qf = -0.7; qdotf = 0; qddotf = 0;
% Coefficient matrix for quintic trajectory and its derivative
% at initial and final values.
A = [1,  to,  to^2, to^3,   to^4,    to^5;   ...
     0,  1,   2*to, 3*to^2, 4*to^3,  5*to^4;   ...
     0,  0,   2,    6*to,   12*to^2, 20*to^3; ...
     1,  tf,  tf^2, tf^3,   tf^4,    tf^5;   ...
     0,  1,   2*tf, 3*tf^2, 4*tf^3,  5*tf^4;   ...
     0,  0,   2,    6*tf,   12*tf^2, 20*tf^3];
% Vector of intial and final joint positions and velocities
b = [qo; qdoto; qddoto; qf; qdotf; qddotf];
% Compute coefficients of trajectory polynomial using
% notion of a = inv(A)*b, but using Gaussian Elimination
a = A\b;
% Evaluate quintic polynomial at times for plotting
t = linspace(to, tf, 501);
q     = a(1) + a(2)*t + a(3)*t.^2 + a(4)*t.^3 + a(5)*t.^4 + a(6)*t.^5;
qdot  = a(2) + 2*a(3)*t + 3*a(4)*t.^2 + 4*a(5)*t.^3 + 5*a(6)*t.^4;
qddot = 2*a(3) + 6*a(4)*t + 12*a(5)*t.^2 + 20*a(6)*t.^3;
% Plot trajectories
subplot(2,2,2);
plot(t,q,'b-',t,qdot,'g--',t,qddot,'r-.','LineWidth',2);
legend('pos','vel','acc');
xlabel('time (sec)'); ylabel('Trajectory');
title('Trajectory using Quintic Polynomial');
grid;
 
%% ** Linear Segments with Parabolic Blends (LSPB) **
clear all;
% initial time, value and velocity
to = 0; qo = 0.5;
% final time, value and velocity
tf = 8; qf = -0.7;
% constant velocity and blend time
V = -0.2; tb = (qo - qf + V*tf)/V;
% check that V is within limits
Vmin = (qf - qo)/tf;
if (V > Vmin || V < 2*Vmin) % this check assumes V negative
    display(['V = ',num2str(V), ' is not within limits',...
            '(',num2str(Vmin),', ',num2str(2*Vmin),')']);
    display('LSPB will not be correct!');
end;
a(1) = qo; a(2) = 0; a(3) = V/(2*tb);
b(1) = qf - (V*tf^2)/(2*tb); b(2) = V*tf/tb; b(3) = -V/(2*tb);
 
% evaluate lspb at times for plotting
t = linspace(to, tf, 501);
q = (a(1) + a(2)*t + a(3)*t.^2).*(t<=tb) + ...
    ((qf + qo - V*tf)/2 + V*t).*((t>tb)-(t>=(tf-tb))) + ...
    (b(1) + b(2)*t + b(3)*t.^2).*(t>=(tf-tb));
qdot = (a(2) + 2*a(3)*t).*(t<=tb) + ...
       V.*((t>tb)-(t>=(tf-tb))) + ...
       (b(2) + 2*b(3)*t).*(t>=(tf-tb));
qddot = 2*a(3)*(t<=tb) + ...
        0*((t>tb)-(t>=(tf-tb))) + ...
        2*b(3)*(t>=(tf-tb));
subplot(2,2,3);
plot(t,q,'b-',t,qdot,'g--',t,qddot,'r-.','LineWidth',2);
legend('pos','vel','acc');
xlabel('time (sec)'); ylabel('Joint Trajectory');
title('Trajectory using LSPB');
grid;
 
%% ** Sinusoidal **
clear all;
% initial time, value and velocity
to = 0; qo = 0.5;
% final time, value and velocity
tf = 8; qf = -0.7;
% frequency of sinusoids
w = pi/tf;
 
% evaluate sinusoid at times for plotting
t = linspace(to, tf, 501);
q = 0.5*(qf - qo)*(1 - cos(w*t)) + qo;
qdot = 0.5*(qf - qo)*sin(w*t)*w;
qddot = 0.5*(qf - qo)*cos(w*t)*w*w;
subplot(2,2,4);
plot(t,q,'b-',t,qdot,'g--',t,qddot,'r-.','LineWidth',2);
legend('pos','vel','acc');
xlabel('time (sec)'); ylabel('Joint Trajectory');
title('Trajectory using Sinusoids');
grid;

Plots generated:

Plots of trajectories