[planning] Add voxelized environment collision checker and supporting tools to planning/dev

[calderpg-tri]

This change is 

[calderpg-tri]

Reviewable status: 1 unresolved discussion, needs platform reviewer assigned, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes (waiting on @calderpg-tri)

a discussion (no related file):
@jwnimmer-tri For both the MbP and voxel checker tests, I'm getting an error like

GoogleTestVerification.UninstantiatedParameterizedTestSuite<SphereRobotModelCollisionCheckerAbstractTestSuite>
planning/dev/test/voxelized_environment_collision_checker_test.cc:80: Failure
Parameterized test suite SphereRobotModelCollisionCheckerAbstractTestSuite is instantiated via INSTANTIATE_TEST_SUITE_P, but no tests are defined via TEST_P . No test cases will run.

Ideally, INSTANTIATE_TEST_SUITE_P should only ever be invoked from code that always depend on code that provides TEST_P. Failing to do so is often an indication of dead code, e.g. the last TEST_P was removed but the rest got left behind.

To suppress this error for this test suite, insert the following line (in a non-header) in the namespace it is defined in:

GTEST_ALLOW_UNINSTANTIATED_PARAMETERIZED_TEST(SphereRobotModelCollisionCheckerAbstractTestSuite);

as far as I know, the structure of the tests and build rules are the same as in anzu, and seem to be basically equivalent to the existing collision checker test suite in //planning, so I don't really know what's different here.

---

[jwnimmer-tri]

Reviewable status: 1 unresolved discussion, needs platform reviewer assigned, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes (waiting on @calderpg-tri)

a discussion (no related file):

Previously, calderpg-tri wrote…

@jwnimmer-tri For both the MbP and voxel checker tests, I'm getting an error like

GoogleTestVerification.UninstantiatedParameterizedTestSuite<SphereRobotModelCollisionCheckerAbstractTestSuite>
planning/dev/test/voxelized_environment_collision_checker_test.cc:80: Failure
Parameterized test suite SphereRobotModelCollisionCheckerAbstractTestSuite is instantiated via INSTANTIATE_TEST_SUITE_P, but no tests are defined via TEST_P . No test cases will run.

Ideally, INSTANTIATE_TEST_SUITE_P should only ever be invoked from code that always depend on code that provides TEST_P. Failing to do so is often an indication of dead code, e.g. the last TEST_P was removed but the rest got left behind.

To suppress this error for this test suite, insert the following line (in a non-header) in the namespace it is defined in:

GTEST_ALLOW_UNINSTANTIATED_PARAMETERIZED_TEST(SphereRobotModelCollisionCheckerAbstractTestSuite);

as far as I know, the structure of the tests and build rules are the same as in anzu, and seem to be basically equivalent to the existing collision checker test suite in //planning, so I don't really know what's different here.

This patch seems to fix it for me:

--- a/planning/dev/BUILD.bazel
+++ b/planning/dev/BUILD.bazel
@@ -106,6 +106,7 @@ drake_cc_library(
         "//planning:collision_checker",
         "@gtest//:without_main",
     ],
