diff --git a/include/hpp/core/continuous-validation.hh b/include/hpp/core/continuous-validation.hh index 58e5fe87..e560a90d 100644 --- a/include/hpp/core/continuous-validation.hh +++ b/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); diff --git a/include/hpp/core/continuous-validation/body-pair-collision.hh b/include/hpp/core/continuous-validation/body-pair-collision.hh index e40e3ecd..da932a23 100644 --- a/include/hpp/core/continuous-validation/body-pair-collision.hh +++ b/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 diff --git a/include/hpp/core/path-optimization/quadratic-program.hh b/include/hpp/core/path-optimization/quadratic-program.hh index 2cfedfa5..a30a9f4e 100644 --- a/include/hpp/core/path-optimization/quadratic-program.hh +++ b/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); /// \} diff --git a/src/continuous-validation.cc b/src/continuous-validation.cc index 93f82632..1c58972c 100644 --- a/src/continuous-validation.cc +++ b/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) { diff --git a/src/continuous-validation/body-pair-collision.cc b/src/continuous-validation/body-pair-collision.cc index 76b35e24..e9ec9781 100644 --- a/src/continuous-validation/body-pair-collision.cc +++ b/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 diff --git a/src/continuous-validation/dichotomy.cc b/src/continuous-validation/dichotomy.cc index 5038f9c7..b4656783 100644 --- a/src/continuous-validation/dichotomy.cc +++ b/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 diff --git a/src/path-optimization/quadratic-program.cc b/src/path-optimization/quadratic-program.cc index 4a275eee..17156c77 100644 --- a/src/path-optimization/quadratic-program.cc +++ b/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); diff --git a/src/path-optimization/spline-gradient-based.cc b/src/path-optimization/spline-gradient-based.cc index 623e3995..6cdf87d2 100644 --- a/src/path-optimization/spline-gradient-based.cc +++ b/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 " diff --git a/src/path/spline.cc b/src/path/spline.cc index db13081f..ef8fe7cd 100644 --- a/src/path/spline.cc +++ b/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 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; diff --git a/tests/test-continuous-validation.cc b/tests/test-continuous-validation.cc index 22de6af0..c801e716 100644 --- a/tests/test-continuous-validation.cc +++ b/tests/test-continuous-validation.cc @@ -54,6 +54,7 @@ using hpp::pinocchio::Device; using hpp::pinocchio::DevicePtr_t; using hpp::pinocchio::urdf::loadModel; +using hpp::pinocchio::urdf::loadModelFromString; using hpp::core::CollisionValidation; using hpp::core::Configuration_t; @@ -68,13 +69,17 @@ using hpp::core::ProblemPtr_t; using hpp::core::size_type; using hpp::core::SteeringMethodPtr_t; using hpp::core::ValidationReportPtr_t; +using hpp::core::value_type; using hpp::core::vector_t; using hpp::core::configurationShooter::Uniform; -using hpp::core::continuousCollisionChecking::Dichotomy; using hpp::core::continuousCollisionChecking::Progressive; +using hpp::core::continuousValidation::Dichotomy; +using hpp::core::continuousValidation::DichotomyPtr_t; using hpp::core::pathValidation::createDiscretizedCollisionChecking; using hpp::core::steeringMethod::Straight; +namespace steeringMethod = hpp::core::steeringMethod; + static size_type i1 = 0, n1 = 100; static size_type i2 = 0, n2 = 10; @@ -427,4 +432,105 @@ BOOST_AUTO_TEST_CASE(continuous_validation_spline) { } #endif +std::string avoid_infinite_loop_urdf_string = R"( + + + + + + + + + + + + + + + + + + + + + + + + + +)"; + +BOOST_AUTO_TEST_CASE(avoid_infinite_loop_straight) { + DevicePtr_t robot(Device::create("test")); + loadModelFromString(robot, 0, "", "anchor", avoid_infinite_loop_urdf_string, + ""); + + // create steering method + ProblemPtr_t problem = Problem::create(robot); + SteeringMethodPtr_t sm(Straight::create(problem)); + + // create path validation objects + DichotomyPtr_t dichotomy(Dichotomy::create(robot, 0)); + dichotomy->distanceLowerBoundThreshold(0.001); + + Configuration_t q1(robot->configSize()), q2(robot->configSize()); + for (hpp::core::value_type min_dist : {0.00101, 0.00099, 1e-6, 0.0}) { + q1 << -1, 1 + min_dist; + q2 << 1.1, 1 + min_dist; + PathPtr_t path((*sm)(q1, q2)); + + PathPtr_t validPart; + PathValidationReportPtr_t report; + bool res(dichotomy->validate(path, false, validPart, report)); + + BOOST_TEST_MESSAGE(min_dist << " - " << res); + if (report) BOOST_TEST_MESSAGE(*report); + // TODO we want to check the number of iterations. + } +} + +BOOST_AUTO_TEST_CASE(avoid_infinite_loop_spline) { + DevicePtr_t robot(Device::create("test")); + loadModelFromString(robot, 0, "", "anchor", avoid_infinite_loop_urdf_string, + ""); + + // create steering method + ProblemPtr_t problem = Problem::create(robot); + typedef steeringMethod::Spline SMSpline_t; + SMSpline_t::Ptr_t sm(SMSpline_t::create(problem)); + Configuration_t q1(robot->configSize()), q2(robot->configSize()); + PathPtr_t path; + bool ok; + + // Calculate shift + std::vector order = {1}; + matrix_t D1(robot->numberDof(), 1), D2(robot->numberDof(), 1); + + q1 << -1, 0; + q2 << 1.1, 0; + D1 << 0, -1; + D2 << 0, 1; + path = sm->steer(q1, order, D1, q2, order, D2, 1.0); + BOOST_REQUIRE_EQUAL(path->length(), 1.0); + value_type shift = -path->eval(0.5, ok)[1]; + + // create path validation objects + DichotomyPtr_t dichotomy(Dichotomy::create(robot, 0)); + dichotomy->distanceLowerBoundThreshold(0.001); + + for (hpp::core::value_type min_dist : {0.00101, 0.00099, 1e-6, 0.0}) { + q1 << -1, shift + 1 + min_dist; + q2 << 1.1, shift + 1 + min_dist; + path = sm->steer(q1, order, D1, q2, order, D2, 1.0); + + PathPtr_t validPart; + PathValidationReportPtr_t report; + bool res(dichotomy->validate(path, false, validPart, report)); + + BOOST_TEST_MESSAGE(min_dist << " - " << res); + if (report) BOOST_TEST_MESSAGE(*report); + // TODO we want to check the number of iterations. + } +} + BOOST_AUTO_TEST_SUITE_END()