Repository: stack-of-tasks/tsid (language: python)

Issue:
Improve computation time of HQP solver
Currently the only QP solver implemented is eiquadprog, which is already rather efficient, but there's room for improvement. In particular, the current version of the solver allocates dynamic memory at each call to the solver, so it should be easy to decrease computation time by pre-allocating all the necessary memory in an initialization phase.


Discussion / hints:
In the last commit 8788221c89d07ddbcc118f4f322c69bb4b284890 I added two improved versions of eiquadprog, neither of which allocates dynamic memory during resolution (verified using Eigen function ```set_is_malloc_allowed```). The only difference between them is that RtEiquadprog uses matrices with size known at compile time, whereas EiquadprogFast does not. In both solvers I optimized a bit the code using some Eigen tricks such as ```noalias```, or ```solveInPlace```.  Finally, both new solvers have also more documentation and provide more information to the interested user, such as number of iterations, number of active inequalities, maximum number of iterations and proper status flag in case the problem could not be solved.
Thiese are the results I got solving random QP on HRP-2's computer (but in a non-real-time thread). More or less we gained 15% of the computation time. The real-time solver is only slightly faster than the "fast" solver.  The size of the problems has been chosen to match the size of the QP we typically have to solve in our inverse-dynamics balance controller.

> 1000 tests with 60 variables, 18 equalities, 40 inequalities
> *** PROFILING RESULTS [ms]                       (min - avg - max - nSamples) ***
> Eiquadprog ...................                                        0.651    0.704    0.870     1000
> Eiquadprog Fast ............                                        0.563    0.605    0.810     1000
> Eiquadprog Real Time ....                                       0.543    0.592    0.712      1000
> 
> *** STATISTICS (min - avg - max - nSamples) ***
> active inequalities       ....                                  16.0    19.8    26.0      1000
> solver iterations  ........                                         17.0    21.5    29.0      1000
FYI: I also tried to warm start the algorithm (see macro ```USE_WARM_START``` in ```eiquadprog_fast.h```) but I didn't manage to make it work properly (it only worked in the trivial case where the active set didn't change). Then I decided to stop working on it, but maybe it's just a matter of fixing a simple bug to make it work, in case someone is interested in finishing the work (the code should be quite self-explaining now because I added plenty of comments).
I've just discovered a new [open-source QP solver](https://github.com/asherikov/qpmad), which is supposed to be faster than eiquadprog, so we may give it a try by integrating it inside TSID. Anybody feels like doing it?
Since this issue has been inactive for a while now I think we can close it.

Fix patch:
diff --git a/CMakeLists.txt b/CMakeLists.txt
index b84f2155..5ded4c56 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -75,6 +75,11 @@ CHECK_MINIMAL_CXX_STANDARD(11 ENFORCE)
 ADD_PROJECT_DEPENDENCY(pinocchio 2.3.1 REQUIRED)
 ADD_PROJECT_DEPENDENCY(eiquadprog 1.1.3 REQUIRED)
 
+FIND_PACKAGE(qpmad QUIET) # optional
+if(qpmad_FOUND)
+  ADD_PROJECT_DEPENDENCY(qpmad QUIET)
+endif()
+
 IF(BUILD_TESTING)
   FIND_PACKAGE(Boost REQUIRED COMPONENTS unit_test_framework)
 ENDIF(BUILD_TESTING)
@@ -143,6 +148,10 @@ SET(${PROJECT_NAME}_SOLVERS_HEADERS
   include/tsid/solvers/solver-HQP-eiquadprog-fast.hpp
   )
 
