From e078cff2471398878b774e2a186a50c4412fdc16 Mon Sep 17 00:00:00 2001 From: Louis Montaut Date: Wed, 20 Mar 2024 21:54:37 +0100 Subject: [PATCH 01/14] Misc: fix HPP_FCL_ASSERT macro --- include/hpp/fcl/fwd.hh | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/include/hpp/fcl/fwd.hh b/include/hpp/fcl/fwd.hh index 2bb040eae..63040d2ab 100644 --- a/include/hpp/fcl/fwd.hh +++ b/include/hpp/fcl/fwd.hh @@ -71,15 +71,17 @@ } #ifdef HPP_FCL_TURN_ASSERT_INTO_EXCEPTION -#define HPP_FCL_ASSERT(check, message, exception) \ - { \ - if (check == false) HPP_FCL_THROW_PRETTY(message, exception); \ - } +#define HPP_FCL_ASSERT(check, message, exception) \ + do { \ + if (!(check)) { \ + HPP_FCL_THROW_PRETTY(message, exception); \ + } \ + } while (0) #else #define HPP_FCL_ASSERT(check, message, exception) \ { \ HPP_FCL_UNUSED_VARIABLE(exception(message)); \ - assert(check&& message); \ + assert((check) && message); \ } #endif From 55e8dd72c8457ead3796711758756056e467bd1e Mon Sep 17 00:00:00 2001 From: Louis Montaut Date: Wed, 20 Mar 2024 22:14:59 +0100 Subject: [PATCH 02/14] GJK: rename `Valid` status to `NoCollision` --- include/hpp/fcl/narrowphase/gjk.h | 2 +- include/hpp/fcl/narrowphase/narrowphase.h | 2 +- python/gjk.cc | 2 +- src/narrowphase/gjk.cpp | 4 ++-- src/narrowphase/narrowphase.cpp | 2 +- test/gjk.cpp | 9 +++++---- 6 files changed, 11 insertions(+), 10 deletions(-) diff --git a/include/hpp/fcl/narrowphase/gjk.h b/include/hpp/fcl/narrowphase/gjk.h index 317d053cb..cf0683095 100644 --- a/include/hpp/fcl/narrowphase/gjk.h +++ b/include/hpp/fcl/narrowphase/gjk.h @@ -169,7 +169,7 @@ struct HPP_FCL_DLLAPI GJK { /// Failed: GJK did not converge. /// EarlyStopped: GJK found a separating hyperplane and exited before /// converting. The shapes are not in collision. - enum Status { DidNotRun, Failed, Valid, Inside, EarlyStopped }; + enum Status { DidNotRun, Failed, NoCollision, Inside, EarlyStopped }; public: FCL_REAL distance_upper_bound; diff --git a/include/hpp/fcl/narrowphase/narrowphase.h b/include/hpp/fcl/narrowphase/narrowphase.h index 7cdb874b4..eb5c49e0d 100644 --- a/include/hpp/fcl/narrowphase/narrowphase.h +++ b/include/hpp/fcl/narrowphase/narrowphase.h @@ -231,7 +231,7 @@ struct HPP_FCL_DLLAPI GJKSolver { "`distance_upper_bound`.", std::logic_error); break; - case details::GJK::Valid: + case details::GJK::NoCollision: // // Case where GJK converged and proved that the shapes are not in // collision, i.e their distance is above GJK's tolerance (default diff --git a/python/gjk.cc b/python/gjk.cc index 91b8750e9..9fc192afa 100644 --- a/python/gjk.cc +++ b/python/gjk.cc @@ -53,7 +53,7 @@ using hpp::fcl::details::MinkowskiDiff; void exposeGJK() { if (!eigenpy::register_symbolic_link_to_registered_type()) { enum_("GJKStatus") - .value("Valid", GJK::Valid) + .value("NoCollision", GJK::NoCollision) .value("Inside", GJK::Inside) .value("Failed", GJK::Failed) .export_values(); diff --git a/src/narrowphase/gjk.cpp b/src/narrowphase/gjk.cpp index 6f880b670..8c3e744d9 100644 --- a/src/narrowphase/gjk.cpp +++ b/src/narrowphase/gjk.cpp @@ -673,7 +673,7 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess, free_v[3] = &store_v[3]; nfree = 4; - status = Valid; + status = NoCollision; shape = &shape_; distance = 0.0; current = 0; @@ -829,7 +829,7 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess, status = ((++iterations) < max_iterations) ? status : Failed; - } while (status == Valid); + } while (status == NoCollision); simplex = &simplices[current]; assert(simplex->rank > 0 && simplex->rank < 5); diff --git a/src/narrowphase/narrowphase.cpp b/src/narrowphase/narrowphase.cpp index 3a8b338bc..75b0949c2 100644 --- a/src/narrowphase/narrowphase.cpp +++ b/src/narrowphase/narrowphase.cpp @@ -489,7 +489,7 @@ bool GJKSolver::shapeDistance( gjk.getClosestPoints(minkowski_difference, p1, p2); - if ((gjk_status == details::GJK::Valid) || + if ((gjk_status == details::GJK::NoCollision) || (gjk_status == details::GJK::Failed)) { // TODO On degenerated case, the closest point may be wrong // (i.e. an object face normal is colinear to gjk.ray diff --git a/test/gjk.cpp b/test/gjk.cpp index 82deaaf8d..b8e32127e 100644 --- a/test/gjk.cpp +++ b/test/gjk.cpp @@ -342,7 +342,7 @@ void test_gjk_unit_sphere(FCL_REAL center_distance, Vec3f ray, if (expect_collision) BOOST_CHECK_EQUAL(status, details::GJK::Inside); else - BOOST_CHECK_EQUAL(status, details::GJK::Valid); + BOOST_CHECK_EQUAL(status, details::GJK::NoCollision); Vec3f w0, w1; gjk.getClosestPoints(shape, w0, w1); @@ -398,17 +398,18 @@ void test_gjk_triangle_capsule(Vec3f T, bool expect_collision, if (expect_collision) BOOST_CHECK_EQUAL(status, details::GJK::Inside); else { - BOOST_CHECK_EQUAL(status, details::GJK::Valid); + BOOST_CHECK_EQUAL(status, details::GJK::NoCollision); // Check that guess works as expected Vec3f guess = gjk.getGuessFromSimplex(); details::GJK gjk2(3, 1e-6); details::GJK::Status status2 = gjk2.evaluate(shape, guess); - BOOST_CHECK_EQUAL(status2, details::GJK::Valid); + BOOST_CHECK_EQUAL(status2, details::GJK::NoCollision); } Vec3f w0, w1; - if (status == details::GJK::Valid || gjk.hasPenetrationInformation(shape)) { + if (status == details::GJK::NoCollision || + gjk.hasPenetrationInformation(shape)) { gjk.getClosestPoints(shape, w0, w1); } else { details::EPA epa(64, 1e-6); From a610ad79b78949ca6033b5dc8832943b7b65bbd4 Mon Sep 17 00:00:00 2001 From: Louis Montaut Date: Wed, 20 Mar 2024 22:20:22 +0100 Subject: [PATCH 03/14] GJK: rename `Inside` status to `Collision` --- include/hpp/fcl/narrowphase/gjk.h | 6 +++--- include/hpp/fcl/narrowphase/narrowphase.h | 6 +++--- python/gjk.cc | 2 +- src/narrowphase/gjk.cpp | 6 +++--- src/narrowphase/narrowphase.cpp | 2 +- test/gjk.cpp | 4 ++-- 6 files changed, 13 insertions(+), 13 deletions(-) diff --git a/include/hpp/fcl/narrowphase/gjk.h b/include/hpp/fcl/narrowphase/gjk.h index cf0683095..d04f54e61 100644 --- a/include/hpp/fcl/narrowphase/gjk.h +++ b/include/hpp/fcl/narrowphase/gjk.h @@ -164,12 +164,12 @@ struct HPP_FCL_DLLAPI GJK { }; /// @brief Status of the GJK algorithm: - /// Valid: GJK converged and the shapes are not in collision. - /// Inside: GJK converged and the shapes are in collision. + /// NoCollision: GJK converged and the shapes are not in collision. + /// Collision: GJK converged and the shapes are in collision. /// Failed: GJK did not converge. /// EarlyStopped: GJK found a separating hyperplane and exited before /// converting. The shapes are not in collision. - enum Status { DidNotRun, Failed, NoCollision, Inside, EarlyStopped }; + enum Status { DidNotRun, Failed, NoCollision, Collision, EarlyStopped }; public: FCL_REAL distance_upper_bound; diff --git a/include/hpp/fcl/narrowphase/narrowphase.h b/include/hpp/fcl/narrowphase/narrowphase.h index eb5c49e0d..f68cce8cd 100644 --- a/include/hpp/fcl/narrowphase/narrowphase.h +++ b/include/hpp/fcl/narrowphase/narrowphase.h @@ -82,7 +82,7 @@ struct HPP_FCL_DLLAPI GJKSolver { if (normal != NULL) *normal = n; if (contact_points != NULL) *contact_points = 0.5 * (p1 + p2); } - return (gjk.status == details::GJK::Inside); + return (gjk.status == details::GJK::Collision); } //// @brief intersection checking between one shape and a triangle with @@ -106,7 +106,7 @@ struct HPP_FCL_DLLAPI GJKSolver { runGJKAndEPA(s, tf1, tri, tf_1M2, distance, compute_penetration, p1, p2, normal, relative_transformation_already_computed); HPP_FCL_UNUSED_VARIABLE(gjk_and_epa_ran_successfully); - return (gjk.status == details::GJK::Inside); + return (gjk.status == details::GJK::Collision); } /// @brief distance computation between two shapes. @@ -244,7 +244,7 @@ struct HPP_FCL_DLLAPI GJKSolver { "distance between the closest points.", std::logic_error); break; - case details::GJK::Inside: + case details::GJK::Collision: // // Case where GJK found the shapes to be in collision, i.e. their // distance is below GJK's tolerance (default 1e-6). diff --git a/python/gjk.cc b/python/gjk.cc index 9fc192afa..2344cfa7e 100644 --- a/python/gjk.cc +++ b/python/gjk.cc @@ -54,7 +54,7 @@ void exposeGJK() { if (!eigenpy::register_symbolic_link_to_registered_type()) { enum_("GJKStatus") .value("NoCollision", GJK::NoCollision) - .value("Inside", GJK::Inside) + .value("Inside", GJK::Collision) .value("Failed", GJK::Failed) .export_values(); } diff --git a/src/narrowphase/gjk.cpp b/src/narrowphase/gjk.cpp index 8c3e744d9..b50136ed4 100644 --- a/src/narrowphase/gjk.cpp +++ b/src/narrowphase/gjk.cpp @@ -708,7 +708,7 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess, // return touch { assert(rl > 0); - status = Inside; + status = Collision; distance = -inflation; // should we take rl into account ? break; } @@ -791,7 +791,7 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess, // TODO When inflation is strictly positive, the distance may be exactly // zero (so the ray is not zero) and we are not in the case rl < // tolerance. - if (distance < tolerance) status = Inside; + if (distance < tolerance) status = Collision; break; } @@ -822,7 +822,7 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess, current = next; if (!inside) rl = ray.norm(); if (inside || rl == 0) { - status = Inside; + status = Collision; distance = -inflation; break; } diff --git a/src/narrowphase/narrowphase.cpp b/src/narrowphase/narrowphase.cpp index 75b0949c2..80ddd5916 100644 --- a/src/narrowphase/narrowphase.cpp +++ b/src/narrowphase/narrowphase.cpp @@ -497,7 +497,7 @@ bool GJKSolver::shapeDistance( dist = gjk.distance; return true; - } else if (gjk_status == details::GJK::Inside) { + } else if (gjk_status == details::GJK::Collision) { if (enable_penetration) { FCL_REAL penetrationDepth = details::computePenetration( t1.a, t1.b, t1.c, t2.a, t2.b, t2.c, normal); diff --git a/test/gjk.cpp b/test/gjk.cpp index b8e32127e..a592606f7 100644 --- a/test/gjk.cpp +++ b/test/gjk.cpp @@ -340,7 +340,7 @@ void test_gjk_unit_sphere(FCL_REAL center_distance, Vec3f ray, details::GJK::Status status = gjk.evaluate(shape, Vec3f(1, 0, 0)); if (expect_collision) - BOOST_CHECK_EQUAL(status, details::GJK::Inside); + BOOST_CHECK_EQUAL(status, details::GJK::Collision); else BOOST_CHECK_EQUAL(status, details::GJK::NoCollision); @@ -396,7 +396,7 @@ void test_gjk_triangle_capsule(Vec3f T, bool expect_collision, details::GJK::Status status = gjk.evaluate(shape, Vec3f(1, 0, 0)); if (expect_collision) - BOOST_CHECK_EQUAL(status, details::GJK::Inside); + BOOST_CHECK_EQUAL(status, details::GJK::Collision); else { BOOST_CHECK_EQUAL(status, details::GJK::NoCollision); From 1a824bb350499cd2ee5ae91c0b8ae74cd2887cd7 Mon Sep 17 00:00:00 2001 From: Louis Montaut Date: Wed, 20 Mar 2024 22:27:50 +0100 Subject: [PATCH 04/14] GJK: rename `EarlyStopped` status to `NoCollisionEarlyStopped` --- include/hpp/fcl/narrowphase/gjk.h | 12 +++++++++--- include/hpp/fcl/narrowphase/narrowphase.h | 2 +- src/narrowphase/gjk.cpp | 2 +- 3 files changed, 11 insertions(+), 5 deletions(-) diff --git a/include/hpp/fcl/narrowphase/gjk.h b/include/hpp/fcl/narrowphase/gjk.h index d04f54e61..7322d8d94 100644 --- a/include/hpp/fcl/narrowphase/gjk.h +++ b/include/hpp/fcl/narrowphase/gjk.h @@ -167,9 +167,15 @@ struct HPP_FCL_DLLAPI GJK { /// NoCollision: GJK converged and the shapes are not in collision. /// Collision: GJK converged and the shapes are in collision. /// Failed: GJK did not converge. - /// EarlyStopped: GJK found a separating hyperplane and exited before - /// converting. The shapes are not in collision. - enum Status { DidNotRun, Failed, NoCollision, Collision, EarlyStopped }; + /// NoCollisionEarlyStopped: GJK found a separating hyperplane and exited + /// before converting. The shapes are not in collision. + enum Status { + DidNotRun, + Failed, + NoCollision, + Collision, + NoCollisionEarlyStopped + }; public: FCL_REAL distance_upper_bound; diff --git a/include/hpp/fcl/narrowphase/narrowphase.h b/include/hpp/fcl/narrowphase/narrowphase.h index f68cce8cd..7ab646d84 100644 --- a/include/hpp/fcl/narrowphase/narrowphase.h +++ b/include/hpp/fcl/narrowphase/narrowphase.h @@ -219,7 +219,7 @@ struct HPP_FCL_DLLAPI GJKSolver { normal); gjk_and_epa_ran_successfully = false; break; - case details::GJK::EarlyStopped: + case details::GJK::NoCollisionEarlyStopped: // // Case where GJK early stopped because the distance was found to be // above the `distance_upper_bound`. diff --git a/src/narrowphase/gjk.cpp b/src/narrowphase/gjk.cpp index b50136ed4..265f06bb1 100644 --- a/src/narrowphase/gjk.cpp +++ b/src/narrowphase/gjk.cpp @@ -759,7 +759,7 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess, FCL_REAL omega = dir.dot(w) / dir.norm(); if (omega > upper_bound) { distance = omega - inflation; - status = EarlyStopped; + status = NoCollisionEarlyStopped; break; } From 5241e5b798dc62f54f5b303ae0c31f6551644093 Mon Sep 17 00:00:00 2001 From: Louis Montaut Date: Thu, 21 Mar 2024 00:08:35 +0100 Subject: [PATCH 05/14] GJK: rename `VDB` cv criterion to `Default` --- include/hpp/fcl/collision_data.h | 2 +- include/hpp/fcl/data_types.h | 2 +- include/hpp/fcl/narrowphase/gjk.h | 11 +++--- include/hpp/fcl/narrowphase/narrowphase.h | 2 +- python/gjk.cc | 10 +++--- src/narrowphase/gjk.cpp | 44 ++++++++--------------- test/gjk_convergence_criterion.cpp | 5 +-- 7 files changed, 34 insertions(+), 42 deletions(-) diff --git a/include/hpp/fcl/collision_data.h b/include/hpp/fcl/collision_data.h index b8be19068..62b129e9c 100644 --- a/include/hpp/fcl/collision_data.h +++ b/include/hpp/fcl/collision_data.h @@ -221,7 +221,7 @@ struct HPP_FCL_DLLAPI QueryRequest { gjk_max_iterations(GJK_DEFAULT_MAX_ITERATIONS), gjk_tolerance(GJK_DEFAULT_TOLERANCE), gjk_variant(GJKVariant::DefaultGJK), - gjk_convergence_criterion(GJKConvergenceCriterion::VDB), + gjk_convergence_criterion(GJKConvergenceCriterion::Default), gjk_convergence_criterion_type(GJKConvergenceCriterionType::Relative), epa_max_iterations(EPA_DEFAULT_MAX_ITERATIONS), epa_tolerance(EPA_DEFAULT_TOLERANCE), diff --git a/include/hpp/fcl/data_types.h b/include/hpp/fcl/data_types.h index a787a1439..cb8a0a128 100644 --- a/include/hpp/fcl/data_types.h +++ b/include/hpp/fcl/data_types.h @@ -89,7 +89,7 @@ enum GJKVariant { DefaultGJK, PolyakAcceleration, NesterovAcceleration }; /// shapes are not in collision). (default) VDB: Van den Bergen (A Fast and /// Robust GJK Implementation, 1999) DG: duality-gap, as used in the Frank-Wolfe /// and the vanilla 1988 GJK algorithms Hybrid: a mix between VDB and DG. -enum GJKConvergenceCriterion { VDB, DualityGap, Hybrid }; +enum GJKConvergenceCriterion { Default, DualityGap, Hybrid }; /// @brief Wether the convergence criterion is scaled on the norm of the /// solution or not diff --git a/include/hpp/fcl/narrowphase/gjk.h b/include/hpp/fcl/narrowphase/gjk.h index 7322d8d94..13c6f7b4a 100644 --- a/include/hpp/fcl/narrowphase/gjk.h +++ b/include/hpp/fcl/narrowphase/gjk.h @@ -164,17 +164,20 @@ struct HPP_FCL_DLLAPI GJK { }; /// @brief Status of the GJK algorithm: + /// DidNotRun: GJK has not been run. + /// Failed: GJK did not converge (it exceeded the maximum number of + /// iterations). + /// NoCollisionEarlyStopped: GJK found a separating hyperplane and exited + /// before converting. The shapes are not in collision. /// NoCollision: GJK converged and the shapes are not in collision. /// Collision: GJK converged and the shapes are in collision. /// Failed: GJK did not converge. - /// NoCollisionEarlyStopped: GJK found a separating hyperplane and exited - /// before converting. The shapes are not in collision. enum Status { DidNotRun, Failed, + NoCollisionEarlyStopped, NoCollision, - Collision, - NoCollisionEarlyStopped + Collision }; public: diff --git a/include/hpp/fcl/narrowphase/narrowphase.h b/include/hpp/fcl/narrowphase/narrowphase.h index 7ab646d84..0d9ea5ef4 100644 --- a/include/hpp/fcl/narrowphase/narrowphase.h +++ b/include/hpp/fcl/narrowphase/narrowphase.h @@ -539,7 +539,7 @@ struct HPP_FCL_DLLAPI GJKSolver { // Default settings for GJK algorithm gjk.gjk_variant = GJKVariant::DefaultGJK; - gjk.convergence_criterion = GJKConvergenceCriterion::VDB; + gjk.convergence_criterion = GJKConvergenceCriterion::Default; gjk.convergence_criterion_type = GJKConvergenceCriterionType::Relative; } diff --git a/python/gjk.cc b/python/gjk.cc index 2344cfa7e..1b330ecd2 100644 --- a/python/gjk.cc +++ b/python/gjk.cc @@ -53,9 +53,11 @@ using hpp::fcl::details::MinkowskiDiff; void exposeGJK() { if (!eigenpy::register_symbolic_link_to_registered_type()) { enum_("GJKStatus") - .value("NoCollision", GJK::NoCollision) - .value("Inside", GJK::Collision) - .value("Failed", GJK::Failed) + .value("Failed", GJK::Status::Failed) + .value("DidNotRun", GJK::Status::DidNotRun) + .value("NoCollision", GJK::Status::NoCollision) + .value("NoCollisionEarlyStopped", GJK::Status::NoCollisionEarlyStopped) + .value("Collision", GJK::Status::Collision) .export_values(); } @@ -103,7 +105,7 @@ void exposeGJK() { if (!eigenpy::register_symbolic_link_to_registered_type< GJKConvergenceCriterion>()) { enum_("GJKConvergenceCriterion") - .value("VDB", GJKConvergenceCriterion::VDB) + .value("Default", GJKConvergenceCriterion::Default) .value("DualityGap", GJKConvergenceCriterion::DualityGap) .value("Hybrid", GJKConvergenceCriterion::Hybrid) .export_values(); diff --git a/src/narrowphase/gjk.cpp b/src/narrowphase/gjk.cpp index 265f06bb1..18ae67c97 100644 --- a/src/narrowphase/gjk.cpp +++ b/src/narrowphase/gjk.cpp @@ -548,7 +548,7 @@ void MinkowskiDiff::set(const ShapeBase* shape0, const ShapeBase* shape1) { void GJK::initialize() { distance_upper_bound = (std::numeric_limits::max)(); gjk_variant = GJKVariant::DefaultGJK; - convergence_criterion = GJKConvergenceCriterion::VDB; + convergence_criterion = GJKConvergenceCriterion::Default; convergence_criterion_type = GJKConvergenceCriterionType::Relative; reset(max_iterations, tolerance); } @@ -838,71 +838,57 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess, bool GJK::checkConvergence(const Vec3f& w, const FCL_REAL& rl, FCL_REAL& alpha, const FCL_REAL& omega) { - FCL_REAL diff; - bool check_passed; // x^* is the optimal solution (projection of origin onto the Minkowski // difference). // x^k is the current iterate (x^k = `ray` in the code). // Each criterion provides a different guarantee on the distance to the // optimal solution. switch (convergence_criterion) { - case VDB: + case Default: { // alpha is the distance to the best separating hyperplane found so far alpha = std::max(alpha, omega); // ||x^*|| - ||x^k|| <= diff - diff = rl - alpha; - switch (convergence_criterion_type) { - case Absolute: - HPP_FCL_THROW_PRETTY("VDB convergence criterion is relative.", - std::logic_error); - break; - case Relative: - check_passed = (diff - (tolerance + tolerance * rl)) <= 0; - break; - default: - HPP_FCL_THROW_PRETTY("Invalid convergence criterion type.", - std::logic_error); - } - break; + const FCL_REAL diff = rl - alpha; + return ((diff - (tolerance + tolerance * rl)) <= 0); + } break; - case DualityGap: + case DualityGap: { // ||x^* - x^k||^2 <= diff - diff = 2 * ray.dot(ray - w); + const FCL_REAL diff = 2 * ray.dot(ray - w); switch (convergence_criterion_type) { case Absolute: - check_passed = (diff - tolerance) <= 0; + return ((diff - tolerance) <= 0); break; case Relative: - check_passed = ((diff / tolerance * rl) - tolerance * rl) <= 0; + return (((diff / tolerance * rl) - tolerance * rl) <= 0); break; default: HPP_FCL_THROW_PRETTY("Invalid convergence criterion type.", std::logic_error); } - break; + } break; - case Hybrid: + case Hybrid: { // alpha is the distance to the best separating hyperplane found so far alpha = std::max(alpha, omega); // ||x^* - x^k||^2 <= diff - diff = rl * rl - alpha * alpha; + const FCL_REAL diff = rl * rl - alpha * alpha; switch (convergence_criterion_type) { case Absolute: - check_passed = (diff - tolerance) <= 0; + return ((diff - tolerance) <= 0); break; case Relative: - check_passed = ((diff / tolerance * rl) - tolerance * rl) <= 0; + return (((diff / tolerance * rl) - tolerance * rl) <= 0); break; default: HPP_FCL_THROW_PRETTY("Invalid convergence criterion type.", std::logic_error); } - break; + } break; default: HPP_FCL_THROW_PRETTY("Invalid convergence criterion.", std::logic_error); } - return check_passed; } inline void GJK::removeVertex(Simplex& simplex) { diff --git a/test/gjk_convergence_criterion.cpp b/test/gjk_convergence_criterion.cpp index 4fc5db68e..ae315540a 100644 --- a/test/gjk_convergence_criterion.cpp +++ b/test/gjk_convergence_criterion.cpp @@ -64,11 +64,12 @@ BOOST_AUTO_TEST_CASE(set_cv_criterion) { GJK gjk(128, 1e-6); // Checking defaults - BOOST_CHECK(solver.gjk.convergence_criterion == GJKConvergenceCriterion::VDB); + BOOST_CHECK(solver.gjk.convergence_criterion == + GJKConvergenceCriterion::Default); BOOST_CHECK(solver.gjk.convergence_criterion_type == GJKConvergenceCriterionType::Relative); - BOOST_CHECK(gjk.convergence_criterion == GJKConvergenceCriterion::VDB); + BOOST_CHECK(gjk.convergence_criterion == GJKConvergenceCriterion::Default); BOOST_CHECK(gjk.convergence_criterion_type == GJKConvergenceCriterionType::Relative); From e928a99de4ebf330bdb9abca50a70a7272f2aed8 Mon Sep 17 00:00:00 2001 From: Louis Montaut Date: Thu, 21 Mar 2024 01:58:00 +0100 Subject: [PATCH 06/14] EPA: remove useless init function The generic `newFace` function actually takes good care of initializing EPA's polytope. --- include/hpp/fcl/narrowphase/gjk.h | 6 --- src/narrowphase/gjk.cpp | 70 ++++--------------------------- 2 files changed, 9 insertions(+), 67 deletions(-) diff --git a/include/hpp/fcl/narrowphase/gjk.h b/include/hpp/fcl/narrowphase/gjk.h index 13c6f7b4a..97e3d3c5c 100644 --- a/include/hpp/fcl/narrowphase/gjk.h +++ b/include/hpp/fcl/narrowphase/gjk.h @@ -512,12 +512,6 @@ struct HPP_FCL_DLLAPI EPA { bool getEdgeDist(SimplexFace* face, const SimplexVertex& a, const SimplexVertex& b, FCL_REAL& dist); - /// @brief Add a new face to the polytope; used at the beginning of EPA. - /// Note: sometimes the origin can be located outside EPA's starting polytope. - /// This is fine, we simply make sure to compute quantities in the right - /// normal direction to set the `ignore` flag correctly. - SimplexFace* createInitialPolytopeFace(size_t id_a, size_t id_b, size_t id_c); - /// @brief Add a new face to the polytope. /// This function sets the `ignore` flag to `true` if the origin does not /// project inside the face. diff --git a/src/narrowphase/gjk.cpp b/src/narrowphase/gjk.cpp index 18ae67c97..d2b672a0a 100644 --- a/src/narrowphase/gjk.cpp +++ b/src/narrowphase/gjk.cpp @@ -1119,6 +1119,7 @@ bool GJK::projectTetrahedraOrigin(const Simplex& current, Simplex& next) { next.rank = 4; \ return true; + // clang-format off if (ba_aa <= 0) { // if AB.AO >= 0 / a10 if (-D.dot(a_cross_b) <= 0) { // if ADB.AO >= 0 / a10.a3 if (ba * da_ba + bd * ba_aa - bb * da_aa <= @@ -1469,6 +1470,7 @@ bool GJK::projectTetrahedraOrigin(const Simplex& current, Simplex& next) { } // end of AD.AO >= 0 } // end of AC.AO >= 0 } // end of AB.AO >= 0 + // clang-format on #undef REGION_INSIDE return false; @@ -1529,60 +1531,6 @@ bool EPA::getEdgeDist(SimplexFace* face, const SimplexVertex& a, return false; } -EPA::SimplexFace* EPA::createInitialPolytopeFace(size_t id_a, size_t id_b, - size_t id_c) { - if (stock.root != nullptr) { - SimplexFace* face = stock.root; - stock.remove(face); - hull.append(face); - face->pass = 0; - face->vertex_id[0] = id_a; - face->vertex_id[1] = id_b; - face->vertex_id[2] = id_c; - const SimplexVertex& a = sv_store[id_a]; - const SimplexVertex& b = sv_store[id_b]; - const SimplexVertex& c = sv_store[id_c]; - face->n = (b.w - a.w).cross(c.w - a.w); - - if (face->n.norm() > Eigen::NumTraits::epsilon()) { - face->n.normalize(); - // When creating the original polytope out of GJK's simplex, - // it's possible the origin lies outside the polytope. - // This is fine, but to check if the origin lies inside/outside the face, - // we simply need to do it in the right direction. - Vec3f n = face->n; - if (face->n.dot(a.w) < -tolerance || // - face->n.dot(b.w) < -tolerance || // - face->n.dot(c.w) < -tolerance) { - n = -face->n; - } - - FCL_REAL a_dot_nab = a.w.dot((b.w - a.w).cross(n)); - FCL_REAL b_dot_nbc = b.w.dot((c.w - b.w).cross(n)); - FCL_REAL c_dot_nca = c.w.dot((a.w - c.w).cross(n)); - if (!(a_dot_nab < -tolerance || // - b_dot_nbc < -tolerance || // - c_dot_nca < -tolerance)) { - face->d = a.w.dot(face->n); - face->ignore = false; - } else { - face->d = std::numeric_limits::max(); - face->ignore = true; - } - return face; - } else - status = Degenerated; - - hull.remove(face); - stock.append(face); - return nullptr; - } - - assert(hull.count >= fc_store.size() && "EPA: should not be out of faces."); - status = OutOfFaces; - return nullptr; -} - EPA::SimplexFace* EPA::newFace(size_t id_a, size_t id_b, size_t id_c) { if (stock.root != nullptr) { SimplexFace* face = stock.root; @@ -1606,9 +1554,9 @@ EPA::SimplexFace* EPA::newFace(size_t id_a, size_t id_b, size_t id_c) { FCL_REAL a_dot_nab = a.w.dot((b.w - a.w).cross(face->n)); FCL_REAL b_dot_nbc = b.w.dot((c.w - b.w).cross(face->n)); FCL_REAL c_dot_nca = c.w.dot((a.w - c.w).cross(face->n)); - if (!(a_dot_nab < -tolerance || // - b_dot_nbc < -tolerance || // - c_dot_nca < -tolerance)) { + if (a_dot_nab >= -tolerance && // + b_dot_nbc >= -tolerance && // + c_dot_nca >= -tolerance) { face->d = a.w.dot(face->n); face->ignore = false; } else { @@ -1686,10 +1634,10 @@ EPA::Status EPA::evaluate(GJK& gjk, const Vec3f& guess) { sv_store[num_vertices++] = *simplex.vertex[i]; } - SimplexFace* tetrahedron[] = {createInitialPolytopeFace(0, 1, 2), // - createInitialPolytopeFace(1, 0, 3), // - createInitialPolytopeFace(2, 1, 3), // - createInitialPolytopeFace(0, 2, 3)}; + SimplexFace* tetrahedron[] = {newFace(0, 1, 2), // + newFace(1, 0, 3), // + newFace(2, 1, 3), // + newFace(0, 2, 3)}; if (hull.count == 4) { // set the face connectivity From de76ac46abe3c58538e35b096824fbd3c8f6b31e Mon Sep 17 00:00:00 2001 From: Louis Montaut Date: Thu, 21 Mar 2024 02:24:00 +0100 Subject: [PATCH 07/14] EPA: clean assert --- src/narrowphase/gjk.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/narrowphase/gjk.cpp b/src/narrowphase/gjk.cpp index d2b672a0a..61fd68bf4 100644 --- a/src/narrowphase/gjk.cpp +++ b/src/narrowphase/gjk.cpp @@ -1816,9 +1816,7 @@ bool EPA::expand(size_t pass, const SimplexVertex& w, SimplexFace* f, size_t e, // Uncomment the following line and the associated EPA method // to debug the infinite loop if needed. // EPAPrintExpandLooping(this, f); - if (f == closest_face) { - assert(false && "EPA is looping indefinitely."); - } + assert(f != closest_face && "EPA is looping indefinitely."); status = InvalidHull; return false; } From 6dfe6458fc2b93e1fecac103f5167281a985fdaa Mon Sep 17 00:00:00 2001 From: Louis Montaut Date: Thu, 21 Mar 2024 02:55:52 +0100 Subject: [PATCH 08/14] EPA: don't fill with nan in edge cases When EPA creates an invalid polytope, often it's due to badly conditioned problems. However, it's probably better to return an estimate of the depth and the normal than returning nans. Nans are for really bad failure cases. --- include/hpp/fcl/narrowphase/narrowphase.h | 48 +++++++++-------------- 1 file changed, 19 insertions(+), 29 deletions(-) diff --git a/include/hpp/fcl/narrowphase/narrowphase.h b/include/hpp/fcl/narrowphase/narrowphase.h index 0d9ea5ef4..85a6ec339 100644 --- a/include/hpp/fcl/narrowphase/narrowphase.h +++ b/include/hpp/fcl/narrowphase/narrowphase.h @@ -324,41 +324,31 @@ struct HPP_FCL_DLLAPI GJKSolver { EPAValidExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal); break; - // - // In the following cases, EPA failed to run, in a bad way. - // The produced witness points, penetration depth and normal - // may make no sense. - case details::EPA::DidNotRun: - HPP_FCL_ASSERT(false, "EPA did not run. It should have!", - std::logic_error); - HPP_FCL_LOG_ERROR("EPA error: did not run. It should have."); - EPAFailedExtractWitnessPointsAndNormal(tf1, distance, p1, p2, - normal); - gjk_and_epa_ran_successfully = false; - break; case details::EPA::Degenerated: - HPP_FCL_ASSERT( - false, "EPA created a polytope with a degenerated face.", - std::logic_error); - HPP_FCL_LOG_ERROR( - "EPA error: created a polytope with a degenerated face."); - EPAFailedExtractWitnessPointsAndNormal(tf1, distance, p1, p2, - normal); - gjk_and_epa_ran_successfully = false; + HPP_FCL_LOG_WARNING( + "EPA warning: created a polytope with a degenerated face."); + EPAValidExtractWitnessPointsAndNormal(tf1, distance, p1, p2, + normal); + gjk_and_epa_ran_successfully = true; break; case details::EPA::NonConvex: - HPP_FCL_ASSERT(false, "EPA got called onto non-convex shapes.", - std::logic_error); - HPP_FCL_LOG_ERROR( - "EPA error: EPA got called onto non-convex shapes."); - EPAFailedExtractWitnessPointsAndNormal(tf1, distance, p1, p2, - normal); - gjk_and_epa_ran_successfully = false; + HPP_FCL_LOG_WARNING( + "EPA warning: EPA got called onto non-convex shapes."); + EPAValidExtractWitnessPointsAndNormal(tf1, distance, p1, p2, + normal); + gjk_and_epa_ran_successfully = true; break; case details::EPA::InvalidHull: - HPP_FCL_ASSERT(false, "EPA created an invalid polytope.", + HPP_FCL_LOG_WARNING( + "EPA warning: created an invalid polytope."); + EPAValidExtractWitnessPointsAndNormal(tf1, distance, p1, p2, + normal); + gjk_and_epa_ran_successfully = true; + break; + case details::EPA::DidNotRun: + HPP_FCL_ASSERT(false, "EPA did not run. It should have!", std::logic_error); - HPP_FCL_LOG_ERROR("EPA error: created an invalid polytope."); + HPP_FCL_LOG_ERROR("EPA error: did not run. It should have."); EPAFailedExtractWitnessPointsAndNormal(tf1, distance, p1, p2, normal); gjk_and_epa_ran_successfully = false; From 3f6f7c3d73f48c60ec6ea53d0304e597967ec36d Mon Sep 17 00:00:00 2001 From: Louis Montaut Date: Thu, 21 Mar 2024 03:18:44 +0100 Subject: [PATCH 09/14] CollisionData: add note on EPA's tolerance --- include/hpp/fcl/collision_data.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/include/hpp/fcl/collision_data.h b/include/hpp/fcl/collision_data.h index 62b129e9c..aaf5a9bce 100644 --- a/include/hpp/fcl/collision_data.h +++ b/include/hpp/fcl/collision_data.h @@ -201,7 +201,8 @@ struct HPP_FCL_DLLAPI QueryRequest { /// @brief max number of iterations for EPA size_t epa_max_iterations; - /// @brief tolerance for EPA + /// @brief tolerance for EPA. + /// Note: setting EPA's tolerance to less than GJK's is not recommended. FCL_REAL epa_tolerance; /// @brief enable timings when performing collision/distance request From e9cd7dff688d54963ce3bdc5340ddaba4dbcf7b0 Mon Sep 17 00:00:00 2001 From: Louis Montaut Date: Thu, 21 Mar 2024 03:18:50 +0100 Subject: [PATCH 10/14] Update changelog --- CHANGELOG.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index cfef96414..fb7aa0006 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -5,6 +5,8 @@ All notable changes to this project will be documented in this file. The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/). ## [Unreleased] +- [API change] Renamed default convergence criterion from `VDB` to `Default` ([#556](https://github.com/humanoid-path-planner/hpp-fcl/pull/556)) +- Fixed EPA returning nans on cases where it could return an estimate of the normal and penetration depth. ([#556](https://github.com/humanoid-path-planner/hpp-fcl/pull/556)) - Fixed too low tolerance in GJK/EPA asserts ([#554](https://github.com/humanoid-path-planner/hpp-fcl/pull/554)) - Fixed `normal_and_nearest_points` test (no need to have Eigen 3.4) ([#553](https://github.com/humanoid-path-planner/hpp-fcl/pull/553)) - [#549](https://github.com/humanoid-path-planner/hpp-fcl/pull/549) From 3cbb51307c5f1268fb2c5ac6f6f79e04c3272bff Mon Sep 17 00:00:00 2001 From: Louis Montaut Date: Thu, 21 Mar 2024 03:54:22 +0100 Subject: [PATCH 11/14] GJK: clearer exit status --- include/hpp/fcl/narrowphase/gjk.h | 9 +- include/hpp/fcl/narrowphase/narrowphase.h | 230 ++++++++++------------ python/gjk.cc | 3 +- src/narrowphase/gjk.cpp | 21 +- test/gjk.cpp | 17 +- 5 files changed, 134 insertions(+), 146 deletions(-) diff --git a/include/hpp/fcl/narrowphase/gjk.h b/include/hpp/fcl/narrowphase/gjk.h index 97e3d3c5c..df8a56572 100644 --- a/include/hpp/fcl/narrowphase/gjk.h +++ b/include/hpp/fcl/narrowphase/gjk.h @@ -177,6 +177,7 @@ struct HPP_FCL_DLLAPI GJK { Failed, NoCollisionEarlyStopped, NoCollision, + CollisionWithPenetrationInformation, Collision }; @@ -261,14 +262,6 @@ struct HPP_FCL_DLLAPI GJK { /// Tells whether the closest points are available. bool hasClosestPoints() { return distance < distance_upper_bound; } - /// Tells whether the penetration information. - /// - /// In such case, most indepth points and penetration depth can be retrieved - /// from GJK. Calling EPA has an undefined behaviour. - bool hasPenetrationInformation(const MinkowskiDiff& shape) { - return distance > -shape.inflation.sum(); - } - /// Get the closest points on each object. /// @return true on success bool getClosestPoints(const MinkowskiDiff& shape, Vec3f& w0, Vec3f& w1); diff --git a/include/hpp/fcl/narrowphase/narrowphase.h b/include/hpp/fcl/narrowphase/narrowphase.h index 85a6ec339..da9374f69 100644 --- a/include/hpp/fcl/narrowphase/narrowphase.h +++ b/include/hpp/fcl/narrowphase/narrowphase.h @@ -244,126 +244,115 @@ struct HPP_FCL_DLLAPI GJKSolver { "distance between the closest points.", std::logic_error); break; + // + // Next are the cases where GJK found the shapes to be in collision, i.e. + // their distance is below GJK's tolerance (default 1e-6). + case details::GJK::CollisionWithPenetrationInformation: + GJKCollisionWithPenetrationInfoExtractWitnessPointsAndNormal( + tf1, distance, p1, p2, normal); + HPP_FCL_ASSERT(distance <= gjk.getTolerance() + dummy_precision, + "The distance found by GJK should be negative (or at )" + "least below GJK's tolerance.", + std::logic_error); + break; case details::GJK::Collision: - // - // Case where GJK found the shapes to be in collision, i.e. their - // distance is below GJK's tolerance (default 1e-6). - if (gjk.hasPenetrationInformation(minkowski_difference)) { - // - // Case where the shapes are inflated (sphere or capsule). - // When the shapes are inflated, the GJK algorithm can provide the - // witness points and the normal. - GJKCollisionWithInflationExtractWitnessPointsAndNormal( - tf1, distance, p1, p2, normal); - HPP_FCL_ASSERT(distance <= gjk.getTolerance() + dummy_precision, - "The distance found by GJK should be negative (or at )" - "least below GJK's tolerance.", - std::logic_error); - // + because the distance is negative. - HPP_FCL_ASSERT(std::abs((p1 - p2).norm() + distance) <= - gjk.getTolerance() + dummy_precision, - "The distance found by GJK should coincide with the " - "distance between the closest points.", - std::logic_error); + if (!compute_penetration) { + // Skip EPA and set the witness points and the normal to nans. + GJKCollisionExtractWitnessPointsAndNormal(tf1, distance, p1, p2, + normal); } else { - if (!compute_penetration) { - GJKCollisionExtractWitnessPointsAndNormal(tf1, distance, p1, p2, - normal); - } else { + // + // Case where the shapes are not inflated (box, cylinder, cone, + // convex meshes etc.). We need to run the EPA algorithm to find the + // witness points, penetration depth and the normal. + + // Reset EPA algorithm. Potentially allocate memory if + // `epa_max_face_num` or `epa_max_vertex_num` are bigger than EPA's + // current storage. + epa.reset(epa_max_iterations, epa_tolerance); + + // TODO: understand why EPA's performance is so bad on cylinders and + // cones. + epa.evaluate(gjk, -guess); + + switch (epa.status) { // - // Case where the shapes are not inflated (box, cylinder, cone, - // convex meshes etc.). We need to run the EPA algorithm to find the - // witness points, penetration depth and the normal. - - // Reset EPA algorithm. Potentially allocate memory if - // `epa_max_face_num` or `epa_max_vertex_num` are bigger than EPA's - // current storage. - epa.reset(epa_max_iterations, epa_tolerance); - - // TODO: understand why EPA's performance is so bad on cylinders and - // cones. - epa.evaluate(gjk, -guess); - - switch (epa.status) { - // - // In the following switch cases, until the "Valid" case, - // EPA either ran out of iterations, of faces or of vertices. - // The depth, witness points and the normal are still valid, - // simply not at the precision of EPA's tolerance. - // The flag `HPP_FCL_ENABLE_LOGGING` enables feebdack on these - // cases. - // - // TODO: Remove OutOfFaces and OutOfVertices statuses and simply - // compute the upper bound on max faces and max vertices as a - // function of the number of iterations. - case details::EPA::OutOfFaces: - HPP_FCL_LOG_WARNING("EPA ran out of faces."); - EPAValidExtractWitnessPointsAndNormal(tf1, distance, p1, p2, - normal); - gjk_and_epa_ran_successfully = false; - break; - case details::EPA::OutOfVertices: - HPP_FCL_LOG_WARNING("EPA ran out of vertices."); - EPAValidExtractWitnessPointsAndNormal(tf1, distance, p1, p2, - normal); - gjk_and_epa_ran_successfully = false; - break; - case details::EPA::Failed: - HPP_FCL_LOG_WARNING("EPA ran out of iterations."); - EPAValidExtractWitnessPointsAndNormal(tf1, distance, p1, p2, - normal); - gjk_and_epa_ran_successfully = false; - break; - case details::EPA::Valid: - case details::EPA::AccuracyReached: - HPP_FCL_ASSERT( - -epa.depth <= epa.getTolerance() + dummy_precision, - "EPA's penetration distance should be negative (or " - "at least below EPA's tolerance).", - std::logic_error); - EPAValidExtractWitnessPointsAndNormal(tf1, distance, p1, p2, - normal); - break; - case details::EPA::Degenerated: - HPP_FCL_LOG_WARNING( - "EPA warning: created a polytope with a degenerated face."); - EPAValidExtractWitnessPointsAndNormal(tf1, distance, p1, p2, - normal); - gjk_and_epa_ran_successfully = true; - break; - case details::EPA::NonConvex: - HPP_FCL_LOG_WARNING( - "EPA warning: EPA got called onto non-convex shapes."); - EPAValidExtractWitnessPointsAndNormal(tf1, distance, p1, p2, - normal); - gjk_and_epa_ran_successfully = true; - break; - case details::EPA::InvalidHull: - HPP_FCL_LOG_WARNING( - "EPA warning: created an invalid polytope."); - EPAValidExtractWitnessPointsAndNormal(tf1, distance, p1, p2, - normal); - gjk_and_epa_ran_successfully = true; - break; - case details::EPA::DidNotRun: - HPP_FCL_ASSERT(false, "EPA did not run. It should have!", - std::logic_error); - HPP_FCL_LOG_ERROR("EPA error: did not run. It should have."); - EPAFailedExtractWitnessPointsAndNormal(tf1, distance, p1, p2, - normal); - gjk_and_epa_ran_successfully = false; - break; - case details::EPA::FallBack: - HPP_FCL_ASSERT( - false, - "EPA went into fallback mode. It should never do that.", - std::logic_error); - HPP_FCL_LOG_ERROR("EPA error: FallBack."); - EPAFailedExtractWitnessPointsAndNormal(tf1, distance, p1, p2, - normal); - gjk_and_epa_ran_successfully = false; - break; - } + // In the following switch cases, until the "Valid" case, + // EPA either ran out of iterations, of faces or of vertices. + // The depth, witness points and the normal are still valid, + // simply not at the precision of EPA's tolerance. + // The flag `HPP_FCL_ENABLE_LOGGING` enables feebdack on these + // cases. + // + // TODO: Remove OutOfFaces and OutOfVertices statuses and simply + // compute the upper bound on max faces and max vertices as a + // function of the number of iterations. + case details::EPA::OutOfFaces: + HPP_FCL_LOG_WARNING("EPA ran out of faces."); + EPAValidExtractWitnessPointsAndNormal(tf1, distance, p1, p2, + normal); + gjk_and_epa_ran_successfully = false; + break; + case details::EPA::OutOfVertices: + HPP_FCL_LOG_WARNING("EPA ran out of vertices."); + EPAValidExtractWitnessPointsAndNormal(tf1, distance, p1, p2, + normal); + gjk_and_epa_ran_successfully = false; + break; + case details::EPA::Failed: + HPP_FCL_LOG_WARNING("EPA ran out of iterations."); + EPAValidExtractWitnessPointsAndNormal(tf1, distance, p1, p2, + normal); + gjk_and_epa_ran_successfully = false; + break; + case details::EPA::Valid: + case details::EPA::AccuracyReached: + HPP_FCL_ASSERT( + -epa.depth <= epa.getTolerance() + dummy_precision, + "EPA's penetration distance should be negative (or " + "at least below EPA's tolerance).", + std::logic_error); + EPAValidExtractWitnessPointsAndNormal(tf1, distance, p1, p2, + normal); + break; + case details::EPA::Degenerated: + HPP_FCL_LOG_WARNING( + "EPA warning: created a polytope with a degenerated face."); + EPAValidExtractWitnessPointsAndNormal(tf1, distance, p1, p2, + normal); + gjk_and_epa_ran_successfully = true; + break; + case details::EPA::NonConvex: + HPP_FCL_LOG_WARNING( + "EPA warning: EPA got called onto non-convex shapes."); + EPAValidExtractWitnessPointsAndNormal(tf1, distance, p1, p2, + normal); + gjk_and_epa_ran_successfully = true; + break; + case details::EPA::InvalidHull: + HPP_FCL_LOG_WARNING("EPA warning: created an invalid polytope."); + EPAValidExtractWitnessPointsAndNormal(tf1, distance, p1, p2, + normal); + gjk_and_epa_ran_successfully = true; + break; + case details::EPA::DidNotRun: + HPP_FCL_ASSERT(false, "EPA did not run. It should have!", + std::logic_error); + HPP_FCL_LOG_ERROR("EPA error: did not run. It should have."); + EPAFailedExtractWitnessPointsAndNormal(tf1, distance, p1, p2, + normal); + gjk_and_epa_ran_successfully = false; + break; + case details::EPA::FallBack: + HPP_FCL_ASSERT( + false, + "EPA went into fallback mode. It should never do that.", + std::logic_error); + HPP_FCL_LOG_ERROR("EPA error: FallBack."); + EPAFailedExtractWitnessPointsAndNormal(tf1, distance, p1, p2, + normal); + gjk_and_epa_ran_successfully = false; + break; } } break; // End of case details::GJK::Inside @@ -416,7 +405,7 @@ struct HPP_FCL_DLLAPI GJKSolver { p2 = tf1.transform(p2); } - void GJKCollisionWithInflationExtractWitnessPointsAndNormal( + void GJKCollisionWithPenetrationInfoExtractWitnessPointsAndNormal( const Transform3f& tf1, FCL_REAL& distance, Vec3f& p1, Vec3f& p2, Vec3f& normal) const { distance = gjk.distance; @@ -439,9 +428,8 @@ struct HPP_FCL_DLLAPI GJKSolver { std::logic_error); distance = gjk.distance; gjk.getClosestPoints(minkowski_difference, p1, p2); - p1 = tf1.transform(p1); - p2 = tf1.transform(p2); - normal = Vec3f::Constant(std::numeric_limits::quiet_NaN()); + p1 = p2 = normal = + Vec3f::Constant(std::numeric_limits::quiet_NaN()); } void EPAValidExtractWitnessPointsAndNormal(const Transform3f& tf1, diff --git a/python/gjk.cc b/python/gjk.cc index 1b330ecd2..f1d68afb4 100644 --- a/python/gjk.cc +++ b/python/gjk.cc @@ -57,6 +57,8 @@ void exposeGJK() { .value("DidNotRun", GJK::Status::DidNotRun) .value("NoCollision", GJK::Status::NoCollision) .value("NoCollisionEarlyStopped", GJK::Status::NoCollisionEarlyStopped) + .value("CollisionWithPenetrationInformation", + GJK::Status::CollisionWithPenetrationInformation) .value("Collision", GJK::Status::Collision) .export_values(); } @@ -135,7 +137,6 @@ void exposeGJK() { .DEF_CLASS_FUNC(GJK, getNumIterations) .DEF_CLASS_FUNC(GJK, getNumIterationsMomentumStopped) .DEF_CLASS_FUNC(GJK, hasClosestPoints) - .DEF_CLASS_FUNC(GJK, hasPenetrationInformation) .DEF_CLASS_FUNC(GJK, getClosestPoints) .DEF_CLASS_FUNC(GJK, setDistanceEarlyBreak) .DEF_CLASS_FUNC(GJK, getGuessFromSimplex); diff --git a/src/narrowphase/gjk.cpp b/src/narrowphase/gjk.cpp index 61fd68bf4..f72a85fdc 100644 --- a/src/narrowphase/gjk.cpp +++ b/src/narrowphase/gjk.cpp @@ -700,13 +700,14 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess, Simplex& next_simplex = simplices[next]; // check A: when origin is near the existing simplex, stop - // TODO this is an early stop which may cause the following issue. - // - EPA will not run correctly because it starts with a tetrahedron which - // does not include the origin. Note that, at this stage, we do not know - // whether a tetrahedron including the origin exists. if (rl < tolerance) // mean origin is near the face of original simplex, // return touch { + // At this point, GJK has converged but we don't know if GJK is enough to + // recover penetration information. + // EPA needs to be run. + // Unless the Minkowski difference is degenerated, EPA will run fine even + // if the final simplex of GJK is not a tetrahedron. assert(rl > 0); status = Collision; distance = -inflation; // should we take rl into account ? @@ -777,9 +778,6 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess, // check C: when the new support point is close to the sub-simplex where the // ray point lies, stop (as the new simplex again is degenerated) bool cv_check_passed = checkConvergence(w, rl, alpha, omega); - // TODO here, we can stop at iteration 0 if this condition is met. - // We stopping at iteration 0, the closest point will not be valid. - // if(diff - tolerance * rl <= 0) if (iterations > 0 && cv_check_passed) { if (iterations > 0) removeVertex(simplices[current]); if (current_gjk_variant != DefaultGJK) { @@ -787,11 +785,16 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess, iterations_momentum_stop = iterations; continue; } - distance = rl - inflation; // TODO When inflation is strictly positive, the distance may be exactly // zero (so the ray is not zero) and we are not in the case rl < // tolerance. - if (distance < tolerance) status = Collision; + + // At this point, GJK has converged and penetration information can always + // be recovered without running EPA. + distance = rl - inflation; + if (distance < tolerance) { + status = CollisionWithPenetrationInformation; + } break; } diff --git a/test/gjk.cpp b/test/gjk.cpp index a592606f7..11b86bf26 100644 --- a/test/gjk.cpp +++ b/test/gjk.cpp @@ -339,10 +339,12 @@ void test_gjk_unit_sphere(FCL_REAL center_distance, Vec3f ray, gjk.gjk_variant = GJKVariant::NesterovAcceleration; details::GJK::Status status = gjk.evaluate(shape, Vec3f(1, 0, 0)); - if (expect_collision) - BOOST_CHECK_EQUAL(status, details::GJK::Collision); - else + if (expect_collision) { + BOOST_CHECK((status == details::GJK::Collision) || + (status == details::GJK::CollisionWithPenetrationInformation)); + } else { BOOST_CHECK_EQUAL(status, details::GJK::NoCollision); + } Vec3f w0, w1; gjk.getClosestPoints(shape, w0, w1); @@ -395,9 +397,10 @@ void test_gjk_triangle_capsule(Vec3f T, bool expect_collision, gjk.gjk_variant = GJKVariant::NesterovAcceleration; details::GJK::Status status = gjk.evaluate(shape, Vec3f(1, 0, 0)); - if (expect_collision) - BOOST_CHECK_EQUAL(status, details::GJK::Collision); - else { + if (expect_collision) { + BOOST_CHECK((status == details::GJK::Collision) || + (status == details::GJK::CollisionWithPenetrationInformation)); + } else { BOOST_CHECK_EQUAL(status, details::GJK::NoCollision); // Check that guess works as expected @@ -409,7 +412,7 @@ void test_gjk_triangle_capsule(Vec3f T, bool expect_collision, Vec3f w0, w1; if (status == details::GJK::NoCollision || - gjk.hasPenetrationInformation(shape)) { + status == details::GJK::CollisionWithPenetrationInformation) { gjk.getClosestPoints(shape, w0, w1); } else { details::EPA epa(64, 1e-6); From dcc335fa4a8a69024031b41d7836fc1344d25084 Mon Sep 17 00:00:00 2001 From: Louis Montaut Date: Thu, 21 Mar 2024 03:57:50 +0100 Subject: [PATCH 12/14] Misc: fix compilation warning --- include/hpp/fcl/narrowphase/narrowphase.h | 1 + 1 file changed, 1 insertion(+) diff --git a/include/hpp/fcl/narrowphase/narrowphase.h b/include/hpp/fcl/narrowphase/narrowphase.h index da9374f69..ad9165ad1 100644 --- a/include/hpp/fcl/narrowphase/narrowphase.h +++ b/include/hpp/fcl/narrowphase/narrowphase.h @@ -420,6 +420,7 @@ struct HPP_FCL_DLLAPI GJKSolver { FCL_REAL& distance, Vec3f& p1, Vec3f& p2, Vec3f& normal) const { + HPP_FCL_UNUSED_VARIABLE(tf1); const FCL_REAL dummy_precision = 3 * std::sqrt(std::numeric_limits::epsilon()); HPP_FCL_UNUSED_VARIABLE(dummy_precision); From 1916aece03bdc8295be172685c485be17effac89 Mon Sep 17 00:00:00 2001 From: Louis Montaut Date: Thu, 21 Mar 2024 03:58:05 +0100 Subject: [PATCH 13/14] GJK: fix constness in methods --- include/hpp/fcl/narrowphase/gjk.h | 6 +++--- src/narrowphase/gjk.cpp | 5 +++-- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/include/hpp/fcl/narrowphase/gjk.h b/include/hpp/fcl/narrowphase/gjk.h index df8a56572..e687a92cd 100644 --- a/include/hpp/fcl/narrowphase/gjk.h +++ b/include/hpp/fcl/narrowphase/gjk.h @@ -260,11 +260,11 @@ struct HPP_FCL_DLLAPI GJK { inline Simplex* getSimplex() const { return simplex; } /// Tells whether the closest points are available. - bool hasClosestPoints() { return distance < distance_upper_bound; } + bool hasClosestPoints() const { return distance < distance_upper_bound; } /// Get the closest points on each object. /// @return true on success - bool getClosestPoints(const MinkowskiDiff& shape, Vec3f& w0, Vec3f& w1); + bool getClosestPoints(const MinkowskiDiff& shape, Vec3f& w0, Vec3f& w1) const; /// @brief get the guess from current simplex Vec3f getGuessFromSimplex() const; @@ -280,7 +280,7 @@ struct HPP_FCL_DLLAPI GJK { /// @brief Convergence check used to stop GJK when shapes are not in /// collision. bool checkConvergence(const Vec3f& w, const FCL_REAL& rl, FCL_REAL& alpha, - const FCL_REAL& omega); + const FCL_REAL& omega) const; /// @brief Get the max number of iterations of GJK. size_t getNumMaxIterations() const { return max_iterations; } diff --git a/src/narrowphase/gjk.cpp b/src/narrowphase/gjk.cpp index f72a85fdc..93c35ef07 100644 --- a/src/narrowphase/gjk.cpp +++ b/src/narrowphase/gjk.cpp @@ -653,7 +653,8 @@ void inflate(const MinkowskiDiff& shape, Vec3f& w0, Vec3f& w1) { } // namespace details -bool GJK::getClosestPoints(const MinkowskiDiff& shape, Vec3f& w0, Vec3f& w1) { +bool GJK::getClosestPoints(const MinkowskiDiff& shape, Vec3f& w0, + Vec3f& w1) const { bool res = details::getClosestPoints(*simplex, w0, w1); if (!res) return false; details::inflate(shape, w0, w1); @@ -840,7 +841,7 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess, } bool GJK::checkConvergence(const Vec3f& w, const FCL_REAL& rl, FCL_REAL& alpha, - const FCL_REAL& omega) { + const FCL_REAL& omega) const { // x^* is the optimal solution (projection of origin onto the Minkowski // difference). // x^k is the current iterate (x^k = `ray` in the code). From 8683c45db6479090dcb65b34eb3640ed6d767ccc Mon Sep 17 00:00:00 2001 From: Louis Montaut Date: Thu, 21 Mar 2024 04:02:15 +0100 Subject: [PATCH 14/14] GJKSolver: update return value w.r.t new exit code --- include/hpp/fcl/narrowphase/narrowphase.h | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/include/hpp/fcl/narrowphase/narrowphase.h b/include/hpp/fcl/narrowphase/narrowphase.h index ad9165ad1..13f34bbf4 100644 --- a/include/hpp/fcl/narrowphase/narrowphase.h +++ b/include/hpp/fcl/narrowphase/narrowphase.h @@ -82,7 +82,8 @@ struct HPP_FCL_DLLAPI GJKSolver { if (normal != NULL) *normal = n; if (contact_points != NULL) *contact_points = 0.5 * (p1 + p2); } - return (gjk.status == details::GJK::Collision); + return (gjk.status == details::GJK::Collision || + gjk.status == details::GJK::CollisionWithPenetrationInformation); } //// @brief intersection checking between one shape and a triangle with @@ -106,7 +107,8 @@ struct HPP_FCL_DLLAPI GJKSolver { runGJKAndEPA(s, tf1, tri, tf_1M2, distance, compute_penetration, p1, p2, normal, relative_transformation_already_computed); HPP_FCL_UNUSED_VARIABLE(gjk_and_epa_ran_successfully); - return (gjk.status == details::GJK::Collision); + return (gjk.status == details::GJK::Collision || + gjk.status == details::GJK::CollisionWithPenetrationInformation); } /// @brief distance computation between two shapes.