文章目录
一. qpOASES
1. 安装
mkdir build
cd build
cmake ..
make
sudo make install
2. CmakeLists 修改
set(QPOASES_INLUDE_DIRS /usr/local/include/qpOASES)
include_directories(${QPOASES_INLUDE_DIRS})
target_link_libraries(qptest1 ${catkin_LIBRARIES} qpOASES)
我也是这样用的,但是编译报错
/usr/bin/ld: error: unable to find library -lqpOASES
collect2: error: ld returned 1 exit status
我是直接把编译生成的静态链接库,拿出来使用,编译的文件在这里
qpOASES-master/build/libs/libqpOASES.a
放到自己的文件路径下
bbox_test/lib/qpOASES/libqpOASES.a
- 修改一下 CmakeLists.txt
link_directories(lib/qpOASES)
set(qpOASES libqpOASES.a)
add_executable(bbox_test ${SOURCES} ${AS_HEADERS} ${AS_SOURCES})
target_link_libraries(bbox_test ${OpenCV_LIBS}
qpOASES
${GTEST}
gflags)
3. 使用测试
举个栗子：
#include <qpOASES.hpp>
using namespace qpOASES;
int main()
{
/* Setup data of first QP. */
real_t H[2 * 2] = {
1.0, 0.0, 0.0, 0.5 };
real_t A[1 * 2] = {
1.0, 1.0 };
real_t g[2] = {
1.5, 1.0 };
real_t lb[2] = {
0.5, -2.0 };
real_t ub[2] = {
5.0, 2.0 };
real_t lbA[1] = {
-1.0 };
real_t ubA[1] = {
2.0 };
/* Setup data of second QP. */
// real_t g_new[2] = { 1.0, 1.5 };
// real_t lb_new[2] = { 0.0, -1.0 };
// real_t ub_new[2] = { 5.0, -0.5 };
// real_t lbA_new[1] = { -2.0 };
// real_t ubA_new[1] = { 1.0 };
/* Setting up QProblem object. */
QProblem example(2, 1);
/* Solve first QP. */
int_t nWSR = 10;
example.init(H, g, A, lb, ub, lbA, ubA, nWSR);
// example.init(H, g, A, lb, ub, nullptr, ubA, nWSR);
/* Solve second QP. */
// nWSR = 10;
// example.hotstart(g_new, lb_new, ub_new, lbA_new, ubA_new, nWSR);
/* Get and print solution of second QP. */
real_t xOpt[2];
example.getPrimalSolution(xOpt);
printf("\n xOpt = [ %e, %e ]; objVal = %e\n\n", xOpt[0], xOpt[1], example.getObjVal());
return 0;
}
结果:
seivl@seivl-Default-string:~/me/bbox_test/build_$ ./bbox_test
#################### qpOASES -- QP NO. 1 #####################
Iter | StepLength | Info | nFX | nAC
----------+------------------+------------------+---------+---------
0 | 5.833333e-01 | ADD CON 0 | 1 | 1
1 | 1.000000e+00 | QP SOLVED | 1 | 1
xOpt = [ 5.000000e-01, -1.500000e+00 ]; objVal = -6.250000e-02
验证一下:
- matlab 测试
H = [1 0; 0 1/2];
f = [1.5; 1];
A = [1 1;-1 -1]; % 两边乘上负号,变成约束
b = [2;1];
lb = [1/2; -2];
ub = [5; 2];
[x,fval,exitflag,output,lambda] = quadprog(H,f,A,b,[],[],lb,ub)
结果：
x =
0.5000
-1.5000
fval =
-0.0625
二. osqp
1. osqp 安装
git clone --recursive https://github.com/oxfordcontrol/osqp
cd osqp
mkdir build
cd build
cmake .. -DBUILD_SHARED_LIBS=ON
make -j6
sudo make install
2. osqp-eigen 安装
git clone https://github.com/robotology/osqp-eigen.git
cd osqp-eigen
mkdir build
cd build
cmake ..
make
make install
编译报错：
解决措施：
- step1：卸载eigen3
sudo rm -rf /usr/include/eigen3
sudo rm -rf /usr/lib/cmake/eigen3
- step2: 下载安装最新eigen3
在这里下载：
https://gitlab.com/libeigen/eigen/-/releases
mkdir build
cd build
cmake ..
sudo make
sudo make install
可以看见其实是安装到：
/usr/local/include/eigen3/
这个目录下了，但是一般使用 #include<Eigen/Dense> 的时候，编译器会默认去
usr/local/include
这里找，因此需要改一下
sudo cp -r /usr/local/include/eigen3/Eigen /usr/local/include
自带的测试例子：
cd osqp-eigen/example
mkdir build && cd build
cmake ..
make
./MPCExample
测试
- CMakeLists.txt 文件
cmake_minimum_required(VERSION 3.1)
set (CMAKE_CXX_STANDARD 11)
project(OsqpEigen-Example-Test)
find_package(OsqpEigen)
find_package(Eigen3)
include_directories(SYSTEM ${
EIGEN3_INCLUDE_DIR})
#osqp_eigen_test
add_executable(osqp_eigen_test src/osqp_eigen_test.cpp)
target_link_libraries(osqp_eigen_test OsqpEigen::OsqpEigen)
#include "OsqpEigen/OsqpEigen.h"
#include <Eigen/Dense>
#include <iostream>
int main()
{
// allocate QP problem matrices and vectores
Eigen::SparseMatrix<double> hessian;
Eigen::VectorXd gradient;
Eigen::SparseMatrix<double> linearMatrix;
Eigen::VectorXd lowerBound;
Eigen::VectorXd upperBound;
hessian.resize(2, 2);
hessian.insert(0, 0) = 1;
hessian.insert(1, 0) = 0;
hessian.insert(0, 1) = 0;
hessian.insert(1, 1) = 0.5;
std::cout << "hessian:" << std::endl << hessian << std::endl;
gradient.resize(2);
gradient << 1.5, 1;
std::cout << "gradient:" << std::endl << gradient << std::endl;
linearMatrix.resize(3, 2);
linearMatrix.insert(0, 0) = 1;
linearMatrix.insert(0, 1) = 1;
linearMatrix.insert(1, 0) = 1;
linearMatrix.insert(1, 1) = 0;
linearMatrix.insert(2, 0) = 0;
linearMatrix.insert(2, 1) = 1;
std::cout << "linearMatrix:" << std::endl << linearMatrix << std::endl;lowerBound.resize(3);
lowerBound.resize(3);
lowerBound << -1, 0.5, -2;
std::cout << "lowerBound:" << std::endl << lowerBound << std::endl;
upperBound.resize(3);
upperBound << 2, 5, 2;
std::cout << "upperBound:" << std::endl << upperBound << std::endl;
int NumberOfVariables = 2; //A矩阵的列数
int NumberOfConstraints = 3; //A矩阵的行数
OsqpEigen::Solver solver;
// settings
//solver.settings()->setVerbosity(false);
solver.settings()->setWarmStart(true);
// set the initial data of the QP solver
solver.data()->setNumberOfVariables(NumberOfVariables); //设置A矩阵的列数，即n
solver.data()->setNumberOfConstraints(NumberOfConstraints); //设置A矩阵的行数，即m
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;
Eigen::VectorXd QPSolution;
// solve the QP problem
if (!solver.solve())
return 1;
// get the controller input
clock_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;
}
结果：
-----------------------------------------------------------------
OSQP v0.6.2 - Operator Splitting QP Solver
(c) Bartolomeo Stellato, Goran Banjac
University of Oxford - Stanford University 2021
-----------------------------------------------------------------
problem: variables n = 2, constraints m = 3
nnz(P) + nnz(A) = 9
settings: linear system solver = qdldl,
eps_abs = 1.0e-03, eps_rel = 1.0e-03,
eps_prim_inf = 1.0e-04, eps_dual_inf = 1.0e-04,
rho = 1.00e-01 (adaptive),
sigma = 1.00e-06, alpha = 1.60, max_iter = 4000
check_termination: on (interval 25),
scaling: on, scaled_termination: off
warm start: on, polish: off, time_limit: off
iter objective pri res dua res rho time
1 -2.0896e+00 2.34e+00 8.25e-01 1.00e-01 1.28e-04s
50 -6.2500e-02 1.02e-07 1.88e-07 8.47e-01 2.12e-04s
status: solved
number of iterations: 50
optimal objective: -0.0625
run time: 2.29e-04s
optimal rho estimate: 7.22e-01
time use:0.009ms
QPSolution:
0.5
-1.5
可以看到和上面一致
3. osqp python
安装
pip install osqp -i https://pypi.tuna.tsinghua.edu.cn/simple