# Creating Trajectories with Splines

## Introduction

This example shows hows to create a trajectory for waypoints in MATLAB with Splines. Trajectories are used to navigate between waypoints smoothly with respect to time.

This smoothness is important because it means that the acceleration or velocity has no discontinuities. Since acceleration is generally tied to the torque input, this means that motors are ramped up and down in a continuous smooth manner that reduces stress on the motors.

First, we'll show how to generate a spline between two points, then move onto an n-point example. There are three methods that we can use:

• Cubic spline polynomial and solving for the coefficients
• "spline" function
• "interp1" function

We will start off by showing the cubic spline polynomial method, followed by spline and briefly show interp1. Spline and interp1 implementations are almost identical. Spline has the added advantage of being able to set the slope at the endpoints.

```x = [0 1];
y = [0 1];
xq = x(1):0.1:x(end);
yq = spline(x,[0 y 0],xq);
plot(xq,yq,':.b',x,y,'or');
axis([-0.5 1.5 -0.5 1.5]);
```

## Two Point Example

Starting with the cubic spline polynomial:

We have two points.

```plot(x,y,'or');
axis([-0.5 1.5 -0.5 1.5]);
```

We would like to connect the two points with a line.

```plot(x,y,x,y,'or');
axis([-0.5 1.5 -0.5 1.5]);
```

We would also like to connect the two points with a spline so that we can specify the slope at the endpoints.

```plot(xq,yq,':.b',x,y,'or');
axis([-0.5 1.5 -0.5 1.5]);
```

We are going to start with x(t). Let's say we have four boundary conditions:

```x(t0) = x0; % position of initial coordinate
x(tf) = xf; % position of final coordinate
xdot(t0) = xdot0; % slope of initial coordinate
xdot(tf) = xdotf; % slope of final coordinate```
```where:
t0 = 0;    tf = 10;
x0 = 1;    xf = 11;
xdot0 = 1; xdotf = 1;```

The general equation for a spline is;

```syms x xdot a0 a1 a2 a3 t
x = a0+a1*t+a2*t^2+a3*t^3;
xdot = a1+2*a2*t+3*a3*t^2;
```

Inserting the values of the initial conditions for t, we get x and xdot as a function of a1,a2,a3,a4.

```subs(x,t,0) % = 1
subs(x,t,10) % = 11
subs(xdot,t,0) % = 1
subs(xdot,t,10) % = 1
```
```
ans =

a0

ans =

a0 + 10*a1 + 100*a2 + 1000*a3

ans =

a1

ans =

a1 + 20*a2 + 300*a3

```

In matrix form, this becomes C*a = bc, where:

```xbc = [1 11 1 1]'; % boundary conditions for y
C = [1 0 0 0; 1 10 100 1000; 0 1 0 0; 0 1 20 300]; % coefficients from spline equations
```

Solving for the spline coefficients a, we obtain the following:

```a = C\xbc; % a = [a1 a2 a3 a4]';
display(a);
```
```a =

1
1
0
0

```

The cubic spline equation then becomes:

```syms x;
f = a(1)+a(2)*x+a(3)*x.^2+a(4)*x.^3;
display(f);
```
```
f =

x + 1

```

calculate spline interpolated data using calculated coefficients

```tbc = [0 10]; % [x0 xf]
xbc = [1 11];
t = 0:0.1:10;
x = a(1)+a(2)*t+a(3)*t.^2+a(4)*t.^3;

% plot spline
plot(t,x,tbc,xbc,'or');
axis([-1 11 0 12]);
title('x vs. t for Cubic Spline Equation')
```

This obtains the spline for the x vs. t motion. We would then have to do this for y vs. t as well.

