[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));
 }
@@ -1393,8 +1396,5 @@ RigidTransformd GeometryState<T>::GetDoubleWorldPose(FrameId frame_id) 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

@@ -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_engine.cc

@@ -230,8 +230,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
@@ -877,7 +878,15 @@ template <typename T>
 std::unique_ptr<ProximityEngine<AutoDiffXd>> ProximityEngine<T>::ToAutoDiffXd()
     const {
   return unique_ptr<ProximityEngine<AutoDiffXd>>(
-      new ProximityEngine<AutoDiffXd>(impl_->ToAutoDiff().release()));
+      new ProximityEngine<AutoDiffXd>(
+          impl_->template ToScalarType<AutoDiffXd>().release()));
+}
+
+template <typename T>
+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>
@@ -977,9 +986,16 @@ bool ProximityEngine<T>::IsFclConvexType(GeometryId id) const {
   return impl_->IsFclConvexType(id);
 }
 
+// Explicit instantiations.
+template std::unique_ptr<ProximityEngine<AutoDiffXd>>
+ProximityEngine<double>::ToScalarType() const;
+
+template std::unique_ptr<ProximityEngine<symbolic::Expression>>
+ProximityEngine<double>::ToScalarType() const;
+
 }  // 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

@@ -80,6 +80,9 @@ class ProximityEngine {
    to using the copy constructor to create a duplicate on the heap.  */
   std::unique_ptr<ProximityEngine<AutoDiffXd>> ToAutoDiffXd() const;
 
+  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/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

@@ -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/utilities.h

@@ -8,6 +8,7 @@
 #include "drake/common/autodiff_overloads.h"
 #include "drake/common/drake_copyable.h"
 #include "drake/common/eigen_types.h"
+#include "drake/common/extract_double.h"
 #include "drake/math/rigid_transform.h"
 
 namespace drake {
@@ -84,9 +85,9 @@ inline const Vector3<double>& convert_to_double(const Vector3<double>& vec) {
   return vec;
 }
 
-template <class VectorType>
+template <class T>
 Vector3<double> convert_to_double(
-    const Vector3<Eigen::AutoDiffScalar<VectorType>>& vec) {
+    const Vector3<T>& vec) {
   Vector3<double> result;
   for (int r = 0; r < 3; ++r) {
     result(r) = ExtractDoubleOrThrow(vec(r));
@@ -114,6 +115,13 @@ math::RigidTransformd convert_to_double(
   return math::RigidTransformd(math::RotationMatrixd(R_converted), p_converted);
 }
 
+inline math::RigidTransformd convert_to_double(
+    const math::RigidTransform<symbolic::Expression>& X_AB) {
+  return math::RigidTransform<double>(
+      ExtractDoubleOrThrow(X_AB.GetAsMatrix34()));
+}
+
+
 //@}
 
 }  // namespace internal