OSQP文档学习
参考博客：
（1）二次规划（QP）与OSQP求解器
（2）如何使用OSQP-Eigen

osqp-eigen

1 osqp-eigen接口

以下列问题的求解为例：
s.t. $1\le x_1\le 1.5$，$1\le x_2\le 1.5$

mkdir -p catkin_ws/src
cd catkin_ws/src
catkin_init_workspace
catkin_create_pkg OSQP_Eigen roscpp std_msgs

main.cpp

// osqp-eigen
#include "OsqpEigen/OsqpEigen.h"// eigen
#include <Eigen/Dense>
#include <iostream>int main()
{// allocate QP problem matrices and vectoresEigen::SparseMatrix<double> hessian(2, 2);      //P: n*n正定矩阵,必须为稀疏矩阵SparseMatrixEigen::VectorXd gradient(2);                    //Q: n*1向量Eigen::SparseMatrix<double> linearMatrix(2, 2); //A: m*n矩阵,必须为稀疏矩阵SparseMatrixEigen::VectorXd lowerBound(2);                  //L: m*1下限向量Eigen::VectorXd upperBound(2);                  //U: m*1上限向量hessian.insert(0, 0) = 2.0; //注意稀疏矩阵的初始化方式,无法使用<<初始化hessian.insert(1, 1) = 2.0;// std::cout << "hessian:" << std::endl//           << hessian << std::endl;gradient << -2, -2;linearMatrix.insert(0, 0) = 1.0; //注意稀疏矩阵的初始化方式,无法使用<<初始化linearMatrix.insert(1, 1) = 1.0;// std::cout << "linearMatrix:" << std::endl//           << linearMatrix << std::endl;lowerBound << 1, 1;upperBound << 1.5, 1.5;// instantiate the solverOsqpEigen::Solver solver;// settingssolver.settings()->setVerbosity(false);solver.settings()->setWarmStart(true);// set the initial data of the QP solversolver.data()->setNumberOfVariables(2);   //变量数nsolver.data()->setNumberOfConstraints(2); //约束数mif (!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 solverif (!solver.initSolver())return 1;Eigen::VectorXd QPSolution;// solve the QP problemif (!solver.solve()){return 1;}QPSolution = solver.getSolution();std::cout << "QPSolution" << std::endl<< QPSolution << std::endl; //输出为m*1的向量return 0;
}

CMakeList.txt

cmake_minimum_required(VERSION 3.0.2)
project(OSQP_Eigen)set(CMAKE_CXX_STANDARD 11)
set(CMAKE_CXX_STANDARD_REQUIRED ON)find_package(catkin REQUIRED COMPONENTSroscppstd_msgs
)include_directories(include${catkin_INCLUDE_DIRS}
)find_package(OsqpEigen)
find_package(Eigen3)link_directories(/usr/local/lib 
)include_directories(SYSTEM ${EIGEN3_INCLUDE_DIR})
catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES planning
#  CATKIN_DEPENDS roscpp std_msgs
#  DEPENDS system_lib
)add_executable(OSQP src/main.cpp)
target_link_libraries(OSQP OsqpEigen::OsqpEigenlibosqp.so  libOsqpEigen.so libqdldl.so)

输出

QPSolution
1.0003
0.750274

2 如何使用OSQP-Eigen

上面主要介绍了用OSQP-Eigen的一般示例，下面给出几个典型不同约束的二次规划形式：
（1）具有线性约束的二次规划

#include "OsqpEigen/OsqpEigen.h"#include <Eigen/Dense>
#include <iostream>int main()
{Eigen::SparseMatrix<double> hessian(2, 2);Eigen::VectorXd gradient(2);Eigen::SparseMatrix<double> linearMatrix(3, 2);Eigen::VectorXd lowerBound(3);Eigen::VectorXd upperBound(3);// 具有线性约束的二次规划hessian.insert(0, 0) = 1;hessian.insert(1, 0) = -1;hessian.insert(0, 1) = -1;hessian.insert(1, 1) = 2;gradient << -2, -6;linearMatrix.insert(0, 0) = 1;linearMatrix.insert(0, 1) = 1;linearMatrix.insert(1, 0) = -1;linearMatrix.insert(1, 1) = 2;linearMatrix.insert(2, 0) = 2;linearMatrix.insert(2, 1) = 1;lowerBound << -OsqpEigen::INFTY, -OsqpEigen::INFTY, -OsqpEigen::INFTY;upperBound << 2, 2, 3;OsqpEigen::Solver solver;solver.settings()->setVerbosity(false);solver.settings()->setWarmStart(true);solver.data()->setNumberOfVariables(2);//A矩阵的列数solver.data()->setNumberOfConstraints(3);//A矩阵的行数//设置P矩阵if (!solver.data()->setHessianMatrix(hessian)) return 1;//设置q or f矩阵if (!solver.data()->setGradient(gradient)) return 1;//设置线性约束的A矩阵if (!solver.data()->setLinearConstraintsMatrix(linearMatrix)) return 1;//设置下边界if (!solver.data()->setLowerBound(lowerBound)) return 1;//设置上边界if (!solver.data()->setUpperBound(upperBound)) return 1;if (!solver.initSolver()) return 1;Eigen::VectorXd QPSolution;if (!solver.solve()) return 1;// get the controller inputclock_t time_start = clock();clock_t time_end = clock();time_start = clock();QPSolution = solver.getSolution();time_end = clock();std::cout << "time use:" << 1000 * (time_end - time_start) / (double)CLOCKS_PER_SEC << "ms" << std::endl;std::cout << "QPSolution" << std::endl << QPSolution << std::endl;return 0;
}

