Backport proxQP solver implementation

CMakeLists.txt

@@ -118,7 +118,6 @@ set(SOURCE_FILES
     ${SRC_DIR}/component/solver/QPInverseProblemSolver.cpp
     ${SRC_DIR}/component/solver/modules/ContactHandler.cpp
     ${SRC_DIR}/component/solver/modules/ConstraintHandler.cpp
-    ${SRC_DIR}/component/solver/modules/LCPQPSolver.cpp
     ${SRC_DIR}/component/solver/modules/NLCPSolver.cpp
     ${SRC_DIR}/component/solver/modules/QPInverseProblem.cpp
     ${SRC_DIR}/component/solver/modules/QPInverseProblemImpl.cpp
@@ -139,13 +138,35 @@ endif()
 set(DOC_FILES README.md)
 file(GLOB_RECURSE EXAMPLE_FILES examples "*.pyscn" "*.py" "*.md" "*.psl" "*.pslx" "*.scn" "*.xml")
 
-set(OASES_LIBRARY_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/extlibs/qpOASES-3.2.0/")
-add_subdirectory(${OASES_LIBRARY_DIRECTORY} extlibs/libqpOASES)
-find_package(libqpOASES REQUIRED)
-message("libqpOASES_INCLUDE_DIRS = ${libqpOASES_INCLUDE_DIRS}")
-include_directories(${libqpOASES_INCLUDE_DIRS})
-set(QPOASES_INCLUDE_DIR ${libqpOASES_INCLUDE_DIRS} CACHE INTERNAL "")
-sofa_install_libraries(PATHS ${libqpOASES_LIBRARY})
+
+option(SOFTROBOTSINVERSE_ENABLE_PROXQP "Build proxQP wrapper for inverse problems" OFF)
+option(SOFTROBOTSINVERSE_ENABLE_QPOASES "Build qpOASES wrapper for inverse problems" ON)
+
+if(NOT SOFTROBOTSINVERSE_ENABLE_PROXQP AND NOT SOFTROBOTSINVERSE_ENABLE_QPOASES)
+    message(FATAL_ERROR "Both proxQP and qpOASES solvers are disabled. Please enable at least one solver.")
+endif()
+
+if(SOFTROBOTSINVERSE_ENABLE_PROXQP)
+    list(APPEND HEADER_FILES
+        ${SRC_DIR}/component/solver/modules/QPInverseProblemProxQP.h
+        ${SRC_DIR}/component/solver/modules/LCPQPSolverProxQP.h
+        )
+    list(APPEND SOURCE_FILES
+        ${SRC_DIR}/component/solver/modules/QPInverseProblemProxQP.cpp
+        ${SRC_DIR}/component/solver/modules/LCPQPSolverProxQP.cpp
+        )
+endif()
+
+if(SOFTROBOTSINVERSE_ENABLE_QPOASES)
+    list(APPEND HEADER_FILES
+        ${SRC_DIR}/component/solver/modules/QPInverseProblemQPOases.h
+        ${SRC_DIR}/component/solver/modules/LCPQPSolverQPOases.h
+        )
+    list(APPEND SOURCE_FILES
+        ${SRC_DIR}/component/solver/modules/QPInverseProblemQPOases.cpp
+        ${SRC_DIR}/component/solver/modules/LCPQPSolverQPOases.cpp
+        )
+endif()
 
 option(SOFTROBOTSINVERSE_INSTALL_HEADERS "Install the headers" ON)
 if(SOFTROBOTSINVERSE_INSTALL_HEADERS)
@@ -154,14 +175,47 @@ else()
     add_library(${PROJECT_NAME} SHARED ${SOURCE_FILES} ${DOC_FILES} ${EXAMPLE_FILES} "${SRC_DIR}/component/config.h.in")
 endif()
 
-target_link_libraries(${PROJECT_NAME}
+
+if(SOFTROBOTSINVERSE_ENABLE_PROXQP)
+    find_package(proxsuite REQUIRED)
+    message("Sofa.SoftRobots.Inverse: proxsuite FOUND. Will build with proxsuite support.")
+
+    target_link_libraries(${PROJECT_NAME} PUBLIC proxsuite::proxsuite)
+    # XXX proxsuite vectorization support via SIMDE and activated by the compilation options '-march=native' or `-mavx2 -mavx512f`
+    # This will not be available if proxsuite has been fetched
+    # target_link_libraries(${PROJECT_NAME} PUBLIC proxsuite::proxsuite-vectorized)
+    # target_compile_options(${PROJECT_NAME}  PUBLIC "-march=native")
+    target_compile_definitions(${PROJECT_NAME} PUBLIC SOFTROBOTSINVERSE_ENABLE_PROXQP)
+endif()
+
+if(SOFTROBOTSINVERSE_ENABLE_QPOASES)
+    find_package(qpOASES QUIET)
+
+    if(qpOASES_FOUND)
+        message("Sofa.SoftRobots.Inverse: external qpOASES FOUND. Will build with qpOASES support.")
+        include_directories(${qpOASES_INCLUDE_DIRS})
+        target_link_libraries(${PROJECT_NAME} PUBLIC ${qpOASES_LIBRARIES})
+        set(QPOASES_INCLUDE_DIR ${qpOASES_INCLUDE_DIRS} CACHE INTERNAL "")
+    else()
+        message("External package qpOASES not found, using embedded version for qpOASES support")
+        set(OASES_LIBRARY_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/extlibs/qpOASES-3.2.0/")
+        add_subdirectory(${OASES_LIBRARY_DIRECTORY} extlibs/libqpOASES)
+        find_package(libqpOASES REQUIRED)
+        include_directories(${libqpOASES_INCLUDE_DIRS})
+        set(QPOASES_INCLUDE_DIR ${libqpOASES_INCLUDE_DIRS} CACHE INTERNAL "")
+        sofa_install_libraries(PATHS ${libqpOASES_LIBRARY})
+        target_link_libraries(${PROJECT_NAME} PUBLIC ${libqpOASES_LIBRARY})
+    endif()
+    target_compile_definitions(${PROJECT_NAME} PUBLIC SOFTROBOTSINVERSE_ENABLE_QPOASES)
+  endif()
+
+target_link_libraries(${PROJECT_NAME} PUBLIC
     SoftRobots
     Sofa.Component.Constraint.Lagrangian.Solver
     Sofa.Component.Collision.Response.Contact)
-target_link_libraries(${PROJECT_NAME} ${libqpOASES_LIBRARY})
 
 sofa_find_package(Sofa.Component.SolidMechanics.FEM.Elastic)
-target_link_libraries(${PROJECT_NAME} Sofa.Component.SolidMechanics.FEM.Elastic)
+target_link_libraries(${PROJECT_NAME} PUBLIC Sofa.Component.SolidMechanics.FEM.Elastic)
 
 find_package(SofaPython3 QUIET)
 if(SofaPython3_FOUND)

SoftRobots.InverseConfig.cmake.in

@@ -7,8 +7,19 @@ find_package(Sofa.Component.Constraint.Lagrangian.Solver QUIET REQUIRED)
 find_package(Sofa.Component.Collision.Response.Contact QUIET REQUIRED)
 find_package(Sofa.Component.SolidMechanics.FEM.Elastic QUIET REQUIRED)
 
-find_package(libqpOASES REQUIRED)
-include_directories(${OASES_INCLUDE_DIRS})
+set(SOFTROBOTSINVERSE_ENABLE_QPOASES @SOFTROBOTSINVERSE_ENABLE_QPOASES@)
+if(SOFTROBOTSINVERSE_ENABLE_QPOASES)
+  find_package(qpOASES QUIET)
+  if(NOT qpOASES_FOUND)
+    find_package(libqpOASES REQUIRED)
+    include_directories(${OASES_INCLUDE_DIRS})
+  endif()
+endif()
+
+set(SOFTROBOTSINVERSE_ENABLE_PROXQP @SOFTROBOTSINVERSE_ENABLE_PROXQP@)
+if(SOFTROBOTSINVERSE_ENABLE_PROXQP)
+  find_package(proxsuite QUIET REQUIRED)
+endif()
 
 if(NOT TARGET @PROJECT_NAME@)
     include("${CMAKE_CURRENT_LIST_DIR}/@[email protected]")

cmake/Modules/FindqpOASES.cmake

@@ -0,0 +1,23 @@
+# Find the qpOASES includes and libraries.
+# The following variables are set if qpOASES is found.  If qpOASES is not
+# found, qpOASES_FOUND is set to false.
+#  qpOASES_FOUND        - True when the qpOASES include directory is found.
+#  qpOASES_INCLUDE_DIRS - the path to where the qpOASES include files are.
+#  qpOASES_LIBRARIES    - The libraries to link against qpOASES
+
+include(FindPackageHandleStandardArgs)
+
+find_path(qpOASES_INCLUDE_DIR
+  NAMES qpOASES.hpp
+  PATH_SUFFIXES include
+)
+
+find_library(qpOASES_LIBRARY
+  NAMES qpOASES
+  PATH_SUFFIXES lib
+)
+
+set(qpOASES_INCLUDE_DIRS ${qpOASES_INCLUDE_DIR})
+set(qpOASES_LIBRARIES ${qpOASES_LIBRARY})
+
+find_package_handle_standard_args(qpOASES DEFAULT_MSG qpOASES_LIBRARIES qpOASES_INCLUDE_DIRS)

src/SoftRobots.Inverse/component/solver/QPInverseProblemSolver.cpp

@@ -53,6 +53,14 @@
 #include <SoftRobots.Inverse/component/solver/modules/QPMechanicalSetConstraint.h>
 #include <SoftRobots.Inverse/component/solver/modules/QPMechanicalAccumulateConstraint.h>
 
+#ifdef SOFTROBOTSINVERSE_ENABLE_QPOASES
+#include <SoftRobots.Inverse/component/solver/modules/QPInverseProblemQPOases.h>
+#endif
+
+#ifdef SOFTROBOTSINVERSE_ENABLE_PROXQP
+#include <SoftRobots.Inverse/component/solver/modules/QPInverseProblemProxQP.h>
+#endif
+
 using sofa::simulation::mechanicalvisitor::MechanicalProjectJacobianMatrixVisitor;
 using sofa::simulation::mechanicalvisitor::MechanicalResetConstraintVisitor;
 using sofa::component::constraint::lagrangian::solver::ConstraintStoreLambdaVisitor ;
@@ -122,6 +130,8 @@ QPInverseProblemSolver::QPInverseProblemSolver()
 
     , d_responseFriction(initData(&d_responseFriction, 0., "responseFriction", "Response friction for contact resolution"))
 
+    , d_qpSolver(initData(&d_qpSolver, "qpSolver", "QP solver implementation to be used"))
+
     , d_epsilon(initData(&d_epsilon, 1e-3, "epsilon",
                          "An energy term is added in the minimization process. \n"
                          "Epsilon has to be chosen sufficiently small so that the deformation \n"
@@ -149,25 +159,77 @@ QPInverseProblemSolver::QPInverseProblemSolver()
     , d_objective(initData(&d_objective, 250.0, "objective", "Erreur between the target and the end effector "))
 
     , m_lastCP(NULL)
+    , m_CP1(nullptr)
+    , m_CP2(nullptr)
+    , m_CP3(nullptr)
 {
-    createProblems();
+    sofa::helper::OptionsGroup qpSolvers{"qpOASES" , "proxQP"};
+#if defined SOFTROBOTSINVERSE_ENABLE_PROXQP && !defined SOFTROBOTSINVERSE_ENABLE_QPOASES
+    qpSolvers.setSelectedItem(QPSolverImpl::PROXQP);
+#else
+    qpSolvers.setSelectedItem(QPSolverImpl::QPOASES);
+#endif
+
+    d_qpSolver.setValue(qpSolvers);
+
     d_graph.setWidget("graph");
+    createProblems();
+
+    m_qpSolverCB.addInput(&d_qpSolver);
+    m_qpSolverCB.addCallback([this]()
+    {
+        deleteProblems();
+        createProblems();
+    });
 }
 
 void QPInverseProblemSolver::createProblems()
 {
-    m_CP1 = new module::QPInverseProblemImpl();
-    m_CP2 = new module::QPInverseProblemImpl();
-    m_CP3 = new module::QPInverseProblemImpl();
+    switch(d_qpSolver.getValue().getSelectedId())
+    {
+#ifdef SOFTROBOTSINVERSE_ENABLE_PROXQP
+    case QPSolverImpl::PROXQP :
+        msg_info() << "Using proxQP solver";
+        m_CP1 = new module::QPInverseProblemProxQP();
+        m_CP2 = new module::QPInverseProblemProxQP();
+        m_CP3 = new module::QPInverseProblemProxQP();
+        break;
+#endif
+#ifdef SOFTROBOTSINVERSE_ENABLE_QPOASES
+    case QPSolverImpl::QPOASES :
+        msg_info() << "Using qpOASES solver";
+        m_CP1 = new module::QPInverseProblemQPOases();
+        m_CP2 = new module::QPInverseProblemQPOases();
+        m_CP3 = new module::QPInverseProblemQPOases();
+        break;
+#endif
+    default :
+        msg_error() << "Unkown specified solver: " << d_qpSolver.getValue();
+        sofa::core::objectmodel::BaseObject::d_componentState.setValue(sofa::core::objectmodel::ComponentState::Invalid);
+        break;
+    }
+
 
     m_currentCP = m_CP1;
 }
 
 void QPInverseProblemSolver::deleteProblems()
 {
-    delete m_CP1;
-    delete m_CP2;
-    delete m_CP3;
+    if(m_CP1)
+    {
+      delete m_CP1;
+      m_CP1 = nullptr;
+    }
+     if(m_CP2)
+    {
+      delete m_CP2;
+      m_CP2 = nullptr;
+    }
+     if(m_CP3)
+    {
+      delete m_CP3;
+      m_CP3 = nullptr;
+    }
 }
 
 QPInverseProblemSolver::~QPInverseProblemSolver()

src/SoftRobots.Inverse/component/solver/QPInverseProblemSolver.h

@@ -32,10 +32,12 @@
 #include <sofa/core/behavior/BaseConstraint.h>
 #include <sofa/core/behavior/ConstraintSolver.h>
 #include <sofa/core/behavior/BaseConstraintCorrection.h>
+#include <sofa/core/objectmodel/DataCallback.h>
 #include <sofa/core/objectmodel/KeypressedEvent.h>
 #include <sofa/simulation/TaskScheduler.h>
 #include <sofa/simulation/InitTasks.h>
 #include <sofa/helper/map.h>
+#include <sofa/helper/OptionsGroup.h>
 
 #include <SoftRobots/component/behavior/SoftRobotsBaseConstraint.h>
 #include <SoftRobots.Inverse/component/solver/modules/QPInverseProblemImpl.h>
@@ -58,6 +60,7 @@ using std::string;
 using sofa::simulation::Node;
 using sofa::core::MultiVecDerivId;
 
+
 /**
  * This component solves an inverse problem set by actuator and effector constraints. The method
  * is based on the formulation of a quadratic program (QP).
@@ -68,6 +71,13 @@ using sofa::core::MultiVecDerivId;
 class SOFA_SOFTROBOTS_INVERSE_API QPInverseProblemSolver : public sofa::component::constraint::lagrangian::solver::ConstraintSolverImpl
 {
 public:
+    // List of availables QP solvers used to solve the inverse problem
+    enum QPSolverImpl
+    {
+        QPOASES = 0,
+        PROXQP
+    };
+
     SOFA_CLASS(QPInverseProblemSolver, ConstraintSolverImpl);
 
     typedef vector<BaseConstraintCorrection*> list_cc;
@@ -136,6 +146,7 @@ class SOFA_SOFTROBOTS_INVERSE_API QPInverseProblemSolver : public sofa::componen
     sofa::Data<int>       d_maxIterations;
     sofa::Data<double>    d_tolerance;
     sofa::Data<double>    d_responseFriction;
+    sofa::Data<sofa::helper::OptionsGroup> d_qpSolver;
 
     sofa::Data<double>    d_epsilon;
     sofa::Data<bool>      d_actuatorsOnly;
@@ -154,6 +165,7 @@ class SOFA_SOFTROBOTS_INVERSE_API QPInverseProblemSolver : public sofa::componen
     module::QPInverseProblemImpl *m_lastCP, *m_currentCP;
     vector<BaseConstraintCorrection*> m_constraintsCorrections;
     vector<char> m_isConstraintCorrectionActive;
+    sofa::core::objectmodel::DataCallback m_qpSolverCB;
 
     Node *m_context;
 

src/SoftRobots.Inverse/component/solver/modules/LCPQPSolver.h

@@ -28,24 +28,17 @@
 ******************************************************************************/
 #pragma once
 
-#include <sofa/type/vector.h>
-
 // LCP solver using qpOASES QP solver
 // QP formulation for solving a LCP with a symmetric matrix M.
 
 namespace softrobotsinverse::solver::module {
 
-using sofa::type::vector;
-
 class LCPQPSolver
 {
-
 public:
+    virtual ~LCPQPSolver() = default;
 
-    LCPQPSolver(){}
-    ~LCPQPSolver(){}
-
-    void solve(int dim, double*q, double**M, double*res);
+    virtual void solve(int dim, double*q, double**M, double*res) = 0;
 };
 
 } // namespace