Autoware.Auto
autoware::common::osqp::OSQPInterface Class Reference

#include <osqp_interface.hpp>

Public Member Functions

 OSQPInterface (const c_float eps_abs=std::numeric_limits< c_float >::epsilon(), const bool8_t polish=true)
 Constructor without problem formulation. More...
 
 OSQPInterface (const Eigen::MatrixXd &P, const Eigen::MatrixXd &A, const std::vector< float64_t > &q, const std::vector< float64_t > &l, const std::vector< float64_t > &u, const c_float eps_abs)
 Constructor with problem setup. More...
 
 OSQPInterface (const CSC_Matrix &P, const CSC_Matrix &A, const std::vector< float64_t > &q, const std::vector< float64_t > &l, const std::vector< float64_t > &u, const c_float eps_abs)
 
std::tuple< std::vector< float64_t >, std::vector< float64_t >, int64_t, int64_t, int64_t > optimize ()
 Solves the stored convex quadratic program (QP) problem using the OSQP solver. More...
 
std::tuple< std::vector< float64_t >, std::vector< float64_t >, int64_t, int64_t, int64_t > optimize (const Eigen::MatrixXd &P, const Eigen::MatrixXd &A, const std::vector< float64_t > &q, const std::vector< float64_t > &l, const std::vector< float64_t > &u)
 Solves convex quadratic programs (QPs) using the OSQP solver. More...
 
int64_t initializeProblem (const Eigen::MatrixXd &P, const Eigen::MatrixXd &A, const std::vector< float64_t > &q, const std::vector< float64_t > &l, const std::vector< float64_t > &u)
 Converts the input data and sets up the workspace object. More...
 
int64_t initializeProblem (CSC_Matrix P, CSC_Matrix A, const std::vector< float64_t > &q, const std::vector< float64_t > &l, const std::vector< float64_t > &u)
 
void updateP (const Eigen::MatrixXd &P_new)
 
void updateCscP (const CSC_Matrix &P_csc)
 
void updateA (const Eigen::MatrixXd &A_new)
 
void updateCscA (const CSC_Matrix &A_csc)
 
void updateQ (const std::vector< double > &q_new)
 
void updateL (const std::vector< double > &l_new)
 
void updateU (const std::vector< double > &u_new)
 
void updateBounds (const std::vector< double > &l_new, const std::vector< double > &u_new)
 
void updateEpsAbs (const double eps_abs)
 
void updateEpsRel (const double eps_rel)
 
void updateMaxIter (const int iter)
 
void updateVerbose (const bool verbose)
 
void updateRhoInterval (const int rho_interval)
 
void updateRho (const double rho)
 
void updateAlpha (const double alpha)
 
int64_t getTakenIter () const
 Get the number of iteration taken to solve the problem. More...
 
std::string getStatusMessage () const
 Get the status message for the latest problem solved. More...
 
int64_t getStatus () const
 Get the status value for the latest problem solved. More...
 
int64_t getStatusPolish () const
 Get the status polish for the latest problem solved. More...
 
float64_t getRunTime () const
 Get the runtime of the latest problem solved. More...
 
float64_t getObjVal () const
 Get the objective value the latest problem solved. More...
 
int64_t getExitFlag () const
 Returns flag asserting interface condition (Healthy condition: 0). More...
 

Detailed Description

Implementation of a native C++ interface for the OSQP solver.

Constructor & Destructor Documentation

◆ OSQPInterface() [1/3]

autoware::common::osqp::OSQPInterface::OSQPInterface ( const c_float  eps_abs = std::numeric_limits<c_float>::epsilon(),
const bool8_t  polish = true 
)
explicit

Constructor without problem formulation.

◆ OSQPInterface() [2/3]

autoware::common::osqp::OSQPInterface::OSQPInterface ( const Eigen::MatrixXd &  P,
const Eigen::MatrixXd &  A,
const std::vector< float64_t > &  q,
const std::vector< float64_t > &  l,
const std::vector< float64_t > &  u,
const c_float  eps_abs 
)

Constructor with problem setup.

Parameters
P(n,n) matrix defining relations between parameters.
A(m,n) matrix defining parameter constraints relative to the lower and upper bound.
q(n) vector defining the linear cost of the problem.
l(m) vector defining the lower bound problem constraint.
u(m) vector defining the upper bound problem constraint.
eps_absAbsolute convergence tolerance.

◆ OSQPInterface() [3/3]

autoware::common::osqp::OSQPInterface::OSQPInterface ( const CSC_Matrix P,
const CSC_Matrix A,
const std::vector< float64_t > &  q,
const std::vector< float64_t > &  l,
const std::vector< float64_t > &  u,
const c_float  eps_abs 
)

Member Function Documentation

◆ getExitFlag()

int64_t autoware::common::osqp::OSQPInterface::getExitFlag ( ) const
inline

Returns flag asserting interface condition (Healthy condition: 0).

◆ getObjVal()

float64_t autoware::common::osqp::OSQPInterface::getObjVal ( ) const
inline

Get the objective value the latest problem solved.

◆ getRunTime()

float64_t autoware::common::osqp::OSQPInterface::getRunTime ( ) const
inline

Get the runtime of the latest problem solved.

◆ getStatus()

int64_t autoware::common::osqp::OSQPInterface::getStatus ( ) const
inline

Get the status value for the latest problem solved.

◆ getStatusMessage()

std::string autoware::common::osqp::OSQPInterface::getStatusMessage ( ) const
inline

