Add setStringParameter and expose initial_guess parameter in proxqp (#15

)
ami-iit · Nov 12, 2024 · 8c1b68c · 8c1b68c
1 parent 1a3c6e6
commit 8c1b68c
Show file tree

Hide file tree

Showing 12 changed files with 106 additions and 4 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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));