diff --git a/geometry/proximity_engine.cc b/geometry/proximity_engine.cc index dd9b40f77381..e2c3d7b69755 100644 --- a/geometry/proximity_engine.cc +++ b/geometry/proximity_engine.cc @@ -465,6 +465,24 @@ void ComputeNarrowPhaseDistance(const fcl::CollisionObjectd* a, fcl::DistanceResultd* result) { const fcl::CollisionGeometryd* a_geometry = a->collisionGeometry().get(); const fcl::CollisionGeometryd* b_geometry = b->collisionGeometry().get(); + + // We use FCL's GJK/EPA fallback in those geometries we haven't explicitly + // supported. However, FCL doesn't support: half spaces, planes, triangles, or + // octtrees in that workflow. We need to give intelligent feedback rather than + // the segfault otherwise produced. + // NOTE: Currently this only tests for halfspace (because it is an otherwise + // supported geometry type in SceneGraph. When meshes, planes, and/or octrees + // are supported, this error would have to be modified. + // TODO(SeanCurtis-TRI): Remove this test when FCL supports signed distance + // queries for halfspaces (see issue #10905). Also see FCL issue + // https://github.com/flexible-collision-library/fcl/issues/383. + if (a_geometry->getNodeType() == fcl::GEOM_HALFSPACE || + b_geometry->getNodeType() == fcl::GEOM_HALFSPACE) { + throw std::logic_error( + "Signed distance queries on halfspaces are not currently supported. " + "Try using a large box instead."); + } + const RigidTransformd X_WA(a->getTransform()); const RigidTransformd X_WB(b->getTransform()); const auto id_A = EncodedData(*a).id(geometry_map); @@ -479,17 +497,16 @@ void ComputeNarrowPhaseDistance(const fcl::CollisionObjectd* a, auto sphere_A = static_cast(a_geometry); auto sphere_B = static_cast(b_geometry); distance_pair(sphere_A, sphere_B); - break; + return; } case fcl::GEOM_BOX: { auto sphere_A = static_cast(a_geometry); auto box_B = static_cast(b_geometry); distance_pair(sphere_A, box_B); - break; - } - default: { - fcl::distance(a, b, request, *result); + return; } + default: + break; } break; } @@ -499,18 +516,17 @@ void ComputeNarrowPhaseDistance(const fcl::CollisionObjectd* a, auto box_A = static_cast(a_geometry); auto sphere_B = static_cast(b_geometry); distance_pair(box_A, sphere_B); - break; - } - default: { - fcl::distance(a, b, request, *result); + return; } + default: + break; } break; } - default: { - fcl::distance(a, b, request, *result); - } + default: + break; } + fcl::distance(a, b, request, *result); } // The callback function in fcl::distance request. The final unnamed parameter diff --git a/geometry/test/proximity_engine_test.cc b/geometry/test/proximity_engine_test.cc index b26c07a1b3e6..3a92f59c7183 100644 --- a/geometry/test/proximity_engine_test.cc +++ b/geometry/test/proximity_engine_test.cc @@ -1715,6 +1715,32 @@ INSTANTIATE_TEST_CASE_P(SphereBoxBoundary, SignedDistancePairTest, INSTANTIATE_TEST_CASE_P(SphereBoxBoundaryTransform, SignedDistancePairTest, testing::ValuesIn(GenDistPairTestSphereBoxBoundaryTransform())); +// This tests that signed distance queries against a halfspace throw an +// intelligible exception rather than a segfault. +GTEST_TEST(SignedDistancePairError, HalfspaceException) { + // Note: this doesn't fully test the condition for emitting the error; we + // have no *real* control over which geometry is the first geometry and + // second geometry in the pair being tested. However, this test isn't + // intended to be long-lasting; we want to correct FCL's short-coming soon + // so that the behavior (and this test) can be removed. + ProximityEngine engine; + std::vector geometry_map; + + Sphere sphere{0.5}; + engine.AddDynamicGeometry(sphere, GeometryIndex(0)); + geometry_map.push_back(GeometryId::get_new_id()); + + HalfSpace halfspace; + engine.AddAnchoredGeometry(halfspace, Isometry3d::Identity(), + GeometryIndex(1)); + geometry_map.push_back(GeometryId::get_new_id()); + + DRAKE_EXPECT_THROWS_MESSAGE( + engine.ComputeSignedDistancePairwiseClosestPoints(geometry_map), + std::logic_error, + "Signed distance .* halfspaces .* not .* supported.* Try .* box .*"); +} + // Concentric geometries A, B do not have a unique pair of witness points // Na, Nb. We inherit another test fixture from SignedDistancePairTest, so we // can define another TEST_P that checks |Na-Nb| = -signed_distance but does