[geometry] Add support for SceneGraph<Expression>

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, double>,
+                          std::unique_ptr<GeometryState<AutoDiffXd>>>
+GeometryState<T>::ToAutoDiffXd() const {
   return std::unique_ptr<GeometryState<AutoDiffXd>>(
       new GeometryState<AutoDiffXd>(*this));
 }
@@ -1390,11 +1393,12 @@ RigidTransformd GeometryState<T>::GetDoubleWorldPose(FrameId frame_id) const {
   return internal::convert_to_double(X_WF_[frame.index()]);
 }
 
+// Explicit instantiation.
+template std::unique_ptr<GeometryState<AutoDiffXd>>
+    GeometryState<double>::ToAutoDiffXd<double>() 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 {
@@ -530,7 +530,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, double>,
+                            std::unique_ptr<GeometryState<AutoDiffXd>>>
+  ToAutoDiffXd() const;
 
   //@}
 
@@ -539,8 +542,7 @@ 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.
@@ -554,7 +556,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

@@ -570,7 +570,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 +579,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_engine.cc

@@ -42,6 +42,7 @@ using std::shared_ptr;
 using std::unique_ptr;
 using std::unordered_map;
 using std::vector;
+using symbolic::Expression;
 
 namespace {
 
@@ -230,8 +231,9 @@ class ProximityEngine<T>::Impl : public ShapeReifier {
   Impl(Impl&& other) = delete;
   Impl& operator=(Impl&&) = delete;
 
-  std::unique_ptr<ProximityEngine<AutoDiffXd>::Impl> ToAutoDiff() const {
-    auto engine = make_unique<ProximityEngine<AutoDiffXd>::Impl>();
+  template <typename U>
+  std::unique_ptr<typename ProximityEngine<U>::Impl> ToScalarType() const {
+    auto engine = make_unique<typename ProximityEngine<U>::Impl>();
 
     // TODO(SeanCurtis-TRI): When AutoDiff is fully supported in the internal
     // types, modify this map to the appropriate scalar and modify consuming
@@ -573,7 +575,9 @@ class ProximityEngine<T>::Impl : public ShapeReifier {
     return data.collisions_exist;
   }
 
-  vector<ContactSurface<T>> ComputeContactSurfaces(
+  template <typename T1 = T>
+  std::enable_if_t<!std::is_same_v<T1, Expression>, vector<ContactSurface<T>>>
+  ComputeContactSurfaces(
       HydroelasticContactRepresentation representation,
       const unordered_map<GeometryId, RigidTransform<T>>& X_WGs) const {
     vector<ContactSurface<T>> surfaces;
@@ -594,7 +598,9 @@ class ProximityEngine<T>::Impl : public ShapeReifier {
     return surfaces;
   }
 
-  void ComputeContactSurfacesWithFallback(
+  template <typename T1 = T>
+  std::enable_if_t<!std::is_same_v<T1, Expression>, void>
+  ComputeContactSurfacesWithFallback(
       HydroelasticContactRepresentation representation,
       const std::unordered_map<GeometryId, RigidTransform<T>>& X_WGs,
       std::vector<ContactSurface<T>>* surfaces,
@@ -874,10 +880,10 @@ double ProximityEngine<T>::distance_tolerance() const {
 }
 
 template <typename T>
-std::unique_ptr<ProximityEngine<AutoDiffXd>> ProximityEngine<T>::ToAutoDiffXd()
-    const {
-  return unique_ptr<ProximityEngine<AutoDiffXd>>(
-      new ProximityEngine<AutoDiffXd>(impl_->ToAutoDiff().release()));
+template <typename U>
+std::unique_ptr<ProximityEngine<U>> ProximityEngine<T>::ToScalarType() const {
+  return std::unique_ptr<ProximityEngine<U>>(
+      new ProximityEngine<U>(impl_->template ToScalarType<U>().release()));
 }
 
 template <typename T>
@@ -934,7 +940,13 @@ template <typename T>
 std::vector<ContactSurface<T>> ProximityEngine<T>::ComputeContactSurfaces(
     HydroelasticContactRepresentation representation,
     const std::unordered_map<GeometryId, RigidTransform<T>>& X_WGs) const {
-  return impl_->ComputeContactSurfaces(representation, X_WGs);
+  if constexpr(std::is_same_v<T, symbolic::Expression>) {
+    unused(representation, X_WGs);
+    throw std::runtime_error(
+        "Hydroelastic contact does not support symbolic::Expression.");
+  } else {
+    return impl_->ComputeContactSurfaces(representation, X_WGs);
+  }
 }
 
 template <typename T>
@@ -943,8 +955,14 @@ void ProximityEngine<T>::ComputeContactSurfacesWithFallback(
     const std::unordered_map<GeometryId, RigidTransform<T>>& X_WGs,
     std::vector<ContactSurface<T>>* surfaces,
     std::vector<PenetrationAsPointPair<T>>* point_pairs) const {
-  return impl_->ComputeContactSurfacesWithFallback(representation, X_WGs,
+  if constexpr(std::is_same_v<T, symbolic::Expression>) {
+    unused(representation, X_WGs, surfaces, point_pairs);
+    throw std::runtime_error(
+        "Hydroelastic contact does not support symbolic::Expression.");
+  } else {
+    return impl_->ComputeContactSurfacesWithFallback(representation, X_WGs,
                                                    surfaces, point_pairs);
+  }
 }
 
 template <typename T>
@@ -977,9 +995,12 @@ bool ProximityEngine<T>::IsFclConvexType(GeometryId id) const {
   return impl_->IsFclConvexType(id);
 }
 
+DRAKE_DEFINE_FUNCTION_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS(
+    (&ProximityEngine<T>::template ToScalarType<U>))
+
 }  // 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::ProximityEngine)

geometry/proximity_engine.h

@@ -52,7 +52,7 @@ namespace internal {
  <h3>Geometry proximity properties</h3>
  -->
 
- @tparam_nonsymbolic_scalar
+ @tparam_default_scalar
 
  @internal Historically, this replaces the DrakeCollision::Model class.  */
 template <typename T>
@@ -75,10 +75,11 @@ class ProximityEngine {
    engine will be returned to its default-initialized state.  */
   ProximityEngine& operator=(ProximityEngine&& other) noexcept;
 
-  /* Returns an independent copy of this engine templated on the AutoDiffXd
-   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;
+  /* Returns an independent copy of this engine templated on a scalar type. If
+   T=U, it is equivalent to using the copy constructor to create a duplicate on
+   the heap. */
+  template <typename U>
+  std::unique_ptr<ProximityEngine<U>> ToScalarType() const;
 
   /* Provides access to the mutable collision filter this engine uses. */
   CollisionFilter& collision_filter();

geometry/query_object.cc

@@ -223,5 +223,5 @@ const GeometryState<T>& QueryObject<T>::geometry_state() const {
 }  // namespace geometry
 }  // namespace drake
 
-DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_NONSYMBOLIC_SCALARS(
+DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS(
     class ::drake::geometry::QueryObject)

geometry/query_object.h

@@ -104,7 +104,7 @@ class SceneGraph;
      report no more than 1e-14 error across all supportable geometry pairs
      and scalars. At that point, the table will simply disappear.
 
- @tparam_nonsymbolic_scalar
+ @tparam_default_scalar
 */
 template <typename T>
 class QueryObject {

geometry/scene_graph.cc

@@ -100,11 +100,7 @@ template <typename T>
 template <typename U>
 SceneGraph<T>::SceneGraph(const SceneGraph<U>& other)
     : SceneGraph() {
-  // TODO(SeanCurtis-TRI) This is very brittle; we are essentially assuming that
-  //  T = AutoDiffXd. For now, that's true. If we ever support
-  //  symbolic::Expression, this U --> T conversion will have to be more
-  //  generic.
-  model_ = *(other.model_.ToAutoDiffXd());
+  model_ = GeometryState<T>(other.model_);
 
   // We need to guarantee that the same source ids map to the same port indices.
   // We'll do this by processing the source ids in monotonically increasing
@@ -444,9 +440,15 @@ const GeometryState<T>& SceneGraph<T>::geometry_state(
       .template get_abstract_parameter<GeometryState<T>>(geometry_state_index_);
 }
 
-// Explicitly instantiates on the most common scalar types.
-template class SceneGraph<double>;
-template class SceneGraph<AutoDiffXd>;
-
 }  // namespace geometry
+
+namespace systems {
+namespace scalar_conversion {
+template <> struct Traits<geometry::SceneGraph> : public FromDoubleTraits {};
+}  // namespace scalar_conversion
+}  // namespace systems
+
 }  // namespace drake
+
+DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS(
+    class ::drake::geometry::SceneGraph)

geometry/scene_graph.h

@@ -264,7 +264,7 @@ with <em style="color:gray">(source name)</em>.
  //   - Finalizing API for topology changes at discrete events.
  @endcond
 
- @tparam_nonsymbolic_scalar
+ @tparam_default_scalar
  @ingroup systems
  */
 template <typename T>
@@ -906,18 +906,7 @@ class SceneGraph final : public systems::LeafSystem<T> {
 };
 
 }  // namespace geometry
-
-// Define the conversion trait to *only* allow double -> AutoDiffXd conversion.
-// Symbolic is not supported yet, and AutoDiff -> double doesn't "make sense".
-namespace systems {
-namespace scalar_conversion {
-template <>
-struct Traits<geometry::SceneGraph> {
-  template <typename T, typename U>
-  using supported = typename std::bool_constant<
-    std::is_same_v<U, double> && !std::is_same_v<T, symbolic::Expression>>;
-};
-}  // namespace scalar_conversion
-}  // namespace systems
-
 }  // namespace drake
+
+DRAKE_DECLARE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS(
+    class ::drake::geometry::SceneGraph)

geometry/scene_graph_inspector.cc

@@ -25,9 +25,8 @@ SceneGraphInspector<T>::CloneGeometryInstance(GeometryId id) const {
   return geometry_instance;
 }
 
-// Explicitly instantiates on the most common scalar types.
-template class SceneGraphInspector<double>;
-template class SceneGraphInspector<AutoDiffXd>;
-
 }  // namespace geometry
 }  // namespace drake
+
+DRAKE_DEFINE_CLASS_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS(
+    class ::drake::geometry::SceneGraphInspector)