From 81b3d02911e891bfb0d262e0ac129b42efd3791b Mon Sep 17 00:00:00 2001 From: Louis Montaut Date: Tue, 19 Mar 2024 23:54:48 +0100 Subject: [PATCH 1/4] GJK/EPA: fix epsilon in asserts --- include/hpp/fcl/narrowphase/narrowphase.h | 12 ++++++------ src/narrowphase/gjk.cpp | 13 ++++++------- 2 files changed, 12 insertions(+), 13 deletions(-) diff --git a/include/hpp/fcl/narrowphase/narrowphase.h b/include/hpp/fcl/narrowphase/narrowphase.h index f7fb7ac3c..ef143ea62 100644 --- a/include/hpp/fcl/narrowphase/narrowphase.h +++ b/include/hpp/fcl/narrowphase/narrowphase.h @@ -199,8 +199,8 @@ struct HPP_FCL_DLLAPI GJKSolver { } HPP_FCL_COMPILER_DIAGNOSTIC_POP - static constexpr FCL_REAL dummy_precision = - std::numeric_limits::epsilon() * 100; + const FCL_REAL dummy_precision = + 3 * std::sqrt(std::numeric_limits::epsilon()); HPP_FCL_UNUSED_VARIABLE(dummy_precision); switch (gjk.status) { case details::GJK::DidNotRun: @@ -410,8 +410,8 @@ struct HPP_FCL_DLLAPI GJKSolver { // In any case, `gjk.ray`'s norm is bigger than GJK's tolerance and thus // it can safely be normalized. distance = gjk.distance; - static constexpr FCL_REAL dummy_precision = - std::numeric_limits::epsilon() * 100; + const FCL_REAL dummy_precision = + 3 * std::sqrt(std::numeric_limits::epsilon()); HPP_FCL_UNUSED_VARIABLE(dummy_precision); HPP_FCL_ASSERT( gjk.ray.norm() > gjk.getTolerance() + dummy_precision, @@ -441,8 +441,8 @@ struct HPP_FCL_DLLAPI GJKSolver { FCL_REAL& distance, Vec3f& p1, Vec3f& p2, Vec3f& normal) const { - static constexpr FCL_REAL dummy_precision = - std::numeric_limits::epsilon() * 100; + const FCL_REAL dummy_precision = + 3 * std::sqrt(std::numeric_limits::epsilon()); HPP_FCL_UNUSED_VARIABLE(dummy_precision); HPP_FCL_ASSERT(gjk.distance <= gjk.getTolerance() + dummy_precision, "The distance should be lower than GJK's tolerance.", diff --git a/src/narrowphase/gjk.cpp b/src/narrowphase/gjk.cpp index fe3ddd257..468ae26f3 100644 --- a/src/narrowphase/gjk.cpp +++ b/src/narrowphase/gjk.cpp @@ -1120,10 +1120,8 @@ bool GJK::projectTetrahedraOrigin(const Simplex& current, Simplex& next) { const Vec3f a_cross_b = A.cross(B); const Vec3f a_cross_c = A.cross(C); - // dummy_precision is 1e-14 if FCL_REAL is double - // 1e-5 if FCL_REAL is float - const FCL_REAL dummy_precision(100 * - std::numeric_limits::epsilon()); + const FCL_REAL dummy_precision( + 3 * std::sqrt(std::numeric_limits::epsilon())); HPP_FCL_UNUSED_VARIABLE(dummy_precision); #define REGION_INSIDE() \ @@ -1747,7 +1745,8 @@ EPA::Status EPA::evaluate(GJK& gjk, const Vec3f& guess) { // the support we just added is in the direction of the normal of // the closest_face. Therefore, the support point will **always** // lie "after" the closest_face, i.e closest_face.n.dot(w.w) > 0. - assert(closest_face->n.dot(w.w) > 0 && + assert(closest_face->n.dot(w.w) > + -3 * std::sqrt(std::numeric_limits::epsilon()) && "The support is not in the right direction."); // // 1) First check: `fdist` (see below) is an upper bound of how much @@ -1921,8 +1920,8 @@ bool EPA::expand(size_t pass, const SimplexVertex& w, SimplexFace* f, size_t e, // recursive nature of `expand`, it is safer to go through the first case. // This is because `expand` can potentially loop indefinitly if the // Minkowski difference is very flat (hence the check above). - const FCL_REAL dummy_precision(100 * - std::numeric_limits::epsilon()); + const FCL_REAL dummy_precision( + 3 * std::sqrt(std::numeric_limits::epsilon())); const SimplexVertex& vf = sv_store[f->vertex_id[e]]; if (f->n.dot(w.w - vf.w) < dummy_precision) { // case 1: the support point is "below" `f`. From 94fabb0eab77cdf9d80276830e9e55b6259828d0 Mon Sep 17 00:00:00 2001 From: Louis Montaut Date: Wed, 20 Mar 2024 00:00:58 +0100 Subject: [PATCH 2/4] Update changelog --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 7ca89b6e2..cfef96414 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -5,6 +5,7 @@ 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] +- 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) - Optimize EPA: ignore useless faces in EPA's polytope; warm-start support computation for `Convex`; fix edge-cases witness points computation. From 18cf6e28ff4736c8a5d4620c70357eaaad068177 Mon Sep 17 00:00:00 2001 From: Louis Montaut Date: Wed, 20 Mar 2024 01:20:03 +0100 Subject: [PATCH 3/4] EPA: fix assert tolerance --- src/narrowphase/gjk.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/narrowphase/gjk.cpp b/src/narrowphase/gjk.cpp index 468ae26f3..6f880b670 100644 --- a/src/narrowphase/gjk.cpp +++ b/src/narrowphase/gjk.cpp @@ -1745,9 +1745,10 @@ EPA::Status EPA::evaluate(GJK& gjk, const Vec3f& guess) { // the support we just added is in the direction of the normal of // the closest_face. Therefore, the support point will **always** // lie "after" the closest_face, i.e closest_face.n.dot(w.w) > 0. - assert(closest_face->n.dot(w.w) > - -3 * std::sqrt(std::numeric_limits::epsilon()) && - "The support is not in the right direction."); + if (iterations > 0) { + assert(closest_face->n.dot(w.w) > -tolerance && + "The support is not in the right direction."); + } // // 1) First check: `fdist` (see below) is an upper bound of how much // more penetration depth we can expect to "gain" by adding `w` to EPA's From bd5f79715e1f801fe1a62f90d2b54d8ff1de3424 Mon Sep 17 00:00:00 2001 From: Louis Montaut Date: Wed, 20 Mar 2024 01:34:20 +0100 Subject: [PATCH 4/4] GJK: fix minus sign in assert --- include/hpp/fcl/narrowphase/narrowphase.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/hpp/fcl/narrowphase/narrowphase.h b/include/hpp/fcl/narrowphase/narrowphase.h index ef143ea62..7cdb874b4 100644 --- a/include/hpp/fcl/narrowphase/narrowphase.h +++ b/include/hpp/fcl/narrowphase/narrowphase.h @@ -414,7 +414,7 @@ struct HPP_FCL_DLLAPI GJKSolver { 3 * std::sqrt(std::numeric_limits::epsilon()); HPP_FCL_UNUSED_VARIABLE(dummy_precision); HPP_FCL_ASSERT( - gjk.ray.norm() > gjk.getTolerance() + dummy_precision, + gjk.ray.norm() > gjk.getTolerance() - dummy_precision, "The norm of GJK's ray should be bigger than GJK's tolerance.", std::logic_error); normal.noalias() = -tf1.getRotation() * gjk.ray;