diff --git a/include/fcl/narrowphase/collision-inl.h b/include/fcl/narrowphase/collision-inl.h index a7442835d..81dc22d5f 100644 --- a/include/fcl/narrowphase/collision-inl.h +++ b/include/fcl/narrowphase/collision-inl.h @@ -159,11 +159,14 @@ std::size_t collide(const CollisionObject* o1, const CollisionObject* o2, case GST_LIBCCD: { detail::GJKSolver_libccd solver; + solver.collision_tolerance = request.gjk_tolerance; return collide(o1, o2, &solver, request, result); } case GST_INDEP: { detail::GJKSolver_indep 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 solver; + solver.collision_tolerance = request.gjk_tolerance; return collide(o1, tf1, o2, tf2, &solver, request, result); } case GST_INDEP: { detail::GJKSolver_indep solver; + solver.gjk_tolerance = request.gjk_tolerance; + solver.epa_tolerance = request.gjk_tolerance; return collide(o1, tf1, o2, tf2, &solver, request, result); } default: diff --git a/include/fcl/narrowphase/collision_request-inl.h b/include/fcl/narrowphase/collision_request-inl.h index 99a4c772b..eb779014a 100644 --- a/include/fcl/narrowphase/collision_request-inl.h +++ b/include/fcl/narrowphase/collision_request-inl.h @@ -57,7 +57,8 @@ CollisionRequest::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_), @@ -65,15 +66,15 @@ CollisionRequest::CollisionRequest( use_approximate_cost(use_approximate_cost_), gjk_solver_type(gjk_solver_type_), enable_cached_gjk_guess(false), - cached_gjk_guess(Vector3::UnitX()) + cached_gjk_guess(Vector3::UnitX()), + gjk_tolerance(gjk_tolerance_) { // Do nothing } //============================================================================== template -bool CollisionRequest::isSatisfied( - const CollisionResult& result) const +bool CollisionRequest::isSatisfied(const CollisionResult& result) const { return (!enable_cost) && result.isCollision() diff --git a/include/fcl/narrowphase/collision_request.h b/include/fcl/narrowphase/collision_request.h index dde2aa58d..5d4da3777 100644 --- a/include/fcl/narrowphase/collision_request.h +++ b/include/fcl/narrowphase/collision_request.h @@ -47,40 +47,60 @@ namespace fcl template struct CollisionResult; -/// @brief request to the collision algorithm +/// @brief Parameters for performing collision request template 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::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>. + /// @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 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& result) const; }; diff --git a/test/test_fcl_collision.cpp b/test/test_fcl_collision.cpp index 86b373f1d..0f39fbc33 100644 --- a/test/test_fcl_collision.cpp +++ b/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 +void CollisionRequestGjkToleranceTest() { + typedef typename Eigen::NumTraits::Real Real; + + using GeometryPtr_t = std::shared_ptr>; + + const S pi = fcl::constants::pi(); + const Real radius = Real(1.0); + const Real size = Real(1.0); + const Real depth = Real(1e-7); + const Vector3 contact_point{0, 0, radius - Real(0.5) * depth}; + + GeometryPtr_t box_geometry(new fcl::Box(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 box_pose{AngleAxis(-pi / 4, Vector3::UnitY()) * + AngleAxis(pi / 4, Vector3::UnitX())}; + Vector3 corner{-size / 2, -size / 2, -size / 2}; + Vector3 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::dummy_precision()); + EXPECT_NEAR(rotated_corner(1), 0, Eigen::NumTraits::dummy_precision()); + EXPECT_NEAR(rotated_corner(2), radius - depth, + Eigen::NumTraits::dummy_precision()); + fcl::CollisionObject box(box_geometry, box_pose); + + GeometryPtr_t sphere_geometry(new fcl::Sphere(1)); + fcl::CollisionObject sphere(sphere_geometry, Transform3::Identity()); + + auto test_tolerance = [&box, &sphere] (fcl::GJKSolverType solver_type) { + fcl::CollisionRequest 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::epsilon(), 0.25); + // 7/8 of the Real's bits in precision (7/8 = 0.875). + const Real tight_tolerance = + std::pow(Eigen::NumTraits::epsilon(), 0.875); + + request.gjk_tolerance = loose_tolerance; + fcl::CollisionResult result_low; + fcl::collide(&box, &sphere, request, result_low); + + request.gjk_tolerance = tight_tolerance; + fcl::CollisionResult 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& pos_low = result_low.getContact(0).pos; + const Vector3& 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(); + // NOTE: FCL doesn't build for float. +// CollisionRequestGjkToleranceTest(); +} + GTEST_TEST(FCL_COLLISION, OBB_Box_test) { // test_OBB_Box_test();