[geometry] Add support for SceneGraph<Expression>

bindings/pydrake/geometry_py_scene_graph.cc

@@ -361,23 +361,6 @@ void DoScalarDependentDefinitions(py::module m, T) {
         .def("ComputePointPairPenetration",
             &QueryObject<T>::ComputePointPairPenetration,
             cls_doc.ComputePointPairPenetration.doc)
-        .def("ComputeContactSurfaces", &Class::ComputeContactSurfaces,
-            py::arg("representation"), cls_doc.ComputeContactSurfaces.doc)
-        .def(
-            "ComputeContactSurfacesWithFallback",
-            [](const Class* self,
-                HydroelasticContactRepresentation representation) {
-              // For the Python bindings, we'll use return values instead of
-              // output pointers.
-              std::vector<ContactSurface<T>> surfaces;
-              std::vector<PenetrationAsPointPair<T>> point_pairs;
-              self->ComputeContactSurfacesWithFallback(
-                  representation, &surfaces, &point_pairs);
-              return std::make_pair(
-                  std::move(surfaces), std::move(point_pairs));
-            },
-            py::arg("representation"),
-            cls_doc.ComputeContactSurfacesWithFallback.doc)
         .def("ComputeSignedDistanceToPoint",
             &QueryObject<T>::ComputeSignedDistanceToPoint, py::arg("p_WQ"),
             py::arg("threshold") = std::numeric_limits<double>::infinity(),
@@ -424,6 +407,28 @@ void DoScalarDependentDefinitions(py::module m, T) {
             py::arg("camera"), py::arg("parent_frame"), py::arg("X_PC"),
             cls_doc.RenderLabelImage.doc);
 
+    if constexpr (scalar_predicate<T>::is_bool) {
+      cls  // BR
+          .def("ComputeContactSurfaces",
+              &Class::template ComputeContactSurfaces<T>,
+              py::arg("representation"), cls_doc.ComputeContactSurfaces.doc)
+          .def(
+              "ComputeContactSurfacesWithFallback",
+              [](const Class* self,
+                  HydroelasticContactRepresentation representation) {
+                // For the Python bindings, we'll use return values instead of
+                // output pointers.
+                std::vector<ContactSurface<T>> surfaces;
+                std::vector<PenetrationAsPointPair<T>> point_pairs;
+                self->template ComputeContactSurfacesWithFallback<T>(
+                    representation, &surfaces, &point_pairs);
+                return std::make_pair(
+                    std::move(surfaces), std::move(point_pairs));
+              },
+              py::arg("representation"),
+              cls_doc.ComputeContactSurfacesWithFallback.doc);
+    }
+
     AddValueInstantiation<QueryObject<T>>(m);
   }
 
@@ -492,7 +497,7 @@ void DoScalarDependentDefinitions(py::module m, T) {
   }
 
   // ContactSurface
-  {
+  if constexpr (scalar_predicate<T>::is_bool) {
     using Class = ContactSurface<T>;
     constexpr auto& cls_doc = doc.ContactSurface;
     auto cls = DefineTemplateClassWithDefault<Class>(
@@ -537,7 +542,7 @@ void DefineGeometrySceneGraph(py::module m) {
   py::module::import("pydrake.systems.framework");
   DoScalarIndependentDefinitions(m);
   type_visit([m](auto dummy) { DoScalarDependentDefinitions(m, dummy); },
-      NonSymbolicScalarPack{});
+      CommonScalarPack{});
 }
 }  // namespace pydrake
 }  // namespace drake

bindings/pydrake/test/geometry_scene_graph_test.py

@@ -7,6 +7,7 @@
 from pydrake.common.test_utilities.deprecation import catch_drake_warnings
 from pydrake.common.value import Value
 from pydrake.math import RigidTransform_
+from pydrake.symbolic import Expression
 from pydrake.systems.framework import InputPort_, OutputPort_
 from pydrake.systems.sensors import (
     CameraInfo,
@@ -22,7 +23,7 @@ def test_hydroelastic_contact_representation_enum(self):
         mut.HydroelasticContactRepresentation.kTriangle
         mut.HydroelasticContactRepresentation.kPolygon
 
-    @numpy_compare.check_nonsymbolic_types
+    @numpy_compare.check_all_types
     def test_scene_graph_api(self, T):
         SceneGraph = mut.SceneGraph_[T]
         InputPort = InputPort_[T]
@@ -293,7 +294,7 @@ def test_scene_graph_api(self, T):
                     role=role),
                 1)
 
-    @numpy_compare.check_nonsymbolic_types
+    @numpy_compare.check_all_types
     def test_frame_pose_vector_api(self, T):
         FramePoseVector = mut.FramePoseVector_[T]
         RigidTransform = RigidTransform_[T]
@@ -309,16 +310,19 @@ def test_frame_pose_vector_api(self, T):
         obj.clear()
         self.assertEqual(obj.size(), 0)
 
-    @numpy_compare.check_nonsymbolic_types
+    @numpy_compare.check_all_types
     def test_penetration_as_point_pair_api(self, T):
         obj = mut.PenetrationAsPointPair_[T]()
         self.assertIsInstance(obj.id_A, mut.GeometryId)
         self.assertIsInstance(obj.id_B, mut.GeometryId)
         self.assertTupleEqual(obj.p_WCa.shape, (3,))
         self.assertTupleEqual(obj.p_WCb.shape, (3,))
-        self.assertEqual(obj.depth, -1.)
+        if T == Expression:
+            self.assertTrue(obj.depth.EqualTo(-1.0))
+        else:
+            self.assertEqual(obj.depth, -1.0)
 
-    @numpy_compare.check_nonsymbolic_types
+    @numpy_compare.check_all_types
     def test_signed_distance_api(self, T):
         obj = mut.SignedDistancePair_[T]()
         self.assertIsInstance(obj.id_A, mut.GeometryId)
@@ -328,15 +332,15 @@ def test_signed_distance_api(self, T):
         self.assertIsInstance(obj.distance, T)
         self.assertTupleEqual(obj.nhat_BA_W.shape, (3,))
 
-    @numpy_compare.check_nonsymbolic_types
+    @numpy_compare.check_all_types
     def test_signed_distance_to_point_api(self, T):
         obj = mut.SignedDistanceToPoint_[T]()
         self.assertIsInstance(obj.id_G, mut.GeometryId)
         self.assertTupleEqual(obj.p_GN.shape, (3,))
         self.assertIsInstance(obj.distance, T)
         self.assertTupleEqual(obj.grad_W.shape, (3,))
 
-    @numpy_compare.check_nonsymbolic_types
+    @numpy_compare.check_all_types
     def test_query_object(self, T):
         RigidTransform = RigidTransform_[float]
         SceneGraph = mut.SceneGraph_[T]
@@ -384,13 +388,15 @@ def test_query_object(self, T):
         self.assertEqual(len(results), 0)
         results = query_object.ComputePointPairPenetration()
         self.assertEqual(len(results), 0)
-        hydro_rep = mut.HydroelasticContactRepresentation.kTriangle
-        results = query_object.ComputeContactSurfaces(representation=hydro_rep)
-        self.assertEqual(len(results), 0)
-        surfaces, results = query_object.ComputeContactSurfacesWithFallback(
-            representation=hydro_rep)
-        self.assertEqual(len(surfaces), 0)
-        self.assertEqual(len(results), 0)
+        if T != Expression:
+            hydro_rep = mut.HydroelasticContactRepresentation.kTriangle
+            results = query_object.ComputeContactSurfaces(
+                representation=hydro_rep)
+            self.assertEqual(len(results), 0)
+            surfaces, results = query_object.ComputeContactSurfacesWithFallback(  # noqa
+                representation=hydro_rep)
+            self.assertEqual(len(surfaces), 0)
+            self.assertEqual(len(results), 0)
         results = query_object.ComputeSignedDistanceToPoint(p_WQ=(1, 2, 3))
         self.assertEqual(len(results), 0)
         results = query_object.FindCollisionCandidates()
@@ -432,7 +438,7 @@ def test_query_object(self, T):
             X_PC=RigidTransform())
         self.assertIsInstance(image, ImageLabel16I)
 
-    @numpy_compare.check_nonsymbolic_types
+    @numpy_compare.check_all_types
     def test_value_instantiations(self, T):
         Value[mut.FramePoseVector_[T]]
         Value[mut.QueryObject_[T]]

geometry/geometry_state.cc

@@ -10,6 +10,7 @@
 
 #include "drake/common/autodiff.h"
 #include "drake/common/default_scalars.h"
+#include "drake/common/extract_double.h"
 #include "drake/common/text_logging.h"
 #include "drake/geometry/geometry_frame.h"
 #include "drake/geometry/geometry_instance.h"
@@ -1024,8 +1025,10 @@ void GeometryState<T>::RenderLabelImage(const ColorRenderCamera& camera,
 }
 
 template <typename T>
-std::unique_ptr<GeometryState<AutoDiffXd>> GeometryState<T>::ToAutoDiffXd()
-    const {
+template <typename T1>
+typename std::enable_if_t<!std::is_same_v<T1, symbolic::Expression>,
+                          std::unique_ptr<GeometryState<AutoDiffXd>>>
+GeometryState<T>::ToAutoDiffXd() const {
   return std::unique_ptr<GeometryState<AutoDiffXd>>(
       new GeometryState<AutoDiffXd>(*this));
 }
@@ -1390,11 +1393,14 @@ RigidTransformd GeometryState<T>::GetDoubleWorldPose(FrameId frame_id) const {
   return internal::convert_to_double(X_WF_[frame.index()]);
 }
 
+// Explicit instantiations.
+template std::unique_ptr<GeometryState<AutoDiffXd>>
+    GeometryState<double>::ToAutoDiffXd<double>() const;
+template std::unique_ptr<GeometryState<AutoDiffXd>>
+    GeometryState<AutoDiffXd>::ToAutoDiffXd<AutoDiffXd>() const;
+
 }  // namespace geometry
 }  // namespace drake
 
-// TODO(SeanCurtis-TRI): Currently assumes that "non-symbolic" implies
-// AutoDiffXd. Update things appropriately when more non-symbolic scalars
-// are available.
-DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS(
+DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS(
     class ::drake::geometry::GeometryState)

geometry/geometry_state.h

@@ -63,7 +63,7 @@ using FrameIdSet = std::unordered_set<FrameId>;
 
  @note This is intended as an internal class only.
 
- @tparam_nonsymbolic_scalar
+ @tparam_default_scalar
 */
 template <typename T>
 class GeometryState {
@@ -403,13 +403,18 @@ class GeometryState {
   }
 
   /** Implementation of QueryObject::ComputeContactSurfaces().  */
-  std::vector<ContactSurface<T>> ComputeContactSurfaces(
+  template <typename T1 = T>
+  typename std::enable_if_t<scalar_predicate<T1>::is_bool,
+                            std::vector<ContactSurface<T>>>
+  ComputeContactSurfaces(
       HydroelasticContactRepresentation representation) const {
     return geometry_engine_->ComputeContactSurfaces(representation, X_WGs_);
   }
 
   /** Implementation of QueryObject::ComputeContactSurfacesWithFallback().  */
-  void ComputeContactSurfacesWithFallback(
+  template <typename T1 = T>
+  typename std::enable_if_t<scalar_predicate<T1>::is_bool, void>
+  ComputeContactSurfacesWithFallback(
       HydroelasticContactRepresentation representation,
       std::vector<ContactSurface<T>>* surfaces,
       std::vector<PenetrationAsPointPair<T>>* point_pairs) const {
@@ -530,7 +535,10 @@ class GeometryState {
    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.  */
-  std::unique_ptr<GeometryState<AutoDiffXd>> ToAutoDiffXd() const;
+  template <typename T1 = T>
+  typename std::enable_if_t<!std::is_same_v<T1, symbolic::Expression>,
+                            std::unique_ptr<GeometryState<AutoDiffXd>>>
+  ToAutoDiffXd() const;
 
   //@}
 
@@ -539,11 +547,14 @@ class GeometryState {
   template <typename>
   friend class GeometryState;
 
-  // Conversion constructor. In the initial implementation, this is only
-  // intended to be used to clone an AutoDiffXd instance from a double instance.
+  // Conversion constructor.
   // It is _vitally_ important that all members are _explicitly_ accounted for
   // (either in the initialization list or in the body). Failure to do so will
   // lead to errors in the converted GeometryState instance.
+  //
+  // TODO(russt): Move this to the .cc file, support
+  // (T=AutoDiffXd,U=Expression), and remove the enable_if restriction on
+  // ToAutoDiffXd().
   template <typename U>
   explicit GeometryState(const GeometryState<U>& source)
       : self_source_(source.self_source_),
@@ -554,7 +565,8 @@ class GeometryState {
         frames_(source.frames_),
         geometries_(source.geometries_),
         frame_index_to_id_map_(source.frame_index_to_id_map_),
-        geometry_engine_(std::move(source.geometry_engine_->ToAutoDiffXd())),
+        geometry_engine_(
+            std::move(source.geometry_engine_->template ToScalarType<T>())),
         render_engines_(source.render_engines_),
         geometry_version_(source.geometry_version_) {
     auto convert_pose_vector = [](const std::vector<math::RigidTransform<U>>& s,

geometry/proximity/distance_to_point_callback.cc

@@ -1,6 +1,7 @@
 #include "drake/geometry/proximity/distance_to_point_callback.h"
 
 #include "drake/common/default_scalars.h"
+#include "drake/common/drake_bool.h"
 
 namespace drake {
 namespace geometry {
@@ -24,7 +25,9 @@ void SphereDistanceInSphereFrame(const fcl::Sphered& sphere,
   const double tolerance = DistanceToPointRelativeTolerance(radius);
   // Unit vector in x-direction of S's frame.
   const Vector3<T> Sx = Vector3<T>::UnitX();
-  const bool non_zero_displacement = (dist_SQ > tolerance);
+  // Note: Skip the zero displacement check for non-numeric T.
+  const bool non_zero_displacement =
+      !scalar_predicate<T>::is_bool || (dist_SQ > tolerance);
   // Gradient vector expressed in S's frame.
   *grad_S = non_zero_displacement ? p_SQ / dist_SQ : Sx;
 
@@ -255,7 +258,10 @@ SignedDistanceToPoint<T> DistanceToPoint<T>::operator()(
   // stored implicitly in a 2D vector, because v_GBr(2) must be zero. If Q is
   // within a tolerance from the center line, v_GBr = <1, 0, 0> by convention.
   const T r_Q = sqrt(p_GQ(0) * p_GQ(0) + p_GQ(1) * p_GQ(1));
+
+  // Note: Skip near_center_line check for non-numeric T.
   const bool near_center_line =
+      scalar_predicate<T>::is_bool &&
       (r_Q < DistanceToPointRelativeTolerance(cylinder.radius));
   const Vector2<T> v_GBr = near_center_line
                                ? Vector2<T>(1., 0.)
@@ -570,7 +576,7 @@ bool Callback(fcl::CollisionObjectd* object_A_ptr,
   return false;  // Returning false tells fcl to continue to other objects.
 }
 
-DRAKE_DEFINE_FUNCTION_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS((
+DRAKE_DEFINE_FUNCTION_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS((
     &Callback<T>
 ))
 
@@ -579,5 +585,5 @@ DRAKE_DEFINE_FUNCTION_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS((
 }  // namespace geometry
 }  // namespace drake
 
-DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS(
+DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS(
     class ::drake::geometry::internal::point_distance::DistanceToPoint)

geometry/proximity/distance_to_point_callback.h

@@ -230,7 +230,7 @@ class DistanceToPoint {
 
 template <typename T>
 struct ScalarSupport {
-  static bool is_supported(fcl::NODE_TYPE node_type) { return false; }
+  static bool is_supported(fcl::NODE_TYPE) { return false; }
 };
 
 /* Primitive support for double-valued query.  */

geometry/proximity/distance_to_shape_callback.cc

@@ -285,7 +285,7 @@ bool Callback(fcl::CollisionObjectd* object_A_ptr,
   return false;
 }
 
-DRAKE_DEFINE_FUNCTION_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS((
+DRAKE_DEFINE_FUNCTION_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS((
     &ComputeNarrowPhaseDistance<T>,
     &Callback<T>
 ))
@@ -295,5 +295,5 @@ DRAKE_DEFINE_FUNCTION_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS((
 }  // namespace geometry
 }  // namespace drake
 
-DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS(
+DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS(
     class ::drake::geometry::internal::shape_distance::DistancePairGeometry)

geometry/proximity/distance_to_shape_callback.h

@@ -230,15 +230,15 @@ void ComputeNarrowPhaseDistance(const fcl::CollisionObjectd& a,
 
 template <typename T>
 struct ScalarSupport {
-  static bool is_supported(fcl::NODE_TYPE node1, fcl::NODE_TYPE node2) {
+  static bool is_supported(fcl::NODE_TYPE, fcl::NODE_TYPE) {
     return false;
   }
 };
 
 /* Primitive support for double-valued query.  */
 template <>
 struct ScalarSupport<double> {
-  static bool is_supported(fcl::NODE_TYPE node1, fcl::NODE_TYPE node2);
+  static bool is_supported(fcl::NODE_TYPE, fcl::NODE_TYPE);
 };
 
 /* Primitive support for AutoDiff-valued query.  */

geometry/proximity/penetration_as_point_pair_callback.cc

@@ -424,7 +424,7 @@ bool Callback(fcl::CollisionObjectd* fcl_object_A_ptr,
   return false;
 }
 
-DRAKE_DEFINE_FUNCTION_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS((
+DRAKE_DEFINE_FUNCTION_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS((
     &Callback<T>
 ))
 

geometry/proximity/test/characterization_utilities.cc

@@ -716,15 +716,15 @@ Sphere CharacterizeResultTest<T>::sphere(bool) {
   return Sphere(kDistance * 100);
 }
 
-DRAKE_DEFINE_FUNCTION_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS((
+DRAKE_DEFINE_FUNCTION_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS((
     &AlignPlanes<T>
 ))
 
 }  // namespace internal
 }  // namespace geometry
 }  // namespace drake
 
-DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS(
+DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS(
   class ::drake::geometry::internal::CharacterizeResultTest)
-DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS(
+DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS(
   class ::drake::geometry::internal::ShapeConfigurations)