Adding scalar-dependent tolerances to GJK solvers

include/fcl/math/constants.h

@@ -39,17 +39,141 @@
 
 #include "fcl/common/types.h"
 
-namespace fcl
-{
+#include <limits>
+#include <cmath>
+
+namespace fcl {
+
+namespace detail {
+
+// Helper struct for determining the underlying numerical type of scalars.
+// Allows us to treat AutoDiffScalar<double> and double as double type and
+// AutoDiffScalar<float> and float as float type.
+template<typename S>
+struct ScalarTrait {
+  // NOTE: This relies on AutoDiffScalar's `Real` class member and serves as
+  // an entry path for any custom scalar class that likewise defines a `Real`
+  // class member.
+  typedef typename S::Real type;
+};
 
+template<>
+struct ScalarTrait<long double> {
+  typedef long double type;
+};
+
+template<>
+struct ScalarTrait<double> {
+  typedef double type;
+};
+
+template<>
+struct ScalarTrait<float> {
+  typedef float type;
+};
+
+}  // namespace detail
+
+/// A collection of scalar-dependent constants. This provides the ability to
+/// get mathematical constants and tolerance values that are appropriately
+/// scaled and typed to the scalar type `S`.
+///
+/// Constants `pi()` and `phi()` are returned in the literal scalar type `S`.
+/// In other words, if `S` is an `AutoDiffScalar<...>`, then the value of `pi`
+/// and `phi` are likewise `AutoDiffScalar<...>` typed.
+///
+/// Tolerances (e.g., `eps()` and its variants) are always provided in the
+/// scalar's numerical representation. In other words, if `S` is a `double` or
+/// `float`, the tolerances are given as `double` and `float`, respectively.
+/// For `AutoDiffScalar` it is more interesting. The `AutoDiffScalar` has an
+/// underlying numerical representation (e.g.,
+/// `AutoDiffScalar<Matrix<double, 1, 3>>` has a double). It is the type of this
+/// underlying numerical representation that is provided by the tolerance
+/// functions.
+///
+/// This is designed to specifically work with `float`, `double`, `long double`,
+/// and corresponding `AutoDiffScalar` types. However, custom scalars will also
+/// work provided that the scalar type provides a class member type `Real`
+/// which must be one of `long double`, `double`, or `float`. E.g.,
+///
+/// ```
+/// struct MyScalar {
+///  public:
+///   typedef double Real;
+///   ...
+/// };
+/// ```
+///
+/// @note The tolerance values provided are defined so as to provide varying
+/// precision that *scales* with the underlying numerical type. The
+/// following contrast will make it clear.
+/// ```
+/// S local_eps = 10 * std::numeric_limit<S>::epsilon();  // DON'T DO THIS!
+/// ```
+/// The above example shows a common but incorrect method for defining a local
+/// epsilon. It defines it as being an order of magnitude larger (base 10) than
+/// the machine epsilon for `S`. However, if `S` is a float, its epsilon is
+/// essentially 1e-7. A full decimal digit of precision is 1/7th of the
+/// available digits. In contrast, double epsilon is approximately 2e-16.
+/// Throwing away a digit there reduces the precision by only 1/16th. This
+/// technique disproportionately punishes lower-precision numerical
+/// representations. Instead, by raising epsilon to a fractional power, we
+/// *scale* the precision. Roughly, `ε^(1/2)` gives us half the
+/// precision (3.5e-4 for floats and 1.5e-8 for doubles). Similarly powers of
+/// 3/4 and 7/8 gives us three quarters and 7/8ths of the bits of precision. By
+/// defining tolerances in this way, one can get some fraction of machine
+/// precision, regardless of the choice of numeric type.
+///
+/// \tparam S The scalar type for which constant values will be retrieved.
 template <typename S>
 struct FCL_EXPORT constants
 {
-/// The mathematical constant pi
+typedef typename detail::ScalarTrait<S>::type Real;
+
+/// The mathematical constant pi.
 static constexpr S pi() { return S(3.141592653589793238462643383279502884197169399375105820974944592L); }
 
-/// The golden ratio
+/// The golden ratio.
 static constexpr S phi() { return S(1.618033988749894848204586834365638117720309179805762862135448623L); }
+
+/// Defines the default accuracy for gjk and epa tolerance. It is defined as
+/// ε^(7/8) -- where ε is the machine precision epsilon for
+/// the in-use Real. The value is a much smaller epsilon for doubles than
+/// for floats (2e-14 vs 9e-7, respectively). The choice of ε^(7/8) as the
+/// default GJK tolerance reflects a tolerance that is a *slightly* tighter
+/// bound than the historical value of 1e-6 used for 32-bit floats.
+static Real gjk_default_tolerance() {
+  static const Real value = eps_78();
+  return value;
+}
+
+/// Returns ε for the precision of the underlying scalar type.
+static Real eps() {
+  static_assert(std::is_floating_point<Real>::value,
+                "Constants can only be evaluated for scalars with floating "
+                "point implementations");
+  static const Real value = std::numeric_limits<Real>::epsilon();
+  return value;
+}
+
+/// Returns ε^(7/8) for the precision of the underlying scalar type.
+static Real eps_78() {
+  static const Real value = std::pow(eps(), 7./8.);
+  return value;
+}
+
+/// Returns ε^(3/4) for the precision of the underlying scalar type.
+static Real eps_34() {
+  static const Real value = std::pow(eps(), 3./4.);
+  return value;
+}
+
+/// Returns ε^(1/2) for the precision of the underlying scalar type.
+static Real eps_12() {
+  static const Real value = std::pow(eps(), 1./2.);
+  return value;
+}
+
 };
 
 using constantsf = constants<float>;

include/fcl/narrowphase/detail/gjk_solver_indep-inl.h

@@ -948,11 +948,11 @@ template <typename S>
 GJKSolver_indep<S>::GJKSolver_indep()
 {
   gjk_max_iterations = 128;
-  gjk_tolerance = 1e-6;
+  gjk_tolerance = constants<S>::gjk_default_tolerance();
   epa_max_face_num = 128;
   epa_max_vertex_num = 64;
   epa_max_iterations = 255;
-  epa_tolerance = 1e-6;
+  epa_tolerance = constants<S>::gjk_default_tolerance();
   enable_cached_guess = false;
   cached_guess = Vector3<S>(1, 0, 0);
 }

include/fcl/narrowphase/detail/gjk_solver_libccd-inl.h

@@ -902,7 +902,7 @@ GJKSolver_libccd<S>::GJKSolver_libccd()
 {
   max_collision_iterations = 500;
   max_distance_iterations = 1000;
-  collision_tolerance = 1e-6;
+  collision_tolerance = constants<S>::gjk_default_tolerance();
   distance_tolerance = 1e-6;
 }
 

test/CMakeLists.txt

@@ -48,6 +48,7 @@ set(tests
     test_fcl_capsule_capsule.cpp
     test_fcl_cylinder_half_space.cpp
     test_fcl_collision.cpp
+    test_fcl_constant_eps.cpp
     test_fcl_distance.cpp
     test_fcl_frontlist.cpp
     test_fcl_general.cpp

test/test_fcl_constant_eps.cpp

@@ -0,0 +1,120 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2018, Toyota Research Institute
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Open Source Robotics Foundation nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** @author Sean Curtis */
+
+#include "fcl/math/constants.h"
+
+#include <cmath>
+#include <limits>
+
+#include <gtest/gtest.h>
+#include <unsupported/Eigen/AutoDiff>
+
+namespace fcl {
+namespace {
+
+// Some autodiff helpers
+template <typename S>
+using Vector2 = Eigen::Matrix<S, 2, 1>;
+template <typename S>
+using AutoDiff2 = Eigen::AutoDiffScalar<Vector2<S>>;
+
+// Utility function for confirming that the value returned by `constants` is
+// the expected value based on the scalar type.
+template<typename S>
+void expect_eps_values(const char* type_name) {
+  static_assert(std::is_floating_point<S>::value,
+                "Only use this helper for float and double types");
+  S expected_eps = std::numeric_limits<S>::epsilon();
+  // This is *explicitly* testing for perfect bit equivalency. The two values
+  // should be absolutely the same.
+  EXPECT_EQ(constants<S>::eps(), expected_eps) << "Failed for " << type_name;
+  EXPECT_EQ(std::pow(expected_eps, S(0.5)), constants<S>::eps_12())
+            << "Failed for " << type_name;
+  EXPECT_EQ(std::pow(expected_eps, S(0.75)), constants<S>::eps_34())
+            << "Failed for " << type_name;
+  EXPECT_EQ(std::pow(expected_eps, S(0.875)), constants<S>::eps_78())
+            << "Failed for " << type_name;
+}
+
+// Test that the values returned are truly a function of the precision of the
+// underlying type.
+GTEST_TEST(FCL_CONSTANTS_EPS, precision_dependent) {
+  expect_eps_values<double>("double");
+  expect_eps_values<float>("float");
+  // Double check that the float value and double values are *not* equal.
+  EXPECT_NE(constantsd::eps(), constantsf::eps());
+  EXPECT_NE(constantsd::eps_12(), constantsf::eps_12());
+  EXPECT_NE(constantsd::eps_34(), constantsf::eps_34());
+  EXPECT_NE(constantsd::eps_78(), constantsf::eps_78());
+}
+
+template <typename S> void expect_autodiff_constants(const char *type_name) {
+  EXPECT_TRUE((std::is_same<decltype(constants<AutoDiff2<S>>::pi()),
+                            AutoDiff2<S>>::value))
+      << "Failed for " << type_name;
+  EXPECT_TRUE((std::is_same<decltype(constants<AutoDiff2<S>>::phi()),
+                            AutoDiff2<S>>::value))
+      << "Failed for " << type_name;
+  EXPECT_TRUE(
+      (std::is_same<decltype(constants<AutoDiff2<S>>::eps()), S>::value))
+      << "Failed for " << type_name;
+  EXPECT_TRUE(
+      (std::is_same<decltype(constants<AutoDiff2<S>>::eps_78()), S>::value))
+      << "Failed for " << type_name;
+  EXPECT_TRUE(
+      (std::is_same<decltype(constants<AutoDiff2<S>>::eps_34()), S>::value))
+      << "Failed for " << type_name;
+  EXPECT_TRUE(
+      (std::is_same<decltype(constants<AutoDiff2<S>>::eps_12()), S>::value))
+      << "Failed for " << type_name;
+}
+
+// Test the types returned by constants. pi and phi should return autodiff, but
+// the tolerances should return real types.
+GTEST_TEST(FCL_CONSTANTS_EPS, autodiff_compatibility) {
+  expect_autodiff_constants<double>("double");
+  expect_autodiff_constants<float>("float");
+}
+
+}  // namespace
+}  // namespace fcl
+
+//==============================================================================
+int main(int argc, char* argv[])
+{
+  ::testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}

test/test_fcl_geometric_shapes.cpp

@@ -464,7 +464,7 @@ void testShapeIntersection(
     std::cerr << "Invalid GJK solver. Test aborted." << std::endl;
     return;
   }
-  EXPECT_EQ(res, expected_res);
+  EXPECT_EQ(expected_res, res);
 
   // Check contact information as well
   if (solver_type == GST_LIBCCD)
@@ -480,7 +480,7 @@ void testShapeIntersection(
     std::cerr << "Invalid GJK solver. Test aborted." << std::endl;
     return;
   }
-  EXPECT_EQ(res, expected_res);
+  EXPECT_EQ(expected_res, res);
   if (expected_res)
   {
     EXPECT_TRUE(inspectContactPointds(s1, tf1, s2, tf2, solver_type,
@@ -497,13 +497,13 @@ void testShapeIntersection(
   request.enable_contact = false;
   result.clear();
   res = (collide(&s1, tf1, &s2, tf2, request, result) > 0);
-  EXPECT_EQ(res, expected_res);
+  EXPECT_EQ(expected_res, res);
 
   // Check contact information as well
   request.enable_contact = true;
   result.clear();
   res = (collide(&s1, tf1, &s2, tf2, request, result) > 0);
-  EXPECT_EQ(res, expected_res);
+  EXPECT_EQ(expected_res, res);
   if (expected_res)
   {
     getContactPointdsFromResult(actual_contacts, result);
@@ -4259,7 +4259,31 @@ void test_shapeIntersectionGJK_spherebox()
   tf2 = Transform3<S>(Translation3<S>(Vector3<S>(22.5, 0, 0)));
   contacts.resize(1);
   contacts[0].normal << 1, 0, 0;
-  testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false, false, true, false, 1e-7);  // built-in GJK solver requires larger tolerance than libccd
+  // TODO(SeanCurtis-TRI): This osculating contact is considered a collision
+  // only for a relatively "loose" gjk tolerance. So, for this test to pass, we
+  // set and restore the default gjk tolerances. We need to determine if this is
+  // the correct/desired behavior and, if so, fix the defect that smaller
+  // tolerances prevent this from reporting as a contact. NOTE: this is *not*
+  // the same tolerance that is passed into the `testShapeIntersection` function
+  // -- which isn't actually *used*.
+  {
+    detail::GJKSolver_indep<S>& solver = solver2<S>();
+    const S old_gjk_tolerance = solver.gjk_tolerance;
+    const S old_epa_tolerance = solver.epa_tolerance;
+
+    // The historical tolerances for which this test passes.
+    solver.gjk_tolerance = 1e-6;
+    solver.epa_tolerance = 1e-6;
+
+    testShapeIntersection(s1, tf1, s2, tf2, GST_INDEP, true, contacts, false,
+                          false, true, false,
+                          1e-7 // built-in GJK solver requires larger tolerance than libccd
+    );
+    // TODO(SeanCurtis-TRI): If testShapeIntersection fails an *assert* this
+    // code will not get fired off and the static solvers will not get reset.
+    solver.gjk_tolerance = old_gjk_tolerance;
+    solver.epa_tolerance = old_epa_tolerance;
+  }
 
   tf1 = transform;
   tf2 = transform * Transform3<S>(Translation3<S>(Vector3<S>(22.51, 0, 0)));