+if(qpmad_FOUND)
+  LIST(APPEND ${PROJECT_NAME}_SOLVERS_HEADERS include/tsid/solvers/solver-HQP-qpmad.hpp)
+endif()
+
 SET(${PROJECT_NAME}_ROBOTS_HEADERS
   include/tsid/robots/fwd.hpp
   include/tsid/robots/robot-wrapper.hpp
@@ -230,6 +239,10 @@ SET(${PROJECT_NAME}_SOLVERS_SOURCES
   src/solvers/utils.cpp
   )
 
+if(qpmad_FOUND)
+  LIST(APPEND ${PROJECT_NAME}_SOLVERS_SOURCES src/solvers/solver-HQP-qpmad.cpp)
+endif()
+
 SET(${PROJECT_NAME}_ROBOTS_SOURCES
   src/robots/robot-wrapper.cpp
   )
@@ -261,6 +274,11 @@ TARGET_INCLUDE_DIRECTORIES(${PROJECT_NAME} PUBLIC $<INSTALL_INTERFACE:include>)
 TARGET_LINK_LIBRARIES(${PROJECT_NAME} PUBLIC
   pinocchio::pinocchio eiquadprog::eiquadprog)
 
+if(qpmad_FOUND)
+  TARGET_INCLUDE_DIRECTORIES(${PROJECT_NAME} PUBLIC ${qpmad_INCLUDE_DIRS})
+  TARGET_COMPILE_DEFINITIONS(${PROJECT_NAME} PUBLIC -DTSID_QPMAD_FOUND)
+endif()
+
 
 IF(SUFFIX_SO_VERSION)
   SET_TARGET_PROPERTIES(${PROJECT_NAME} PROPERTIES SOVERSION ${PROJECT_VERSION})
diff --git a/include/tsid/solvers/fwd.hpp b/include/tsid/solvers/fwd.hpp
index f48c56ec..5c6c9023 100644
--- a/include/tsid/solvers/fwd.hpp
+++ b/include/tsid/solvers/fwd.hpp
@@ -39,8 +39,11 @@ namespace tsid
       SOLVER_HQP_EIQUADPROG = 0,
       SOLVER_HQP_EIQUADPROG_FAST = 1,
       SOLVER_HQP_EIQUADPROG_RT = 2
+#ifdef TSID_QPMAD_FOUND
+      ,SOLVER_HQP_QPMAD
+#endif
 #ifdef QPOASES_FOUND
-      ,SOLVER_HQP_OASES = 3
+      ,SOLVER_HQP_OASES
 #endif
     };
     
diff --git a/include/tsid/solvers/solver-HQP-qpmad.hpp b/include/tsid/solvers/solver-HQP-qpmad.hpp
new file mode 100644
index 00000000..b0fd3e71
--- /dev/null
+++ b/include/tsid/solvers/solver-HQP-qpmad.hpp
@@ -0,0 +1,84 @@
+//
+// Copyright (c) 2022 Inria
+//
+// This file is part of tsid
+// tsid 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
+// 3 of the License, or (at your option) any later version.
+// tsid 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
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// tsid If not, see
+// <http://www.gnu.org/licenses/>.
+//
+
+#ifndef __invdyn_solvers_hqp_qpmad_hpp__
+#define __invdyn_solvers_hqp_qpmad_hpp__
+
+#include <tsid/solvers/solver-HQP-base.hpp>
+
+#include <qpmad/solver.h>
+
+namespace tsid
+{
+  namespace solvers
+  {
+    /**
+     * @brief Implementation of Quadratic Program (HQP) solver using qpmad.
+     */
+    class TSID_DLLAPI SolverHQpmad:
+        public SolverHQPBase
+    {
+    public:
+      EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+      
+      typedef math::Matrix Matrix;
+      typedef math::Vector Vector;
+      typedef math::RefVector RefVector;
+      typedef math::ConstRefVector ConstRefVector;
+      typedef math::ConstRefMatrix ConstRefMatrix;
+
+      typedef qpmad::SolverParameters Settings;
+
+      SolverHQpmad(const std::string & name);
+
+      void resize(unsigned int n, unsigned int neq, unsigned int nin);
+
+      /** Solve the given Hierarchical Quadratic Program
+       */
+      const HQPOutput & solve(const HQPData & problemData);
+
+      /** Get the objective value of the last solved problem. */
+      double getObjectiveValue();
+
+      Settings& settings() { return m_settings; }
+
+    protected:
+
+      void sendMsg(const std::string & s);
+
+      qpmad::Solver m_solver;
+      Settings m_settings;
+
+      bool m_has_bounds;
+      
+      Matrix m_H;   // hessian matrix
+      Vector m_g;   // gradient vector
+      Vector m_lb;  // gradient vector
+      Vector m_ub;  // gradient vector
+      Matrix m_C;   // constraint matrix
+      Vector m_cl;  // constraints lower bound
+      Vector m_cu;  // constraints upper bound
+
+      double m_hessian_regularization;
+
+      unsigned int m_nc;  /// number of equality-inequality constraints
+      unsigned int m_n;    /// number of variables
+    };
+  }
+}
+
+#endif // ifndef __invdyn_solvers_hqp_qpmad_hpp__
diff --git a/src/solvers/solver-HQP-factory.cpp b/src/solvers/solver-HQP-factory.cpp
index f7f6def1..14bbcae6 100644
--- a/src/solvers/solver-HQP-factory.cpp
+++ b/src/solvers/solver-HQP-factory.cpp
@@ -19,6 +19,10 @@
 #include <tsid/solvers/solver-HQP-eiquadprog.hpp>
 #include <tsid/solvers/solver-HQP-eiquadprog-fast.hpp>
 
