Copra

This is library has been developed to answer to the model preview control problem. It solves discrete systems using a QP solver. The library has been design to be fast and user-friendly. Python bindings are also provided.

It is also my first big library! In this article i will describe to what a mpc is made for, why you should use it and how.

So, what is a MPC?

MPC stands for Model Predictive Control. It is used to solve a problem in a horizon of time by minimizing a cost function. Be aware that the library here is made to solve Linear Time Invariant (LTI) discrete systems. This means that the behavior of the system does not change through time.

So, in simple words, you can use a mpc to find the control that leads to a given trajectory/target.

Continuous system

Generally, all start with a beautiful continuous system. As it is hard to solve a continuous system fastly, a discretization is done, and this system can be solved by the MPC.

First, let’s see what kind of system can work. Any system of the form

\[\mathbf{\dot{s}} = A_c\mathbf{s} + B_c\mathbf{u} + \mathbf{d}_c\]

is a candidate. $\mathbf{s}$ is the state vector of the system and $\mathbf{u}$ is the control vector. After discretization with a time step $T$, the system becomes:

\[\mathbf{s}_{k+1} = A\mathbf{s}_k + B\mathbf{u}_k + \mathbf{d}\]

where $A$, $B$ and $\mathbf{d}$ are respectively the state matrix, the control matrix and the bias vector. To understand more about how to get the discretized system, Wikipedia is your friend

How the horizon is computed

To show how the MPC finds out the solution, we need to develop the equation above. We need first to perform a recurrence and see what comes out at a step $k$.

\[\begin{align} \mathbf{x}_1 & = A\mathbf{x}_0 + B\mathbf{u}_0 + \mathbf{d} \\\\ \mathbf{x}_2 & = A\mathbf{x}_1 + B\mathbf{u}_1 + \mathbf{d} \\\\ & = A(A\mathbf{x}_0 + B\mathbf{u}_0 + \mathbf{d}) + B\mathbf{u}_1 + \mathbf{d} \\\\ & = A^2\mathbf{x}_0 + [AB\ B][\mathbf{u}^T_0\ \mathbf{u}^T_1]^T + A\mathbf{d} + \mathbf{d} \\\\ \mathbf{x}_3 & = A\mathbf{x}_2 + B\mathbf{u}_2 + \mathbf{d} \\\\ & = A(A^2\mathbf{x}_0 + [AB\ B][\mathbf{u}^T_0\ \mathbf{u}^T_1]^T + A\mathbf{d} + \mathbf{d}) + B\mathbf{u}_1 + \mathbf{d} \\\\ & = A^3\mathbf{x}_0 + [A^2B\ AB\ B][\mathbf{u}^T_0\ \mathbf{u}^T_1\ \mathbf{u}^T_2]^T + A^2\mathbf{d} + A\mathbf{d} + \mathbf{d} \end{align}\]

It is then easy to show that the recurrence of $N$ steps can be written:

\[\mathbf{X}_N = \Phi\mathbf{x}_0 + \Psi\mathbf{U}_{N-1} + \mathbf{\xi}\]

where we have

\[\Phi = \left[ \begin{array}{c} I \\ A \\ A^2 \\ \vdots \\ A^N \end{array} \right], \ \Psi = \left[ \begin{array}{cccc} 0 & 0 & \cdots & 0 \\ B & 0 & \cdots & 0 \\ AB & B & \ddots & \vdots \\ \vdots & \vdots & \ddots & 0 \\ A^{N-1}B & A^{N-2}B & \cdots & B \end{array} \right], \ \mathbf{\xi} = \left[ \begin{array}{c} \mathbf{0} \\ \mathbf{d} \\ A\mathbf{d} + \mathbf{d} \\ \vdots \\ \sum_{i=0}^{N-1} A^{i}\mathbf{d} \end{array} \right]\]

and the whole-state vector $\mathbf{X}$ and whole-control vector $\mathbf{U}$ are

\[\mathbf{X} = \left[ \begin{array}{c} \mathbf{x}_0 \\ \mathbf{x}_1 \\ \vdots \\ \mathbf{x}_N \end{array} \right], \ \mathbf{U} = \left[ \begin{array}{c} \mathbf{u}_0 \\ \mathbf{u}_1 \\ \vdots \\ \mathbf{u}_{N-1} \end{array} \right].\]

Finally, given a desired target/trajectory $\mathbf{X}_T$ we compute the horizon using a QP solver with a desired cost function.

Let’s dive into the library

