# Quintic Polynomial Planner

We can describe one-dimensional robot motion as a quintic polynomial based on distance as

$x(s) = a_0 + a_1s + a_2s^2 + a_3s^3 + a_4s^4 + a_5s^5$

The boundary conditions are $x_i, v_i, a_i, x_f, v_f, a_f$ where

$x_i=\text{initial position}$
$v_i=\text{initial velocity}$
$a_i=\text{initial acceleration}$
$x_f=\text{final position}$
$v_f=\text{final velocity}$
$a_f=\text{final acceleration}$

When $s=0$, the polynomial can be represented as
$x(0)=a_0$
$\therefore x_i=a_0$

To solve for velocity, we can differentiate the polynomial and represent it as

$\frac{dx}{dt}=a_1+2a_2s+3a_3s^2+4a_4s^3+5a_5s^4$

We can also solve the equation for initial velocity similarly, where $s=0$, represented by the following as

$\frac{dx}{dt}=a_1$
$\therefore v_i=a_1$

It is now obvious that the initial acceleration can be represented as

$\frac{d^2x}{dt^2}=2a_2$
$\therefore a_i=2a_2$

To solve for the remaining three unknowns, we can assume that the final distance for a manoeuvre is $S$ and thus, represented as

$x(S)=a_0+a_1S+a_2S^2+a_3S^3+a_4S^4+a_5S^5$
$\frac{dx}{dS}=a_1+2a_2S+3a_3S^2+4a_4S^3+5a_5S^4$
$\frac{d^2x}{dS^2}=2a_2+6a_3S+12a_4S^2+20a_5S^3$

We can represent these three equations in the form of a linear equation to solve for $a_3, a_4, a_5$

$\begin{bmatrix} S^3 & S^4 & S^5 \\ 3S^2 & 4S^3 & 5S^4 \\ 6S & 12S^2 & 20S^3 \end{bmatrix} \begin{bmatrix} a_3 \\ a_4 \\ a_5 \end{bmatrix} = \begin{bmatrix} x_f-x_i-v_iS-0.5a_iS^2 & v_f-v_i-a_iS & a_f-a_i \end{bmatrix}$

However, for two-dimensional robot motion, there are now two more terminal states, $\theta_i$ and $\theta_f$ where

$\theta_i=\text{initial orientation}$
$\theta_f=\text{final orientation}$

Each velocity and acceleration boundary condition can be calculated with each orientation, represented as

$v_{xi}=v_i\cos(\theta_i), v_{xf}=v_f\cos(\theta_f)$
$v_{yi}=v_i\sin(\theta_i), v_{yf}=v_f\sin(\theta_f)$

$a_{xi}=a_i\cos(\theta_i), a_{xf}=a_f\cos(\theta_f)$
$a_{yi}=a_i\sin(\theta_i), a_{yf}=a_f\sin(\theta_f)$

My python implementation for generating a quintic polynomial trajectory:

import numpy as np

class QuinticPolynomial:

    def __init__(self, init_pos, init_vel, init_accel, final_pos, final_vel, final_accel, dist):

        # Derived coefficients
        self.a_0 = init_pos
        self.a_1 = init_vel
        self.a_2 = init_accel / 2.0

        # Solve the linear equation (Ax = B)
        A = np.array([[dist ** 3,      dist ** 4,        dist ** 5], 
                     [3 * dist ** 2,   4 * dist ** 3,    5 * dist ** 4],
                     [6 * dist,        12 * dist ** 2,   20 * dist ** 3]])

        B = np.array([final_pos - self.a_0 - self.a_1 * dist - self.a_2 * dist ** 2, final_vel - self.a_1 - init_accel * dist, final_accel - init_accel])

        x = np.linalg.solve(A, B)

        self.a_3 = x[0]
        self.a_4 = x[1]
        self.a_5 = x[2]

    def calc_point(self, s):

        xs = self.a_0 + self.a_1 * s + self.a_2 * s ** 2 + self.a_3 * s ** 3 + self.a_4 * s ** 4 + self.a_5 * s ** 5

        return xs

    def calc_first_derivative(self, s):

        xs = self.a_1 + 2 * self.a_2 * s + 3 * self.a_3 * s ** 2 + 4 * self.a_4 * s ** 3 + 5 * self.a_5 * s ** 4

        return xs

    def calc_second_derivative(self, s):

        xs = 2 * self.a_2 + 6 * self.a_3 * s + 12 * self.a_4 * s ** 2 + 20 * self.a_5 * s ** 3

        return xs

    def calc_third_derivative(self, s):

        xs = 6 * self.a_3 + 24 * self.a_4 * s + 60 * self.a_5 * s ** 2

        return xs

def quintic_polynomial_planner(x_i, y_i, yaw_i, v_i, a_i, x_f, y_f, yaw_f, v_f, a_f, max_accel, max_jerk, ds):
    
    # Calculate the velocity boundary conditions based on vehicle's orientation
    v_xi = v_i * np.cos(yaw_i)
    v_xf = v_f * np.cos(yaw_f)
    v_yi = v_i * np.sin(yaw_i)
    v_yf = v_f * np.sin(yaw_f)

    # Calculate the acceleration boundary conditions based on vehicle's orientation
    a_xi = a_i * np.cos(yaw_i)
    a_xf = a_f * np.cos(yaw_f)
    a_yi = a_i * np.sin(yaw_i)
    a_yf = a_f * np.sin(yaw_f)

    for S in np.arange(1.0, 1000.0, 1.0):
        # Initialise the class
        xqp = QuinticPolynomial(x_i, v_xi, a_xi, x_f, v_xf, a_xf, S)
        yqp = QuinticPolynomial(y_i, v_yi, a_yi, y_f, v_yf, a_yf, S)

        # Instantiate/clear the arrays
        x = []
        y = []
        v = []
        a = []
        j = []
        yaw = []

        for s in np.arange(0.0, S + ds, ds):
            # Solve for position (m)
            x.append(xqp.calc_point(s))
            y.append(yqp.calc_point(s))

            # Solve for velocity (m/s)
            v_x = xqp.calc_first_derivative(s)
            v_y = yqp.calc_first_derivative(s)
            v.append(np.hypot(v_x, v_y))

            # Solve for orientation (rad)
            yaw.append(np.arctan2(v_y, v_x))

            # Solve for acceleration (m/s^2)
            a_x = xqp.calc_second_derivative(s)
            a_y = yqp.calc_second_derivative(s)

            if len(v) >= 2 and v[-1] - v[-2] < 0.0:
                a.append(-1.0 * np.hypot(a_x, a_y))

            else:
                a.append(np.hypot(a_x, a_y))
        
            # Solve for jerk (m/s^3)
            j_x = xqp.calc_third_derivative(s)
            j_y = yqp.calc_third_derivative(s)
            
            if len(a) >= 2 and a[-1] - a[-2] < 0.0:
                j.append(-1 * np.hypot(j_x, j_y))

            else:
                j.append(np.hypot(j_x, j_y))

        if max([abs(i) for i in a]) <= max_accel and max([abs(i) for i in j]) <= max_jerk:
            break

    return x, y, v, a, j, yaw