From 6fe10f6d446323dbbaf474faa4b7829d944dbc90 Mon Sep 17 00:00:00 2001 From: dmronga Date: Wed, 5 Jun 2024 15:34:31 +0200 Subject: [PATCH 1/2] Add osqp solver --- CMakeLists.txt | 2 + scripts/run_tests.sh | 6 + src/solvers/CMakeLists.txt | 3 + src/solvers/osqp/CMakeLists.txt | 27 +++ src/solvers/osqp/OsqpSolver.cpp | 108 +++++++++++ src/solvers/osqp/OsqpSolver.hpp | 47 +++++ src/solvers/osqp/test/CMakeLists.txt | 4 + src/solvers/osqp/test/test_osqp_solver.cpp | 201 +++++++++++++++++++++ src/solvers/osqp/wbc-solvers-osqp.pc.in | 12 ++ 9 files changed, 410 insertions(+) create mode 100644 src/solvers/osqp/CMakeLists.txt create mode 100644 src/solvers/osqp/OsqpSolver.cpp create mode 100644 src/solvers/osqp/OsqpSolver.hpp create mode 100644 src/solvers/osqp/test/CMakeLists.txt create mode 100644 src/solvers/osqp/test/test_osqp_solver.cpp create mode 100644 src/solvers/osqp/wbc-solvers-osqp.pc.in diff --git a/CMakeLists.txt b/CMakeLists.txt index 200803d1..dcadc39d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -4,6 +4,7 @@ project(wbc) find_package(PkgConfig REQUIRED) find_package(Boost REQUIRED COMPONENTS system filesystem unit_test_framework serialization) +set(CMAKE_CXX_STANDARD 14) set(PROJECT_VERSION 0.2) set(API_VERSION ${PROJECT_VERSION}) @@ -15,6 +16,7 @@ option(ROBOT_MODEL_HYRODYN "Also build the HyRoDyn-based robot model, by default option(SOLVER_PROXQP "Build the ProxQP-based solver, by default only hls and qpoases are built" OFF) option(SOLVER_EIQUADPROG "Build the Eiquadprog-based solver, by default only hls and qpoases are built" OFF) option(SOLVER_QPSWIFT "Build the QPSwift-based solver, by default only hls and qpoases are built" OFF) +option(SOLVER_OSQP "Build the OSQP-based solver, by default only hls and qpoases are built" OFF) add_subdirectory(src) add_subdirectory(tutorials) diff --git a/scripts/run_tests.sh b/scripts/run_tests.sh index 9f025709..a1c6a277 100644 --- a/scripts/run_tests.sh +++ b/scripts/run_tests.sh @@ -96,4 +96,10 @@ if [ -d "qpswift" ]; then ./test_qpswift_solver cd ../.. fi +if [ -d "osqp" ]; then + echo "Testing OSQPsolver ..." + cd osqp/test + ./test_osqp_solver + cd ../.. +fi cd .. diff --git a/src/solvers/CMakeLists.txt b/src/solvers/CMakeLists.txt index de02170d..486fbc85 100644 --- a/src/solvers/CMakeLists.txt +++ b/src/solvers/CMakeLists.txt @@ -9,3 +9,6 @@ endif() if(SOLVER_PROXQP) add_subdirectory(proxqp) endif() +if(SOLVER_OSQP) + add_subdirectory(osqp) +endif() diff --git a/src/solvers/osqp/CMakeLists.txt b/src/solvers/osqp/CMakeLists.txt new file mode 100644 index 00000000..f25b8db2 --- /dev/null +++ b/src/solvers/osqp/CMakeLists.txt @@ -0,0 +1,27 @@ +SET(TARGET_NAME wbc-solvers-osqp) + +find_package(OsqpEigen REQUIRED) + +set(SOURCES OsqpSolver.cpp) +set(HEADERS OsqpSolver.hpp) + +list(APPEND PKGCONFIG_REQUIRES wbc-core) +string (REPLACE ";" " " PKGCONFIG_REQUIRES "${PKGCONFIG_REQUIRES}") + +add_library(${TARGET_NAME} SHARED ${SOURCES} ${HEADERS}) +target_link_libraries(${TARGET_NAME} PUBLIC + wbc-core) +target_link_libraries(${TARGET_NAME} PUBLIC OsqpEigen::OsqpEigen) + +set_target_properties(${TARGET_NAME} PROPERTIES + VERSION ${PROJECT_VERSION} + SOVERSION ${API_VERSION}) + +install(TARGETS ${TARGET_NAME} + LIBRARY DESTINATION lib) + +CONFIGURE_FILE(${CMAKE_CURRENT_SOURCE_DIR}/${TARGET_NAME}.pc.in ${CMAKE_CURRENT_BINARY_DIR}/${TARGET_NAME}.pc @ONLY) +install(FILES ${CMAKE_CURRENT_BINARY_DIR}/${TARGET_NAME}.pc DESTINATION lib/pkgconfig) +INSTALL(FILES ${HEADERS} DESTINATION include/${PROJECT_NAME}/solvers/osqp) + +add_subdirectory(test) diff --git a/src/solvers/osqp/OsqpSolver.cpp b/src/solvers/osqp/OsqpSolver.cpp new file mode 100644 index 00000000..c45433ac --- /dev/null +++ b/src/solvers/osqp/OsqpSolver.cpp @@ -0,0 +1,108 @@ +#include "OsqpSolver.hpp" +#include "../../core/QuadraticProgram.hpp" + +namespace wbc { + +OsqpSolver::OsqpSolver() : configured(false){ +} + +OsqpSolver::~OsqpSolver(){ +} + +void OsqpSolver::solve(const HierarchicalQP& hierarchical_qp, base::VectorXd& solver_output){ + + if(hierarchical_qp.size() != 1) + throw std::runtime_error("OsqpSolver::solve: Constraints vector size must be 1 for the current implementation"); + + const QuadraticProgram& qp = hierarchical_qp[0]; + uint nc = qp.bounded ? qp.neq + qp.nin + qp.nq : qp.neq + qp.nin; + + if(!configured){ + hessian_sparse.resize(qp.nq,qp.nq); + gradient.resize(qp.nq); + constraint_mat_dense.resize(nc,qp.nq); + constraint_mat_dense.setZero(); + constraint_mat_sparse.resize(nc,qp.nq); + lower_bound.resize(nc); + upper_bound.resize(nc); + lower_bound.setZero(); + upper_bound.setZero(); + hessian_sparse.setZero(); + gradient.setZero(); + constraint_mat_sparse.setZero(); + solver.settings()->setVerbosity(false); + solver.settings()->setWarmStart(true); + solver.data()->setNumberOfVariables(qp.nq); + configured = true; + solver.data()->setNumberOfConstraints(nc); + solver.data()->setHessianMatrix(hessian_sparse); + solver.data()->setGradient(gradient); + solver.data()->setLinearConstraintsMatrix(constraint_mat_sparse); + solver.data()->setLowerBound(lower_bound); + solver.data()->setUpperBound(upper_bound); + solver.initSolver(); + } + + + if(solver.data()->getData()->m != nc){ + solver.data()->setNumberOfConstraints(nc); + solver.data()->clearLinearConstraintsMatrix(); + solver.data()->clearHessianMatrix(); + hessian_sparse.resize(qp.nq,qp.nq); + gradient.resize(qp.nq); + constraint_mat_dense.resize(nc,qp.nq); + constraint_mat_dense.setZero(); + constraint_mat_sparse.resize(nc,qp.nq); + lower_bound.resize(nc); + upper_bound.resize(nc); + lower_bound.setZero(); + upper_bound.setZero(); + hessian_sparse.setZero(); + gradient.setZero(); + constraint_mat_sparse.setZero(); + solver.settings()->setVerbosity(false); + solver.settings()->setWarmStart(true); + solver.data()->setNumberOfVariables(qp.nq); + solver.data()->setNumberOfConstraints(nc); + solver.data()->setHessianMatrix(hessian_sparse); + solver.data()->setGradient(gradient); + solver.data()->setLinearConstraintsMatrix(constraint_mat_sparse); + solver.data()->setLowerBound(lower_bound); + solver.data()->setUpperBound(upper_bound); + } + + // OSQP treats bounds, eq. and ineq. constraints in one constraint matrix / vector, so we have to merge: + if(qp.neq != 0){ // Has equality constraints + constraint_mat_dense.middleRows(0,qp.neq) = qp.A; + lower_bound.segment(0,qp.neq) = qp.b; + upper_bound.segment(0,qp.neq) = qp.b; + } + if(qp.C.rows() != 0){ // Has inequality constraints + constraint_mat_dense.middleRows(qp.neq,qp.nin) = qp.C; + lower_bound.segment(qp.neq,qp.nin) = qp.lower_y; + upper_bound.segment(qp.neq,qp.nin) = qp.upper_y; + } + if(qp.bounded){ // Has bounds + constraint_mat_dense.middleRows(qp.neq+qp.nin,qp.nq).diagonal().setConstant(1.0); + lower_bound.segment(qp.neq+qp.nin,qp.nq) = qp.lower_x; + upper_bound.segment(qp.neq+qp.nin,qp.nq) = qp.upper_x; + } + + constraint_mat_sparse = constraint_mat_dense.sparseView(); + hessian_dense = qp.H; + hessian_sparse = hessian_dense.sparseView(); + gradient = qp.g; + solver.updateHessianMatrix(hessian_sparse); + solver.updateLinearConstraintsMatrix(constraint_mat_sparse); + solver.updateGradient(gradient); + solver.updateBounds(lower_bound,upper_bound); + + OsqpEigen::ErrorExitFlag flag = solver.solveProblem(); + if(flag != OsqpEigen::ErrorExitFlag::NoError){ + qp.print(); + throw std::runtime_error("Error solving QP: " + exitFlagToString(flag)); + } + solver_output = solver.getSolution(); +} + +} diff --git a/src/solvers/osqp/OsqpSolver.hpp b/src/solvers/osqp/OsqpSolver.hpp new file mode 100644 index 00000000..86993ad7 --- /dev/null +++ b/src/solvers/osqp/OsqpSolver.hpp @@ -0,0 +1,47 @@ +#ifndef WBC_OSQP_SOLVER_HPP +#define WBC_OSQP_SOLVER_HPP + +#include +#include +#include "../../core/QPSolver.hpp" +#include "../../core/QuadraticProgram.hpp" + +namespace wbc { + +class OsqpSolver : public QPSolver{ +public: + OsqpSolver(); + ~OsqpSolver(); + + virtual void solve(const HierarchicalQP& hierarchical_qp, base::VectorXd& solver_output); + +protected: + bool configured; + OsqpEigen::Solver solver; + + Eigen::MatrixXd hessian_dense; + Eigen::SparseMatrix hessian_sparse; + Eigen::MatrixXd constraint_mat_dense; + Eigen::SparseMatrix constraint_mat_sparse; + Eigen::VectorXd gradient; + Eigen::VectorXd lower_bound; + Eigen::VectorXd upper_bound; + + void resetData(uint nq, uint nc); + std::string exitFlagToString(OsqpEigen::ErrorExitFlag flag){ + switch(flag){ + case OsqpEigen::ErrorExitFlag::DataValidationError: return "DataValidationError"; + case OsqpEigen::ErrorExitFlag::SettingsValidationError: return "SettingsValidationError"; + case OsqpEigen::ErrorExitFlag::LinsysSolverLoadError: return "LinsysSolverLoadError"; + case OsqpEigen::ErrorExitFlag::LinsysSolverInitError: return "LinsysSolverInitError"; + case OsqpEigen::ErrorExitFlag::NonCvxError: return "NonCvxError"; + case OsqpEigen::ErrorExitFlag::MemAllocError: return "MemAllocError"; + case OsqpEigen::ErrorExitFlag::WorkspaceNotInitError: return "WorkspaceNotInitError"; + default: return "NoError"; + } + } +}; + +} + +#endif diff --git a/src/solvers/osqp/test/CMakeLists.txt b/src/solvers/osqp/test/CMakeLists.txt new file mode 100644 index 00000000..a3d42c79 --- /dev/null +++ b/src/solvers/osqp/test/CMakeLists.txt @@ -0,0 +1,4 @@ +add_executable(test_osqp_solver test_osqp_solver.cpp) +target_link_libraries(test_osqp_solver + wbc-solvers-osqp + Boost::unit_test_framework) diff --git a/src/solvers/osqp/test/test_osqp_solver.cpp b/src/solvers/osqp/test/test_osqp_solver.cpp new file mode 100644 index 00000000..1bf7305f --- /dev/null +++ b/src/solvers/osqp/test/test_osqp_solver.cpp @@ -0,0 +1,201 @@ +#define BOOST_TEST_DYN_LINK +#define BOOST_TEST_MAIN +#include +#include +#include +#include "../../../core/QuadraticProgram.hpp" +#include "../OsqpSolver.hpp" + +using namespace wbc; +using namespace std; + +BOOST_AUTO_TEST_CASE(solver_without_constraints) +{ + srand (time(NULL)); + + const int NO_JOINTS = 6; + const int NO_EQ_CONSTRAINTS = 0; + const int NO_IN_CONSTRAINTS = 0; + const bool WITH_BOUNDS = false; + + // Solve the problem min(||Ax-b||) without constraints --> encode the task as part of the cost function + // Standard form of QP is x^T*H*x + x^T*g --> Choose H = A^T*A and g = -(A^T*y)^T + // For a 6x6 Constraint matrix this is approx. 3-5 times faster than encoding the task as constraint as below + // With warm start, this solver is much faster (approx. 5 times) than in the initial run + + wbc::QuadraticProgram qp; + qp.resize(NO_JOINTS, NO_EQ_CONSTRAINTS, NO_IN_CONSTRAINTS, WITH_BOUNDS); + + qp.lower_x.resize(0); + qp.upper_x.resize(0); + qp.lower_y.resize(0); + qp.upper_y.resize(0); + qp.A.setIdentity(); + // Task Jacobian + base::Matrix6d A; + A << 0.642, 0.706, 0.565, 0.48, 0.59, 0.917, + 0.553, 0.087, 0.43, 0.71, 0.148, 0.87, + 0.249, 0.632, 0.711, 0.13, 0.426, 0.963, + 0.682, 0.123, 0.998, 0.716, 0.961, 0.901, + 0.891, 0.019, 0.716, 0.534, 0.725, 0.633, + 0.315, 0.551, 0.462, 0.221, 0.638, 0.244; + // Desired task space reference + base::Vector6d y; + y << 0.833, 0.096, 0.078, 0.971, 0.883, 0.366; + + qp.H = A.transpose()*A; + qp.g = -(A.transpose()*y).transpose(); + + qp.check(); + wbc::HierarchicalQP hqp; + hqp << qp; + + OsqpSolver solver; + base::VectorXd solver_output; + + struct timeval start, end; + gettimeofday(&start, NULL); + + BOOST_CHECK_NO_THROW(solver.solve(hqp, solver_output)); + gettimeofday(&end, NULL); + long useconds = end.tv_usec - start.tv_usec; + + cout<<"\n----------------------- Test Results ----------------------"< encode the task as constraint + // Standard form of QP is x^T*H*x + x^T*g --> Choose H = I and g = 0 + // For a 6x6 Constraint matrix this is approx. 3-5 times slower than encoding the task in the cost function as above + + wbc::QuadraticProgram qp; + qp.resize(NO_JOINTS, NO_EQ_CONSTRAINTS, NO_IN_CONSTRAINTS, WITH_BOUNDS); + + qp.g.setZero(); + qp.H.setIdentity(); + // Task Jacobian + base::Matrix6d A; + A << 0.642, 0.706, 0.565, 0.48, 0.59, 0.917, + 0.553, 0.087, 0.43, 0.71, 0.148, 0.87, + 0.249, 0.632, 0.711, 0.13, 0.426, 0.963, + 0.682, 0.123, 0.998, 0.716, 0.961, 0.901, + 0.891, 0.019, 0.716, 0.534, 0.725, 0.633, + 0.315, 0.551, 0.462, 0.221, 0.638, 0.244; + qp.A = A; + // Desired task space reference + base::Vector6d y; + y << 0.833, 0.096, 0.078, 0.971, 0.883, 0.366; + qp.b = y; + + qp.check(); + wbc::HierarchicalQP hqp; + hqp << qp; + + OsqpSolver solver; + + base::VectorXd solver_output; + + struct timeval start, end; + gettimeofday(&start, NULL); + BOOST_CHECK_NO_THROW(solver.solve(hqp, solver_output)); + gettimeofday(&end, NULL); + long useconds = end.tv_usec - start.tv_usec; + + cout<<"\n----------------------- Test Results ----------------------"< encode the task as part of the cost function + // Standard form of QP is x^T*H*x + x^T*g --> Choose H = A^T*A and g = -(A^T*y)^T + // For a 6x6 Constraint matrix this is approx. 3-5 times faster than encoding the task as constraint as below + // With warm start, this solver is much faster (approx. 5 times) than in the initial run + + wbc::QuadraticProgram qp; + qp.resize(NO_JOINTS, NO_EQ_CONSTRAINTS, NO_IN_CONSTRAINTS, WITH_BOUNDS); + + // Task Jacobian + base::Matrix6d A; + A << 0.642, 0.706, 0.565, 0.48, 0.59, 0.917, + 0.553, 0.087, 0.43, 0.71, 0.148, 0.87, + 0.249, 0.632, 0.711, 0.13, 0.426, 0.963, + 0.682, 0.123, 0.998, 0.716, 0.961, 0.901, + 0.891, 0.019, 0.716, 0.534, 0.725, 0.633, + 0.315, 0.551, 0.462, 0.221, 0.638, 0.244; + // Desired task space reference + base::Vector6d y; + y << 0.833, 0.096, 0.078, 0.971, 0.883, 0.366; + + qp.H = A.transpose()*A; + qp.g = -(A.transpose()*y).transpose(); + + qp.lower_x.setConstant(-10.0); + qp.upper_x.setConstant(+10.0); + qp.bounded = true; + + qp.check(); + wbc::HierarchicalQP hqp; + hqp << qp; + + OsqpSolver solver; + + base::VectorXd solver_output; + + struct timeval start, end; + gettimeofday(&start, NULL); + + BOOST_CHECK_NO_THROW(solver.solve(hqp, solver_output)); + gettimeofday(&end, NULL); + long useconds = end.tv_usec - start.tv_usec; + + for(uint j = 0; j < NO_JOINTS; ++j) + BOOST_CHECK((qp.lower_x(j)-1e-9) <= solver_output(j) && solver_output(j) <= (qp.upper_x(j)+1e-9)); + +} diff --git a/src/solvers/osqp/wbc-solvers-osqp.pc.in b/src/solvers/osqp/wbc-solvers-osqp.pc.in new file mode 100644 index 00000000..dff22b9c --- /dev/null +++ b/src/solvers/osqp/wbc-solvers-osqp.pc.in @@ -0,0 +1,12 @@ +prefix=@CMAKE_INSTALL_PREFIX@ +exec_prefix=@CMAKE_INSTALL_PREFIX@ +libdir=${prefix}/lib +includedir=${prefix}/include + +Name: @TARGET_NAME@ +Description: @PROJECT_DESCRIPTION@ +Version: @PROJECT_VERSION@ +Requires: @PKGCONFIG_REQUIRES@ +Libs: -L${libdir} -l@TARGET_NAME@ @PKGCONFIG_LIBS@ +Cflags: -I${includedir} @PKGCONFIG_CFLAGS@ + From a10e50f9ced371ed1d234e9638be6c605d294308 Mon Sep 17 00:00:00 2001 From: dmronga Date: Wed, 5 Jun 2024 15:56:45 +0200 Subject: [PATCH 2/2] Add install instructions for osqp --- scripts/full_install.sh | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/scripts/full_install.sh b/scripts/full_install.sh index 98917026..24e3ea1a 100644 --- a/scripts/full_install.sh +++ b/scripts/full_install.sh @@ -89,9 +89,21 @@ mkdir build && cd build cmake .. -DBUILD_TESTING=OFF -DBUILD_PYTHON_INTERFACE=OFF -DBUILD_WITH_VECTORIZATION_SUPPORT=OFF make -j8 && sudo make install && cd ../.. +# OSQP +git clone https://github.com/osqp/osqp.git +cd osqp +mkdir build && cd build +cmake .. +make -j8 && sudo make install && cd ../.. +git clone https://github.com/robotology/osqp-eigen.git +cd osqp-eigen +mkdir build && cd build +cmake .. +make -j8 && sudo make install && cd ../.. + # WBC mkdir wbc/build && cd wbc/build -cmake .. -DROBOT_MODEL_RBDL=ON -DROBOT_MODEL_KDL=ON -DSOLVER_PROXQP=ON -DSOLVER_EIQUADPROG=ON -DSOLVER_QPSWIFT=ON -DCMAKE_BUILD_TYPE=RELEASE +cmake .. -DROBOT_MODEL_RBDL=ON -DROBOT_MODEL_KDL=ON -DSOLVER_PROXQP=ON -DSOLVER_EIQUADPROG=ON -DSOLVER_QPSWIFT=ON -DSOLVER_OSQP=ON -DCMAKE_BUILD_TYPE=RELEASE make -j8 && sudo make install && cd .. sudo ldconfig