The definition of the matrices above is really important to have a proper use of the library. The library has been developed using c++14 tools and it uses eigen matrix library internally. Later, i will call step-matrix (resp. step-vector), a matrix (resp. vector) that has the dimension of one step (same dimension as $\mathbf{x}_k$) and whole-matrix (resp. whole-vector), a matrix (resp. vector) that has the full dimension (same dimension as $\mathbf{X}$ ).

The system

The system is written in the PreviewSystem.h file. It defines the whole system and creates the $\Phi$, $\Psi$ and $\mathbf{\xi}$ matrices and vector. It is better to create a shared pointer of the instance.

auto ps = std::make_shared<copra::PreviewSystem>();
// Initialize the system
ps->system(A, B, d, x_0, nrStep);
// Initialize the system in the constructor
auto ps = std::make_shared<copra::PreviewSystem>(A, B, d, x_0, nrStep);

The mpc

The mpc is written in the LMPC.h file. Creating an instance of an mpc is also fairly easy.

copra::LMPC controller(ps);
// Build the system and solve the mpc
controller.solve();

It is also possible to (re)initialize the system used by the mpc.

copra::LMPC controller();
// (Re)initialize the mpc with the new system
controller.initializeController(ps);

To get the results, you just need to call it :)

Eigen::VectorXd trajectory = controller.trajectory();
Eigen::VectorXd control = controller.control();

The constraints

The library provides several types of constraints. All the constraints can be created using step-matrix or whole-matrix. We will see that a mixed of the two is also possible. They also need to be wrapped in a shared pointer in order to be passed to the mpc.

Trajectory Constraints

We distinguish two types of trajectory constraints: The trajectory constraint $E\mathbf{X} \leq \mathbf{f}$ or $E\mathbf{X} = \mathbf{f}$ and the trajectory bound constraint $\underline{\mathbf{X}} \leq \mathbf{X} \leq \overline{\mathbf{X}}$. Note that the latter can be rewritten to produce the former.

// Create an inequality constraint
// E is step-matrix or whole-matrix, f is step-vector or whole-vector.
auto ineqTrajConstr = std::make_shared<copra::TrajectoryConstraint>(E, f);
// Create an equality constraint
auto eqTrajConstr = std::make_shared<copra::TrajectoryConstraint>(E, f, false);
// Create a bound constraint
// lower and upper are step-vector or whole-vector
auto boundTrajConstr = std::make_shared<copra::TrajectoryBoundConstraint>(lower, upper);

Note: to assign infinite born, you have to include the stl <limits> and use std::numeric_limits<double>::max(). In Python side, you have to use float('Inf').

Control Constraints

This is pretty much the same idea as above, there is two type: The control constraint $G\mathbf{U} \leq \mathbf{f}$ or $G\mathbf{U} = \mathbf{f}$ and the control bound constraint $\underline{\mathbf{U}} \leq \mathbf{U} \leq \overline{\mathbf{U}}$.

// Create an inequality constraint
// G is step-matrix or whole-matrix, f is step-vector or whole-vector.
auto ineqControlConstr = std::make_shared<copra::ControlConstraint>(E, f);
// Create an equality constraint
auto eqControlConstr = std::make_shared<copra::ControlConstraint>(E, f, false);
// Create a bound constraint
// lower and upper are step-vector or whole-vector
auto boundControlConstr = std::make_shared<copra::ControlBoundConstraint>(lower, upper);

Mixed Constraints

Mixed constraints allow you to add constraints that involve both the trajectory and the control. It is written as $E\mathbf{X} + G\mathbf{U} \leq \mathbf{f}$ or $E\mathbf{X} + G\mathbf{U} = \mathbf{f}$. As always:

// Create an inequality constraint
// E is step-matrix or whole-matrix,  G is step-matrix or whole-matrix, f is step-vector or whole-vector.
auto ineqMixedConstr = std::make_shared<copra::MixedConstraint>(E, G, f);
// Create an equality constraint
auto eqMixedConstr = std::make_shared<copra::MixedConstraint>(E, G, f, false);

Adding and updating a constraint

All that needs to be done is to make the mpc aware of the constraints. It will update the constraints when its solve function is called.

// Adding an inequality control constraint
controller.addConstraint(ineqControlConstr);

Cost functions

As for constraints, the library provides several types of cost functions. All the costs (but one) can be created using step-matrix or whole-matrix. They also need to be wrapped in a shared pointer in order to be passed to the mpc and may more than one.

Target Cost

The target cost function minimizes $||M\mathbf{x}_N + \mathbf{p}||$. $M$ must be a step-matrix and $p$ must be a step-vector.

// Create a target cost
auto targetCost = std::make_shared<copra::TargetCost>(M, p);

Trajectory Cost

