osqp-eigen  0.3.0
osqp-eigen

A simple C++ Eigen wrapper for osqp library.

Author
Giulio Romualdi giuli.nosp@m.o.ro.nosp@m.muald.nosp@m.i@ii.nosp@m.t.it

How to build the wrapper

Please run the following command in your terminal

1 git clone https://github.com/robotology/osqp-eigen.git
2 cd osqp-eigen
3 mkdir build && cd build
4 cmake ../
5 make
6 [sudo] make install
Note
sudo is not necessary if you specify the CMAKE_INSTALL_PREFIX. In this case it is necessary to add in the .bashrc the following lines:
1 export OsqpEigen_DIR=/path/where/you/installed/

How to embed the wrapper inside your project

osqp-eigen provides native CMake support which allows the library to be easily used in CMake projects.

Note
CMake 3.0 (or later) is required to enable this functionality.

osqp-eigen exports a CMake target called OsqpEigen::OsqpEigen which can be imported using the find_package CMake command and used by calling target_link_libraries as fllows:

1 cmake_minimum_required(VERSION 3.0)
2 find_package(OsqpEigen REQUIRED)
3 add_executable(myproject src/main.cpp)
4 target_link_libraries(myproject OsqpEigen::OsqpEigen osqp::osqp Eigen3::Eigen)

Eigen and osqp libraries are required by the wrapper.

How to implement an MPC controller with osqp-eigen

In this section a simple implementation of a MPC controller using osqp-eigen is shown. Here you can find the original version of this example.

For further details please refer to the code.

Problem

The problem is to develop a controller that allows a linear system to track a constant reference state \(x_r\). This kind of problem can be solved using a lot of different controller architectures, however in order to write a tutorial for osqp-eigen library the MPC approach will be chosen. Thus we have to find a controller low \(u_0^*\) such that:

\[ \begin{split}\begin{array}{ll} u_0 ^* = \mbox{arg min}_{x_k, u_k} & (x_N-x_r)^T Q_N (x_N-x_r) + \sum_{k=0}^{N-1} (x_k-x_r)^T Q (x_k-x_r) + u_k^T R u_k \\ \mbox{subject to} & x_{k+1} = A x_k + B u_k \\ & x_{\rm min} \le x_k \le x_{\rm max} \\ & u_{\rm min} \le u_k \le u_{\rm max} \\ & x_0 = \bar{x} \end{array}\end{split} \]

where \(Q\), \(Q_N\) and \(R\) are symmetric positive definite matrices; the states \(x_k\) and the inputs \(u_k\) have to be constrained between some lower and upper bounds and the reference state \(x_r\) is

\[ x_r = \begin{bmatrix} 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 \end{bmatrix} ^T \]

MPC to QP

First of all the MPC problem has to be casted to a standard QP problem.

\[ \begin{split}\begin{array}{ll} \mbox{minimize} & \frac{1}{2} x^T P x + q^T x \\ \mbox{subject to} & l \leq A_c x \leq u \end{array}\end{split} \]

where the hessian matrix \(P\) is equal to

\[ P = \text{diag}(Q, Q, ..., Q_N, R, ..., R) \]

while the gradient vector is

\[ q = \begin{bmatrix} -Q x_r \\ -Q x_r \\ \vdots \\ -Q_N x_r \\ 0\\ \vdots\\ 0 \end{bmatrix} \]

The linear constraint matrix \(A_c\) is

\[ A_c = \left[ \begin{array}{ccccc|cccc} -I & 0 & 0 & \cdots & 0 & 0 & 0 & \cdots & 0\\ A & -I & 0 & \cdots & 0 & B & 0 & \cdots & 0 \\ 0 & A & -I & \cdots & 0 & 0 & B & \cdots & 0\\ \vdots & \vdots & \vdots & \ddots & \vdots & \vdots & \vdots & \ddots & \vdots \\ 0 & 0 & 0 & \cdots & -I & 0 & 0 & \cdots & B\\ \hline I & 0 & 0 & \cdots & 0 & 0 & 0 & \cdots & 0\\ 0 & I & 0 & \cdots & 0 & 0 & 0 & \cdots & 0\\ 0 & 0 & I & \cdots & 0 & 0 & 0 & \cdots & 0\\ \vdots & \vdots & \vdots & \ddots & \vdots & \vdots & \vdots & \ddots & \vdots \\ 0 & 0 & 0 & \cdots & I & 0 & 0 & \cdots & 0\\ 0 & 0 & 0 & \cdots & 0 & I & 0 & \cdots & 0\\ 0 & 0 & 0 & \cdots & 0 & 0 & I & \cdots & 0\\ \vdots & \vdots & \vdots & \ddots & \vdots & \vdots & \vdots & \ddots & \vdots \\ 0 & 0 & 0 & \cdots & 0 & 0 & 0 & \cdots & I \end{array} \right] \]