Get the status message for the latest problem solved.

◆ getStatusPolish()

int64_t autoware::common::osqp::OSQPInterface::getStatusPolish ( ) const
inline

Get the status polish for the latest problem solved.

◆ getTakenIter()

int64_t autoware::common::osqp::OSQPInterface::getTakenIter ( ) const
inline

Get the number of iteration taken to solve the problem.

◆ initializeProblem() [1/2]

int64_t autoware::common::osqp::OSQPInterface::initializeProblem ( const Eigen::MatrixXd &  P,
const Eigen::MatrixXd &  A,
const std::vector< float64_t > &  q,
const std::vector< float64_t > &  l,
const std::vector< float64_t > &  u 
)

Converts the input data and sets up the workspace object.

Parameters
P(n,n) matrix defining relations between parameters.
A(m,n) matrix defining parameter constraints relative to the lower and upper bound.
q(n) vector defining the linear cost of the problem.
l(m) vector defining the lower bound problem constraint.
u(m) vector defining the upper bound problem constraint.

◆ initializeProblem() [2/2]

int64_t autoware::common::osqp::OSQPInterface::initializeProblem ( CSC_Matrix  P,
CSC_Matrix  A,
const std::vector< float64_t > &  q,
const std::vector< float64_t > &  l,
const std::vector< float64_t > &  u 
)

◆ optimize() [1/2]

std::tuple< std::vector< float64_t >, std::vector< float64_t >, int64_t, int64_t, int64_t > autoware::common::osqp::OSQPInterface::optimize ( )

Solves the stored convex quadratic program (QP) problem using the OSQP solver.

Returns
The function returns a tuple containing the solution as two float vectors.
The first element of the tuple contains the 'primal' solution.
The second element contains the 'lagrange multiplier' solution.
The third element contains an integer with solver polish status information.

How to use:

  1. Generate the Eigen matrices P, A and vectors q, l, u according to the problem.
  2. Initialize the interface and set up the problem.

osqp_interface = OSQPInterface(P, A, q, l, u, 1e-6);

  1. Call the optimization function.

std::tuple<std::vector<float64_t>, std::vector<float64_t>> result;

result = osqp_interface.optimize();

  1. Access the optimized parameters.

std::vector<float> param = std::get<0>(result);

float64_t x_0 = param[0];

float64_t x_1 = param[1];

◆ optimize() [2/2]

std::tuple< std::vector< float64_t >, std::vector< float64_t >, int64_t, int64_t, int64_t > autoware::common::osqp::OSQPInterface::optimize ( const Eigen::MatrixXd &  P,
const Eigen::MatrixXd &  A,
const std::vector< float64_t > &  q,
const std::vector< float64_t > &  l,
const std::vector< float64_t > &  u 
)

Solves convex quadratic programs (QPs) using the OSQP solver.

Returns
The function returns a tuple containing the solution as two float vectors.
The first element of the tuple contains the 'primal' solution.
The second element contains the 'lagrange multiplier' solution.
The third element contains an integer with solver polish status information.

How to use:

  1. Generate the Eigen matrices P, A and vectors q, l, u according to the problem.
  2. Initialize the interface.

osqp_interface = OSQPInterface(1e-6);

  1. Call the optimization function with the problem formulation.

std::tuple<std::vector<float64_t>, std::vector<float64_t>> result;

result = osqp_interface.optimize(P, A, q, l, u, 1e-6);

  1. Access the optimized parameters.

std::vector<float> param = std::get<0>(result);

float64_t x_0 = param[0];

float64_t x_1 = param[1];

◆ updateA()

void autoware::common::osqp::OSQPInterface::updateA ( const Eigen::MatrixXd &  A_new)

◆ updateAlpha()

void autoware::common::osqp::OSQPInterface::updateAlpha ( const double  alpha)

◆ updateBounds()

void autoware::common::osqp::OSQPInterface::updateBounds ( const std::vector< double > &  l_new,
const std::vector< double > &  u_new 
)

◆ updateCscA()

void autoware::common::osqp::OSQPInterface::updateCscA ( const CSC_Matrix A_csc)

◆ updateCscP()

void autoware::common::osqp::OSQPInterface::updateCscP ( const CSC_Matrix P_csc)

◆ updateEpsAbs()

void autoware::common::osqp::OSQPInterface::updateEpsAbs ( const double  eps_abs)

◆ updateEpsRel()

void autoware::common::osqp::OSQPInterface::updateEpsRel ( const double  eps_rel)

◆ updateL()

void autoware::common::osqp::OSQPInterface::updateL ( const std::vector< double > &  l_new)

◆ updateMaxIter()

void autoware::common::osqp::OSQPInterface::updateMaxIter ( const int  iter)

◆ updateP()

void autoware::common::osqp::OSQPInterface::updateP ( const Eigen::MatrixXd &  P_new)

◆ updateQ()

void autoware::common::osqp::OSQPInterface::updateQ ( const std::vector< double > &  q_new)

◆ updateRho()

void autoware::common::osqp::OSQPInterface::updateRho ( const double  rho)

◆ updateRhoInterval()

void autoware::common::osqp::OSQPInterface::updateRhoInterval ( const int  rho_interval)

◆ updateU()

void autoware::common::osqp::OSQPInterface::updateU ( const std::vector< double > &  u_new)

◆ updateVerbose()

void autoware::common::osqp::OSQPInterface::updateVerbose ( const bool  verbose)

The documentation for this class was generated from the following files: