diff --git a/CMakeLists.txt b/CMakeLists.txt index 429cf78..151aa6c 100644 --- a/CMakeLists.txt +++ b/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,12 +138,6 @@ endif() set(DOC_FILES README.md) file(GLOB_RECURSE EXAMPLE_FILES examples "*.pyscn" "*.py" "*.md" "*.psl" "*.pslx" "*.scn" "*.xml") -option(SOFTROBOTSINVERSE_INSTALL_HEADERS "Install the headers" ON) -if(SOFTROBOTSINVERSE_INSTALL_HEADERS) - add_library(${PROJECT_NAME} SHARED ${SOURCE_FILES} ${DOC_FILES} ${EXAMPLE_FILES} ${HEADER_FILES}) -else() - add_library(${PROJECT_NAME} SHARED ${SOURCE_FILES} ${DOC_FILES} ${EXAMPLE_FILES} "${SRC_DIR}/component/config.h.in") -endif() option(SOFTROBOTSINVERSE_ENABLE_PROXQP "Build proxQP wrapper for inverse problems" OFF) option(SOFTROBOTSINVERSE_ENABLE_QPOASES "Build qpOASES wrapper for inverse problems" ON) @@ -154,15 +147,39 @@ if(NOT SOFTROBOTSINVERSE_ENABLE_PROXQP AND NOT SOFTROBOTSINVERSE_ENABLE_QPOASES) endif() if(SOFTROBOTSINVERSE_ENABLE_PROXQP) - find_package(proxsuite REQUIRED) - message("Sofa.SoftRobots.Inverse: proxsuite FOUND. Will build with proxsuite support.") - 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) + add_library(${PROJECT_NAME} SHARED ${SOURCE_FILES} ${DOC_FILES} ${EXAMPLE_FILES} ${HEADER_FILES}) +else() + add_library(${PROJECT_NAME} SHARED ${SOURCE_FILES} ${DOC_FILES} ${EXAMPLE_FILES} "${SRC_DIR}/component/config.h.in") +endif() + + +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 @@ -174,19 +191,13 @@ endif() if(SOFTROBOTSINVERSE_ENABLE_QPOASES) find_package(qpOASES QUIET) - list(APPEND HEADER_FILES - ${SRC_DIR}/component/solver/modules/QPInverseProblemQPOases.h - ) - list(APPEND SOURCE_FILES - ${SRC_DIR}/component/solver/modules/QPInverseProblemQPOases.cpp - ) - 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...") + 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) diff --git a/src/SoftRobots.Inverse/component/solver/QPInverseProblemSolver.cpp b/src/SoftRobots.Inverse/component/solver/QPInverseProblemSolver.cpp index 9a0020f..e9fb21a 100644 --- a/src/SoftRobots.Inverse/component/solver/QPInverseProblemSolver.cpp +++ b/src/SoftRobots.Inverse/component/solver/QPInverseProblemSolver.cpp @@ -164,7 +164,12 @@ QPInverseProblemSolver::QPInverseProblemSolver() , m_CP3(nullptr) { 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"); diff --git a/src/SoftRobots.Inverse/component/solver/modules/LCPQPSolver.h b/src/SoftRobots.Inverse/component/solver/modules/LCPQPSolver.h index 549267c..c08b4ce 100644 --- a/src/SoftRobots.Inverse/component/solver/modules/LCPQPSolver.h +++ b/src/SoftRobots.Inverse/component/solver/modules/LCPQPSolver.h @@ -28,24 +28,17 @@ ******************************************************************************/ #pragma once -#include - // 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 diff --git a/src/SoftRobots.Inverse/component/solver/modules/LCPQPSolverProxQP.cpp b/src/SoftRobots.Inverse/component/solver/modules/LCPQPSolverProxQP.cpp new file mode 100644 index 0000000..9ea2afa --- /dev/null +++ b/src/SoftRobots.Inverse/component/solver/modules/LCPQPSolverProxQP.cpp @@ -0,0 +1,143 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Plugin SoftRobots.Inverse * +* * +* This plugin is distributed under the GNU AGPL v3 (Affero General * +* Public License) license. * +* * +* Authors: Christian Duriez, Eulalie Coevoet, Yinoussa Adagolodjo * +* * +* (c) 2023 INRIA * +* * +* Contact information: https://project.inria.fr/softrobot/contact/ * +******************************************************************************/ + +#include +#include + +#include + +#include + +namespace softrobotsinverse::solver::module +{ + +void LCPQPSolverProxQP::solve(int dim, double*q, double**M, double*res) +{ + const int n_in = dim; + const int n_eq = 0; + + ////////////////////////////////////////// + // Constraints on lambda + ////////////////////////////////////////// + Eigen::VectorXd lbox(dim); // l + Eigen::VectorXd ubox(dim); // u + for (int i=0; i qp(dim, n_eq, n_in, true); // create the QP object with box constraints + + const double rho = 1e-14; // default is 1e-6, but has to be set smaller due to low values in Hessian when using data in meters (e.g. from URDF models) + + qp.init(H, g, std::nullopt, std::nullopt, C, l, u, lbox, ubox, true, rho); // initialize the model + // qp.init(H, g, A, b, C, l, u, lbox, ubox, true, rho); // initialize the model + + qp.settings.eps_rel = 0.; + qp.settings.eps_abs = 1e-12; // default is 1e-5 + qp.settings.check_duality_gap = true; + qp.settings.verbose = false; + qp.settings.max_iter = 3000; + // same here, default is 1e-4 for these epsilons, but has to be set smaller due to low values in Hessian when using data in meters (e.g. from URDF models) + qp.settings.eps_primal_inf = 1e-12; + qp.settings.eps_dual_inf = 1e-12; + + qp.solve(); + + switch(qp.results.info.status) + { + case proxsuite::proxqp::QPSolverOutput::PROXQP_SOLVED: + break; + case proxsuite::proxqp::QPSolverOutput::PROXQP_MAX_ITER_REACHED: + msg_warning("QPInverseProblemImpl") << "Solver status: PROXQP_MAX_ITER_REACHED"; + break; + case proxsuite::proxqp::QPSolverOutput::PROXQP_PRIMAL_INFEASIBLE: + msg_warning("QPInverseProblemImpl") << "Solver status: PROXQP_PRIMAL_INFEASIBLE"; + break; + case proxsuite::proxqp::QPSolverOutput::PROXQP_SOLVED_CLOSEST_PRIMAL_FEASIBLE: + msg_warning("QPInverseProblemImpl") << "Solver status: PROXQP_SOLVED_CLOSEST_PRIMAL_FEASIBLE"; + break; + case proxsuite::proxqp::QPSolverOutput::PROXQP_DUAL_INFEASIBLE: + msg_warning("QPInverseProblemImpl") << "Solver status: PROXQP_DUAL_INFEASIBLE"; + break; + case proxsuite::proxqp::QPSolverOutput::PROXQP_NOT_RUN: + msg_warning("QPInverseProblemImpl") << "Solver status: PROXQP_NOT_RUN"; + break; + default: + msg_error("QPInverseProblemImpl") << "Unknown solver status"; + } + + // get primal solution + for (int i = 0; i < dim; ++i) + { + res[i] = qp.results.x[i]; + } +} + +} // namespace diff --git a/src/SoftRobots.Inverse/component/solver/modules/LCPQPSolverProxQP.h b/src/SoftRobots.Inverse/component/solver/modules/LCPQPSolverProxQP.h new file mode 100644 index 0000000..e3a0a19 --- /dev/null +++ b/src/SoftRobots.Inverse/component/solver/modules/LCPQPSolverProxQP.h @@ -0,0 +1,50 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Plugin SoftRobots.Inverse * +* * +* This plugin is distributed under the GNU AGPL v3 (Affero General * +* Public License) license. * +* * +* Authors: Christian Duriez, Eulalie Coevoet, Yinoussa Adagolodjo * +* * +* (c) 2023 INRIA * +* * +* Contact information: https://project.inria.fr/softrobot/contact/ * +******************************************************************************/ +#pragma once + +#include + +// LCP solver using proxQP solver +// QP formulation for solving a LCP with a symmetric matrix M. + +namespace softrobotsinverse::solver::module { + +class LCPQPSolverProxQP: public LCPQPSolver +{ + +public: + + LCPQPSolverProxQP() = default; + virtual ~LCPQPSolverProxQP() = default; + + void solve(int dim, double*q, double**M, double*res) override; +}; + +} // namespace + diff --git a/src/SoftRobots.Inverse/component/solver/modules/LCPQPSolver.cpp b/src/SoftRobots.Inverse/component/solver/modules/LCPQPSolverQPOases.cpp similarity index 97% rename from src/SoftRobots.Inverse/component/solver/modules/LCPQPSolver.cpp rename to src/SoftRobots.Inverse/component/solver/modules/LCPQPSolverQPOases.cpp index dd42962..3a7012c 100644 --- a/src/SoftRobots.Inverse/component/solver/modules/LCPQPSolver.cpp +++ b/src/SoftRobots.Inverse/component/solver/modules/LCPQPSolverQPOases.cpp @@ -28,8 +28,9 @@ ******************************************************************************/ #include +#include #include -#include +#include namespace softrobotsinverse::solver::module @@ -45,7 +46,7 @@ using qpOASES::int_t; using sofa::type::vector; -void LCPQPSolver::solve(int dim, double*q, double**M, double*res) +void LCPQPSolverQPOases::solve(int dim, double*q, double**M, double*res) { ////////////////////////////////////////// // Constraints on lambda diff --git a/src/SoftRobots.Inverse/component/solver/modules/LCPQPSolverQPOases.h b/src/SoftRobots.Inverse/component/solver/modules/LCPQPSolverQPOases.h new file mode 100644 index 0000000..3c8c2df --- /dev/null +++ b/src/SoftRobots.Inverse/component/solver/modules/LCPQPSolverQPOases.h @@ -0,0 +1,51 @@ +/****************************************************************************** +* SOFA, Simulation Open-Framework Architecture * +* (c) 2006 INRIA, USTL, UJF, CNRS, MGH * +* * +* This program is free software; you can redistribute it and/or modify it * +* under the terms of the GNU Lesser General Public License as published by * +* the Free Software Foundation; either version 2.1 of the License, or (at * +* your option) any later version. * +* * +* This program is distributed in the hope that it will be useful, but WITHOUT * +* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or * +* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public License * +* for more details. * +* * +* You should have received a copy of the GNU Lesser General Public License * +* along with this program. If not, see . * +******************************************************************************* +* Plugin SoftRobots.Inverse * +* * +* This plugin is distributed under the GNU AGPL v3 (Affero General * +* Public License) license. * +* * +* Authors: Christian Duriez, Eulalie Coevoet, Yinoussa Adagolodjo * +* * +* (c) 2023 INRIA * +* * +* Contact information: https://project.inria.fr/softrobot/contact/ * +******************************************************************************/ +#pragma once + +#include + +#include + +// LCP solver using qpOASES QP solver +// QP formulation for solving a LCP with a symmetric matrix M. + +namespace softrobotsinverse::solver::module { + +class LCPQPSolverQPOases: public LCPQPSolver +{ + +public: + LCPQPSolverQPOases() = default; + virtual ~LCPQPSolverQPOases() = default; + + void solve(int dim, double*q, double**M, double*res) override; +}; + +} // namespace + diff --git a/src/SoftRobots.Inverse/component/solver/modules/QPInverseProblem.cpp b/src/SoftRobots.Inverse/component/solver/modules/QPInverseProblem.cpp index 4b3fd82..156a8c2 100644 --- a/src/SoftRobots.Inverse/component/solver/modules/QPInverseProblem.cpp +++ b/src/SoftRobots.Inverse/component/solver/modules/QPInverseProblem.cpp @@ -33,7 +33,6 @@ #include #include -#include #include #include @@ -52,12 +51,6 @@ using std::ceil; using sofa::linearalgebra::FullVector; using sofa::linearalgebra::LPtrFullMatrix; - -using qpOASES::QProblem; -using qpOASES::Options; -using qpOASES::real_t; -using qpOASES::int_t; - using sofa::helper::logging::Message; using sofa::core::objectmodel::Base; diff --git a/src/SoftRobots.Inverse/component/solver/modules/QPInverseProblemImpl.cpp b/src/SoftRobots.Inverse/component/solver/modules/QPInverseProblemImpl.cpp index 5a9fdd6..4fec6a2 100644 --- a/src/SoftRobots.Inverse/component/solver/modules/QPInverseProblemImpl.cpp +++ b/src/SoftRobots.Inverse/component/solver/modules/QPInverseProblemImpl.cpp @@ -32,7 +32,6 @@ #include #include -#include #include #include @@ -57,13 +56,6 @@ using sofa::component::collision::response::contact::CollisionResponse; using sofa::core::objectmodel::BaseContext; using sofa::core::behavior::BaseConstraint; -using qpOASES::QProblemB; -using qpOASES::QProblem; - -using qpOASES::Options; -using qpOASES::real_t; -using qpOASES::int_t; - using Eigen::MatrixXd; using Eigen::VectorXd; using Eigen::LDLT; @@ -72,7 +64,8 @@ using sofa::helper::AdvancedTimer; QPInverseProblemImpl::QPInverseProblemImpl() - : QPInverseProblem() + : QPInverseProblem(), + m_lcpQpSolver{nullptr} { m_currentSequence.clear(); m_previousSequence.clear(); @@ -434,11 +427,10 @@ void QPInverseProblemImpl::solveContacts(vector& res) } else { - LCPQPSolver* lcpSolver = new LCPQPSolver(); x.clear(); x.resize(nbContactRows); - lcpSolver->solve(nbContactRows, q.ptr(), M.lptr(), x.ptr()); - delete lcpSolver; + // will use overrided solve depending on its solver implementation + m_lcpQpSolver->solve(nbContactRows, q.ptr(), M.lptr(), x.ptr()); for (unsigned int i=0; i #include +#include #include @@ -66,6 +67,7 @@ class SOFA_SOFTROBOTS_INVERSE_API QPInverseProblemImpl : public QPInverseProblem ConstraintHandler* m_constraintHandler; ConstraintHandler::QPConstraintParams* m_qpCParams; + std::unique_ptr m_lcpQpSolver; // Utils to prevent cycling in pivot algorithm vector m_currentSequence; diff --git a/src/SoftRobots.Inverse/component/solver/modules/QPInverseProblemProxQP.cpp b/src/SoftRobots.Inverse/component/solver/modules/QPInverseProblemProxQP.cpp index 67990c5..180b3d2 100644 --- a/src/SoftRobots.Inverse/component/solver/modules/QPInverseProblemProxQP.cpp +++ b/src/SoftRobots.Inverse/component/solver/modules/QPInverseProblemProxQP.cpp @@ -34,6 +34,7 @@ #include +#include #include namespace softrobotsinverse::solver::module @@ -41,6 +42,12 @@ namespace softrobotsinverse::solver::module using sofa::helper::AdvancedTimer; +QPInverseProblemProxQP::QPInverseProblemProxQP() : + QPInverseProblemImpl() +{ + m_lcpQpSolver = std::make_unique(); +} + void QPInverseProblemProxQP::solveInverseProblem(double& objective, vector &result, vector &dual) diff --git a/src/SoftRobots.Inverse/component/solver/modules/QPInverseProblemProxQP.h b/src/SoftRobots.Inverse/component/solver/modules/QPInverseProblemProxQP.h index 3a93584..b3e65b5 100644 --- a/src/SoftRobots.Inverse/component/solver/modules/QPInverseProblemProxQP.h +++ b/src/SoftRobots.Inverse/component/solver/modules/QPInverseProblemProxQP.h @@ -44,7 +44,7 @@ class SOFA_SOFTROBOTS_INVERSE_API QPInverseProblemProxQP : public QPInverseProbl { public: - QPInverseProblemProxQP() = default; + QPInverseProblemProxQP(); virtual ~QPInverseProblemProxQP() = default; protected: diff --git a/src/SoftRobots.Inverse/component/solver/modules/QPInverseProblemQPOases.cpp b/src/SoftRobots.Inverse/component/solver/modules/QPInverseProblemQPOases.cpp index 2279a53..5a349e5 100644 --- a/src/SoftRobots.Inverse/component/solver/modules/QPInverseProblemQPOases.cpp +++ b/src/SoftRobots.Inverse/component/solver/modules/QPInverseProblemQPOases.cpp @@ -32,6 +32,7 @@ #include +#include #include #include @@ -40,6 +41,12 @@ namespace softrobotsinverse::solver::module using sofa::helper::AdvancedTimer; +QPInverseProblemQPOases::QPInverseProblemQPOases() : + QPInverseProblemImpl() +{ + m_lcpQpSolver = std::make_unique(); +} + void QPInverseProblemQPOases::solveInverseProblem(double& objective, vector &result, vector &dual) diff --git a/src/SoftRobots.Inverse/component/solver/modules/QPInverseProblemQPOases.h b/src/SoftRobots.Inverse/component/solver/modules/QPInverseProblemQPOases.h index 52d78bd..7e6ca53 100644 --- a/src/SoftRobots.Inverse/component/solver/modules/QPInverseProblemQPOases.h +++ b/src/SoftRobots.Inverse/component/solver/modules/QPInverseProblemQPOases.h @@ -55,7 +55,7 @@ class SOFA_SOFTROBOTS_INVERSE_API QPInverseProblemQPOases : public QPInverseProb { public: - QPInverseProblemQPOases() = default; + QPInverseProblemQPOases(); virtual ~QPInverseProblemQPOases() = default; protected: