# Motion Planning for Mobile Robots -- Assignment 05 Minimum-Snap Trajectory Generation
**NOTE** Please open this in **VSCode** with **MATLAB plugin**
Solution guide for **Assignment 05, Minimum-Snap Trajectory Generation**.
---
## Introduction
Welcome to **Solution Guide for Assignment 05**! Here I will guide you through the **MATLAB** implementations of both
* **Minimum-Snap Numeric Solver**
* **Minimum-Snap Analytic Solver**
---
## Q & A
Please send e-mail to alexgecontrol@qq.com with title **Motion-Planning-for-Mobile-Robots--Assignment-04--Q&A-[XXXX]**. I will respond to your questions at my earliest convenience.
**NOTE**
* I will **NOT** help you debug your code and will only give you suggestions on how should you do it on your own.
---
## Minimum-Snap, Numeric Solver
### Overview
The workflow of **numeric solver** can be summed up as follows:
* Build **objective matrix**, which is defined by **Minimum-Snap** or **Minimum-Jerk**
* Build **equality constraint matrix**, which is defined by:
* **Boundary conditions**, **start** / **end** ego states
* **Intermediate waypoint continuity**
* Solve the above **QP** problem
* Use the result monomial coeffs. to generate the optimal trajectory
The workflow can be implemented in MATLAB as follows:
```matlab
% plan the minimum snap trajectory
poly_coef_x = MinimumSnapQPSolver(path(:, 1), ts, K, t_order);
poly_coef_y = MinimumSnapQPSolver(path(:, 2), ts, K, t_order);
% display the trajectory
X_n = [];
Y_n = [];
k = 1;
tstep = 0.01;
for i=0:K-1
%#####################################################
% STEP 3: get the coefficients of i-th segment
%#####################################################
% 1. get segment index:
segment_index = (i*N + 1):((i+1)*N);
% 2. extract segment coeffs:
Pxi = flipud(poly_coef_x(segment_index));
Pyi = flipud(poly_coef_y(segment_index));
% 3. calculate planned waypoints:
for t = 0:tstep:ts(i+1)
X_n(k) = polyval(Pxi, t / ts(i + 1));
Y_n(k) = polyval(Pyi, t / ts(i + 1));
k = k + 1;
end
end
% minimum snap trajectory generator:
function poly_coef = MinimumSnapQPSolver(waypoints, ts, K, t_order)
start_cond = [waypoints(1), 0, 0, 0];
end_cond = [waypoints(end), 0, 0, 0];
%#####################################################
% TODO -- STEP 1: compute Q of p'Qp
%#####################################################
%#####################################################
% TODO -- STEP 2: compute Aeq and beq
%#####################################################
%#####################################################
% TODO -- STEP 3: solve the problem with QP
%#####################################################
end
```
### Objective Matrix
The objective matrix is defined by **the L2-norm of the optimization target**:
* **Minimum Snap**, which is equivalent to **t_order = 4** in the implementation below
* **Minimum Jerk**, which is equivalent to **t_order = 3** in the implementation below
```matlab
function Q = getQ(K, t_order, ts)
% num. of polynomial coeffs:
N = 2*t_order;
% ###############################################
% 1. pre-compute constants used in Q construction
% ###############################################
% 1.1 factorial from derivative
Q_k = zeros(N - t_order);
Q_v = zeros(N - t_order);
for n = t_order:(N - 1)
Q_k(n - t_order + 1) = n;
Q_v(n - t_order + 1) = factorial(n) / factorial(n - t_order);
end
Q_factorial = containers.Map(Q_k, Q_v);
% 1.2 time power:
ts_power = ts .^ t_order;
% ###############################################
% 2. populate Q
% ###############################################
Q_i = [];
Q_j = [];
Q_v = [];
index = 1;
for k = 1:K
for m = t_order:(N - 1)
for n = t_order:(N - 1)
% TODO -- fill in elements of Q
index = index + 1;
end
end
end
Q = sparse(Q_i, Q_j, Q_v);
end
```
**Implementation Nodes**
* First, the above code uses **time normalization** to improve numeric stability
<img src="images/hints/01-time-normalization.png" alt="Normalize Each Trajectory Segment Duration to 1.0" width="100%">
* Pay attention to the extra multiplication of **ts** introduced by change of variable in integration
* Here **sparse matrix** is used for efficient matrix manipulation. Please search the corresponding doc in MATLAB for details.
### Equality Constraint Matrix
The equality constraint matrix consists of following constraints:
* **Boundary Conditions**, which are defined by **start** and **end** states of target trajectory
* **Intermediate Waypoint Continuity**, which requires the planned trajectory should be **t_order - 1** order continuous at each intermediate waypoint
```matlab
function [Aeq, beq]= getAbeq(K, t_order, waypoints, ts, start_cond, end_cond)
% num. of polynomial coeffs:
N = 2*t_order;
% num. of constraints:
C = (K + 1)*t_order + (K - 1);
% ###############################################
% 1. pre-compute constants used in A construction
% ###############################################
% 1.1 factorial from derivative
A_factorial_k = [];
A_factorial_v = [];
index = 1;
for c = 1:t_order
for n = c:N
A_factorial_k(index) = (c-1)*N + n - 1;
A_factorial_v(index) = factorial(n - 1) / factorial(n - c);
index = index + 1;
end
end
A_factorial = containers.Map(A_factorial_k, A_factorial_v);
% ###############################################
% 2. populate A & b
% ###############################################
A_i = [];
A_j = [];
A_v = [];
beq = zeros(C, 1);
index = 1;
c_index = 1;
% 2.1 start & goal states:
for c = 1:t_order
% TODO -- fill in start state:
index = index + 1;
% move to next constraint:
c_index = c_index + 1;
% TODO -- fill in end state:
for n = c:N
index = index + 1;
end
% move to next constraint:
c_index = c_index + 1;
end
% 2.2 intermediate waypoint passing constraints:
for k = 1:(K - 1)
% TODO -- next segment start position:
index = index + 1;
% TODO -- should equal to the specified value:
% move to next constraint:
c_index = c_index + 1;
end
% 2.3 intermediate waypoint continuity constraints:
for c = 1:t_order
for k = 1:(K - 1)
% TODO -- current segment end state:
for n = c:N
index = index + 1;
end
% TODO -- should equal to next segment start state:
index = index + 1;
% move to next constraint:
c_index = c_index + 1;
end
end
Aeq = sparse(A_i, A_j, A_v);
end
```
**Implementation Nodes**
* According to the original paper, if your objective function is the L2-norm of **t_order** trajectory derivative, then the trajectory should be **t_order - 1** order continuous at each intermediate waypoint.
---
## Minimum-Snap, Analytic Solver
### Overview
The workflow of **analytic solver** can be summed up as follows:
* Build **objective matrix**, which is defined by **Minimum-Snap** or **Minimum-Jerk**. This is the same as previous question.
* Map **equality constraints** to **actual decision variables**:
* The **equality constraints** are the same as previous question
* The **actual decision variables** are the **t_order - 1** derivatives at the start and end of each trajectory segment
* Solve the above **least square** problem.
* Map the optimal decision variables to monomial coe