[geometry] Add support for SceneGraph<Expression>

[RussTedrake]

This change is 

[RussTedrake]

@jwnimmer-tri -- before I cover this with documentation and tests, could you please take a quick look? Anything fundamentally wrong with this?

FTR -- this was not inspired by the visualizer support for Expression, but from a system identification through contact project (cc @abhishekunique). With only this PR, the SceneGraph will still throw with any non-double Expressions moving through it, but I plan to follow up with some preliminary support for narrow workflows (e.g. sphere on sphere) and grow it from there.

[jwnimmer-tri]

I don't expect that I'll have any concerns, but it's difficult to do a high-level look without at least some of the documentation being fixed. Please add the @tparam_foo documentation to remark which scalars are going to supported for which classes and functions, and then I'll take a look.

Also, you should run this by a scene graph subject matter expert / feature area owner from Dynamics for approval as well.

Reviewed 2 of 8 files at r1, all commit messages.
Reviewable status: 1 unresolved discussion, LGTM missing from assignee jwnimmer-tri(platform), needs at least two assigned reviewers, missing label for release notes (waiting on @jwnimmer-tri and @RussTedrake)

---

geometry/proximity_engine.h, line 81 at r1 (raw file):

   scalar type. If the engine is already an AutoDiffXd engine, it is equivalent
   to using the copy constructor to create a duplicate on the heap.  */
  std::unique_ptr<ProximityEngine<AutoDiffXd>> ToAutoDiffXd() const;

nit Since the engine is in the internal namespace, we should purge this function in lieu of the newly-added broader one, below. (No deprecation required.)

[RussTedrake]

I've updated the tparams.

Reviewable status: LGTM missing from assignee jwnimmer-tri(platform), needs at least two assigned reviewers, missing label for release notes (waiting on @jwnimmer-tri)

---

geometry/proximity_engine.h, line 81 at r1 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

nit Since the engine is in the internal namespace, we should purge this function in lieu of the newly-added broader one, below. (No deprecation required.)

Done.

[RussTedrake]

It looks like I have to dig in a little deeper to make the libdrake tests happy (I was only passing the local tests at first). Will have another update soon.

[jwnimmer-tri]

Reviewed 2 of 8 files at r1, 6 of 7 files at r2, all commit messages.
Reviewable status: 3 unresolved discussions, LGTM missing from assignee jwnimmer-tri(platform), needs at least two assigned reviewers, missing label for release notes (waiting on @jwnimmer-tri and @RussTedrake)

a discussion (no related file):
nit The scene_graph_inspector.cc file needs DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS added, so that we will fail-fast if there is anything amiss here.

---

---

geometry/geometry_state.h, line 532 at r2 (raw file):

   scalar values initialized from the current values. If this is invoked on an
   instance already instantiated on AutoDiffXd, it is equivalent to cloning
   the instance.  */

This documentation implies that GeometryState<AutoDiffXd>::ToAutoDiffXd() was a valid function call, but with this new SFINAE check, it is no longer so.

I assume there was something with GeometryState<Expression>::ToAutoDiffXd() that didn't compile? However, the documentation of ProximityEngine::ToScalarType does not disclaim any of the possible conversions, so it's not yet clear why this call site has to take any additional care. I would have thought that all combinations should be allowed for here, as well.

(... later ...)

Ah, it's the convert_pose_vector isn't it?

In the future, the GeoemtryState scalar-conversion constructor definition should move to the cc file (due to inline functions GSG rule), and we should update it to use the any-scalar-to-any-scalar helpers instead of hand-rolling its own bad implementation.

It would be OK to leave that to a TODO as of this PR, but in that case the SFINAE check here should exclude Expression, rather than only allow double. Or alternatively, it could allow any time per the signature, but the function definition would if constexpr throw from the cc file in case we have are doing Expression. That would leave the header file tidy.

---

geometry/proximity_engine.cc, line 981 at r2 (raw file):

}

// Explicit instantiations.

These much match what's in the header file -- the header allows for all 9 combinations, so we must here as well.

The best way to resolve this is probably:

DRAKE_DEFINE_FUNCTION_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS((
    &ProximityEngine<T>::template ToScalarType<U>
))

[RussTedrake]

Pushed a version that should pass all tests.

Reviewable status: 2 unresolved discussions, LGTM missing from assignee jwnimmer-tri(platform), needs at least two assigned reviewers, missing label for release notes (waiting on @jwnimmer-tri and @RussTedrake)

