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: