From 8c1b68c0e3a5bbd724eba6d103174743eac30d39 Mon Sep 17 00:00:00 2001 From: Silvio Traversaro Date: Tue, 12 Nov 2024 13:58:22 +0100 Subject: [PATCH] Add setStringParameter and expose initial_guess parameter in proxqp (#15) --- CMakeLists.txt | 2 +- core/QpSolversEigen/NullSolver.cpp | 5 +++ core/QpSolversEigen/NullSolver.hpp | 1 + core/QpSolversEigen/Solver.cpp | 5 +++ core/QpSolversEigen/Solver.hpp | 10 +++++ core/QpSolversEigen/SolverInterface.hpp | 1 + examples/mpc/MPCExample.cpp | 10 ++++- plugins/osqp/QpSolversEigenOsqp.cpp | 8 ++++ plugins/proxqp/QpSolversEigenProxqp.cpp | 50 +++++++++++++++++++++++++ plugins/proxqp/README.md | 4 +- tests/MPCTest.cpp | 6 ++- tests/MPCUpdateMatricesTest.cpp | 8 ++++ 12 files changed, 106 insertions(+), 4 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 22e651c..74116b7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,6 +1,6 @@ cmake_minimum_required(VERSION 3.16...3.31) -project(QpSolversEigen VERSION 0.0.2) +project(QpSolversEigen VERSION 0.1.0) include(GNUInstallDirs) set(CMAKE_RUNTIME_OUTPUT_DIRECTORY "${CMAKE_BINARY_DIR}/${CMAKE_INSTALL_BINDIR}") diff --git a/core/QpSolversEigen/NullSolver.cpp b/core/QpSolversEigen/NullSolver.cpp index 3456dca..6b8248e 100644 --- a/core/QpSolversEigen/NullSolver.cpp +++ b/core/QpSolversEigen/NullSolver.cpp @@ -157,6 +157,11 @@ bool NullSolver::setRealNumberParameter(const std::string& settingName, double v return false; } +bool NullSolver::setStringParameter(const std::string& parameterName, const std::string& value) +{ + return false; +} + SolverInterface* NullSolver::allocateInstance() const { return new NullSolver(); diff --git a/core/QpSolversEigen/NullSolver.hpp b/core/QpSolversEigen/NullSolver.hpp index 42e3c51..7e0e191 100644 --- a/core/QpSolversEigen/NullSolver.hpp +++ b/core/QpSolversEigen/NullSolver.hpp @@ -65,6 +65,7 @@ class NullSolver final: public SolverInterface bool setBooleanParameter(const std::string& settingName, bool value) override; bool setIntegerParameter(const std::string& settingName, int64_t value) override; bool setRealNumberParameter(const std::string& settingName, double value) override; + bool setStringParameter(const std::string& parameterName, const std::string& value) override; SolverInterface* allocateInstance() const override; }; diff --git a/core/QpSolversEigen/Solver.cpp b/core/QpSolversEigen/Solver.cpp index 73ce9c5..b2a7c65 100644 --- a/core/QpSolversEigen/Solver.cpp +++ b/core/QpSolversEigen/Solver.cpp @@ -284,6 +284,11 @@ bool Solver::setRealNumberParameter(const std::string& parameterName, double val return m_pimpl->solver->setRealNumberParameter(parameterName, value); } +bool Solver::setStringParameter(const std::string& parameterName, const std::string& value) +{ + return m_pimpl->solver->setStringParameter(parameterName, value); +} + Solver* Solver::data() { return this; diff --git a/core/QpSolversEigen/Solver.hpp b/core/QpSolversEigen/Solver.hpp index 4fa8ed7..9d61d9e 100644 --- a/core/QpSolversEigen/Solver.hpp +++ b/core/QpSolversEigen/Solver.hpp @@ -275,6 +275,16 @@ class Solver */ bool setRealNumberParameter(const std::string& parameterName, double value); + /** + * Set a string parameter. + * + * @note Sometimes Enum parameters in a specific solvers are wrapped as a string parameter. + * + * @param parameterName the name of the parameter to bet set. + * @return true/false in case of success/failure. + */ + bool setStringParameter(const std::string& parameterName, const std::string& value); + /** * Return a pointer to this class. * diff --git a/core/QpSolversEigen/SolverInterface.hpp b/core/QpSolversEigen/SolverInterface.hpp index 7299621..c45e3ce 100644 --- a/core/QpSolversEigen/SolverInterface.hpp +++ b/core/QpSolversEigen/SolverInterface.hpp @@ -64,6 +64,7 @@ class SolverInterface virtual bool setBooleanParameter(const std::string& settingName, bool value) = 0; virtual bool setIntegerParameter(const std::string& settingName, int64_t value) = 0; virtual bool setRealNumberParameter(const std::string& settingName, double value) = 0; + virtual bool setStringParameter(const std::string& settingName, const std::string& value) = 0; /** * Allocate a new instance of this class, and return a pointer to it. diff --git a/examples/mpc/MPCExample.cpp b/examples/mpc/MPCExample.cpp index 7d74e2d..8ea2935 100644 --- a/examples/mpc/MPCExample.cpp +++ b/examples/mpc/MPCExample.cpp @@ -250,13 +250,21 @@ int main() } // settings + + // solver.setBooleanParameter("verbose", false); + // Set osqp-specific parameters if (solver.getSolverName() == "osqp") { - // solver.setBooleanParameter("verbose", false); solver.setBooleanParameter("warm_starting", true); } + // Set proxqp-specific parameters + if (solver.getSolverName() == "proxqp") + { + solver.setStringParameter("initial_guess", "WARM_START_WITH_PREVIOUS_RESULT"); + } + // set the initial data of the QP solver solver.setNumberOfVariables(12 * (mpcWindow + 1) + 4 * mpcWindow); solver.setNumberOfConstraints(2 * 12 * (mpcWindow + 1) + 4 * mpcWindow); diff --git a/plugins/osqp/QpSolversEigenOsqp.cpp b/plugins/osqp/QpSolversEigenOsqp.cpp index 7452ec9..6bbc1bc 100644 --- a/plugins/osqp/QpSolversEigenOsqp.cpp +++ b/plugins/osqp/QpSolversEigenOsqp.cpp @@ -71,6 +71,7 @@ class OsqpSolver final: public SolverInterface bool setBooleanParameter(const std::string& settingName, bool value) override; bool setIntegerParameter(const std::string& settingName, int64_t value) override; bool setRealNumberParameter(const std::string& settingName, double value) override; + bool setStringParameter(const std::string& parameterName, const std::string& value) override; SolverInterface* allocateInstance() const override; }; @@ -417,6 +418,13 @@ bool OsqpSolver::setRealNumberParameter(const std::string& settingName, double v return false; } +bool OsqpSolver::setStringParameter(const std::string& parameterName, const std::string& value) +{ + QpSolversEigen::debugStream() << "QpSolversEigen::OsqpSolver::setStringParameter: unknown setting name: " << parameterName << std::endl; + return false; +} + + SolverInterface* OsqpSolver::allocateInstance() const { return new OsqpSolver(); diff --git a/plugins/proxqp/QpSolversEigenProxqp.cpp b/plugins/proxqp/QpSolversEigenProxqp.cpp index 6ef3f09..3cf9936 100644 --- a/plugins/proxqp/QpSolversEigenProxqp.cpp +++ b/plugins/proxqp/QpSolversEigenProxqp.cpp @@ -102,6 +102,7 @@ class ProxqpSolver final: public SolverInterface bool setBooleanParameter(const std::string& settingName, bool value) override; bool setIntegerParameter(const std::string& settingName, int64_t value) override; bool setRealNumberParameter(const std::string& settingName, double value) override; + bool setStringParameter(const std::string& parameterName, const std::string& value) override; SolverInterface* allocateInstance() const override; }; @@ -533,6 +534,55 @@ bool ProxqpSolver::setRealNumberParameter(const std::string& settingName, double return false; } +bool ProxqpSolver::setStringParameter(const std::string& parameterName, const std::string& value) +{ + bool settingFound = false; + bool valueFound = false; + + if (parameterName == "initial_guess") + { + settingFound = true; + if (value == "NO_INITIAL_GUESS") + { + proxqpSettings.initial_guess = proxsuite::proxqp::InitialGuessStatus::NO_INITIAL_GUESS; + valueFound = true; + } else if (value == "EQUALITY_CONSTRAINED_INITIAL_GUESS") + { + proxqpSettings.initial_guess = proxsuite::proxqp::InitialGuessStatus::EQUALITY_CONSTRAINED_INITIAL_GUESS; + valueFound = true; + } else if (value == "WARM_START_WITH_PREVIOUS_RESULT") + { + proxqpSettings.initial_guess = proxsuite::proxqp::InitialGuessStatus::WARM_START_WITH_PREVIOUS_RESULT; + valueFound = true; + } else if (value == "WARM_START") + { + proxqpSettings.initial_guess = proxsuite::proxqp::InitialGuessStatus::WARM_START; + valueFound = true; + } else if (value == "COLD_START_WITH_PREVIOUS_RESULT") + { + proxqpSettings.initial_guess = proxsuite::proxqp::InitialGuessStatus::COLD_START_WITH_PREVIOUS_RESULT; + valueFound = true; + } + } + + + if (settingFound && valueFound) + { + syncSettings(); + return true; + } + + if (!settingFound) + { + QpSolversEigen::debugStream() << "QpSolversEigen::ProxqpSolver::setStringParameter: unknown setting name: " << parameterName << std::endl; + } else if (!valueFound) + { + QpSolversEigen::debugStream() << "QpSolversEigen::ProxqpSolver::setStringParameter: unknown value << " << value << " for parameter with name: " << parameterName << std::endl; + } + return false; +} + + SolverInterface* ProxqpSolver::allocateInstance() const { return new ProxqpSolver(); diff --git a/plugins/proxqp/README.md b/plugins/proxqp/README.md index 432dc22..9e016d9 100644 --- a/plugins/proxqp/README.md +++ b/plugins/proxqp/README.md @@ -63,4 +63,6 @@ If you need support for more parameters, please open an issue. ### String parameters -TODO +| Name | Notes | +|:------------:|:-------------------------:| +| `initial_guess` | Possible values are `NO_INITIAL_GUESS`, `EQUALITY_CONSTRAINED_INITIAL_GUESS`, `WARM_START_WITH_PREVIOUS_RESULT`, `WARM_START` or `COLD_START_WITH_PREVIOUS_RESULT`. See https://simple-robotics.github.io/proxsuite/md_doc_22-ProxQP__api.html#OverviewInitialGuess for more details. | diff --git a/tests/MPCTest.cpp b/tests/MPCTest.cpp index 8eea550..d936877 100644 --- a/tests/MPCTest.cpp +++ b/tests/MPCTest.cpp @@ -285,11 +285,15 @@ TEST_CASE("MPCTest") // settings REQUIRE(solver.setBooleanParameter("verbose", false)); - // Some parameters are only supported by osqp + // Set solver-specific parameters if (solver.getSolverName() == "osqp") { REQUIRE(solver.setBooleanParameter("warm_start", true)); } + if (solver.getSolverName() == "proxqp") + { + REQUIRE(solver.setStringParameter("initial_guess", "WARM_START_WITH_PREVIOUS_RESULT")); + } // set the initial data of the QP solver solver.data()->setNumberOfVariables(12 * (mpcWindow + 1) + 4 * mpcWindow); diff --git a/tests/MPCUpdateMatricesTest.cpp b/tests/MPCUpdateMatricesTest.cpp index 794c95e..d119726 100644 --- a/tests/MPCUpdateMatricesTest.cpp +++ b/tests/MPCUpdateMatricesTest.cpp @@ -263,6 +263,14 @@ TEST_CASE("MPCTest Update matrices") REQUIRE(solver.setBooleanParameter("warm_start", true)); } + if (solver.getSolverName() == "proxqp") + { + REQUIRE(solver.setStringParameter("initial_guess", "WARM_START_WITH_PREVIOUS_RESULT")); + // Check that setStringParameter fail for unknown setting or unknown value + REQUIRE_FALSE(solver.setStringParameter("initial_guess", "THIS_IS_NOT_A_VALID_INITIAL_GUESS_VALUE")); + REQUIRE_FALSE(solver.setStringParameter("this_is_not_a_valid_proqp_parameter_name", "THIS_IS_NOT_A_VALID_INITIAL_GUESS_VALUE")); + } + // set the initial data of the QP solver solver.data()->setNumberOfVariables(2 * (mpcWindow + 1) + 1 * mpcWindow); solver.data()->setNumberOfConstraints(2 * (mpcWindow + 1));