```ybc = [1 5 0 0]';
C = [1 0 0 0; 1 10 100 1000; 0 1 0 0; 0 1 20 300]; % coefficients from spline equations
a = C\ybc; % a = [a1 a2 a3 a4]';

tbc = [0 10];
ybc = [1 5];
t = 0:0.1:10;
y = a(1)+a(2)*t+a(3)*t.^2+a(4)*t.^3;

% plot spline
plot(t,y,tbc,ybc,'or');
axis([-1 12 0 6]);
title('y vs. t for Cubic Spline Equation')
```

After obtaining these two trajectories with respect to time, we can then show the y vs. x trajectory

```plot(x,y,xbc,ybc,'or');
axis([-1 12 0 6]);
title('y vs. x for Cubic Spline Equation')
```

MATLAB offers a function called "spline" which does the polynomial coefficient solving for you.

If we did not want to specify the slope boundary conditions, the equation would be:

y = spline(xbc,ybc,x)

The result would be a straight line between both points.

To specify the slopes we use the following equation:

```tbc = [0 10];
xbc = [1 3];
xdotbc = [0 0];
t = 0:0.1:10;
x =  spline(tbc,[xdotbc(1) xbc xdotbc(2)],t);

plot(t,x,tbc,xbc,'or');
title('Spline Equation')
xlabel('t')
ylabel('x')
axis([-1 11 0 4]);
```

## N-Point Example

This is a "Cubic polynomials for a path with via points" problem

```points = [0 0; 1 0; 1 1; 0 1; 0 2; 1 2];
x = points(:,1);
y = points(:,2);

plot(x,y,x,y,'o');
axis([-0.5 1.5 -0.5 2.5]);
title('Waypoints for N-Point example');
```

We can not fit a spline directly to the x-y data since the x data values are not monotonically increasing.

```% Instead, we need to introduce an independent variable that is always
% monotonically increasing, like time!
```

First, let's define the waypoints

```t = [0 1 2 3 4 5];
points = [0 0; 1 0; 1 1; 0 1; 0 2; 1 2];
x = points(:,1);
y = points(:,2);
```

Then, we'll plot the original waypoints in t-x, t-y and x-y space

```figure;
plot(t,x,t,x,'o');
title('x vs. t');
axis([-0.5 5.5 -0.5 1.5]);

figure;
plot(t,y,t,y,'o');
title('y vs. t');
axis([-0.5 5.5 -0.5 2.5]);

figure;
plot(x,y,x,y,'o');
title('y vs. x');
axis([-0.5 1.5 -0.5 2.5]);
```

To calculate the spline for waypoints, we will use the spline function

```tq = 0:0.1:5;
slope0 = 0;
slopeF = 0;
xq = spline(t,[slope0; x; slopeF],tq);
yq = spline(t,[slope0; y; slopeF],tq);

% plot spline in t-x, t-y and x-y space
figure;
plot(t,x,'o',tq,xq,':.');
axis([-0.5 5.5 -0.5 1.5]);
title('x vs. t');

figure;
plot(t,y,'o',tq,yq,':.');
axis([-0.5 5.5 -0.5 2.5]);
title('y vs. t');

figure;
plot(x,y,'o',xq,yq,':.');
axis([-0.5 1.5 -0.5 2.5]);
title('y vs. x');
```

## Interp1

Interp1 can be used to create a trajectory as long as the user does not need to define the slope at the beginning and end.

If this were the desired trajectory for a mobile robot, the robot would be requested to go from v = 0 to v = "some value" immediately causing the required torque to spike.

```xq = interp1(t,x,tq,'spline');
yq = interp1(t,y,tq,'spline');

figure;
plot(t,x,'o',tq,xq,':.');
axis([-0.5 5.5 -0.5 1.5]);
title('x vs. t');

figure;
plot(t,y,'o',tq,yq,':.');
axis([-0.5 5.5 -0.5 2.5]);
title('y vs. t');

figure;
plot(x,y,'o',xq,yq,':.');
axis([-0.5 1.5 -0.5 2.5]);
title('y vs. x');

close all
```

Open example for Trajectory Planning

Open Example