+#ifdef TSID_QPMAD_FOUND
+  #include <tsid/solvers/solver-HQP-qpmad.hpp>
+#endif
+
 #ifdef QPOASES_FOUND
   #include <tsid/solvers/solver-HQP-qpoases.hh>
 #endif
@@ -36,7 +40,12 @@ namespace tsid
       
       if(solverType==SOLVER_HQP_EIQUADPROG_FAST)
         return new SolverHQuadProgFast(name);
-      
+
+#ifdef TSID_QPMAD_FOUND
+      if(solverType==SOLVER_HQP_QPMAD)
+        return new SolverHQpmad(name);
+#endif
+
 #ifdef QPOASES_FOUND
       if(solverType==SOLVER_HQP_QPOASES)
         return new Solver_HQP_qpoases(name);
diff --git a/src/solvers/solver-HQP-qpmad.cpp b/src/solvers/solver-HQP-qpmad.cpp
new file mode 100644
index 00000000..998e776d
--- /dev/null
+++ b/src/solvers/solver-HQP-qpmad.cpp
@@ -0,0 +1,224 @@
+//
+// Copyright (c) 2022 Inria
+//
+// This file is part of tsid
+// tsid 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
+// 3 of the License, or (at your option) any later version.
+// tsid 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
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// tsid If not, see
+// <http://www.gnu.org/licenses/>.
+//
+
+#include "tsid/solvers/solver-HQP-qpmad.hpp"
+#include "tsid/math/utils.hpp"
+#include "tsid/utils/stop-watch.hpp"
+
+#include <limits>
+
+using namespace tsid::math;
+using namespace tsid::solvers;
+using namespace Eigen;
+
+namespace tsid
+{
+  namespace solvers
+  {
+
+    SolverHQpmad::SolverHQpmad(const std::string & name):
+      SolverHQPBase(name),
+      m_hessian_regularization(DEFAULT_HESSIAN_REGULARIZATION)
+    {
+      m_n = 0;
+      m_nc = 0;
+    }
+
+    void SolverHQpmad::sendMsg(const std::string & s)
+    {
+      std::cout<<"[SolverHQpmad."<<m_name<<"] "<<s<<std::endl;
+    }
+
+    void SolverHQpmad::resize(unsigned int n, unsigned int neq, unsigned int nin)
+    {
+      unsigned int nc = neq+nin;
+
+      const bool resizeVar = n!=m_n;
+      const bool resizeEqIn = (resizeVar || nc!=m_nc);
+
+      if(resizeEqIn)
+      {
+    #ifndef NDEBUG
+        sendMsg("Resizing equality-inequality constraints from "+toString(m_nc)+" to "+toString(nc));
+    #endif
+        m_C.resize(nc, n);
+        m_cl.resize(nc);
+        m_cu.resize(nc);
+      }
+      if(resizeVar)
+      {
+    #ifndef NDEBUG
+        sendMsg("Resizing Hessian from "+toString(m_n)+" to "+toString(n));
+    #endif
+        m_H.resize(n, n);
+        m_g.resize(n);
+        m_output.x.resize(n);
+
+        if(m_has_bounds)
+        {
+          m_lb.resize(n);
+          m_ub.resize(n);
+        }
+      }
+
+      m_n = n;
+      m_nc = nc;
+    }
+
+    const HQPOutput & SolverHQpmad::solve(const HQPData & problemData)
+    {
+      if(problemData.size()>2)
+      {
+        assert(false && "Solver not implemented for more than 2 hierarchical levels.");
+      }
+
+      // Compute the constraint matrix sizes
+      m_has_bounds = false;
+      unsigned int nin = 0, neq=0;
+      const ConstraintLevel & cl0 = problemData[0];
+      if(cl0.size()>0)
+      {
+        const unsigned int n = cl0[0].second->cols();
+        for(ConstraintLevel::const_iterator it=cl0.begin(); it!=cl0.end(); it++)
+        {
+          auto constr = it->second;
+          assert(n==constr->cols());
+          if(constr->isEquality())
+            neq += constr->rows();
+          else if(constr->isInequality())
+            nin += constr->rows();
+          else if(constr->isBound())
+            m_has_bounds = true;
+        }
+        // If necessary, resize the constraint matrices
+        resize(n, neq, nin);
+
+        if(m_has_bounds)
+        {
+          m_lb.setConstant(std::numeric_limits<double>::min());
+          m_ub.setConstant(std::numeric_limits<double>::max());
+        }
+
+        int i_eq_in=0;
+        for(ConstraintLevel::const_iterator it=cl0.begin(); it!=cl0.end(); it++)
+        {
+          auto constr = it->second;
+          if(constr->isEquality())
+          {
+            m_C.middleRows(i_eq_in, constr->rows()) = constr->matrix();
+            m_cl.segment(i_eq_in, constr->rows())   = constr->vector();
+            m_cu.segment(i_eq_in, constr->rows())   = constr->vector();
+            i_eq_in += constr->rows();
+          }
+          else if(constr->isInequality())
+          {
+            m_C.middleRows(i_eq_in, constr->rows()) = constr->matrix();
+            m_cl.segment(i_eq_in, constr->rows())   = constr->lowerBound();
+            m_cu.segment(i_eq_in, constr->rows())   = constr->upperBound();
+            i_eq_in += constr->rows();
+          }
+          else if(constr->isBound())
+          {
+            // not considering masks
+            m_lb = m_lb.cwiseMax(constr->lowerBound());
+            m_ub = m_ub.cwiseMin(constr->upperBound());
+          }
+        }
+      }
+      else
+        resize(m_n, neq, nin);
+
+      if(problemData.size()>1)
+      {
+        const ConstraintLevel & cl1 = problemData[1];
+        m_H.setZero();
+        m_g.setZero();
+        for(ConstraintLevel::const_iterator it=cl1.begin(); it!=cl1.end(); it++)
+        {
+          const double & w = it->first;
+          auto constr = it->second;
+          if(!constr->isEquality())
+            assert(false && "Inequalities in the cost function are not implemented yet");
+
+          m_H.noalias() += w*constr->matrix().transpose()*constr->matrix();
+          m_g.noalias() -= w*(constr->matrix().transpose()*constr->vector());
+        }
+        m_H.diagonal().array() += m_hessian_regularization;
+      }
+
+      //  min 0.5 * x H x + g x
+      //  s.t.
+      //  cl <= C^T x <= cu
+      //  lb <= x <= ub
+
+      qpmad::Solver::ReturnStatus solve_status;
+
+      if(m_has_bounds)
+        solve_status = m_solver.solve(m_output.x, m_H, m_g, m_lb, m_ub, m_C, m_cl, m_cu, m_settings);
+      else
+        solve_status = m_solver.solve(m_output.x, m_H, m_g, Eigen::VectorXd(), Eigen::VectorXd(), m_C, m_cl, m_cu, m_settings);
+
+      if(solve_status != qpmad::Solver::OK)
+        m_output.status = HQP_STATUS_INFEASIBLE;
+      else
+      {
+        m_output.status = HQP_STATUS_OPTIMAL;
+
+    #ifndef NDEBUG
+        const Vector & x = m_output.x;
+
+        if(cl0.size()>0)
+        {
+          for(ConstraintLevel::const_iterator it=cl0.begin(); it!=cl0.end(); it++)
+          {
+            auto constr = it->second;
+            if(constr->checkConstraint(x)==false)
+            {
+              if(constr->isEquality())
+              {
+                sendMsg("Equality "+constr->name()+" violated: "+
+                        toString((constr->matrix()*x-constr->vector()).norm()));
+              }
+              else if(constr->isInequality())
+              {
+                sendMsg("Inequality "+constr->name()+" violated: "+
+                        toString((constr->matrix()*x-constr->lowerBound()).minCoeff())+"\n"+
+                        toString((constr->upperBound()-constr->matrix()*x).minCoeff()));
+              }
+              else if(constr->isBound())
+              {
+                sendMsg("Bound "+constr->name()+" violated: "+
+                        toString((x-constr->lowerBound()).minCoeff())+"\n"+
+                        toString((constr->upperBound()-x).minCoeff()));
+              }
+            }
+          }
+        }
+    #endif
+      }
+
+      return m_output;
+    }
+
+    double SolverHQpmad::getObjectiveValue()
+    {
+      // objective values is not stored by qpmad
+      return std::numeric_limits<double>::infinity();
+    }
+
+  } // namespace solvers
+} // namespace tsid
\ No newline at end of file


