From 31ee175ff831345780e67d9960786797a57615fb Mon Sep 17 00:00:00 2001 From: Sean Curtis Date: Tue, 6 Apr 2021 14:03:47 -0700 Subject: [PATCH] [proximity] Characterize point-shape signed distance query (#14857) * Characterize point-shape signed distance query Extend the framework defined in characterization_utilities.{h|cc} to accommodate shape-point signed distance: - introduce a "point" primitive - Introduce the concept of "symmetry" in the test -- for this test we always test "point-shape" and *never* "shape-point". Add the supporting test and update the documentation accordingly. --- geometry/proximity/BUILD.bazel | 8 + .../test/characterization_utilities.cc | 26 ++- .../test/characterization_utilities.h | 15 +- .../distance_to_point_characterize_test.cc | 156 ++++++++++++++++++ geometry/query_object.h | 34 ++-- 5 files changed, 214 insertions(+), 25 deletions(-) create mode 100644 geometry/proximity/test/distance_to_point_characterize_test.cc diff --git a/geometry/proximity/BUILD.bazel b/geometry/proximity/BUILD.bazel index ef8a839a937f..4899345c88fe 100644 --- a/geometry/proximity/BUILD.bazel +++ b/geometry/proximity/BUILD.bazel @@ -648,6 +648,14 @@ drake_cc_googletest( ], ) +drake_cc_googletest( + name = "distance_to_point_characterize_test", + deps = [ + ":characterization_utilities", + ":distance_to_point_callback", + ], +) + drake_cc_googletest( name = "distance_to_shape_characterize_test", deps = [ diff --git a/geometry/proximity/test/characterization_utilities.cc b/geometry/proximity/test/characterization_utilities.cc index 5cd468b578f0..a2ded708bce5 100644 --- a/geometry/proximity/test/characterization_utilities.cc +++ b/geometry/proximity/test/characterization_utilities.cc @@ -46,6 +46,9 @@ std::ostream& operator<<(std::ostream& out, GeometryType s) { case kMesh: out << "Mesh"; break; + case kPoint: + out << "Point"; + break; case kSphere: out << "Sphere"; break; @@ -67,6 +70,7 @@ QueryInstance::QueryInstance(GeometryType shape1_in, GeometryType shape2_in, : shape1(shape1_in), shape2(shape2_in), error(-1), outcome(outcome_in) { DRAKE_DEMAND(outcome != kSupported); } + std::ostream& operator<<(std::ostream& out, const QueryInstance& c) { out << c.shape1 << " vs " << c.shape2; switch (c.outcome) { @@ -76,6 +80,9 @@ std::ostream& operator<<(std::ostream& out, const QueryInstance& c) { case kThrows: out << " should throw"; break; + case kIgnores: + out << " should be ignored"; + break; } return out; } @@ -454,8 +461,10 @@ void CharacterizeResultTest::RunCallback( ASSERT_EQ(callback_->GetNumResults(), 0); switch (query.outcome) { case kSupported: + case kIgnores: ASSERT_FALSE(callback_->Invoke(obj_A, obj_B, collision_filter, X_WGs)); - ASSERT_EQ(callback_->GetNumResults(), 1) << "No results reported!"; + ASSERT_EQ(callback_->GetNumResults(), + query.outcome == kSupported ? 1 : 0); break; case kThrows: DRAKE_ASSERT_THROWS_MESSAGE( @@ -527,7 +536,7 @@ vector> CharacterizeResultTest::MakeConfigurations( template void CharacterizeResultTest::RunCharacterization( const QueryInstance& query, const Shape& shape_A, const Shape& shape_B, - const vector>& configs) { + const vector>& configs, bool is_symmetric) { fcl::CollisionObjectd object_A = MakeFclShape(shape_A).object(); const GeometryId id_A = EncodeData(&object_A); @@ -571,7 +580,9 @@ void CharacterizeResultTest::RunCharacterization( object_B.setTransform(convert_to_double(X_WGs.at(id_B)).GetAsIsometry3()); evaluate_callback('A', &object_A, 'B', &object_B, config, X_WGs); - evaluate_callback('B', &object_B, 'A', &object_A, config, X_WGs); + if (is_symmetric) { + evaluate_callback('B', &object_B, 'A', &object_A, config, X_WGs); + } } } @@ -602,12 +613,13 @@ void CharacterizeResultTest::RunCharacterization( } template -void CharacterizeResultTest::RunCharacterization( - const QueryInstance& query) { +void CharacterizeResultTest::RunCharacterization(const QueryInstance& query, + bool is_symmetric) { auto shape1 = MakeShape(query.shape1, false /* use_alt */); auto shape2 = MakeShape(query.shape2, query.shape1 == query.shape2); RunCharacterization(query, *shape1, *shape2, - MakeConfigurations(*shape1, *shape2, TestDistances())); + MakeConfigurations(*shape1, *shape2, TestDistances()), + is_symmetric); } template @@ -648,6 +660,8 @@ std::unique_ptr CharacterizeResultTest::MakeShape(GeometryType shape, return std::make_unique(half_space(use_alt)); case kMesh: return std::make_unique(mesh(use_alt)); + case kPoint: + return std::make_unique(0.0); case kSphere: return std::make_unique(sphere(use_alt)); } diff --git a/geometry/proximity/test/characterization_utilities.h b/geometry/proximity/test/characterization_utilities.h index 52a5c34cd9d7..f2199ce3d998 100644 --- a/geometry/proximity/test/characterization_utilities.h +++ b/geometry/proximity/test/characterization_utilities.h @@ -58,8 +58,8 @@ class DistanceCallback { /* Encodes the possible outcomes of performing a proximity query. For a given set of query parameters/scalar attempting to perform the query should either - work or not. */ -enum Outcome { kSupported, kThrows }; + work or not in some enumerable fashion. */ +enum Outcome { kSupported, kThrows, kIgnores }; /* The types of geometries that can be considered for characterization tests. Note: the geometries enumerated here includes a Point (which is not a @@ -72,6 +72,7 @@ enum GeometryType { kEllipsoid, kHalfSpace, kMesh, + kPoint, kSphere }; std::ostream& operator<<(std::ostream& out, GeometryType s); @@ -371,12 +372,12 @@ class CharacterizeResultTest : public ::testing::Test { evaluate the callback. */ void RunCharacterization(const QueryInstance& query, const Shape& shape_A, const Shape& shape_B, - const std::vector>& configs); + const std::vector>& configs, + bool is_symmetric = true); - /* Variant of RunCharacterization that creates a collection of configurations - with using the distances given by TestDistances applied to the shapes - implied by the query. */ - void RunCharacterization(const QueryInstance& query); + /* Evaluates the query instance. */ + void RunCharacterization(const QueryInstance& query, + bool is_symmetric = true); /* Each subclass should define the distances over which it should be evaluated. */ diff --git a/geometry/proximity/test/distance_to_point_characterize_test.cc b/geometry/proximity/test/distance_to_point_characterize_test.cc new file mode 100644 index 000000000000..b80a5f2a5068 --- /dev/null +++ b/geometry/proximity/test/distance_to_point_characterize_test.cc @@ -0,0 +1,156 @@ +#include +#include + +#include + +#include "drake/geometry/proximity/distance_to_point_callback.h" +#include "drake/geometry/proximity/test/characterization_utilities.h" +#include "drake/geometry/query_results/signed_distance_to_point.h" +#include "drake/math/rigid_transform.h" +#include "drake/math/rotation_matrix.h" + +/* @file This provides the test that supports the values contained in the + table documented for QueryObject::ComputeSignedDistanceToPoint in + query_object.h. */ + +namespace drake { +namespace geometry { +namespace internal { +namespace point_distance { +namespace { + +using std::vector; + +/* Implementation of DistanceCallback for signed distance. */ +template +class PointDistanceCallback : public DistanceCallback { + public: + bool Invoke(fcl::CollisionObjectd* obj_A, fcl::CollisionObjectd* obj_B, + const CollisionFilterLegacy*, + const std::unordered_map>* + X_WGs) override { + // We treat the first sphere as representing the query point. + DRAKE_DEMAND(obj_A->collisionGeometry()->getNodeType() == fcl::GEOM_SPHERE); + const GeometryId point_id = EncodedData(*obj_A).id(); + const Vector3 p_WQ = X_WGs->at(point_id).translation(); + CallbackData data(obj_A, std::numeric_limits::infinity(), p_WQ, + X_WGs, &results_); + double max_distance = std::numeric_limits::infinity(); + return Callback(obj_A, obj_B, &data, max_distance); + } + + void ClearResults() override { results_.clear(); } + + int GetNumResults() const override { + return static_cast(results_.size()); + } + + T GetFirstSignedDistance() const override { return results_[0].distance; } + + private: + vector> results_; +}; + +template +class CharacterizePointDistanceResultTest : public CharacterizeResultTest { + public: + CharacterizePointDistanceResultTest() + : CharacterizeResultTest( + std::make_unique>()) {} + + vector TestDistances() const final { + return {-this->kDistance, this->kDistance}; + } + + /* Although we're representing the point as a sphere to fit into the + framework, we can't allow the default configurations to be generated because + the zero-radius sphere will fail the tests that seek to confirm it has + sufficient measure to support penetration. So, we'll create our own + configurations with a single arbitrary tangent plane to the point, and the + default sample tangent planes for the other shape. */ + vector> MakeConfigurations( + const Shape& shape_A, const Shape& shape_B, + const vector& signed_distances) const override { + // For this test, we require shape A to be a zero-radius sphere. + DRAKE_DEMAND(ShapeName(shape_A).name() == "Sphere"); + vector> configs; + // We'll create a tangent plane to the point with an arbitrary normal + // direction. + const Vector3 p_WA{0, 0, 0}; + const Vector3 a_norm_W = Vector3{1, -2, 0.5}.normalized(); + for (const double signed_distance : signed_distances) { + const Vector3 p_WC = p_WA + a_norm_W * signed_distance; + for (const auto& sample_B : + ShapeConfigurations(shape_B, signed_distance).configs()) { + /* If the sample point is not going to be the nearest point to the + query point, skip it. */ + if (sample_B.max_depth < -signed_distance) continue; + const Vector3& p_WB = sample_B.point; + const Vector3& b_norm_W = sample_B.normal; + const std::string relates = + signed_distance < 0 + ? " inside near " + : (signed_distance > 0 ? " outside near " : " touching at "); + configs.push_back( + {AlignPlanes(p_WC, a_norm_W, p_WB, b_norm_W), signed_distance, + "point" + relates + sample_B.description}); + } + } + return configs; + } +}; + +/* *-Mesh has not been implemented because Mesh is represented by Convex. + However, this single test will detect when that condition is no longer true + and call for implementation of *-Mesh tests. */ +GTEST_TEST(CharacterizePointDistanceResultTest, MeshMesh) { + ASSERT_TRUE(MeshIsConvex()); +} + +class DoubleTest : public CharacterizePointDistanceResultTest, + public testing::WithParamInterface {}; + +// clang-format off +INSTANTIATE_TEST_SUITE_P( + PointDistance, DoubleTest, + testing::Values( + QueryInstance(kPoint, kBox, 2e-15), + QueryInstance(kPoint, kCapsule, 4e-15), + QueryInstance(kPoint, kConvex, kIgnores), + QueryInstance(kPoint, kCylinder, 3e-15), + QueryInstance(kPoint, kEllipsoid, kIgnores), + QueryInstance(kPoint, kHalfSpace, 5e-15), + QueryInstance(kPoint, kSphere, 4e-15)), + QueryInstanceName); +// clang-format on + +TEST_P(DoubleTest, Characterize) { + this->RunCharacterization(GetParam(), false /* is_symmetric */); +} + +class AutoDiffTest : public CharacterizePointDistanceResultTest, + public testing::WithParamInterface {}; + +// clang-format off +INSTANTIATE_TEST_SUITE_P( + PointDistance, AutoDiffTest, + testing::Values( + QueryInstance(kPoint, kBox, 1e-15), + QueryInstance(kPoint, kCapsule, 4e-15), + QueryInstance(kPoint, kConvex, kIgnores), + QueryInstance(kPoint, kCylinder, kIgnores), + QueryInstance(kPoint, kEllipsoid, kIgnores), + QueryInstance(kPoint, kHalfSpace, 5e-15), + QueryInstance(kPoint, kSphere, 3e-15)), + QueryInstanceName); +// clang-format on + +TEST_P(AutoDiffTest, Characterize) { + this->RunCharacterization(GetParam(), false /* is_symmetric */); +} + +} // namespace +} // namespace point_distance +} // namespace internal +} // namespace geometry +} // namespace drake diff --git a/geometry/query_object.h b/geometry/query_object.h index 78ef48f2f5f7..3782d5681765 100644 --- a/geometry/query_object.h +++ b/geometry/query_object.h @@ -547,9 +547,6 @@ class QueryObject { Computes the signed distances and gradients to a query point from each geometry in the scene. - @warning Currently supports spheres, boxes, and cylinders only. Silently - ignores other kinds of geometries, which will be added later. - This query provides φᵢ(p), φᵢ:ℝ³→ℝ, the signed distance to the position p of a query point from geometry Gᵢ in the scene. It returns an array of the signed distances from all geometries. @@ -571,11 +568,24 @@ class QueryObject { Note that ∇φᵢ(p) is also defined on Gᵢ's surface, but we cannot use the above formula. -

Scalar support

- This query only supports computing distances from the point to spheres, - boxes, and cylinders for both `double` and `AutoDiffXd` scalar types. If - the SceneGraph contains any other geometry shapes, they will be silently - ignored. +

Characterizing the returned values

+ + This table is a variant of that described in this + @ref query_object_precision_methodology "class's documentation". The query + evaluates signed distance between *one* shape and a point (in contrast to + other queries which involve two shapes). Therefore, we don't need a matrix + of shape pairs, but a list of shapes. Otherwise, the methodology is the same + as described, with the point being represented as a zero-radius sphere. + + | Scalar | %Box | %Capsule | %Convex | %Cylinder | %Ellipsoid | %HalfSpace | %Mesh | %Sphere | + | :----: | :-----: | :------: | :-----: | :-------: | :--------: | :--------: | :-----: | :-----: | + | double | 2e-15 | 4e-15 | ᵃ | 3e-15 | ᵃ | 5e-15 | ᵃ | 4e-15 | + | ADXd | 1e-15 | 4e-15 | ᵃ | ᵃ | ᵃ | 5e-15 | ᵃ | 3e-15 | + __*Table 5*__: Worst observed error (in m) for 2mm penetration/separation + between geometry approximately 20cm in size and a point. + + - ᵃ Unsupported geometry/scalar combinations are simply ignored; no results + are reported for that geometry. @note For a sphere G, the signed distance function φᵢ(p) has an undefined gradient vector at the center of the sphere--every point on the sphere's @@ -611,10 +621,10 @@ class QueryObject { @param[in] threshold We ignore any object beyond this distance. By default, it is infinity, so we report distances from the query point to every object. - @retval signed_distances A vector populated with per-object signed - distance values (and supporting data). - See SignedDistanceToPoint. - */ + @retval signed_distances A vector populated with per-object signed distance + values (and supporting data) for every supported + geometry as shown in the table. See + SignedDistanceToPoint. */ std::vector> ComputeSignedDistanceToPoint(const Vector3 &p_WQ, const double threshold