Misc gjk epa

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)

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
@@ -221,7 +222,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),

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

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
 

include/hpp/fcl/narrowphase/gjk.h

@@ -164,12 +164,22 @@ 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.
+  /// 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.
-  /// 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,
+    NoCollisionEarlyStopped,
+    NoCollision,
+    CollisionWithPenetrationInformation,
+    Collision
+  };
 
  public:
   FCL_REAL distance_upper_bound;
@@ -250,19 +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; }
-
-  /// 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();
-  }
+  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;
@@ -278,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; }
@@ -503,12 +505,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.