输出：

time use:0.002ms
QPSolution0.6665
1.33322

（2）具有线性等式约束的二次规划

#include "OsqpEigen/OsqpEigen.h"#include <Eigen/Dense>
#include <iostream>int main()
{Eigen::SparseMatrix<double> hessian(2, 2);Eigen::VectorXd gradient(2);Eigen::SparseMatrix<double> linearMatrix(1, 2);Eigen::VectorXd lowerBound(1);Eigen::VectorXd upperBound(1);// 具有线性等式约束的二次规划hessian.insert(0, 0) = 1;hessian.insert(1, 0) = -1;hessian.insert(0, 1) = -1;hessian.insert(1, 1) = 2;gradient << -2, -6;linearMatrix.insert(0,0) = 1;linearMatrix.insert(0,1) = 1;lowerBound << 0;upperBound << 0;OsqpEigen::Solver solver;solver.settings()->setVerbosity(false);solver.settings()->setWarmStart(true);solver.data()->setNumberOfVariables(2);solver.data()->setNumberOfConstraints(1);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;if (!solver.initSolver()) return 1;Eigen::VectorXd QPSolution;if (!solver.solve()) return 1;// get the controller inputclock_t time_start = clock();clock_t time_end = clock();time_start = clock();QPSolution = solver.getSolution();time_end = clock();std::cout << "time use:" << 1000 * (time_end - time_start) / (double)CLOCKS_PER_SEC << "ms" << std::endl;std::cout << "QPSolution" << std::endl << QPSolution << std::endl;return 0;
}

time use:0.002ms
QPSolution
-0.8000020.800003

（3）具有线性约束和边界的二次最小化

#include "OsqpEigen/OsqpEigen.h"#include <Eigen/Dense>
#include <iostream>int main()
{Eigen::SparseMatrix<double> hessian(3, 3);Eigen::VectorXd gradient(3);Eigen::SparseMatrix<double> linearMatrix(4, 3);Eigen::VectorXd lowerBound(4);Eigen::VectorXd upperBound(4);// 具有线性约束和边界的二次最小化hessian.insert(0,0) = 1;hessian.insert(1,0) = -1;hessian.insert(2,0) = 1;hessian.insert(0,1) = -1;hessian.insert(1,1) = 2;hessian.insert(2,1) = -2;hessian.insert(0,2) = 1;hessian.insert(1,2) = -2;hessian.insert(2,2) = 4;gradient << 2, -3, 1;linearMatrix.insert(0,0) = 1;linearMatrix.insert(1,0) = 0;linearMatrix.insert(2,0) = 0;linearMatrix.insert(3,0) = 1;linearMatrix.insert(0,1) = 0;linearMatrix.insert(1,1) = 1;linearMatrix.insert(2,1) = 0;linearMatrix.insert(3,1) = 1;linearMatrix.insert(0,2) = 0;linearMatrix.insert(1,2) = 0;linearMatrix.insert(2,2) = 1;linearMatrix.insert(3,2) = 1;lowerBound << 0, 0, 0, 0.5;upperBound << 1, 1, 1, 0.5;OsqpEigen::Solver solver;solver.settings()->setVerbosity(false);solver.settings()->setWarmStart(true);solver.data()->setNumberOfVariables(3);solver.data()->setNumberOfConstraints(4);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;if (!solver.initSolver()) return 1;Eigen::VectorXd QPSolution;if (!solver.solve()) return 1;// get the controller inputclock_t time_start = clock();clock_t time_end = clock();time_start = clock();QPSolution = solver.getSolution();time_end = clock();std::cout << "time use:" << 1000 * (time_end - time_start) / (double)CLOCKS_PER_SEC << "ms" << std::endl;std::cout << "QPSolution" << std::endl << QPSolution << std::endl;return 0;
}

time use:0.003ms
QPSolution
7.42006e-120.5
1.09987e-11

---

本文详解了osqp-eigen的具体用法，方便自己日后回顾总结