From 91311af1012fc24a7968934f091cc57d64729863 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@eurekarobotics.com>
Date: Mon, 4 Dec 2023 22:34:24 +0100
Subject: [PATCH] Fix numerical issue in SplineGradientBased

---
 .../path-optimization/quadratic-program.hh    |  2 +-
 src/path-optimization/quadratic-program.cc    |  7 ++++++-
 .../spline-gradient-based.cc                  | 19 ++++++++++++++++---
 3 files changed, 23 insertions(+), 5 deletions(-)

diff --git a/include/hpp/core/path-optimization/quadratic-program.hh b/include/hpp/core/path-optimization/quadratic-program.hh
index ed09a984..5254367a 100644
--- a/include/hpp/core/path-optimization/quadratic-program.hh
+++ b/include/hpp/core/path-optimization/quadratic-program.hh
@@ -128,7 +128,7 @@ struct QuadraticProgram {
   /// \param ce equality constraints
   /// \param ci inequality constraints: \f$ ci.J * x \ge ci.b \f$
   /// \note \ref computeLLT must have been called before.
-  double solve(const LinearConstraint& ce, const LinearConstraint& ci);
+  double solve(const LinearConstraint& ce, const LinearConstraint& ci, bool& ok);
 
   /// \}
 
diff --git a/src/path-optimization/quadratic-program.cc b/src/path-optimization/quadratic-program.cc
index fb205f16..4a39b6bf 100644
--- a/src/path-optimization/quadratic-program.cc
+++ b/src/path-optimization/quadratic-program.cc
@@ -64,7 +64,8 @@ void QuadraticProgram::computeLLT() {
 }
 
 double QuadraticProgram::solve(const LinearConstraint& ce,
-                               const LinearConstraint& ci) {
+                               const LinearConstraint& ci,
+                               bool& ok) {
   if (ce.J.rows() > ce.J.cols())
     throw std::runtime_error(
         "The QP is over-constrained. QuadProg cannot handle it.");
@@ -79,10 +80,14 @@ double QuadraticProgram::solve(const LinearConstraint& ce,
   switch (status) {
     case Eigen::UNBOUNDED:
       hppDout(warning, "Quadratic problem is not bounded");
+      ok = false;
+      break;
     case Eigen::CONVERGED:
+      ok = true;
       break;
     case Eigen::CONSTRAINT_LINEARLY_DEPENDENT:
       hppDout(error, "Constraint of quadratic problem are linearly dependent.");
+      ok = false;
       break;
   }
   return cost;
diff --git a/src/path-optimization/spline-gradient-based.cc b/src/path-optimization/spline-gradient-based.cc
index ee48c07c..58728af7 100644
--- a/src/path-optimization/spline-gradient-based.cc
+++ b/src/path-optimization/spline-gradient-based.cc
@@ -492,7 +492,10 @@ PathVectorPtr_t SplineGradientBased<_PB, _SO>::optimize(
     // There are no variables left for optimization.
     return this->buildPathVector(splines);
   QPc.computeLLT();
-  QPc.solve(collisionReduced, boundConstraintReduced);
+  bool qpSolved;
+  QPc.solve(collisionReduced, boundConstraintReduced, qpSolved);
+  if (!qpSolved)
+    return this->buildPathVector(splines);
 
   while (!(noCollision && minimumReached) && !this->shouldStop()) {
     // 6.1
@@ -533,7 +536,10 @@ PathVectorPtr_t SplineGradientBased<_PB, _SO>::optimize(
       if (linearizeAtEachStep) {
         collisionFunctions.linearize(splines, solvers, collision);
         constraint.reduceConstraint(collision, collisionReduced);
-        QPc.solve(collisionReduced, boundConstraintReduced);
+        QPc.solve(collisionReduced, boundConstraintReduced, qpSolved);
+        if (!qpSolved) {
+          hppDout(error, "could not solve qp problem");
+        }
         hppDout(info, "linearized");
         computeOptimum = true;
       }
@@ -567,6 +573,13 @@ PathVectorPtr_t SplineGradientBased<_PB, _SO>::optimize(
               collisionFunctions.functions.size() - 1,
               splines[reports[i].second], solvers[reports[i].second]);
           if (!ok) break;
+          QPc.solve(collisionReduced, boundConstraintReduced, ok);
+          if (!ok) {
+            hppDout(info, "could not solve QP. Removing constraint");
+            collisionFunctions.removeLastConstraint(1, collision);
+            constraint.reduceConstraint(collision, collisionReduced);
+            break;
+          }
           if (QPc.H.rows() <= collisionReduced.rank) break;
         }
 
@@ -583,7 +596,7 @@ PathVectorPtr_t SplineGradientBased<_PB, _SO>::optimize(
 
           computeInterpolatedSpline = true;
         } else {
-          QPc.solve(collisionReduced, boundConstraintReduced);
+          QPc.solve(collisionReduced, boundConstraintReduced, qpSolved);
           hppDout(info, "Added " << reports.size()
                                  << " constraints. "
                                     "Constraints size "