Test patch:
diff --git a/tests/hqp_solvers.cpp b/tests/hqp_solvers.cpp
index daec9df2..bd5e6ff7 100644
--- a/tests/hqp_solvers.cpp
+++ b/tests/hqp_solvers.cpp
@@ -23,6 +23,11 @@
 #include <tsid/solvers/solver-HQP-factory.hxx>
 #include <tsid/solvers/solver-HQP-eiquadprog.hpp>
 #include <tsid/solvers/solver-HQP-eiquadprog-rt.hpp>
+
+#ifdef TSID_QPMAD_FOUND
+  #include <tsid/solvers/solver-HQP-qpmad.hpp>
+#endif
+
 #include <tsid/math/utils.hpp>
 #include <tsid/math/constraint-equality.hpp>
 #include <tsid/math/constraint-inequality.hpp>
@@ -164,6 +169,7 @@ BOOST_AUTO_TEST_SUITE ( BOOST_TEST_MODULE )
 #define PROFILE_EIQUADPROG "Eiquadprog"
 #define PROFILE_EIQUADPROG_RT "Eiquadprog Real Time"
 #define PROFILE_EIQUADPROG_FAST "Eiquadprog Fast"
+#define PROFILE_QPMAD "QPMAD"
 
 BOOST_AUTO_TEST_CASE ( test_eiquadprog_classic_vs_rt_vs_fast)
 {
@@ -208,6 +214,12 @@ BOOST_AUTO_TEST_CASE ( test_eiquadprog_classic_vs_rt_vs_fast)
                                                              "eiquadprog");
   solver->resize(n, neq, nin);
 
