Merge pull request #101 from ARC-OPT/osqp

Add osqp solver

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)

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

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 ..

src/solvers/CMakeLists.txt

@@ -9,3 +9,6 @@ endif()
 if(SOLVER_PROXQP)
     add_subdirectory(proxqp)
 endif()
+if(SOLVER_OSQP)
+    add_subdirectory(osqp)
+endif()

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)

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();
+}
+
+}

src/solvers/osqp/OsqpSolver.hpp

@@ -0,0 +1,47 @@
+#ifndef WBC_OSQP_SOLVER_HPP
+#define WBC_OSQP_SOLVER_HPP
+
+#include <Eigen/Sparse>
+#include <OsqpEigen/Solver.hpp>
+#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<double> hessian_sparse;
+    Eigen::MatrixXd constraint_mat_dense;
+    Eigen::SparseMatrix<double> 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

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)