while the upper and the lower bound are

\[ l = \begin{bmatrix} -x_0 \\ 0 \\ \vdots \\ 0 \\ x_{min}\\ \vdots\\ x_{min}\\ u_{min}\\ \vdots\\ u_{min}\\ \end{bmatrix} \quad u = \begin{bmatrix} -x_0 \\ 0 \\ \vdots \\ 0 \\ x_{max}\\ \vdots\\ x_{max}\\ u_{max}\\ \vdots\\ u_{max}\\ \end{bmatrix} \]

Since the osqp-eigen handles only QP problem this operation shall be done by the user. You can find the implementation of the following functions here.

castMPCToQPHessian(Q, R, mpcWindow, hessian);
castMPCToQPGradient(Q, xRef, mpcWindow, gradient);
castMPCToQPConstraintMatrix(a, b, mpcWindow, linearMatrix);
castMPCToQPConstraintVectores(xMax, xMin, uMax, uMin, x0, mpcWindow, lowerBound, upperBound);

Solver initialization

Now you are able to use the OSQP solver. We first create an instance of the solver

// instantiate the solver

when the solver is instantiated the default settings are automatically loaded, however you can change each setting using the following function

solver.settings()->set<Setting>()

where set<Setting>() is a setter function. You can find the list of all the setter functions in the OsqpEigen::Settings class. For example you can use the warm start variables in the optimization problem by calling

solver.settings()->setWarmStart(true);

Now you can set the data of the optimization problem (number of variables, number of constraints and so on)

solver.data()->setNumberOfVariables(numberOfVariable);
solver.data()->setNumberOfConstraints(numberOfConstraints);
if(!solver.data()->setHessianMatrix(hessian)) return 1;
if(!solver.data()->setGradient(gradient)) return 1;
if(!solver.data()->setLinearConstraintMatrix(linearMatrix)) return 1;
if(!solver.data()->setLowerBound(lowerBound)) return 1;
if(!solver.data()->setUpperBound(upperBound)) return 1;

The setter functions return True in case of success and False otherwise.

Now you are able to initialize the solver. All data and settings will be stored inside the osqp struct and the optimization problem will be initialized.

if(!solver.initSolver()) return 1;

The optimization problem can be solved calling the following method

if(!solver.solve()) return 1;

and the solution can be easily got by calling the following method

Eigen::VectorXd QPSolution = solver.getSolution();

If you need to update the bounds constraints and the gradient vector you can use the following methods:

Example

In the following the example of MPC controller is shown.

// osqp-eigen
#include "OsqpEigen/OsqpEigen.h"
// eigen
#include <Eigen/Dense>
#include <iostream>
void setDynamicsMatrices(Eigen::Matrix<double, 12, 12> &a, Eigen::Matrix<double, 12, 4> &b)
{
a << 1., 0., 0., 0., 0., 0., 0.1, 0., 0., 0., 0., 0. ,
0., 1., 0., 0., 0., 0., 0., 0.1, 0., 0., 0., 0. ,
0., 0., 1., 0., 0., 0., 0., 0., 0.1, 0., 0., 0. ,
0.0488, 0., 0., 1., 0., 0., 0.0016, 0., 0., 0.0992, 0., 0. ,
0., -0.0488, 0., 0., 1., 0., 0., -0.0016, 0., 0., 0.0992, 0. ,
0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0.0992,
0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0. ,
0., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0. ,
0., 0., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0. ,
0.9734, 0., 0., 0., 0., 0., 0.0488, 0., 0., 0.9846, 0., 0. ,
0., -0.9734, 0., 0., 0., 0., 0., -0.0488, 0., 0., 0.9846, 0. ,
0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.9846;
b << 0., -0.0726, 0., 0.0726,
-0.0726, 0., 0.0726, 0. ,
-0.0152, 0.0152, -0.0152, 0.0152,
-0., -0.0006, -0., 0.0006,
0.0006, 0., -0.0006, 0.0000,
0.0106, 0.0106, 0.0106, 0.0106,
0, -1.4512, 0., 1.4512,
-1.4512, 0., 1.4512, 0. ,
-0.3049, 0.3049, -0.3049, 0.3049,
-0., -0.0236, 0., 0.0236,
0.0236, 0., -0.0236, 0. ,
0.2107, 0.2107, 0.2107, 0.2107;
}
void setInequalityConstraints(Eigen::Matrix<double, 12, 1> &xMax, Eigen::Matrix<double, 12, 1> &xMin,
Eigen::Matrix<double, 4, 1> &uMax, Eigen::Matrix<double, 4, 1> &uMin)
{
double u0 = 10.5916;
// input inequality constraints
uMin << 9.6 - u0,
9.6 - u0,
9.6 - u0,
9.6 - u0;
uMax << 13 - u0,
13 - u0,
13 - u0,
13 - u0;
// state inequality constraints
xMin << -M_PI/6,-M_PI/6,-OsqpEigen::INFTY,-OsqpEigen::INFTY,-OsqpEigen::INFTY,-1.,
}
void setWeightMatrices(Eigen::DiagonalMatrix<double, 12> &Q, Eigen::DiagonalMatrix<double, 4> &R)
{
Q.diagonal() << 0, 0, 10., 10., 10., 10., 0, 0, 0, 5., 5., 5.;
R.diagonal() << 0.1, 0.1, 0.1, 0.1;
}
void castMPCToQPHessian(const Eigen::DiagonalMatrix<double, 12> &Q, const Eigen::DiagonalMatrix<double, 4> &R, int mpcWindow,
Eigen::SparseMatrix<double> &hessianMatrix)
{
hessianMatrix.resize(12*(mpcWindow+1) + 4 * mpcWindow, 12*(mpcWindow+1) + 4 * mpcWindow);
//populate hessian matrix
for(int i = 0; i<12*(mpcWindow+1) + 4 * mpcWindow; i++){
if(i < 12*(mpcWindow+1)){
int posQ=i%12;
float value = Q.diagonal()[posQ];
if(value != 0)
hessianMatrix.insert(i,i) = value;
}
else{
int posR=i%4;
float value = R.diagonal()[posR];
if(value != 0)
hessianMatrix.insert(i,i) = value;
}
}
}
void castMPCToQPGradient(const Eigen::DiagonalMatrix<double, 12> &Q, const Eigen::Matrix<double, 12, 1> &xRef, int mpcWindow,
Eigen::VectorXd &gradient)
{
Eigen::Matrix<double,12,1> Qx_ref;
Qx_ref = Q * (-xRef);
// populate the gradient vector
gradient = Eigen::VectorXd::Zero(12*(mpcWindow+1) + 4*mpcWindow, 1);
for(int i = 0; i<12*(mpcWindow+1); i++){
int posQ=i%12;
float value = Qx_ref(posQ,0);
gradient(i,0) = value;
}
}
void castMPCToQPConstraintMatrix(const Eigen::Matrix<double, 12, 12> &dynamicMatrix, const Eigen::Matrix<double, 12, 4> &controlMatrix,
int mpcWindow, Eigen::SparseMatrix<double> &constraintMatrix)
{
constraintMatrix.resize(12*(mpcWindow+1) + 12*(mpcWindow+1) + 4 * mpcWindow, 12*(mpcWindow+1) + 4 * mpcWindow);
// populate linear constraint matrix
for(int i = 0; i<12*(mpcWindow+1); i++){
constraintMatrix.insert(i,i) = -1;
}
for(int i = 0; i < mpcWindow; i++)
for(int j = 0; j<12; j++)
for(int k = 0; k<12; k++){
float value = dynamicMatrix(j,k);
if(value != 0){
constraintMatrix.insert(12 * (i+1) + j, 12 * i + k) = value;
}
}
for(int i = 0; i < mpcWindow; i++)
for(int j = 0; j < 12; j++)
for(int k = 0; k < 4; k++){
float value = controlMatrix(j,k);
if(value != 0){
constraintMatrix.insert(12*(i+1)+j, 4*i+k+12*(mpcWindow + 1)) = value;
}
}
for(int i = 0; i<12*(mpcWindow+1) + 4*mpcWindow; i++){
constraintMatrix.insert(i+(mpcWindow+1)*12,i) = 1;
}
}
void castMPCToQPConstraintVectors(const Eigen::Matrix<double, 12, 1> &xMax, const Eigen::Matrix<double, 12, 1> &xMin,
const Eigen::Matrix<double, 4, 1> &uMax, const Eigen::Matrix<double, 4, 1> &uMin,
const Eigen::Matrix<double, 12, 1> &x0,
int mpcWindow, Eigen::VectorXd &lowerBound, Eigen::VectorXd &upperBound)
{
// evaluate the lower and the upper inequality vectors
Eigen::VectorXd lowerInequality = Eigen::MatrixXd::Zero(12*(mpcWindow+1) + 4 * mpcWindow, 1);
Eigen::VectorXd upperInequality = Eigen::MatrixXd::Zero(12*(mpcWindow+1) + 4 * mpcWindow, 1);
for(int i=0; i<mpcWindow+1; i++){
lowerInequality.block(12*i,0,12,1) = xMin;
upperInequality.block(12*i,0,12,1) = xMax;
}
for(int i=0; i<mpcWindow; i++){
lowerInequality.block(4 * i + 12 * (mpcWindow + 1), 0, 4, 1) = uMin;
upperInequality.block(4 * i + 12 * (mpcWindow + 1), 0, 4, 1) = uMax;
}
// evaluate the lower and the upper equality vectors
Eigen::VectorXd lowerEquality = Eigen::MatrixXd::Zero(12*(mpcWindow+1),1 );
Eigen::VectorXd upperEquality;
lowerEquality.block(0,0,12,1) = -x0;
upperEquality = lowerEquality;
lowerEquality = lowerEquality;
// merge inequality and equality vectors
lowerBound = Eigen::MatrixXd::Zero(2*12*(mpcWindow+1) + 4*mpcWindow,1 );
lowerBound << lowerEquality,
lowerInequality;
upperBound = Eigen::MatrixXd::Zero(2*12*(mpcWindow+1) + 4*mpcWindow,1 );
upperBound << upperEquality,
upperInequality;
}
void updateConstraintVectors(const Eigen::Matrix<double, 12, 1> &x0,
Eigen::VectorXd &lowerBound, Eigen::VectorXd &upperBound)
{
lowerBound.block(0,0,12,1) = -x0;
upperBound.block(0,0,12,1) = -x0;
}
double getErrorNorm(const Eigen::Matrix<double, 12, 1> &x,
const Eigen::Matrix<double, 12, 1> &xRef)
{
// evaluate the error
Eigen::Matrix<double, 12, 1> error = x - xRef;
// return the norm
return error.norm();
}
int main()
{
// set the preview window
int mpcWindow = 20;
// allocate the dynamics matrices
Eigen::Matrix<double, 12, 12> a;
Eigen::Matrix<double, 12, 4> b;
// allocate the constraints vector
Eigen::Matrix<double, 12, 1> xMax;
Eigen::Matrix<double, 12, 1> xMin;
Eigen::Matrix<double, 4, 1> uMax;
Eigen::Matrix<double, 4, 1> uMin;
// allocate the weight matrices
Eigen::DiagonalMatrix<double, 12> Q;
Eigen::DiagonalMatrix<double, 4> R;
// allocate the initial and the reference state space
Eigen::Matrix<double, 12, 1> x0;
Eigen::Matrix<double, 12, 1> xRef;
// allocate QP problem matrices and vectores
Eigen::SparseMatrix<double> hessian;
Eigen::VectorXd gradient;
Eigen::SparseMatrix<double> linearMatrix;
Eigen::VectorXd lowerBound;
Eigen::VectorXd upperBound;
// set the initial and the desired states
x0 << 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 ;
xRef << 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0;
// set MPC problem quantities
setDynamicsMatrices(a, b);
setInequalityConstraints(xMax, xMin, uMax, uMin);
setWeightMatrices(Q, R);
// cast the MPC problem as QP problem
castMPCToQPHessian(Q, R, mpcWindow, hessian);
castMPCToQPGradient(Q, xRef, mpcWindow, gradient);
castMPCToQPConstraintMatrix(a, b, mpcWindow, linearMatrix);
castMPCToQPConstraintVectors(xMax, xMin, uMax, uMin, x0, mpcWindow, lowerBound, upperBound);
// instantiate the solver
// settings
//solver.settings()->setVerbosity(false);
solver.settings()->setWarmStart(true);
// set the initial data of the QP solver
solver.data()->setNumberOfVariables(12 * (mpcWindow + 1) + 4 * mpcWindow);
solver.data()->setNumberOfConstraints(2 * 12 * (mpcWindow + 1) + 4 * mpcWindow);
if(!solver.data()->setHessianMatrix(hessian)) return 1;
if(!solver.data()->setGradient(gradient)) return 1;
if(!solver.data()->setLinearConstraintsMatrix(linearMatrix)) return 1;
if(!solver.data()->setLowerBound(lowerBound)) return 1;
if(!solver.data()->setUpperBound(upperBound)) return 1;
// instantiate the solver
if(!solver.initSolver()) return 1;
// controller input and QPSolution vector
Eigen::Vector4d ctr;
Eigen::VectorXd QPSolution;
// number of iteration steps
int numberOfSteps = 50;
for (int i = 0; i < numberOfSteps; i++){
// solve the QP problem
if(!solver.solve()) return 1;
// get the controller input
QPSolution = solver.getSolution();
ctr = QPSolution.block(12 * (mpcWindow + 1), 0, 4, 1);
// save data into file
auto x0Data = x0.data();
// propagate the model
x0 = a * x0 + b * ctr;
// update the constraint bound
updateConstraintVectors(x0, lowerBound, upperBound);
if(!solver.updateBounds(lowerBound, upperBound)) return 1;
}
return 0;
}

The example presented generates the following results

mpc_result.png