a discussion (no related file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

nit The scene_graph_inspector.cc file needs DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS added, so that we will fail-fast if there is anything amiss here.

Done.

---

---

geometry/proximity_engine.cc, line 981 at r2 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

These much match what's in the header file -- the header allows for all 9 combinations, so we must here as well.

The best way to resolve this is probably:

DRAKE_DEFINE_FUNCTION_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS((
    &ProximityEngine<T>::template ToScalarType<U>
))

Done.

[jwnimmer-tri]

Seems like its on the right track. I can finish a review once you're ready.

Reviewed 7 of 10 files at r3, all commit messages.
Reviewable status: 1 unresolved discussion, LGTM missing from assignee jwnimmer-tri(platform), needs at least two assigned reviewers, missing label for release notes (waiting on @jwnimmer-tri and @RussTedrake)

[RussTedrake]

+@jwnimmer-tri for feature review, please.

This got pretty big with all of the unit tests. Want me to break it up, or are you ok to swallow it whole?

Reviewable status: 1 unresolved discussion, LGTM missing from assignee jwnimmer-tri(platform), needs at least two assigned reviewers, missing label for release notes (waiting on @jwnimmer-tri)

---

geometry/geometry_state.h, line 532 at r2 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

This documentation implies that GeometryState<AutoDiffXd>::ToAutoDiffXd() was a valid function call, but with this new SFINAE check, it is no longer so.

I assume there was something with GeometryState<Expression>::ToAutoDiffXd() that didn't compile? However, the documentation of ProximityEngine::ToScalarType does not disclaim any of the possible conversions, so it's not yet clear why this call site has to take any additional care. I would have thought that all combinations should be allowed for here, as well.

(... later ...)

Ah, it's the convert_pose_vector isn't it?

In the future, the GeoemtryState scalar-conversion constructor definition should move to the cc file (due to inline functions GSG rule), and we should update it to use the any-scalar-to-any-scalar helpers instead of hand-rolling its own bad implementation.

It would be OK to leave that to a TODO as of this PR, but in that case the SFINAE check here should exclude Expression, rather than only allow double. Or alternatively, it could allow any time per the signature, but the function definition would if constexpr throw from the cc file in case we have are doing Expression. That would leave the header file tidy.

Done.

[RussTedrake]

I'm now able to reproduce the error on bionic gcc (using docker)... but I think my innocuous changes must have uncovered some old landmine. It seems to have been an issue before, too:

drake/systems/sensors/rgbd_sensor.cc

Lines 169 to 172 in 1944304

	// Note: Ideally, this would be inlined. However, inlining this caused GCC
	// to do something weird with GeometryState such that the rgbd_sensor_test.cc
	// was unable to instantiate an GeometryStateTester that GCC recognized as a
	// friend to GeometryState.

[jwnimmer-tri]

Checkpoint.

Given the amount of cross-checking needed during review for documentation and type-compatibility, I don't think it makes sense to split up any of the geometry changes. If you wanted to split out the rotation_matrix and utilities changes into a small first PR, that would be a small mitigation, but those changes are small enough that it's not a big deal one way or another.

Reviewed 1 of 10 files at r3, 8 of 14 files at r4, 1 of 1 files at r5, all commit messages.
Reviewable status: 7 unresolved discussions, LGTM missing from assignee jwnimmer-tri(platform), needs at least two assigned reviewers (waiting on @jwnimmer-tri and @RussTedrake)

---

geometry/geometry_state.h, line 406 at r5 (raw file):

  /** Implementation of QueryObject::ComputeContactSurfaces().  */
  std::vector<ContactSurface<T>> ComputeContactSurfaces(

In contact_surface.h it currently says:

/** ...
  @tparam_nonsymbolic_scalar
 */
template <typename T>
class ContactSurface {

Either that datatype must add symbolic support (such that ContactSurface<Expresion> is a valid type), or else this query function must SFINAE away for Expression.

Ditto for ComputeContactSurfacesWithFallback.

---

geometry/geometry_state.cc, line 1398 at r5 (raw file):

// Explicit instantiation.
template std::unique_ptr<GeometryState<AutoDiffXd>>
    GeometryState<double>::ToAutoDiffXd<double>() const;

To avoid breaking compatibility, we also need to explicitly instantiate with T=T1=AutoDiffXd here.

---

geometry/proximity_engine.h, line 227 at r5 (raw file):

   @param X_WGs the current poses of all geometries in World in the
                current scalar type, keyed on each geometry's GeometryId.  */
  std::vector<ContactSurface<T>> ComputeContactSurfaces(

In contact_surface.h it currently says:

/** ...
  @tparam_nonsymbolic_scalar
 */
template <typename T>
class ContactSurface {

Either that datatype must add symbolic support (such that ContactSurface<Expresion> is a valid type), or else this query function (and its twin on the Impl) must SFINAE away for Expression.

Ditto for ComputeContactSurfacesWithFallback.

---

geometry/query_object.h, line 322 at r5 (raw file):

            consistent -- for fixed geometry poses, the results will remain
            the same.  */
  std::vector<ContactSurface<T>> ComputeContactSurfaces(

In contact_surface.h it currently says:

/** ...
  @tparam_nonsymbolic_scalar
 */
template <typename T>
class ContactSurface {

Either that datatype must add symbolic support (such that ContactSurface<Expresion> is a valid type), or else this query function must SFINAE away for Expression.

Ditto for ComputeContactSurfacesWithFallback.

---

geometry/test/geometry_state_test.cc, line 880 at r5 (raw file):

  // 4. Compare RigidTransform<AutoDiffXd> with RigidTransformd.
  auto test_T_vs_double_pose = [](const RigidTransform<T>& test,
                                   const RigidTransformd& ref) {

nit Indentation fixup.

Suggestion:

  auto test_T_vs_double_pose = [](const RigidTransform<T>& test,
                                  const RigidTransformd& ref) {

---

geometry/test/geometry_state_test.cc, line 922 at r5 (raw file):

// This tests the ability to assign a GeometryState<double> to a
// GeometryState<T>. Implicitly uses transmogrification.
TEST_F(GeometryStateTest, AssignDoubleToAutoDiff) {

nit This test case name is now inaccurate.

---

math/test/rotation_matrix_test.cc, line 1169 at r5 (raw file):

      const RotationMatrix<symbolic::Expression> R_AB_sym =
          RotationMatrix<symbolic::Expression>::MakeFromOneVector(b_A,
                                                                  axis_index);

nit In most places in this file, when dealing with Expression there is a function-local using statement. I suggest doing the same here, both to improve readability (line wrapping) and also maintain the custom.

I'd also be OK with moving the using symbolic::Expression atop the file at namespace scope, and nixing all of the function-local using statements.

Suggestion:

      using symbolic::Expression;
      const RotationMatrix<Expression> R_AB_sym =
          RotationMatrix<Expression>::MakeFromOneVector(b_A, axis_index);

[jwnimmer-tri]

I'm now able to reproduce the error on bionic gcc ...

I don't think the far-flung unit test needs (or should use in the first place!) the private friendship. Does it work to call the public function GetRenderEngineByName instead of the private function GetRenderEngineOrThrow?

[RussTedrake]

Yes! That resolved it. I've cleaned that usage up.
Will leave it all in a single PR. That's certainly easier for me at this point.

Reviewable status: 4 unresolved discussions, LGTM missing from assignee jwnimmer-tri(platform), needs at least two assigned reviewers (waiting on @jwnimmer-tri)

---

geometry/geometry_state.h, line 406 at r5 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

In contact_surface.h it currently says:

/** ...
  @tparam_nonsymbolic_scalar
 */
template <typename T>
class ContactSurface {

Either that datatype must add symbolic support (such that ContactSurface<Expresion> is a valid type), or else this query function must SFINAE away for Expression.

Ditto for ComputeContactSurfacesWithFallback.

Done. I took a short trip down the ContactSurface rabbit hole, but aborted that mission!

---

geometry/geometry_state.cc, line 1398 at r5 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

To avoid breaking compatibility, we also need to explicitly instantiate with T=T1=AutoDiffXd here.

Done.

---

geometry/proximity_engine.h, line 227 at r5 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

In contact_surface.h it currently says:

/** ...
  @tparam_nonsymbolic_scalar
 */
template <typename T>
class ContactSurface {

Either that datatype must add symbolic support (such that ContactSurface<Expresion> is a valid type), or else this query function (and its twin on the Impl) must SFINAE away for Expression.

Ditto for ComputeContactSurfacesWithFallback.

Done.

---

geometry/query_object.h, line 322 at r5 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

In contact_surface.h it currently says:

/** ...
  @tparam_nonsymbolic_scalar
 */
template <typename T>
class ContactSurface {

Either that datatype must add symbolic support (such that ContactSurface<Expresion> is a valid type), or else this query function must SFINAE away for Expression.

Ditto for ComputeContactSurfacesWithFallback.

Done.

---

geometry/test/geometry_state_test.cc, line 880 at r5 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

nit Indentation fixup.

Done.

---

geometry/test/geometry_state_test.cc, line 922 at r5 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

nit This test case name is now inaccurate.

Done.

---

math/test/rotation_matrix_test.cc, line 1169 at r5 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

nit In most places in this file, when dealing with Expression there is a function-local using statement. I suggest doing the same here, both to improve readability (line wrapping) and also maintain the custom.

I'd also be OK with moving the using symbolic::Expression atop the file at namespace scope, and nixing all of the function-local using statements.

Done.

[RussTedrake]

---

geometry/proximity_engine.cc, line 998 at r6 (raw file):

    (&ProximityEngine<T>::template ComputeContactSurfaces<T>))

// The DEFINE_FUNCTION_TEMPLATE macro doesn't handle 4 arguments yet.

btw -- i would have liked to have just implemented the 3 and 4 argument cases for the macro, but this PR is already growing beyond it's original boundaries...

[jwnimmer-tri]

Reviewed 3 of 14 files at r4, 11 of 12 files at r6, 1 of 1 files at r7, all commit messages.
Reviewable status: 10 unresolved discussions, LGTM missing from assignee jwnimmer-tri(platform), needs at least two assigned reviewers (waiting on @jwnimmer-tri and @RussTedrake)

---

geometry/proximity_engine.cc, line 998 at r6 (raw file):

Previously, RussTedrake (Russ Tedrake) wrote…

btw -- i would have liked to have just implemented the 3 and 4 argument cases for the macro, but this PR is already growing beyond it's original boundaries...

The macro is not sensitive to function arity. Maybe there was some other typo? In any case, this patch builds OK for me:

--- a/geometry/proximity_engine.cc
+++ b/geometry/proximity_engine.cc
@@ -989,24 +989,14 @@ bool ProximityEngine<T>::IsFclConvexType(GeometryId id) const {
   return impl_->IsFclConvexType(id);
 }
 
-DRAKE_DEFINE_FUNCTION_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS(
-    (&ProximityEngine<T>::template ToScalarType<U>))
-
-DRAKE_DEFINE_FUNCTION_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS(
-    (&ProximityEngine<T>::template ComputeContactSurfaces<T>))
-
-// The DEFINE_FUNCTION_TEMPLATE macro doesn't handle 4 arguments yet.
-template void ProximityEngine<double>::ComputeContactSurfacesWithFallback<
-    double>(HydroelasticContactRepresentation representation,
-            const std::unordered_map<GeometryId, RigidTransform<double>>& X_WGs,
-            std::vector<ContactSurface<double>>* surfaces,
-            std::vector<PenetrationAsPointPair<double>>* point_pairs) const;
-template void
-ProximityEngine<AutoDiffXd>::ComputeContactSurfacesWithFallback<AutoDiffXd>(
-    HydroelasticContactRepresentation representation,
-    const std::unordered_map<GeometryId, RigidTransform<AutoDiffXd>>& X_WGs,
-    std::vector<ContactSurface<AutoDiffXd>>* surfaces,
-    std::vector<PenetrationAsPointPair<AutoDiffXd>>* point_pairs) const;
+DRAKE_DEFINE_FUNCTION_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS((
+    &ProximityEngine<T>::template ToScalarType<U>
+))
+
+DRAKE_DEFINE_FUNCTION_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS((
+    &ProximityEngine<T>::template ComputeContactSurfaces<T>,
+    &ProximityEngine<T>::template ComputeContactSurfacesWithFallback<T>
+))
 
 }  // namespace internal
 }  // namespace geometry

---

geometry/query_object.h, line 107 at r7 (raw file):

     and scalars. At that point, the table will simply disappear.

 @tparam_default_scalar

Several places in the API docs for this class that discuss scalar support specifically. All of them need to be fixed up to reflect the new innovations as of this PR.

This includes the "Characterizing the returned values" tables of support, as well as various prose like "This method provides support for both double and AutoDiffXd."

---

geometry/query_object.cc, line 231 at r7 (raw file):

    (&QueryObject<T>::template ComputeContactSurfaces<T>))

// The DEFINE_FUNCTION_TEMPLATE macro doesn't handle 3 arguments yet.

nit The macro is not sensitive to function arity. Maybe there was some other typo? See the other file for the patch that worked in that case.

---

geometry/proximity/test/distance_to_shape_characterize_test.cc, line 12 at r7 (raw file):

/* @file This provides the test that supports the values contained in the
 table documented for QueryObject::ComputeSignedDistancePairwiseClosestPoints in
 query_object.h. */

At some point, the function documentation cited here needs to gain another table for Expression so that the docs and test can say in alignment. If that change isn't part of this PR, there needs to be a clear plan where it will happen in a separately PR. (It's probably easiest to do it in this PR, though.)

---

geometry/proximity/test/penetration_as_point_pair_callback_test.cc, line 496 at r7 (raw file):

TEST_F(PenetrationAsPointPairCallbackTest, UnsupportedExpression) {
  // We don't support penetration queries between for Expression.

nit The grammar got lost somewhere along the way. Maybe the "between" should have been nixed?

---

geometry/proximity/test/penetration_as_point_pair_callback_test.cc, line 499 at r7 (raw file):

  std::vector<std::pair<fcl::CollisionObjectd, GeometryId>>
      unsupported_geometries;
  unsupported_geometries.emplace_back(sphere_A_, id_A_);

Is "ellipsoid" missing here (as a negative test)? Seems like yes, at first glance.

---

systems/sensors/test/rgbd_sensor_test.cc, line 86 at r7 (raw file):

const DummyRenderEngine* GetDummyRenderEngine(
  const systems::Context<T>& context, const std::string& name) {
// Technically brittle, but relatively safe assumption that GeometryState

nit Needs +2 function indent for this whole block.

---

systems/sensors/test/rgbd_sensor_test.cc, line 91 at r7 (raw file):

    context.get_parameters()
        .template get_abstract_parameter<geometry::GeometryState<T>>(0);
return dynamic_cast<const DummyRenderEngine*>(

nit In the prior code, the dynamic_cast used a reference so that it fails-fast if the type was mismatched. We need to preserve that fail-fast, either by changing back to casting a reference, or else checking for nullptr here before returning.

---

bindings/pydrake/geometry_py_scene_graph.cc, line 411 at r7 (raw file):

    if constexpr (!std::is_same_v<T, symbolic::Expression>) {
      cls.def("ComputeContactSurfaces",

nit Our clang-format line wrapping tends to become finicky over time if we do cls.def directly on the same line. As elsewhere in this file, we should force a linebreak when resuming a cls definition.

Suggestion:

      cls  // BR
          .def("ComputeContactSurfaces",

---

bindings/pydrake/test/geometry_scene_graph_test.py, line 321 at r7 (raw file):

        self.assertTupleEqual(obj.p_WCb.shape, (3,))
        if T != Expression:
            self.assertEqual(obj.depth, -1.)

nit This opt-out of Expression shows up out of the blue. In general, the other opt-outs throughout this file are clear, because the functions under test in those cases are documented as nonsymbolic. Here, it's unclear why we're skipping this check in this case. The class documentation does not seem to indicate that this member is unsupported.

[RussTedrake]

Reviewable status: 4 unresolved discussions, LGTM missing from assignee jwnimmer-tri(platform), needs at least two assigned reviewers (waiting on @DamrongGuoy and @jwnimmer-tri)

---

geometry/proximity_engine.cc, line 998 at r6 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

The macro is not sensitive to function arity. Maybe there was some other typo? In any case, this patch builds OK for me:

--- a/geometry/proximity_engine.cc
+++ b/geometry/proximity_engine.cc
@@ -989,24 +989,14 @@ bool ProximityEngine<T>::IsFclConvexType(GeometryId id) const {
   return impl_->IsFclConvexType(id);
 }
 
-DRAKE_DEFINE_FUNCTION_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS(
-    (&ProximityEngine<T>::template ToScalarType<U>))
-
-DRAKE_DEFINE_FUNCTION_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS(
-    (&ProximityEngine<T>::template ComputeContactSurfaces<T>))
-
-// The DEFINE_FUNCTION_TEMPLATE macro doesn't handle 4 arguments yet.
-template void ProximityEngine<double>::ComputeContactSurfacesWithFallback<
-    double>(HydroelasticContactRepresentation representation,
-            const std::unordered_map<GeometryId, RigidTransform<double>>& X_WGs,
-            std::vector<ContactSurface<double>>* surfaces,
-            std::vector<PenetrationAsPointPair<double>>* point_pairs) const;
-template void
-ProximityEngine<AutoDiffXd>::ComputeContactSurfacesWithFallback<AutoDiffXd>(
-    HydroelasticContactRepresentation representation,
-    const std::unordered_map<GeometryId, RigidTransform<AutoDiffXd>>& X_WGs,
-    std::vector<ContactSurface<AutoDiffXd>>* surfaces,
-    std::vector<PenetrationAsPointPair<AutoDiffXd>>* point_pairs) const;
+DRAKE_DEFINE_FUNCTION_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS((
+    &ProximityEngine<T>::template ToScalarType<U>
+))
+
+DRAKE_DEFINE_FUNCTION_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS((
+    &ProximityEngine<T>::template ComputeContactSurfaces<T>,
+    &ProximityEngine<T>::template ComputeContactSurfacesWithFallback<T>
+))
 
 }  // namespace internal
 }  // namespace geometry

Done. Thanks. IDK what I did wrong before.

---

geometry/query_object.h, line 107 at r7 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

Several places in the API docs for this class that discuss scalar support specifically. All of them need to be fixed up to reflect the new innovations as of this PR.

This includes the "Characterizing the returned values" tables of support, as well as various prose like "This method provides support for both double and AutoDiffXd."

Done.

---

geometry/proximity/test/distance_to_shape_characterize_test.cc, line 12 at r7 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

At some point, the function documentation cited here needs to gain another table for Expression so that the docs and test can say in alignment. If that change isn't part of this PR, there needs to be a clear plan where it will happen in a separately PR. (It's probably easiest to do it in this PR, though.)

Done.

---

geometry/proximity/test/penetration_as_point_pair_callback_test.cc, line 499 at r7 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

Is "ellipsoid" missing here (as a negative test)? Seems like yes, at first glance.

Yes, but it's missing from the entire test family. I've added a TODO.

@DamrongGuoy -- i hope it's ok that i've added a TODO with your name on it?

---

systems/sensors/test/rgbd_sensor_test.cc, line 91 at r7 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

nit In the prior code, the dynamic_cast used a reference so that it fails-fast if the type was mismatched. We need to preserve that fail-fast, either by changing back to casting a reference, or else checking for nullptr here before returning.

Done. Looks funny to me, but I guess it should do the trick.

---

bindings/pydrake/test/geometry_scene_graph_test.py, line 321 at r7 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

nit This opt-out of Expression shows up out of the blue. In general, the other opt-outs throughout this file are clear, because the functions under test in those cases are documented as nonsymbolic. Here, it's unclear why we're skipping this check in this case. The class documentation does not seem to indicate that this member is unsupported.

Done.

[jwnimmer-tri]

Reviewed 6 of 7 files at r8, all commit messages.
Reviewable status: LGTM missing from assignee jwnimmer-tri(platform), needs at least two assigned reviewers (waiting on @jwnimmer-tri)

[RussTedrake]

+@sherm1 for platform review, please. (Jeremy says he needs to review a few more details on the tests, but that it's ok to start platform).

Reviewable status: LGTM missing from assignees jwnimmer-tri(platform),sherm1(platform) (waiting on @jwnimmer-tri and @sherm1)

[jwnimmer-tri]

Small checkpoint, progress on one test.

Reviewed 1 of 14 files at r4, 1 of 7 files at r8.
Reviewable status: 3 unresolved discussions, LGTM missing from assignees jwnimmer-tri(platform),sherm1(platform) (waiting on @jwnimmer-tri, @RussTedrake, and @sherm1)

---

geometry/query_object.h, line 307 at r8 (raw file):

     - We do not support contact between two rigid geometries. One geometry
       *must* be compliant, and the other could be rigid or compliant. If two
       rigid geometries collide, an exception will be thrown. More

nit Gratuitous line-wrap change.

Suggestion:

       *must* be compliant, and the other could be rigid or compliant. If
       two rigid geometries collide, an exception will be thrown. More

---

geometry/query_object.h, line 335 at r8 (raw file):

            contact surfaces. The ordering of the results is guaranteed to be
            consistent -- for fixed geometry poses, the results will remain the
            same.  */

nit Gratuitous line-wrap change.

Suggestion:

            consistent -- for fixed geometry poses, the results will remain
            the same.  */

---

geometry/proximity/test/distance_to_point_callback_test.cc, line 628 at r8 (raw file):

int ExpectedResult<AutoDiffXd, fcl::Ellipsoidd>() {
  return 0;
}

Using template specialization for this table of data is difficult to understand and follow. I think it would be much clearer with only one ExpectedResult function, with some sequence of "if-else" within it.

// Helper function to indicate expectation on whether I get a distance result
// based on scalar type T and fcl shape S.
template <typename T, typename S>
int ExpectedResult() {
  if constexpr (std::is_same_v<T, double>) {
    return 1;
  }
  if constexpr (std::is_same_v<T, AutoDiffXd>) {
    if (std::is_same_v<S, fcl::Cylinderd> ||
        std::is_same_v<S, fcl::Ellipsoidd>) {
      return 0;
    }
    return 1;
  }
  if constexpr (std::is_same_v<T, symbolic::Expression>) {
    return 0;
  }
  DRAKE_UNREACHABLE();
}

There is probably a further respelling to makes it even easier to use (as a constexpr literal, instead of a function), but I think getting everything into one place is sufficient for now.

[sherm1]

Platform checkpoint.

Reviewed 1 of 8 files at r1, 1 of 7 files at r2, 1 of 10 files at r3, 2 of 14 files at r4, 6 of 12 files at r6, 6 of 7 files at r8, all commit messages.
Reviewable status: 7 unresolved discussions, LGTM missing from assignees jwnimmer-tri(platform),sherm1(platform) (waiting on @jwnimmer-tri, @RussTedrake, and @sherm1)

---

math/test/rotation_matrix_test.cc, line 1168 at r8 (raw file):

      EXPECT_TRUE(CompareMatrices(R_AB.matrix(), R_AB_basic.matrix()));

      // Verify that it also works for Expression.

nit: change to "also works for an all-numerical Expression" or something like that since it still won't work for symbolic expressions (as I understand it anyway)

---

bindings/pydrake/geometry_py_scene_graph.cc, line 410 at r8 (raw file):

            cls_doc.RenderLabelImage.doc);

    if constexpr (!std::is_same_v<T, symbolic::Expression>) {

minor: this is not the same condition used in the C++ declarations (those used scalar_predicate<T1>::is_bool). Should be the same in both places.

---

bindings/pydrake/geometry_py_scene_graph.cc, line 420 at r8 (raw file):

                  HydroelasticContactRepresentation representation) {
                // For the Python bindings, we'll use return values instead of
                // output pointers.

BTW is this reflected in the Python documentation at all?

---

bindings/pydrake/test/geometry_scene_graph_test.py, line 323 at r8 (raw file):

            self.assertTrue(obj.depth.EqualTo(-1.0))
        else:
            self.assertEqual(obj.depth, -1.)

nit: format the numbers identically in the two branches

[RussTedrake]

Reviewable status: 1 unresolved discussion, LGTM missing from assignees jwnimmer-tri(platform),sherm1(platform) (waiting on @jwnimmer-tri and @sherm1)

---

geometry/query_object.h, line 335 at r8 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

nit Gratuitous line-wrap change.

Done. (but not sure why these are worth mentioning? the new version is an improvement, i think, and the cost of a few lines of obviously frivolous diff seems aok to me)

---

geometry/proximity/test/distance_to_point_callback_test.cc, line 628 at r8 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

Using template specialization for this table of data is difficult to understand and follow. I think it would be much clearer with only one ExpectedResult function, with some sequence of "if-else" within it.

// Helper function to indicate expectation on whether I get a distance result
// based on scalar type T and fcl shape S.
template <typename T, typename S>
int ExpectedResult() {
  if constexpr (std::is_same_v<T, double>) {
    return 1;
  }
  if constexpr (std::is_same_v<T, AutoDiffXd>) {
    if (std::is_same_v<S, fcl::Cylinderd> ||
        std::is_same_v<S, fcl::Ellipsoidd>) {
      return 0;
    }
    return 1;
  }
  if constexpr (std::is_same_v<T, symbolic::Expression>) {
    return 0;
  }
  DRAKE_UNREACHABLE();
}

There is probably a further respelling to makes it even easier to use (as a constexpr literal, instead of a function), but I think getting everything into one place is sufficient for now.

Done. (I agree. I inherited the template specialization, and tried to improve it a little already).

---

bindings/pydrake/geometry_py_scene_graph.cc, line 420 at r8 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW is this reflected in the Python documentation at all?

yes. (note that I didn't change it here)

[jwnimmer-tri]

feature review complete.

Reviewed 1 of 10 files at r3, 1 of 12 files at r6, 5 of 5 files at r9, all commit messages.
Reviewable status: 2 unresolved discussions, LGTM missing from assignee sherm1(platform) (waiting on @RussTedrake)

---

geometry/test/proximity_engine_test.cc, line 1683 at r9 (raw file):

  EXPECT_EQ(results.size(), 0);

  std::unique_ptr<ProximityEngine<Expression>> sym_engine =

As I study the layout of these test cases, my intuition is that keeping the symbolic-engine test for signed-distance as distinct cases is going to be notably better for long-term maintenance, i.e., inserting this line immediately above:

}

TEST_P(SignedDistanceToPointTest, SingleQueryPointSymbolic) {

It means duplicating a const double large_threshold ... line, but that doesn't see like a large cost to pay, in order to be able to customize / run / breakpoint / debug each case separately.

WDYT?

(This change is not part of the fixup PR that I filed.)

---

geometry/test/scene_graph_test.cc, line 291 at r9 (raw file):

// allocated or not, subsequent operations should be allowed.
template <typename T>
void TransmogrifyWithoutAllocation(SceneGraph<double>* scene_graph) {

These test cases were a bit of a mess already, and this PR made them measurable less idiomatic. I fixed the various problems via RussTedrake#46.

[DamrongGuoy]

Reviewable status: 2 unresolved discussions, LGTM missing from assignee sherm1(platform) (waiting on @RussTedrake)

---

geometry/proximity/test/penetration_as_point_pair_callback_test.cc, line 499 at r7 (raw file):

Previously, RussTedrake (Russ Tedrake) wrote…

Yes, but it's missing from the entire test family. I've added a TODO.

@DamrongGuoy -- i hope it's ok that i've added a TODO with your name on it?

It's appropriate. Please go ahead.

[sherm1]

Whew! Platform

Reviewed 1 of 8 files at r1, 6 of 10 files at r3, 3 of 14 files at r4, 1 of 1 files at r5, 2 of 12 files at r6, 1 of 7 files at r8, 5 of 5 files at r9, all commit messages.
Reviewable status: 2 unresolved discussions (waiting on @jwnimmer-tri and @RussTedrake)

---

geometry/proximity/test/distance_to_shape_characterize_test.cc, line 221 at r9 (raw file):

        QueryInstance(kHalfSpace, kSphere, kThrows),

        QueryInstance(kSphere, kSphere, kThrows)),

BTW nowhere to go but up from here!

---

geometry/test/scene_graph_test.cc, line 291 at r9 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

These test cases were a bit of a mess already, and this PR made them measurable less idiomatic. I fixed the various problems via RussTedrake#46.

FYI I platform reviewed Jeremy's fix PR assuming you'll merge it Russ. LMK if not.

[RussTedrake]

Reviewable status: 2 unresolved discussions, commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @jwnimmer-tri and @sherm1)

---

geometry/proximity/test/distance_to_shape_characterize_test.cc, line 221 at r9 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW nowhere to go but up from here!

😆

---

geometry/test/proximity_engine_test.cc, line 1683 at r9 (raw file):

Previously, jwnimmer-tri (Jeremy Nimmer) wrote…

As I study the layout of these test cases, my intuition is that keeping the symbolic-engine test for signed-distance as distinct cases is going to be notably better for long-term maintenance, i.e., inserting this line immediately above:

}

TEST_P(SignedDistanceToPointTest, SingleQueryPointSymbolic) {

It means duplicating a const double large_threshold ... line, but that doesn't see like a large cost to pay, in order to be able to customize / run / breakpoint / debug each case separately.

WDYT?

(This change is not part of the fixup PR that I filed.)

Done.

---

geometry/test/scene_graph_test.cc, line 291 at r9 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

FYI I platform reviewed Jeremy's fix PR assuming you'll merge it Russ. LMK if not.

Done.

[jwnimmer-tri]

Reviewed 1 of 1 files at r10, 1 of 1 files at r11, all commit messages.
Reviewable status: commits need curation (https://drake.mit.edu/reviewable.html#curated-commits) (waiting on @RussTedrake)