A tiny Eigen OSQP wrapper.

qpsolver.h

#pragma once

/*
Description: A tiny Eigen OSQP wrapper.
Author: Yotam Gingold <yotam (strudel) yotamgingold.com>
License: Public Domain [CC0](http://creativecommons.org/publicdomain/zero/1.0/)
On GitHub as a gist: https://gist.github.com/yig/f8fb55fc37dbc11c4fc60cbca77b7dc7
*/

#include <Eigen/Dense>
#include <Eigen/Sparse>
#include <iostream> // std::cerr

#include "osqp.h"

struct QPSolver {
    QPSolver(
        const Eigen::SparseMatrix<double>& P_in,
        const Eigen::VectorXd& q_in,
        const Eigen::SparseMatrix<double>& A_in,
        const Eigen::VectorXd& l_in,
        const Eigen::VectorXd& u_in,
        const OSQPSettings& settings_in = QPSolver::DefaultSettings()
        )
        :
        P( P_in.triangularView<Upper>() ),
        q( q_in ),
        A( A_in ),
        l( l_in ),
        u( u_in ),
        settings( settings_in )
    {
        /*
        settings.eps_abs = 1e-5;
        settings.eps_rel = 1e-5;
        settings.alpha = 1.0;
        */
        
        P.makeCompressed();
        A.makeCompressed();
        csc_set_data( &P_solver, P.rows(), P.cols(), P.nonZeros(), P.valuePtr(), P.innerIndexPtr(), P.outerIndexPtr() );
        csc_set_data( &A_solver, A.rows(), A.cols(), A.nonZeros(), A.valuePtr(), A.innerIndexPtr(), A.outerIndexPtr() );
        
        status = osqp_setup( &solver, &P_solver, q.data(), &A_solver, l.data(), u.data(), A.rows(), P.rows(), &settings );
        std::cout << "QPSolver setup() exit flag (0 is good): " << status << '\n';
    }
    
    Eigen::VectorXd solve()
    {
        status = osqp_solve( solver );
        std::cout << "QPSolver solve() exit flag (0 is good): " << status << '\n';
        
        OSQPInt m, n;
        osqp_get_dimensions( solver, &m, &n );
        
        return Eigen::Map< Eigen::VectorXd >( solver->solution->x, P.rows(), 1 );
    }
    
    OSQPInt update_lower_bound( const Eigen::VectorXd& l_in )
    {
        l = l_in;
        
        status = osqp_update_data_vec( solver, q.data(), l.data(), u.data() );
        std::cout << "QPSolver update_lower_bound() exit flag (0 is good): " << status << '\n';
        return status;
    }
    OSQPInt update_upper_bound( const Eigen::VectorXd& u_in )
    {
        u = u_in;
        
        status = osqp_update_data_vec( solver, q.data(), l.data(), u.data() );
        std::cout << "QPSolver update_upper_bound() exit flag (0 is good): " << status << '\n';
        return status;
    }
    
    static OSQPSettings DefaultSettings()
    {
        OSQPSettings settings;
        osqp_set_default_settings( &settings );
        return settings;
    }
    
    ~QPSolver() { osqp_cleanup( solver ); }
    
    OSQPInt status = 0;
    
private:
    Eigen::SparseMatrix<double> P;
    Eigen::VectorXd q;
    Eigen::SparseMatrix<double> A;
    Eigen::VectorXd l;
    Eigen::VectorXd u;
    
    OSQPSettings settings{};
    OSQPSolver* solver{};
    OSQPCscMatrix P_solver{};
    OSQPCscMatrix A_solver{};
};