Adds gjk tolerance to the CollisionRequest

Sets the tolerance in the request and propagate it through to the solvers.
NOTE: This does *not* provide the same functionality for continuous
collision request.

include/fcl/narrowphase/collision-inl.h

@@ -159,11 +159,14 @@ std::size_t collide(const CollisionObject<S>* o1, const CollisionObject<S>* o2,
   case GST_LIBCCD:
     {
       detail::GJKSolver_libccd<S> solver;
+      solver.collision_tolerance = request.gjk_tolerance;
       return collide(o1, o2, &solver, request, result);
     }
   case GST_INDEP:
     {
       detail::GJKSolver_indep<S> solver;
+      solver.gjk_tolerance = request.gjk_tolerance;
+      solver.epa_tolerance = request.gjk_tolerance;
       return collide(o1, o2, &solver, request, result);
     }
   default:
@@ -187,11 +190,14 @@ std::size_t collide(
   case GST_LIBCCD:
     {
       detail::GJKSolver_libccd<S> solver;
+      solver.collision_tolerance = request.gjk_tolerance;
       return collide(o1, tf1, o2, tf2, &solver, request, result);
     }
   case GST_INDEP:
     {
       detail::GJKSolver_indep<S> solver;
+      solver.gjk_tolerance = request.gjk_tolerance;
+      solver.epa_tolerance = request.gjk_tolerance;
       return collide(o1, tf1, o2, tf2, &solver, request, result);
     }
   default:

include/fcl/narrowphase/collision_request-inl.h

@@ -57,23 +57,24 @@ CollisionRequest<S>::CollisionRequest(
     size_t num_max_cost_sources_,
     bool enable_cost_,
     bool use_approximate_cost_,
-    GJKSolverType gjk_solver_type_)
+    GJKSolverType gjk_solver_type_,
+    Real gjk_tolerance_)
   : num_max_contacts(num_max_contacts_),
     enable_contact(enable_contact_),
     num_max_cost_sources(num_max_cost_sources_),
     enable_cost(enable_cost_),
     use_approximate_cost(use_approximate_cost_),
     gjk_solver_type(gjk_solver_type_),
     enable_cached_gjk_guess(false),
-    cached_gjk_guess(Vector3<S>::UnitX())
+    cached_gjk_guess(Vector3<S>::UnitX()),
+    gjk_tolerance(gjk_tolerance_)
 {
   // Do nothing
 }
 
 //==============================================================================
 template <typename S>
-bool CollisionRequest<S>::isSatisfied(
-    const CollisionResult<S>& result) const
+bool CollisionRequest<S>::isSatisfied(const CollisionResult<S>& result) const
 {
   return (!enable_cost)
       && result.isCollision()

include/fcl/narrowphase/collision_request.h

@@ -47,40 +47,60 @@ namespace fcl
 template <typename S>
 struct CollisionResult;
 
-/// @brief request to the collision algorithm
+/// @brief Parameters for performing collision request
 template <typename S>
 struct FCL_EXPORT CollisionRequest
-{  
-  /// @brief The maximum number of contacts will return
+{
+  /// The underlying numerical representation of the request's scalar (e.g.,
+  /// float or double).
+  typedef typename Eigen::NumTraits<S>::Real Real;
+
+  /// @brief The maximum number of contacts that can be returned.
   size_t num_max_contacts;
 
-  /// @brief whether the contact information (normal, penetration depth and contact position) will return
+  /// @brief If true, contact information (e.g., normal, penetration depth, and
+  /// contact position) will be returned.
   bool enable_contact;
 
-  /// @brief The maximum number of cost sources will return
+  // TODO(SeanCurtis-TRI): Provide clear definitions for what "cost sources"
+  // are.
+
+  /// @brief The maximum number of cost sources that can be returned.
   size_t num_max_cost_sources;
 
-  /// @brief whether the cost sources will be computed
+  /// @brief If true, the cost sources will be computed.
   bool enable_cost;
 
-  /// @brief whether the cost computation is approximated
+  /// @brief If true, the cost computation is approximated (if computed).
   bool use_approximate_cost;
 
-  /// @brief narrow phase solver
+  /// @brief Enumeration indicating the GJK solver implementation to use.
   GJKSolverType gjk_solver_type;
 
-  /// @brief whether enable gjk intial guess
+  // TODO(SeanCurtis-TRI): Consider swapping these *two* parameters with a
+  // single std::optional<Vector3<S>>.
+  /// @brief If true, uses the provided initial guess for the GJK algorithm.
   bool enable_cached_gjk_guess;
 
-  /// @brief the gjk intial guess set by user
+  /// @brief The initial guess to use in the GJK algorithm.
   Vector3<S> cached_gjk_guess;
 
+  // TODO(SeanCurtis-TRI): Document the implications of this tolerance; right
+  // now it is not clear *at all* what turning this knob will do to the results.
+  /// @brief Numerical tolerance to use in the GJK algorithm.
+  /// NOTE: The default value is currently set as 1e-6 to provide backwards
+  /// compatibility; historically it has been 1e-6. Future code should provide
+  /// a value that is consistent with the precision of `S`.
+  Real gjk_tolerance{1e-6};
+
+  /// @brief Default constructor
   CollisionRequest(size_t num_max_contacts_ = 1,
                    bool enable_contact_ = false,
                    size_t num_max_cost_sources_ = 1,
                    bool enable_cost_ = false,
                    bool use_approximate_cost_ = true,
-                   GJKSolverType gjk_solver_type_ = GST_LIBCCD);
+                   GJKSolverType gjk_solver_type_ = GST_LIBCCD,
+                   Real gjk_tolerance_ = 1e-6);
 
   bool isSatisfied(const CollisionResult<S>& result) const;
 };

test/test_fcl_collision.cpp

@@ -827,6 +827,108 @@ void test_mesh_mesh()
   }
 }
 