The trajectory cost function minimizes $||M\mathbf{X} + \mathbf{p}||$.

// Create a trajectory cost
// M is step-matrix or whole-matrix, p is step-vector or whole-vector.
auto trajectoryCost = std::make_shared<copra::TrajectoryCost>(M, p);

Control Cost

The control cost minimizes $||N\mathbf{U} + \mathbf{p}||$.

// Create a control cost
// M is step-matrix or whole-matrix, p is step-vector or whole-vector.
auto ControlCost = std::make_shared<copra::ControlCost>(N, p);

Mixed Cost

The mixed cost minimizes $||M\mathbf{X} + N\mathbf{U} + \mathbf{p}||$.

// Create a mixed cost
// M is step-matrix or whole-matrix, N is step-matrix or whole-matrix, p is step-vector or whole-vector.
auto mixedCost = std::make_shared<copra::MixedCost>(M, N, p);

Adding and updating a cost function

All that needs to be done is to make the mpc aware of the costs. It will update the costs when its solve function is called.

// Adding a target cost
controller.addCost(targetCost);

Cost and constraint specificities

There are several helper tools for cost functions and constraints.

Removal

There are two ways of deleting a constraint/cost from the mpc. The main way is to let the mpc handles it. In the solve() function and at the end, the mpc checks whether or not a constraint needs to be deleted. A constraint/cost is deleted if the user (you) has deleted his shared pointer of the constraint. If you keep a shared pointer of the constraint/cost alive, the constraint/cost remains in the mpc. Another way is to call the function removeCost()/removeConstraint()

The autoSpan function

The autoSpan() function is present in all constraints and cost. This method needs to be called if you have created a constraint/cost that mixes step-matrix/vector with whole-matrix/vector. For example, if you create a control constraint with a step-matrix $G$ and a whole-vector $\mathbf{f}$, you can pass the two matrices to the constraint and then call the autoSpan function.

Changing the QP solver

Depending on your compilation state, 4 QP solvers are implemented. You can change at anytime the QP to use for solving your problem. The current available QP are:

QuadProg (Default one)
QLD
Gurobi (need license)
LSSOL (need commercial license)
OSQP

To change the QP:

// Create an instance of a mpc with a different QP
copra::LMPC controller(copra::SolverFlag::QLD)
// Change the QP at any time
controller.selectQPSolver(copra::SolverFlag::GUROBIDense)

If the default setting of a QP solver is not enough, it is possible to give the controller a user-defined QP.

// Create your QP
std::unique_ptr<Eigen::QLD> solver = solverFactory(copra::SolverFlag::QLD);
// Make change here
solver->SI_feasibilityTolerance(1e-8);
// ...
// Give it to the QP
controller.useSolver(std::move(solver));

Python users

Python users can use all the library, the same way as C++ users. There are some change though. To feed C++ functions Eigen types, converters have been created using pygen-convert. This allows the user to provide functions either numpy arrays or python lists. You need to be careful that the python type are float (which is also a float64 in Numpy). For example, [1, 2, 3] is a list of int and [1., 2., 3.] is a list of float. Finally, to respect the PEP8 norm, functions have been rewritten in lower case with words separated by underscores.

Improving performance and building time

First, you can measure building and solving time of the mpc using solveTime() and solveAndBuildTime() functions. This computes internally the time needed to build the problem and to solve it.

Updating the system yourself

The initialization of the preview system is just meant to allocate memory for all the needed matrices ($\Phi$, $\Psi$, etc…) Normally, when calling the solve() function of the mpc, there is no need to do anything else. If those matrices are not updated yet, then they will be built before solving the problem. In the case you can (and want to) update the system yourself, you just need to call the updateSystem() function of the preview system. The solve() function will be then faster since it does not have to build the preview system.

Note that you have a full control of the preview system. You can voluntary modify the matrices inside the preview system if you have specific matrices. Don’t forget to (re)initialize the mpc if you have changed the dimension of the problem.

Give rvalue to the constraints and costs

Constraints and costs constructor and weights functions use universal reference to allow move semantic of rvalue and copy of lvalue. In the case you don’t need to keep the constraint matrices, prefer rvalue as parameters. This feature is not available for Python users.

Privilege step-matrix and step-vector

The library lets you use both step-matrix/vector and whole-matrix/vector. When only step-matrix and step-vector are given and because of the sparsity of the problem, the computation will run faster.

Sparse matrix will be used to improve performance in future release.

Examples

You find a c++ example here and a Python example here

Pymanoid

Pymanoid is library developed in Python by Stéphane Caron which allows you to use the mpc for motion planning and ele.