From 15090376adbe1ea2b38b22f38bbc78a6937ec6ce Mon Sep 17 00:00:00 2001 From: Joseph Mirabel Date: Mon, 27 Nov 2023 22:30:23 +0100 Subject: [PATCH 1/6] WIP: fix asymptotically infinite loop with Dichotomy --- .../body-pair-collision.cc | 10 ++++ src/continuous-validation/dichotomy.cc | 1 + tests/test-continuous-validation.cc | 56 +++++++++++++++++++ 3 files changed, 67 insertions(+) diff --git a/src/continuous-validation/body-pair-collision.cc b/src/continuous-validation/body-pair-collision.cc index 76b35e24..087134c9 100644 --- a/src/continuous-validation/body-pair-collision.cc +++ b/src/continuous-validation/body-pair-collision.cc @@ -194,6 +194,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 +205,19 @@ bool BodyPairCollision::computeDistanceLowerBound( return false; } if (result.distance_lower_bound < distanceLowerBound) { + iSmallest = i; distanceLowerBound = result.distance_lower_bound; assert(distanceLowerBound > 0); } } + if (distanceLowerBound < 0.001) { + fcl::CollisionRequest req (rqsts[iSmallest]); + req.security_margin += distanceLowerBound; + fcl::CollisionResult result; + prs[iSmallest].collide(data, req, result); + 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..713ddf10 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, diff --git a/tests/test-continuous-validation.cc b/tests/test-continuous-validation.cc index 22de6af0..1b6ee7da 100644 --- a/tests/test-continuous-validation.cc +++ b/tests/test-continuous-validation.cc @@ -49,11 +49,13 @@ namespace bpt = boost::posix_time; #include #include #include +#include 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; @@ -427,4 +429,58 @@ BOOST_AUTO_TEST_CASE(continuous_validation_spline) { } #endif +BOOST_AUTO_TEST_CASE(avoid_infinite_loop) { + //hpp::debug::setVerbosityLevel(60); + DevicePtr_t robot(Device::create("test")); + loadModelFromString(robot, 0, "", "anchor", R"( + + + + + + + + + + + + + + + + + + + + + + + + + +)", + ""); + + // create steering method + ProblemPtr_t problem = Problem::create(robot); + SteeringMethodPtr_t sm(Straight::create(problem)); + + 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)); + + // create path validation objects + PathValidationPtr_t dichotomy(Dichotomy::create(robot, 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); + } +} + BOOST_AUTO_TEST_SUITE_END() From ff3f8da33f8f4ccd3291ba450476f001c13c63af Mon Sep 17 00:00:00 2001 From: Joseph Mirabel Date: Tue, 28 Nov 2023 21:54:29 +0100 Subject: [PATCH 2/6] add distance lower bound threshold to continuous validation. --- include/hpp/core/continuous-validation.hh | 7 ++ .../body-pair-collision.hh | 7 +- src/continuous-validation.cc | 23 +++++++ .../body-pair-collision.cc | 15 +++- tests/test-continuous-validation.cc | 68 ++++++++++++++++--- 5 files changed, 107 insertions(+), 13 deletions(-) diff --git a/include/hpp/core/continuous-validation.hh b/include/hpp/core/continuous-validation.hh index 58e5fe87..ed77ca64 100644 --- a/include/hpp/core/continuous-validation.hh +++ b/include/hpp/core/continuous-validation.hh @@ -219,6 +219,12 @@ 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 +306,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/src/continuous-validation.cc b/src/continuous-validation.cc index 93f82632..5753dcc8 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,10 @@ 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 +269,22 @@ 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 +431,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 087134c9..8db1f967 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) { @@ -210,11 +214,16 @@ bool BodyPairCollision::computeDistanceLowerBound( assert(distanceLowerBound > 0); } } - if (distanceLowerBound < 0.001) { - fcl::CollisionRequest req (rqsts[iSmallest]); - req.security_margin += distanceLowerBound; + if (distanceLowerBound < distanceLowerBoundThr_) { + 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; } diff --git a/tests/test-continuous-validation.cc b/tests/test-continuous-validation.cc index 1b6ee7da..65f2082c 100644 --- a/tests/test-continuous-validation.cc +++ b/tests/test-continuous-validation.cc @@ -49,7 +49,6 @@ namespace bpt = boost::posix_time; #include #include #include -#include using hpp::pinocchio::Device; using hpp::pinocchio::DevicePtr_t; @@ -61,6 +60,7 @@ using hpp::core::CollisionValidation; using hpp::core::Configuration_t; using hpp::core::ConfigurationShooterPtr_t; using hpp::core::ConfigValidationPtr_t; +using hpp::core::value_type; using hpp::core::matrix_t; using hpp::core::PathPtr_t; using hpp::core::PathValidationPtr_t; @@ -72,11 +72,14 @@ using hpp::core::SteeringMethodPtr_t; using hpp::core::ValidationReportPtr_t; using hpp::core::vector_t; using hpp::core::configurationShooter::Uniform; -using hpp::core::continuousCollisionChecking::Dichotomy; +using hpp::core::continuousValidation::Dichotomy; +using hpp::core::continuousValidation::DichotomyPtr_t; using hpp::core::continuousCollisionChecking::Progressive; 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; @@ -429,10 +432,7 @@ BOOST_AUTO_TEST_CASE(continuous_validation_spline) { } #endif -BOOST_AUTO_TEST_CASE(avoid_infinite_loop) { - //hpp::debug::setVerbosityLevel(60); - DevicePtr_t robot(Device::create("test")); - loadModelFromString(robot, 0, "", "anchor", R"( +std::string avoid_infinite_loop_urdf_string = R"( @@ -458,21 +458,70 @@ BOOST_AUTO_TEST_CASE(avoid_infinite_loop) { -)", +)"; + +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)); - // create path validation objects - PathValidationPtr_t dichotomy(Dichotomy::create(robot, 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_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; @@ -480,6 +529,7 @@ BOOST_AUTO_TEST_CASE(avoid_infinite_loop) { BOOST_TEST_MESSAGE(min_dist << " - " << res); if (report) BOOST_TEST_MESSAGE(*report); + // TODO we want to check the number of iterations. } } From 7f19830956e3ba975e2504853b61c96e5301853c Mon Sep 17 00:00:00 2001 From: Joseph Mirabel Date: Thu, 30 Nov 2023 12:30:15 +0100 Subject: [PATCH 3/6] In dichotomy validation, use -tolerance as distance lower bound threshold --- src/continuous-validation/body-pair-collision.cc | 2 ++ src/continuous-validation/dichotomy.cc | 8 ++++---- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/src/continuous-validation/body-pair-collision.cc b/src/continuous-validation/body-pair-collision.cc index 8db1f967..e9ec9781 100644 --- a/src/continuous-validation/body-pair-collision.cc +++ b/src/continuous-validation/body-pair-collision.cc @@ -215,6 +215,8 @@ bool BodyPairCollision::computeDistanceLowerBound( } } 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; diff --git a/src/continuous-validation/dichotomy.cc b/src/continuous-validation/dichotomy.cc index 713ddf10..b4656783 100644 --- a/src/continuous-validation/dichotomy.cc +++ b/src/continuous-validation/dichotomy.cc @@ -138,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 From fa6e6d9f0cae1a497b7c3b6887f6e00ed56e9615 Mon Sep 17 00:00:00 2001 From: Joseph Mirabel Date: Mon, 4 Dec 2023 22:34:24 +0100 Subject: [PATCH 4/6] Fix numerical issue in SplineGradientBased --- .../path-optimization/quadratic-program.hh | 3 ++- src/path-optimization/quadratic-program.cc | 9 ++++++++- .../spline-gradient-based.cc | 19 ++++++++++++++++--- 3 files changed, 26 insertions(+), 5 deletions(-) diff --git a/include/hpp/core/path-optimization/quadratic-program.hh b/include/hpp/core/path-optimization/quadratic-program.hh index 2cfedfa5..d156b9b8 100644 --- a/include/hpp/core/path-optimization/quadratic-program.hh +++ b/include/hpp/core/path-optimization/quadratic-program.hh @@ -164,11 +164,12 @@ 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/path-optimization/quadratic-program.cc b/src/path-optimization/quadratic-program.cc index 4a275eee..1186d632 100644 --- a/src/path-optimization/quadratic-program.cc +++ b/src/path-optimization/quadratic-program.cc @@ -66,7 +66,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."); @@ -82,11 +83,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 +115,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..c0202ae1 100644 --- a/src/path-optimization/spline-gradient-based.cc +++ b/src/path-optimization/spline-gradient-based.cc @@ -496,7 +496,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 @@ -537,7 +540,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 +577,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 +600,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 " From 331d12368a607b3b6a5e0f9236cd354671656129 Mon Sep 17 00:00:00 2001 From: Joseph Mirabel Date: Thu, 7 Dec 2023 15:56:31 +0100 Subject: [PATCH 5/6] Fix numerical issue in path::Spline. --- src/path/spline.cc | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/src/path/spline.cc b/src/path/spline.cc index db13081f..fc58fbdf 100644 --- a/src/path/spline.cc +++ b/src/path/spline.cc @@ -381,8 +381,12 @@ 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 = + 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 +403,12 @@ template void Spline<_SplineType, _Order>::impl_paramDerivative( vectorOut_t res, const value_type& s) const { BasisFunctionVector_t basisFunc; - const value_type u = + 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; From f2549adc2d7fce6e970c34cfbd3c55e657b38178 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 11 Dec 2023 12:11:44 +0000 Subject: [PATCH 6/6] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- include/hpp/core/continuous-validation.hh | 4 +++- .../hpp/core/path-optimization/quadratic-program.hh | 3 ++- src/continuous-validation.cc | 6 ++++-- src/path-optimization/quadratic-program.cc | 3 +-- src/path-optimization/spline-gradient-based.cc | 3 +-- src/path/spline.cc | 10 ++++------ tests/test-continuous-validation.cc | 12 ++++++------ 7 files changed, 21 insertions(+), 20 deletions(-) diff --git a/include/hpp/core/continuous-validation.hh b/include/hpp/core/continuous-validation.hh index ed77ca64..e560a90d 100644 --- a/include/hpp/core/continuous-validation.hh +++ b/include/hpp/core/continuous-validation.hh @@ -220,7 +220,9 @@ class HPP_CORE_DLLAPI ContinuousValidation : public PathValidation, void breakDistance(value_type distance); /// TODO - value_type distanceLowerBoundThreshold() const { return distanceLowerBoundThr_; } + value_type distanceLowerBoundThreshold() const { + return distanceLowerBoundThr_; + } /// TODO void distanceLowerBoundThreshold(value_type distance); diff --git a/include/hpp/core/path-optimization/quadratic-program.hh b/include/hpp/core/path-optimization/quadratic-program.hh index d156b9b8..a30a9f4e 100644 --- a/include/hpp/core/path-optimization/quadratic-program.hh +++ b/include/hpp/core/path-optimization/quadratic-program.hh @@ -169,7 +169,8 @@ struct QuadraticProgram { /// /// \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, bool& ok); + double solve(const LinearConstraint& ce, const LinearConstraint& ci, + bool& ok); /// \} diff --git a/src/continuous-validation.cc b/src/continuous-validation.cc index 5753dcc8..1c58972c 100644 --- a/src/continuous-validation.cc +++ b/src/continuous-validation.cc @@ -256,7 +256,8 @@ 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 " + throw std::invalid_argument( + "Break distance must be strictly greater than " "the distance lower bound threshold."); } breakDistance_ = distance; @@ -272,7 +273,8 @@ 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 " + throw std::invalid_argument( + "Distance lower bound threshold must be " "strictly smaller than the break distance."); } distanceLowerBoundThr_ = distance; diff --git a/src/path-optimization/quadratic-program.cc b/src/path-optimization/quadratic-program.cc index 1186d632..17156c77 100644 --- a/src/path-optimization/quadratic-program.cc +++ b/src/path-optimization/quadratic-program.cc @@ -66,8 +66,7 @@ void QuadraticProgram::computeLLT() { } double QuadraticProgram::solve(const LinearConstraint& ce, - const LinearConstraint& ci, - bool& ok) { + 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."); diff --git a/src/path-optimization/spline-gradient-based.cc b/src/path-optimization/spline-gradient-based.cc index c0202ae1..6cdf87d2 100644 --- a/src/path-optimization/spline-gradient-based.cc +++ b/src/path-optimization/spline-gradient-based.cc @@ -498,8 +498,7 @@ PathVectorPtr_t SplineGradientBased<_PB, _SO>::optimize( QPc.computeLLT(); bool qpSolved; QPc.solve(collisionReduced, boundConstraintReduced, qpSolved); - if (!qpSolved) - return this->buildPathVector(splines); + if (!qpSolved) return this->buildPathVector(splines); while (!(noCollision && minimumReached) && !this->shouldStop()) { // 6.1 diff --git a/src/path/spline.cc b/src/path/spline.cc index fc58fbdf..ef8fe7cd 100644 --- a/src/path/spline.cc +++ b/src/path/spline.cc @@ -381,11 +381,10 @@ 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; - 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); + assert(u - 1 <= 1e-12); u = std::min(1.0, std::max(u, 0.0)); basisFunctionDerivative(order, u, basisFunc); res.noalias() = parameters_.transpose() * basisFunc; @@ -403,11 +402,10 @@ template void Spline<_SplineType, _Order>::impl_paramDerivative( vectorOut_t res, const value_type& s) const { BasisFunctionVector_t basisFunc; - 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); + 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 65f2082c..c801e716 100644 --- a/tests/test-continuous-validation.cc +++ b/tests/test-continuous-validation.cc @@ -60,7 +60,6 @@ using hpp::core::CollisionValidation; using hpp::core::Configuration_t; using hpp::core::ConfigurationShooterPtr_t; using hpp::core::ConfigValidationPtr_t; -using hpp::core::value_type; using hpp::core::matrix_t; using hpp::core::PathPtr_t; using hpp::core::PathValidationPtr_t; @@ -70,11 +69,12 @@ 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::Progressive; using hpp::core::continuousValidation::Dichotomy; using hpp::core::continuousValidation::DichotomyPtr_t; -using hpp::core::continuousCollisionChecking::Progressive; using hpp::core::pathValidation::createDiscretizedCollisionChecking; using hpp::core::steeringMethod::Straight; @@ -497,22 +497,22 @@ BOOST_AUTO_TEST_CASE(avoid_infinite_loop_spline) { // create steering method ProblemPtr_t problem = Problem::create(robot); typedef steeringMethod::Spline SMSpline_t; - SMSpline_t::Ptr_t sm (SMSpline_t::create(problem)); + 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 }; + 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; + 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]; + value_type shift = -path->eval(0.5, ok)[1]; // create path validation objects DichotomyPtr_t dichotomy(Dichotomy::create(robot, 0));