+// Tests the tolerance value in the CollisionRequest, confirming that it
+// propagates down to the solution. It creates a box and sphere and positions
+// them so that they *barely* intersect and show that we get different answers
+// when changing the collision request's gjk_tolerance value.
+//
+// The unit sphere is located at the origin. The box is rotated and positioned
+// so that a single corner most deeply penetrates into the sphere from the +z
+// direction.
+//
+// The point of this test is *not* to prove that the collision algorithm is
+// correct. But merely to show that different tolerances produce different
+// answers (i.e., the tolerances are getting through). The answer based on a
+// smaller tolerance may or may not be a better answer; that is the subject of
+// other unit tests.
+//
+// The test is formulated this way because in the disparity in behavior between
+// the ccd and indep GJK implementations. They differ enough in response to
+// tolerance, that a test that actually tested for convergence to "truth"
+// became unwieldy. (Specifically, it seems the ccd implementation's answers
+// would converge to an answer that differed from the *real* answer by 1e-8 or
+// half the precision that I would've hoped for.
+// TODO(SeanCurtis-TRI): Create test that confirms that smaller tolerances lead
+// to better answers.
+template <typename S>
+void CollisionRequestGjkToleranceTest() {
+  typedef typename Eigen::NumTraits<S>::Real Real;
+
+  using GeometryPtr_t = std::shared_ptr<fcl::CollisionGeometry<S>>;
+
+  const S pi = fcl::constants<S>::pi();
+  const Real radius = Real(1.0);
+  const Real size = Real(1.0);
+  const Real depth = Real(1e-7);
+  const Vector3<S> contact_point{0, 0, radius - Real(0.5) * depth};
+
+  GeometryPtr_t box_geometry(new fcl::Box<S>(size, size, size));
+  // Rotate the box twice. The first time brings the edge into contact with the
+  // sphere. The second brings the corner into contact. Then position it so
+  // that the corner lies at the position (0, 0, Z), where Z = radius - depth.
+  Transform3<S> box_pose{AngleAxis<S>(-pi / 4, Vector3<S>::UnitY()) *
+      AngleAxis<S>(pi / 4, Vector3<S>::UnitX())};
+  Vector3<Real> corner{-size / 2, -size / 2, -size / 2};
+  Vector3<Real> rotated_corner = box_pose.linear() * corner;
+  box_pose.translation() << -rotated_corner(0),
+      -rotated_corner(1), -rotated_corner(2) + radius - depth;
+  rotated_corner = box_pose * corner;
+  // Confirm corner is where it is expected.
+  EXPECT_NEAR(rotated_corner(0), 0, Eigen::NumTraits<S>::dummy_precision());
+  EXPECT_NEAR(rotated_corner(1), 0, Eigen::NumTraits<S>::dummy_precision());
+  EXPECT_NEAR(rotated_corner(2), radius - depth,
+              Eigen::NumTraits<S>::dummy_precision());
+  fcl::CollisionObject<S> box(box_geometry, box_pose);
+
+  GeometryPtr_t sphere_geometry(new fcl::Sphere<S>(1));
+  fcl::CollisionObject<S> sphere(sphere_geometry, Transform3<S>::Identity());
+
+  auto test_tolerance = [&box, &sphere] (fcl::GJKSolverType solver_type) {
+    fcl::CollisionRequest<S> request;
+    request.num_max_contacts = 1;
+    request.enable_contact = true;
+    request.gjk_solver_type = solver_type;
+
+    // 1/4 of the Real's bits in precision.
+    const Real loose_tolerance = std::pow(Eigen::NumTraits<S>::epsilon(), 0.25);
+    // 7/8 of the Real's bits in precision (7/8 = 0.875).
+    const Real tight_tolerance =
+        std::pow(Eigen::NumTraits<S>::epsilon(), 0.875);
+
+    request.gjk_tolerance = loose_tolerance;
+    fcl::CollisionResult<S> result_low;
+    fcl::collide(&box, &sphere, request, result_low);
+
+    request.gjk_tolerance = tight_tolerance;
+    fcl::CollisionResult<S> result_high;
+    fcl::collide(&box, &sphere, request, result_high);
+
+    if (result_low.numContacts() == result_high.numContacts()) {
+      // If there are *no* contacts reported, differences cannot be detected.
+      GTEST_ASSERT_EQ(1u, result_low.numContacts());
+      const Vector3<S>& pos_low = result_low.getContact(0).pos;
+      const Vector3<S>& pos_high = result_high.getContact(0).pos;
+      // Doing literal bit-wise comparisons. Different bits is sufficient
+      // evidence to prove difference.
+      bool is_same = pos_low(0) == pos_high(0) &&
+          pos_low(1) == pos_high(1) &&
+          pos_low(2) == pos_high(2);
+      EXPECT_FALSE(is_same);
+    }
+    // If the number of contacts don't match, then *clearly* the tests have
+    // produced different results and the tolerance made a difference.
+  };
+
+  test_tolerance(fcl::GJKSolverType::GST_INDEP);
+  test_tolerance(fcl::GJKSolverType::GST_LIBCCD);
+}
+
+GTEST_TEST(FCL_COLLISION, CollisionRequestGjkTolerance) {
+  CollisionRequestGjkToleranceTest<double>();
+  // NOTE: FCL doesn't build for float.
+//  CollisionRequestGjkToleranceTest<float>();
+}
+
 GTEST_TEST(FCL_COLLISION, OBB_Box_test)
 {
 //  test_OBB_Box_test<float>();