WIP: fix asymptotically infinite loop with Dichotomy

include/hpp/core/continuous-validation.hh

@@ -219,6 +219,14 @@ class HPP_CORE_DLLAPI ContinuousValidation : public PathValidation,
   /// \sa value_type breakDistance() const
   void breakDistance(value_type distance);
 
+  /// TODO
+  value_type distanceLowerBoundThreshold() const {
+    return distanceLowerBoundThr_;
+  }
+
+  /// TODO
+  void distanceLowerBoundThreshold(value_type distance);
+
   DevicePtr_t robot() const { return robot_; }
   /// Iteratively call method doExecute of delegate classes Initialize
   /// \sa ContinuousValidation::add, ContinuousValidation::Initialize.
@@ -300,6 +308,7 @@ class HPP_CORE_DLLAPI ContinuousValidation : public PathValidation,
   DevicePtr_t robot_;
   value_type tolerance_;
   value_type breakDistance_;
+  value_type distanceLowerBoundThr_;
 
   /// Store weak pointer to itself.
   void init(ContinuousValidationWkPtr_t weak);

include/hpp/core/continuous-validation/body-pair-collision.hh

@@ -114,6 +114,9 @@ class BodyPairCollision : public IntervalValidation {
   void securityMargin(const value_type& securityMargin) {
     for (auto& request : m_->requests) request.security_margin = securityMargin;
   }
+
+  /// TODO
+  void distanceLowerBoundThreshold(value_type distance);
   /// \}
  protected:
   /// Constructor of body pair collision
@@ -126,7 +129,8 @@ class BodyPairCollision : public IntervalValidation {
   BodyPairCollision(const BodyPairCollision& other)
       : IntervalValidation(other),
         m_(other.m_),
-        maximalVelocity_(other.maximalVelocity_) {}
+        maximalVelocity_(other.maximalVelocity_),
+        distanceLowerBoundThr_(other.distanceLowerBoundThr_) {}
 
   virtual void setReport(ValidationReportPtr_t& report,
                          fcl::CollisionResult result,
@@ -146,6 +150,7 @@ class BodyPairCollision : public IntervalValidation {
 
   mutable vector_t Vb_;
   value_type maximalVelocity_;
+  value_type distanceLowerBoundThr_;
 
   /// Compute maximal velocity for a given velocity bound
   /// \param Vb velocity

include/hpp/core/path-optimization/quadratic-program.hh

@@ -164,11 +164,13 @@ struct QuadraticProgram {
   /// Compute solution using quadprog
   /// \param ce equality constraints
   /// \param ci inequality constraints: \f$ ci.J * x \ge ci.b \f$
+  /// \param[out] ok is set to true if a solution has been found.
   /// \return the cost of the solution.
   ///
   /// \note \ref computeLLT must have been called before.
   /// \note if the problem is ill-conditioned, member xStar is left unchanged.
-  double solve(const LinearConstraint& ce, const LinearConstraint& ci);
+  double solve(const LinearConstraint& ce, const LinearConstraint& ci,
+               bool& ok);
 
   /// \}
 

src/continuous-validation.cc

@@ -86,6 +86,7 @@ void ContinuousValidation::Initialize::doExecute() const {
       continuousValidation::SolidSolidCollisionPtr_t ss(
           SolidSolidCollision::create(joint2, joint1, owner().tolerance_));
       ss->breakDistance(owner().breakDistance());
+      ss->distanceLowerBoundThreshold(owner().distanceLowerBoundThreshold());
       owner().addIntervalValidation(ss);
       bodyPairMap[jp] = ss;
     }
@@ -112,6 +113,7 @@ void ContinuousValidation::AddObstacle::doExecute(
       continuousValidation::SolidSolidCollisionPtr_t ss(
           SolidSolidCollision::create(joint, objects, owner().tolerance()));
       ss->breakDistance(owner().breakDistance());
+      ss->distanceLowerBoundThreshold(owner().distanceLowerBoundThreshold());
       owner().addIntervalValidation(ss);
     }
   }
@@ -253,6 +255,11 @@ void ContinuousValidation::removeObstacleFromJoint(
 
 void ContinuousValidation::breakDistance(value_type distance) {
   assert(distance >= 0);
+  if (distance <= distanceLowerBoundThr_) {
+    throw std::invalid_argument(
+        "Break distance must be strictly greater than "
+        "the distance lower bound threshold.");
+  }
   breakDistance_ = distance;
 
   bodyPairCollisionPool_.clear();
@@ -263,6 +270,23 @@ void ContinuousValidation::breakDistance(value_type distance) {
   }
 }
 
+void ContinuousValidation::distanceLowerBoundThreshold(value_type distance) {
+  assert(distance >= 0);
+  if (distance >= breakDistance_) {
+    throw std::invalid_argument(
+        "Distance lower bound threshold must be "
+        "strictly smaller than the break distance.");
+  }
+  distanceLowerBoundThr_ = distance;
+
+  bodyPairCollisionPool_.clear();
+  for (IntervalValidationPtr_t &val : intervalValidations_) {
+    continuousValidation::BodyPairCollisionPtr_t bp(
+        HPP_DYNAMIC_PTR_CAST(continuousValidation::BodyPairCollision, val));
+    if (bp) bp->distanceLowerBoundThreshold(distance);
+  }
+}
+
 void ContinuousValidation::filterCollisionPairs(
     const RelativeMotion::matrix_type &relMotion) {
   // Loop over collision pairs and remove disabled ones.
@@ -409,6 +433,7 @@ ContinuousValidation::ContinuousValidation(const DevicePtr_t &robot,
     : robot_(robot),
       tolerance_(tolerance),
       breakDistance_(1e-2),
+      distanceLowerBoundThr_(0.0),
       intervalValidations_(),
       weak_() {
   if (tolerance < 0) {

src/continuous-validation/body-pair-collision.cc

@@ -42,6 +42,10 @@ namespace core {
 namespace continuousValidation {
 using ::pinocchio::toFclTransform3f;
 
+void BodyPairCollision::distanceLowerBoundThreshold(value_type distance) {
+  distanceLowerBoundThr_ = distance;
+}
+
 bool BodyPairCollision::validateConfiguration(
     const value_type& t, interval_t& interval, ValidationReportPtr_t& report,
     const pinocchio::DeviceData& data) {
@@ -194,6 +198,7 @@ bool BodyPairCollision::computeDistanceLowerBound(
   const CollisionPairs_t& prs(pairs());
   CollisionRequests_t& rqsts(requests());
   assert(rqsts.size() == prs.size());
+  std::size_t iSmallest = prs.size();
   for (std::size_t i = 0; i < prs.size(); ++i) {
     assert(rqsts[i].enable_distance_lower_bound == true);
     fcl::CollisionResult result;
@@ -204,10 +209,26 @@ bool BodyPairCollision::computeDistanceLowerBound(
       return false;
     }
     if (result.distance_lower_bound < distanceLowerBound) {
+      iSmallest = i;
       distanceLowerBound = result.distance_lower_bound;
       assert(distanceLowerBound > 0);
     }
   }
+  if (distanceLowerBound < distanceLowerBoundThr_) {
+    hppDout(info, "Trigerring a fake collision because distance lower bound is "
+                      << distanceLowerBound);
+    fcl::CollisionRequest req(rqsts[iSmallest]);
+    req.security_margin += distanceLowerBoundThr_;
+    fcl::CollisionResult result;
+    prs[iSmallest].collide(data, req, result);
+    if (!result.isCollision()) {
+      hppDout(warning,
+              "No collision detected while distance lower bound is small.");
+      return true;
+    }
+    setReport(report, result, prs[iSmallest]);
+    return false;
+  }
   return true;
 }
 }  // namespace continuousValidation

src/continuous-validation/dichotomy.cc

@@ -122,6 +122,7 @@ bool Dichotomy::validateStraightPath(IntervalValidations_t& bodyPairCollisions,
   } else {
     validPart = path;
   }
+  std::cout << niters << '\n';
   HPP_STOP_AND_DISPLAY_TIMECOUNTER(CV_Dichotomy_validateStraightPath);
   if (niters > 1000) {
     hppDout(notice,
@@ -137,14 +138,14 @@ void Dichotomy::init(const DichotomyWkPtr_t weak) {
 }
 
 Dichotomy::Dichotomy(const DevicePtr_t& robot, const value_type& tolerance)
-    : ContinuousValidation(robot, tolerance), weak_() {
+    : ContinuousValidation(robot, 0.0), weak_() {
   // Tolerance should be equal to 0, otherwise end of valid
   // sub-path might be in collision.
-  if (tolerance != 0) {
+  if (tolerance > 0) {
     throw std::runtime_error(
-        "Dichotomy path validation method does not"
-        "support penetration.");
+        "Tolerance should be negative for continuous validation by dichotomy.");
   }
+  distanceLowerBoundThreshold(-tolerance);
 }
 }  // namespace continuousValidation
 }  // namespace core

src/path-optimization/quadratic-program.cc

@@ -66,7 +66,7 @@ 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.");
@@ -82,11 +82,15 @@ 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;
@@ -110,8 +114,10 @@ double QuadraticProgram::solve(const LinearConstraint& ce,
     value_type res(0);
     Eigen::IOFormat fullPrecision(Eigen::FullPrecision, Eigen::DontAlignCols,
                                   ", ", ", ", "", "", " << ", ";");
+    ok = false;
     switch (qp.results.info.status) {
       case QPSolverOutput::PROXQP_SOLVED:
+        ok = true;
         xStar = qp.results.x;
         res += (.5 * xStar.transpose() * H * xStar)(0, 0);
         res += (b.transpose() * xStar)(0);

src/path-optimization/spline-gradient-based.cc

@@ -496,7 +496,9 @@ 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
@@ -537,7 +539,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;
       }
@@ -571,6 +576,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;
         }
 
@@ -587,7 +599,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 "

src/path/spline.cc

@@ -381,8 +381,11 @@ void Spline<_SplineType, _Order>::impl_derivative(vectorOut_t res,
   // at a higher order. At the d2+/dv2 is not available for SE(n) and SO(n).
   assert(order == 1 || robot_->configSpace()->isVectorSpace());
   BasisFunctionVector_t basisFunc;
-  const value_type u =
-      (length() == 0 ? 0 : (s - paramRange().first) / paramLength());
+  value_type u = (length() == 0 ? 0 : (s - paramRange().first) / paramLength());
+  // Due to numerical errors, u might be outside range [0, 1]
+  assert(u >= -1e-12);
+  assert(u - 1 <= 1e-12);
+  u = std::min(1.0, std::max(u, 0.0));
   basisFunctionDerivative(order, u, basisFunc);
   res.noalias() = parameters_.transpose() * basisFunc;
 
@@ -399,8 +402,11 @@ template <int _SplineType, int _Order>
 void Spline<_SplineType, _Order>::impl_paramDerivative(
     vectorOut_t res, const value_type& s) const {
   BasisFunctionVector_t basisFunc;
-  const value_type u =
-      (length() == 0 ? 0 : (s - paramRange().first) / paramLength());
+  value_type u = (length() == 0 ? 0 : (s - paramRange().first) / paramLength());
+  // Due to numerical errors, u might be outside range [0, 1]
+  assert(u >= -1e-12);
+  assert(u - 1 <= 1e-12);
+  u = std::min(1.0, std::max(u, 0.0));
   basisFunctionDerivative(0, u, basisFunc);
   res = basisFunc;