Fix numerical issue in SplineGradientBased

humanoid-path-planner · Dec 4, 2023 · 91311af · 91311af
1 parent 503e1b5
commit 91311af
Show file tree

Hide file tree

Showing 3 changed files with 23 additions and 5 deletions.
diff --git 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
@@ -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
@@ -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 "