+    alwayslink = True,
 )
 
 drake_cc_googletest(

Best guess:

(1) Anzu doesn't need this because it creates shared libraries by default; Drake crates static libraries by default.

(2) The collision_checker_abstract_test_suite.h doesn't need this because it provides other symbols beyond just the suite, which then induces an actual dependency.

---

[calderpg-tri]

Reviewable status: needs platform reviewer assigned, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes (waiting on @calderpg-tri)

a discussion (no related file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

This patch seems to fix it for me:

--- a/planning/dev/BUILD.bazel
+++ b/planning/dev/BUILD.bazel
@@ -106,6 +106,7 @@ drake_cc_library(
         "//planning:collision_checker",
         "@gtest//:without_main",
     ],
+    alwayslink = True,
 )
 
 drake_cc_googletest(

Best guess:

(1) Anzu doesn't need this because it creates shared libraries by default; Drake crates static libraries by default.

(2) The collision_checker_abstract_test_suite.h doesn't need this because it provides other symbols beyond just the suite, which then induces an actual dependency.

Thanks!

---

[calderpg-tri]

+@tomstewart-woven for review, thanks!

Structure-wise, this code all comes from our internal codebase except for the following changes:

1. New voxel_signed_distance_field.{cc, h} to encapsulate ownership of a VGT SignedDistanceField<float> in a way compatible with Drake's hidden namespacing of VGT
2. The internal template functions of the voxel self filter and voxelized environment builder have been factored out to separate headers voxel_self_filter_internal.h and voxelized_environment_builder_internal.h
3. Minor changes to the tests to remove dependencies on other parts of the internal codebase

[tomstewart-woven]

@calderpg-tri ACK! Let me take a look at this on the weekend 👍

[tomstewart-woven]

Ok! Sorry for the slow review. I left a few comments mainly to confirm my understanding of a few things but nothing blocking. The diff is pretty big so I can't say that I've read every line in close detail, but since this is destined for a dev folder I think that's ok. At minimum, the structure of the code and the core algorithms make sense.

If you happened to have any python bindings for any of these classes, it'd be great if you could throw those in here too! But if not then no problem.

[tomstewart-woven]

planning/dev/sphere_robot_model_collision_checker.h line 490 at r3 (raw file):

  void UpdateCollisionFilters() override {}

  // TODO(sean.curtis): This implementation is not compliant with the

I had the same thought as Sean when I first read this implementation. I guess by combining the gradients together we get a smoother clearance gradient. Is that the idea here?

[tomstewart-woven]

planning/dev/voxelized_environment_collision_checker.h line 36 at r3 (raw file):

  /// Construct a collision checker for voxelized environments.
  /// @param model a Diagram+MbP+SG model of the robot and environment.

What's meant by SG here?

[tomstewart-woven]

Reviewed 18 of 24 files at r1, 1 of 1 files at r2, 5 of 5 files at r3, all commit messages.
Reviewable status: 4 unresolved discussions, LGTM missing from assignee tomstewart-woven, needs platform reviewer assigned, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes (waiting on @calderpg-tri)

---

planning/dev/voxelized_environment_builder.cc line 67 at r3 (raw file):

}  // namespace

CollisionMap BuildCollisionMap(

Do you have any examples for how to build a CollisionMap based on a depth image or a point cloud? I guess that's all kept in voxelized_geometry_tools?

---

planning/dev/voxelized_environment_collision_checker.cc line 152 at r3 (raw file):

        const double window = internal_sdf.GetResolution() * 0.25;
        const auto gradient_query =
            internal_sdf.GetLocationFineGradient4d(p_BQ, window);

Very nice! How do you calculate the gradient of the SDF? Do you use finite differences?

[calderpg-tri]

We have some fairly simple bindings in anzu, but nothing extensive or incorporating the actual voxel grid types - from a higher level perspective, most of our planning/control uses in python just see a CollisionChecker and don't need anything deeper.

Reviewable status: 4 unresolved discussions, LGTM missing from assignee tomstewart-woven, needs platform reviewer assigned, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes (waiting on @calderpg-tri)

---

planning/dev/sphere_robot_model_collision_checker.h line 490 at r3 (raw file):

Previously, tomstewart-woven wrote…

I had the same thought as Sean when I first read this implementation. I guess by combining the gradients together we get a smoother clearance gradient. Is that the idea here?

The aggregated gradients here are mostly a performance choice, since it significantly reduces the number of RobotClearance entries returned. There's a bit of smoothing here, and pragmatically the SDF gradient is also a semi-aggregation of collision gradients when compared to the per-geometry gradients you get from SceneGraph.

(also, as a historical note, the aggregation here long predates the formalization of nominal RobotClearance behavior)

---

planning/dev/voxelized_environment_builder.cc line 67 at r3 (raw file):

Previously, tomstewart-woven wrote…

Do you have any examples for how to build a CollisionMap based on a depth image or a point cloud? I guess that's all kept in voxelized_geometry_tools?

The actual pointcloud voxelizers live in voxelized_geometry_tools, we do have a thin wrapper in anzu but it requires pointcloud datatypes that aren't dependencies we want in Drake (e.g. PCL, ROS 2) and while a wrapper shim for using the Drake PointCloud type would be trivial, I suspect very few users actually use the Drake PointCloud in real settings.

---

planning/dev/voxelized_environment_collision_checker.h line 36 at r3 (raw file):

Previously, tomstewart-woven wrote…

What's meant by SG here?

SG refers to SceneGraph, but really the whole docs here are wrong and predate the consolidation to CollisionCheckerParams

---

planning/dev/voxelized_environment_collision_checker.cc line 152 at r3 (raw file):

Previously, tomstewart-woven wrote…

Very nice! How do you calculate the gradient of the SDF? Do you use finite differences?

Yes, "fine" gradient uses finite differencing on estimated distances over a small window, while the "coarse" gradient uses finite differencing on raw cell values and a 2-cell window.

[calderpg-tri]

Reviewable status: 3 unresolved discussions, LGTM missing from assignee tomstewart-woven, needs platform reviewer assigned, needs at least two assigned reviewers, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits), missing label for release notes (waiting on @calderpg-tri)

---

planning/dev/voxelized_environment_collision_checker.h line 36 at r3 (raw file):

Previously, calderpg-tri wrote…

SG refers to SceneGraph, but really the whole docs here are wrong and predate the consolidation to CollisionCheckerParams

Updated the docs here and in the MbP checker to match the style in SceneGraphCollisionChecker