forked from RobotLocomotion/drake
-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
[proximity] Characterize point-shape signed distance query (RobotLoco…
…motion#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.
- Loading branch information
1 parent
c0ebdea
commit 31ee175
Showing
5 changed files
with
214 additions
and
25 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
156 changes: 156 additions & 0 deletions
156
geometry/proximity/test/distance_to_point_characterize_test.cc
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,156 @@ | ||
| #include <memory> | ||
| #include <vector> | ||
|
|
||
| #include <gtest/gtest.h> | ||
|
|
||
| #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 <typename T> | ||
| class PointDistanceCallback : public DistanceCallback<T> { | ||
| public: | ||
| bool Invoke(fcl::CollisionObjectd* obj_A, fcl::CollisionObjectd* obj_B, | ||
| const CollisionFilterLegacy*, | ||
| const std::unordered_map<GeometryId, math::RigidTransform<T>>* | ||
| 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<T> p_WQ = X_WGs->at(point_id).translation(); | ||
| CallbackData<T> data(obj_A, std::numeric_limits<double>::infinity(), p_WQ, | ||
| X_WGs, &results_); | ||
| double max_distance = std::numeric_limits<double>::infinity(); | ||
| return Callback<T>(obj_A, obj_B, &data, max_distance); | ||
| } | ||
|
|
||
| void ClearResults() override { results_.clear(); } | ||
|
|
||
| int GetNumResults() const override { | ||
| return static_cast<int>(results_.size()); | ||
| } | ||
|
|
||
| T GetFirstSignedDistance() const override { return results_[0].distance; } | ||
|
|
||
| private: | ||
| vector<SignedDistanceToPoint<T>> results_; | ||
| }; | ||
|
|
||
| template <typename T> | ||
| class CharacterizePointDistanceResultTest : public CharacterizeResultTest<T> { | ||
| public: | ||
| CharacterizePointDistanceResultTest() | ||
| : CharacterizeResultTest<T>( | ||
| std::make_unique<PointDistanceCallback<T>>()) {} | ||
|
|
||
| vector<double> 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<Configuration<T>> MakeConfigurations( | ||
| const Shape& shape_A, const Shape& shape_B, | ||
| const vector<double>& 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<Configuration<T>> configs; | ||
| // We'll create a tangent plane to the point with an arbitrary normal | ||
| // direction. | ||
| const Vector3<T> p_WA{0, 0, 0}; | ||
| const Vector3<T> a_norm_W = Vector3<T>{1, -2, 0.5}.normalized(); | ||
| for (const double signed_distance : signed_distances) { | ||
| const Vector3<T> p_WC = p_WA + a_norm_W * signed_distance; | ||
| for (const auto& sample_B : | ||
| ShapeConfigurations<T>(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<T>& p_WB = sample_B.point; | ||
| const Vector3<T>& 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<double>, | ||
| public testing::WithParamInterface<QueryInstance> {}; | ||
|
|
||
| // 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<AutoDiffXd>, | ||
| public testing::WithParamInterface<QueryInstance> {}; | ||
|
|
||
| // 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 |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters