diff --git a/CHANGELOG.md b/CHANGELOG.md index 26ff90059..205d9e2d3 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -33,6 +33,8 @@ [#472](https://github.com/flexible-collision-library/fcl/pull/472) * Documentation for OcTree no longer mistakenly excluded from doxygen: [#472](https://github.com/flexible-collision-library/fcl/pull/472) + * Another failure mode in the GJK/EPA signed distance query patched: + [#494](https://github.com/flexible-collision-library/fcl/pull/494) * Build/Test/Misc diff --git a/include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h b/include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h index 106e132ad..7fee2166d 100644 --- a/include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h +++ b/include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h @@ -966,28 +966,6 @@ static bool triangle_area_is_zero(const ccd_vec3_t& a, const ccd_vec3_t& b, return false; } -/** - * Determines if the point P is on the line segment AB. - * If A, B, P are coincident, report true. - */ -static bool is_point_on_line_segment(const ccd_vec3_t& p, const ccd_vec3_t& a, - const ccd_vec3_t& b) { - if (are_coincident(a, b)) { - return are_coincident(a, p); - } - // A and B are not coincident, if the triangle ABP has non-zero area, then P - // is not on the line adjoining AB, and hence not on the line segment AB. - if (!triangle_area_is_zero(a, b, p)) { - return false; - } - // P is on the line adjoinging AB. If P is on the line segment AB, then - // PA.dot(PB) <= 0. - ccd_vec3_t PA, PB; - ccdVec3Sub2(&PA, &p, &a); - ccdVec3Sub2(&PB, &p, &b); - return ccdVec3Dot(&PA, &PB) <= 0; -} - /** * Computes the normal vector of a triangular face on a polytope, and the normal * vector points outward from the polytope. Notice we assume that the origin @@ -1691,6 +1669,8 @@ static int __ccdGJK(const void *obj1, const void *obj2, */ static void validateNearestFeatureOfPolytopeBeingEdge(ccd_pt_t* polytope) { assert(polytope->nearest_type == CCD_PT_EDGE); + + constexpr ccd_real_t kEps = constants::eps(); // Only verify the feature if the nearest feature is an edge. const ccd_pt_edge_t* const nearest_edge = @@ -1701,6 +1681,14 @@ static void validateNearestFeatureOfPolytopeBeingEdge(ccd_pt_t* polytope) { // normalized. std::array face_normals; std::array origin_to_face_distance; + + // We define the plane equation using vertex[0]. If vertex[0] is far away + // from the origin, it can magnify rounding error. We scale epsilon to account + // for this possibility. + const ccd_real_t v0_dist = + std::sqrt(ccdVec3Len2(&nearest_edge->vertex[0]->v.v)); + const ccd_real_t plane_threshold = kEps * std::max(1.0, v0_dist); + for (int i = 0; i < 2; ++i) { face_normals[i] = faceNormalPointingOutward(polytope, nearest_edge->faces[i]); @@ -1709,27 +1697,29 @@ static void validateNearestFeatureOfPolytopeBeingEdge(ccd_pt_t* polytope) { // n̂ ⋅ (o - vₑ) ≤ 0 or, with simplification, -n̂ ⋅ vₑ ≤ 0 (since n̂ ⋅ o = 0). origin_to_face_distance[i] = -ccdVec3Dot(&face_normals[i], &nearest_edge->vertex[0]->v.v); - if (origin_to_face_distance[i] > 0) { + // If the origin lies *on* the edge, then it also lies on the two adjacent + // faces. Rather than failing on strictly *positive* signed distance, we + // introduce an epsilon threshold. This usage of epsilon is to account for a + // discrepancy in the signed distance computation. How GJK (and partially + // EPA) compute the signed distance from origin to face may *not* be exactly + // the same as done in this test (i.e. for a given set of vertices, the + // plane equation can be defined various ways. Those ways are + // *mathematically* equivalent but numerically will differ due to rounding). + // We account for those differences by allowing a very small positive signed + // distance to be considered zero. We assume that the GJK/EPA code + // ultimately classifies inside/outside around *zero* and *not* epsilon. + if (origin_to_face_distance[i] > plane_threshold) { FCL_THROW_FAILED_AT_THIS_CONFIGURATION( "The origin is outside of the polytope. This should already have " "been identified as separating."); } } - // We compute the projection of the origin onto the plane as - // -face_normals[i] * origin_to_face_distance[i] - // If the projection to both faces are on the edge, the the edge is the - // closest feature. - bool is_edge_closest_feature = true; - for (int i = 0; i < 2; ++i) { - ccd_vec3_t origin_projection_to_plane = face_normals[i]; - ccdVec3Scale(&(origin_projection_to_plane), -origin_to_face_distance[i]); - if (!is_point_on_line_segment(origin_projection_to_plane, - nearest_edge->vertex[0]->v.v, - nearest_edge->vertex[1]->v.v)) { - is_edge_closest_feature = false; - break; - } - } + + // We know the reported squared distance to the edge. If that distance is + // functionally zero, then the edge must *truly* be the nearest feature. + // If it isn't, then it must be one of the adjacent faces. + const bool is_edge_closest_feature = nearest_edge->dist < kEps * kEps; + if (!is_edge_closest_feature) { // We assume that libccd is not crazily wrong. Although the closest // feature is not the edge, it is near that edge. Hence we select the @@ -1779,7 +1769,7 @@ static int __ccdEPA(const void *obj1, const void *obj2, return -2; } - while (1){ + while (1) { // get triangle nearest to origin *nearest = ccdPtNearest(polytope); if (polytope->nearest_type == CCD_PT_EDGE) { @@ -1791,14 +1781,13 @@ static int __ccdEPA(const void *obj1, const void *obj2, *nearest = ccdPtNearest(polytope); } - // get next support point - if (nextSupport(polytope, obj1, obj2, ccd, *nearest, &supp) != 0) { - break; - } + // get next support point + if (nextSupport(polytope, obj1, obj2, ccd, *nearest, &supp) != 0) { + break; + } - // expand nearest triangle using new point - supp - if (expandPolytope(polytope, *nearest, &supp) != 0) - return -2; + // expand nearest triangle using new point - supp + if (expandPolytope(polytope, *nearest, &supp) != 0) return -2; } return 0; diff --git a/test/test_fcl_signed_distance.cpp b/test/test_fcl_signed_distance.cpp index b2850a1e4..cca037ab8 100644 --- a/test/test_fcl_signed_distance.cpp +++ b/test/test_fcl_signed_distance.cpp @@ -325,12 +325,22 @@ void test_distance_box_box_helper(const Vector3& box1_size, // p_B1P1 is the position of the witness point P1 on box 1, measured // and expressed in the box 1 frame B1. const Vector3 p_B1P1 = X_WB1.inverse() * result.nearest_points[0]; - const double tol = 10 * std::numeric_limits::epsilon(); - EXPECT_TRUE((p_B1P1.array().abs() <= (box1_size / 2).array() + tol).all()); + constexpr double tol = 10 * constants::eps(); + const double tol_1 = tol * std::max(S(1), (box1_size / 2).maxCoeff()); + EXPECT_TRUE( + (p_B1P1.array().abs() <= (box1_size / 2).array() + tol_1).all()) + << "\n p_B1P1: " << p_B1P1.transpose() + << "\n box1_size / 2: " << (box1_size / 2).transpose() + << "\n tol: " << tol_1; // p_B2P2 is the position of the witness point P2 on box 2, measured // and expressed in the box 2 frame B2. + const double tol_2 = tol * std::max(S(1), (box2_size / 2).maxCoeff()); const Vector3 p_B2P2 = X_WB2.inverse() * result.nearest_points[1]; - EXPECT_TRUE((p_B2P2.array().abs() <= (box2_size / 2).array() + tol).all()); + EXPECT_TRUE( + (p_B2P2.array().abs() <= (box2_size / 2).array() + tol_2).all()) + << "\n p_B2P2: " << p_B2P2.transpose() + << "\n box2_size / 2: " << (box2_size / 2).transpose() + << "\n tol: " << tol_2; // An expected distance has been provided; let's test that the value is as // expected. @@ -345,6 +355,7 @@ void test_distance_box_box_helper(const Vector3& box1_size, // reported in https://github.com/flexible-collision-library/fcl/issues/388 template void test_distance_box_box_regression1() { + SCOPED_TRACE("test_distance_box_box_regression1"); const Vector3 box1_size(0.03, 0.12, 0.1); Transform3 X_WB1 = Transform3::Identity(); X_WB1.matrix() << -3.0627937852578681533e-08, -0.99999999999999888978, @@ -370,6 +381,7 @@ void test_distance_box_box_regression1() { // reported in https://github.com/flexible-collision-library/fcl/issues/395 template void test_distance_box_box_regression2() { + SCOPED_TRACE("test_distance_box_box_regression2"); const Vector3 box1_size(0.46, 0.48, 0.01); Transform3 X_WB1 = Transform3::Identity(); X_WB1.matrix() << 1,0,0, -0.72099999999999997424, @@ -393,6 +405,7 @@ void test_distance_box_box_regression2() { // reported in https://github.com/flexible-collision-library/fcl/issues/415 template void test_distance_box_box_regression3() { + SCOPED_TRACE("test_distance_box_box_regression3"); const Vector3 box1_size(0.49, 0.05, 0.21); Transform3 X_WB1 = Transform3::Identity(); // clang-format off @@ -416,6 +429,7 @@ void test_distance_box_box_regression3() { // reported in https://github.com/flexible-collision-library/fcl/issues/398 template void test_distance_box_box_regression4() { + SCOPED_TRACE("test_distance_box_box_regression4"); const Vector3 box1_size(0.614, 3, 0.37); Transform3 X_WB1 = Transform3::Identity(); X_WB1.translation() << -0.675, 0, 0.9115; @@ -429,6 +443,7 @@ void test_distance_box_box_regression4() { // reported in https://github.com/flexible-collision-library/fcl/issues/428 template void test_distance_box_box_regression5() { + SCOPED_TRACE("test_distance_box_box_regression5"); const Vector3 box1_size(0.2, 0.33, 0.1); Transform3 X_WB1 = Transform3::Identity(); X_WB1.translation() << -0.071000000000000035305, -0.77200000000000001954, 0.79999999999999993339; @@ -440,6 +455,7 @@ void test_distance_box_box_regression5() { template void test_distance_box_box_regression6() { + SCOPED_TRACE("test_distance_box_box_regression6"); const Vector3 box1_size(0.31650000000000000355, 0.22759999999999999676, 0.1768000000000000127); Transform3 X_WB1 = Transform3::Identity(); // clang-format off @@ -460,11 +476,64 @@ void test_distance_box_box_regression6() { test_distance_box_box_helper(box1_size, X_WB1, box2_size, X_WB2, &expected_distance); } +// Issue #493 outlines a number of scenarios that caused signed distance +// failure. They consisted of two identical, stacked boxes. The boxes are +// slightly tilted. The boxes were essentially touching but were separated by +// infinitesimally small distances. The issue outlines three different examples. +// Rather than reproducing each of them verbatim, this test attempts to +// generalize those cases by testing the stacked scenario across various box +// sizes and separation amounts (ranging from slightly penetrating to slightly +// separated). These should essentially cover the variations described in the +// issue. +template +void test_distance_box_box_regression_tilted_kissing_contact() { + SCOPED_TRACE("test_distance_box_box_regression_tilted_kissing_contact"); + // The boxes are posed relative to each other in a common frame F (such that + // it is easy to reason about their separation). The stack is rotated around + // box A's origin and translated into the world frame. + Matrix3 R_WF; + R_WF << + 0.94096063217417758029, 0.29296840037289501035, 0.16959541586174811667, + -0.23569836841299879326, 0.92661523595848427348, -0.29296840037289506586, + -0.2429801799032638987, 0.23569836841299884878, 0.94096063217417758029; + + for (const S dim : {S(0.01), S(0.25), S(0.5), S(10), S(1000)}) { + const Vector3 box_size(dim, dim, dim); + + const Vector3 p_WA(0, 0, 5 * dim); + Transform3 X_WA; + X_WA.linear() = R_WF; + X_WA.translation() = p_WA; + Transform3 X_WB; + X_WB.linear() = R_WF; + + // Both boxes always have the same orientation and the *stack* is always + // located at p_WA. Only the translational component of X_WB changes with + // varying separation distance. + + // By design, the distances are all near epsilon. We'll scale them up for + // larger boxes to make sure the distance doesn't simply disappear in + // the rounding noise. + for (const S distance : {S(-1e-15), S(-2.5e-16), S(-1e-16), S(0), S(1e-16), + S(2.5e-16), S(1e-15)}) { + const S scaled_distance = distance * std::max(S(1), dim); + const Vector3 p_AB_F = Vector3(0, dim + scaled_distance, 0); + + X_WB.translation() = p_WA + R_WF * p_AB_F; + SCOPED_TRACE("dim: " + std::to_string(dim) + + ", distance: " + std::to_string(distance)); + test_distance_box_box_helper(box_size, X_WA, box_size, X_WB, + &scaled_distance); + } + } +} + // This is a *specific* case that has cropped up in the wild that reaches the // unexpected `validateNearestFeatureOfPolytopeBeingEdge` error. This error was // reported in https://github.com/flexible-collision-library/fcl/issues/408 template void test_distance_sphere_box_regression1() { + SCOPED_TRACE("test_distance_sphere_box_regression1"); using CollisionGeometryPtr_t = std::shared_ptr>; const S sphere_radius = 0.06; CollisionGeometryPtr_t sphere_geo(new fcl::Sphere(sphere_radius)); @@ -533,6 +602,7 @@ GTEST_TEST(FCL_SIGNED_DISTANCE, RealWorldRegression) { test_distance_box_box_regression4(); test_distance_box_box_regression5(); test_distance_box_box_regression6(); + test_distance_box_box_regression_tilted_kissing_contact(); test_distance_sphere_box_regression1(); }