General Splines Library

This is a library to compute generalized splines passing through a series of points given a sequence of intervals.

Motivation

Generalized splines appear naturally in problems of trajectory optimization when waypoint constraints are added. In other words, if we desire to optimize a motion which pass trough a sequence of positions we will meet with generalized splines.

Generalized splines trajectories arising in such kind of optimization problems are uniquely characterized by the waypoints that they attain, the time intervals between waypoints and the speed, and possible higher order derivatives at the boundaries. Moreover, such a relation is synthesised in the expression of the type

$\mathbf{A}(\boldsymbol\tau)\mathbf{y} = \mathbf{b}(\mathbf{w})\ \ \ \ \ \ \ \ \ \ \ \ \ \ (0)$

where $\mathbf{A}(\boldsymbol\tau)$ is a matrix which depends on the time intervals $\boldsymbol\tau$ , $\mathbf{b}(\mathbf{w})$ is a column vector which depends on the waypoints $\mathbf{w}$ , the speed, and possible higher order derivatives at the boundaries, and $\mathbf{y}$ is a column vector which represents uniquely the curve at each interval.

The main challenge to build into a computer a trajectory optimization problems with waypoint constraints is to compute the derivatives of $\mathbf{y}$ with respect to $\mathbf{\tau}$ .

This library provides a uniform and simple interface to formulate gradient-based optimization problems for waypoint-constrained trajectory planning. The library leverage on the representation (0) to compute the "derivatives of the splines" with respect to the time intervals (and possibly the waypoints) as the corresponding derivatives of $\mathbf{y}$

Background

This library is aimed to find a trajectory passing trough a sequence of waypoints $\{\mathbf{w}_0, ...,\mathbf{w}_{N + 1}\}$ such that the following integral is minized

$I=\int_0^T \alpha_1\left\|\frac{\mathsf{d}\mathbf{q}}{\mathsf{d} t }\right\|^2 + \alpha_2 \left\|\frac{\mathsf{d}^2\mathbf{q}}{\mathsf{d} t^2 }\right\|^2 + \alpha_3\left\|\frac{\mathsf{d}^3\mathbf{q}}{\mathsf{d} t^3 }\right\|^2 + \alpha_4\left\|\frac{\mathsf{d}^4\mathbf{q}}{\mathsf{d} t^4 }\right\|^2 \mathsf{d} t \ \ \ \ \ (1)$

It may be proven that such a problem can be subdivided in two steps

Find the family of optimal curves that joint waypoints
Compute time instants $\{t_0, t_1, ...,t_N, t_{N + 1}\}$ where the optimal curves must be attached

The step 1. is done by solving a linear ordinary differential equation. One method to achieve 2. is to formulate an optimization problem (e.g. a gradient based one).

Optimal curves

We underline that this library leverages on the general theory of linear ODEs. It may be proven that any optimal of (1) solves the following linear ODE, which turn out to be the Euler-Lagrange equations at each interval $[t_i, t_{i + 1}]$

$-\alpha_1\frac{\mathsf{d}^2\mathbf{q}}{\mathsf{d} t^2 } + \alpha_2 \frac{\mathsf{d}^4\mathbf{q}}{\mathsf{d} t^4 } - \alpha_3\frac{\mathsf{d}^6\mathbf{q}}{\mathsf{d} t^6 } + \alpha_4 \frac{\mathsf{d}^8\mathbf{q}}{\mathsf{d} t^8 } = 0\ \ \ \ \ (2)$

with the following boundary conditions

$\mathbf{q}(t_i) = \mathbf{w}_i\ \ \ \ \ \ \ \mathbf{q}(t_{i+1}) = \mathbf{w}_{i+1}\ \ \ \ \ \ \ \ \ \ \ \ \ \ (3)$

Because the ODE (2) is linear, we can compute its general suction depending on the value of the coefficients

$\alpha_i$

In fact, the general solution of (2) may be written as a piecewise function defined at each interval as

$\mathbf{q} = \sum_{i=1}^{n_b} \mathbf{y}_i^j B_i(t) \ \ \ \ \text{if}\ \ \ \ t \in [t_{j}, t_{j + 1}]\ \ \ \ \ \ \ \ \ \ \ \ \ \ \ \ (4)$

where $n_b$ , $B_i(t)$ depend on the coefficients $\alpha_i$ and $\mathbf{y}_i^j$ are column vectors in which represents the curve uniquely at the interval $[t_j, t_{j + 1}]$ .

If we stack the column vectors $\mathbf{y}_i^j$ in a suitable way we obtain the column vector $\mathbf{y}$ used in (0). In fact, the equation (0) is obtained after applying to (4) the waypoint constrains and the boundary conditions.

After substituting (4) in (1) we obtain the following expression

$I=\mathbf{y}^\top \mathbf{Q}(\boldsymbol{\tau}) \mathbf{y}\ \ \ \ \ \ \ \ \ \ \ \ \ (5)$

Finally we can substitute (0) in (5) to obtain the representation of (1) subject to the waypoint constrains as a function of $N$ real variables:

$I=I(\boldsymbol{\tau})=\mathbf{b}^{\top}\mathbf{A}^{-\top}(\boldsymbol{\tau})\mathbf{Q}(\boldsymbol{\tau}) \mathbf{A}^{-1}(\boldsymbol{\tau})\mathbf{b}\ \ \ \ \ \ \ \ \ \ \ \ \ (6)$

Software architecture

From the formalization of the optimization problem, we derive that a flexible and uniform methodology for the construction of the problem of optimizing (1) consists in designing an abstract representation of the basis $B_i(t)$ in (4) capable of building in an automatic fashion the constraint (0), the cost function (5) and their derivatives.

In fact, note that the input of any gradient-based optimizer is the expression (6) and its derivatives. This library provides a template class to represent the basis $B_i(t)$ and a series of procedures which utilizes these basis as an input and then generate (0), (6) and their derivatives.

Implemented basis

Up to now we have implemented tree types of basis

Basis for the minimum jerk problems, called cBasis0010, because their optimize the L2-norm of the jerk

$I=\int_0^T \left\|\frac{\mathsf{d}^3\mathbf{q}}{\mathsf{d} t^3 }\right\|^2 d t$

Basis for the weighed speed-jerk problems, called cBasis1010, because their optimize convex combination of the L2-norm of the speed and the L2 norm of the jerk

$I=\int_0^T \alpha \left\|\frac{\mathsf{d}\mathbf{q}}{\mathsf{d} t }\right\|^2 + (\alpha-1)\left\|\frac{\mathsf{d}^3\mathbf{q}}{\mathsf{d} t^3 }\right\|^2 d t$

Requirements

numpy
scipy
matplotlib