+#ifdef TSID_QPMAD_FOUND
+  SolverHQPBase * solver_qpmad = SolverHQPFactory::createNewSolver(SOLVER_HQP_QPMAD,
+                                                             "qpmad");
+  solver_qpmad->resize(n, neq, nin);
+#endif
+
   // CREATE PROBLEM DATA
   HQPData HQPData(2);
 
@@ -274,6 +286,12 @@ BOOST_AUTO_TEST_CASE ( test_eiquadprog_classic_vs_rt_vs_fast)
     const HQPOutput & output    = solver->solve(HQPData);
     getProfiler().stop(PROFILE_EIQUADPROG);
 
+  #ifdef TSID_QPMAD_FOUND
+    getProfiler().start(PROFILE_QPMAD);
+    const HQPOutput & output_qpmad = solver_qpmad->solve(HQPData);
+    getProfiler().stop(PROFILE_QPMAD);
+  #endif
+
     getStatistics().store("active inequalities", (double)output_rt.activeSet.size());
     getStatistics().store("solver iterations", output_rt.iterations);
 
@@ -303,6 +321,11 @@ BOOST_AUTO_TEST_CASE ( test_eiquadprog_classic_vs_rt_vs_fast)
   //                        "Sol RT"+toString(output_rt.x.transpose())+
   //                        "\nSol FAST "+toString(output_fast.x.transpose())+
                           "\nDiff FAST: "+toString((output_rt.x-output_fast.x).norm()));
+
+    #ifdef TSID_QPMAD_FOUND
+      BOOST_CHECK_MESSAGE(output.x.isApprox(output_qpmad.x, EPS),
+                          "\nDiff QPMAD: "+toString((output.x-output_qpmad.x).norm()));
+    #endif
     }
   }