Adding scalar-dependent tolerances to GJK solvers

1. Extends fcl::constants to have scalar-dependent tolerances
  a. Documentation
  b. Tests
2. Modify default constructors for solvers to use default values

include/fcl/math/constants.h

@@ -39,17 +39,132 @@
 
 #include "fcl/common/types.h"
 
+#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 {
+  typedef typename S::Real 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 constant 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.
+///
+/// Generally, this is designed to work with `float`, `double`, `long double`,
+/// and `AutoDiffScalar`. 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();
+/// ```
+/// The above example shows a common basis for defining a local epsilon. It
+/// defines it as being an order of magnitude larger than the machine epsilon
+/// for `S`. However, if `S` is a float, it's machine precision is essentially
+/// 1e-7. A full decimal digit of precision is 1/7th of the available digits.
+/// In contrast, double machine precision 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 taking the machine precision, epsilon, and raising it to
+/// fractional values, we *scale* the precision. Roughly, `pow(epsilon, 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.
+///
+/// \tparam S The scalar type for which constant values will be retrieved.
 template <typename S>
 struct FCL_EXPORT constants
 {
+typedef typename detail::ScalarTrait<S>::type RealType;
+
 /// The mathematical constant pi
 static constexpr S pi() { return S(3.141592653589793238462643383279502884197169399375105820974944592L); }
 
 /// The golden ratio
 static constexpr S phi() { return S(1.618033988749894848204586834365638117720309179805762862135448623L); }
+
+/// Defines the default accuracy for gjk and epa tolerance. It is defined as
+/// pow(epsilon, 7/8) -- where epsilon is the machine precision epsilon. This
+/// value reflects a *slightly* tighter bound than the historical value of 1e-6
+/// used for 32-bit floats.
+static RealType gjk_default_tolerance() {
+  static const RealType value = eps_78();
+  return value;
+}
+
+/// Returns ε for the precision of the underlying scalar type.
+static RealType eps() {
+  static_assert(std::is_floating_point<RealType>::value,
+                "Constants can only be evaluated for scalars with floating "
+                "point implementations");
+  static const RealType value = std::numeric_limits<RealType>::epsilon();
+  return value;
+}
+
+/// Returns pow(ε, 7/8) for the precision of the underlying scalar type.
+static RealType eps_78() {
+  static const RealType value = std::pow(eps(), 7./8.);
+  return value;
+}
+
+/// Returns pow(ε, 3/4) for the precision of the underlying scalar type.
+static RealType eps_34() {
+  static const RealType value = std::pow(eps(), 3./4.);
+  return value;
+}
+
+/// Returns pow(ε, 1/2) for the precision of the underlying scalar type.
+static RealType eps_12() {
+  static const RealType 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,118 @@
+/*
+ * 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();
+  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();
+}