Stripping out Isometry3 from geometry/ (RobotLocomotion#11869)

* Stripping out Isometry3 from geometry/

1. Remove all internal references to Isometry3 with the following
   exceptions:
   - ProximityEngine gets Iso3 from RT to pass to FCL (2X)
   - SceneGraph's PoseBundle output port still uses Iso3
2. Modify public API with a single deprecated method:
  - GeometryInstance::GeometryInstance(Isometry3)
    and a number of methods relying on the implicit 
    RT->ISo3 conversion to change return types.
3. Account for deprecated methods in various call sites in attic, bindings,
   examples, multibody, and sensors.

attic/manipulation/util/frame_pose_tracker.cc

@@ -11,6 +11,7 @@ namespace manipulation {
 namespace util {
 
 using drake::systems::KinematicsResults;
+using drake::math::RigidTransformd;
 
 FramePoseTracker::FramePoseTracker(
     const RigidBodyTree<double>& tree,
@@ -120,8 +121,8 @@ void FramePoseTracker::OutputStatus(
        it != frame_name_to_frame_map_.end(); ++it) {
     output->set_value(
         frame_name_to_id_map_.at(it->first),
-        tree_->CalcFramePoseInWorldFrame(
-            kinematic_results->get_cache(), *(it->second.get())));
+        RigidTransformd(tree_->CalcFramePoseInWorldFrame(
+            kinematic_results->get_cache(), *(it->second.get()))));
   }
 }
 

attic/multibody/rigid_body_plant/rigid_body_plant_bridge.cc

@@ -31,6 +31,7 @@ using geometry::render::RenderLabel;
 using geometry::SceneGraph;
 using geometry::Shape;
 using geometry::Sphere;
+using math::RigidTransform;
 
 namespace {
 
@@ -148,23 +149,23 @@ void RigidBodyPlantBridge<T>::RegisterTree(SceneGraph<T>* scene_graph) {
 
     // Default to the world body configuration.
     FrameId body_id = scene_graph->world_frame_id();
-    RenderLabel label;
     if (body.get_body_index() != tree_->world().get_body_index()) {
-      // All other bodies register a frame and (possibly) get a unique label.
+      // All other bodies register a frame.
       body_id = scene_graph->RegisterFrame(
           source_id_,
           GeometryFrame(body.get_name(), body.get_model_instance_id()));
-
-      if (body.get_visual_elements().size() > 0) {
-        // We'll have the render label map to the body index.
-        // NOTE: This is only valid if the RBT is the only source of geometry.
-        // But given that the RBT is on the way out, why not?
-        label = RenderLabel(static_cast<int>(body.get_body_index()));
-        label_to_index_[label] = body.get_body_index();
-      }
     }
     body_ids_.push_back(body_id);
 
+    // By default, the render label value is the body index value.
+    RenderLabel label(static_cast<int>(body.get_body_index()));
+    if (body.get_visual_elements().size() > 0) {
+      // We'll have the render label map to the body index.
+      // NOTE: This is only valid if the RBT is the only source of geometry.
+      // But given that the RBT is on the way out, why not?
+      label_to_index_[label] = body.get_body_index();
+    }
+
     // TODO(SeanCurtis-TRI): Detect if equivalent shapes are used for visual
     // and collision and then simply assign it additional roles. This is an
     // optimization.
@@ -231,8 +232,8 @@ void RigidBodyPlantBridge<T>::CalcFramePoseOutput(
   // When we start skipping welded frames, or frames without geometry, this
   // mapping won't be so trivial.
   for (size_t i = 1; i < tree_->get_bodies().size(); ++i) {
-    poses->set_value(body_ids_[i],
-                     tree_->relativeTransform(cache, world_body, i));
+    poses->set_value(body_ids_[i], RigidTransform<T>(tree_->relativeTransform(
+                                       cache, world_body, i)));
   }
 }
 

bindings/pydrake/test/geometry_test.py

@@ -8,10 +8,9 @@
 
 from pydrake.autodiffutils import AutoDiffXd
 from pydrake.common import FindResourceOrThrow
-from pydrake.common.eigen_geometry import Isometry3_
 from pydrake.common.test_utilities import numpy_compare
 from pydrake.lcm import DrakeMockLcm
-from pydrake.math import RigidTransform
+from pydrake.math import RigidTransform_
 from pydrake.symbolic import Expression
 from pydrake.systems.framework import DiagramBuilder_, InputPort_, OutputPort_
 from pydrake.systems.sensors import (
@@ -75,13 +74,13 @@ def test_connect_drake_visualizer(self):
     @numpy_compare.check_nonsymbolic_types
     def test_frame_pose_vector_api(self, T):
         FramePoseVector = mut.FramePoseVector_[T]
-        Isometry3 = Isometry3_[T]
+        RigidTransform = RigidTransform_[T]
         obj = FramePoseVector()
         frame_id = mut.FrameId.get_new_id()
 
-        obj.set_value(id=frame_id, value=Isometry3.Identity())
+        obj.set_value(id=frame_id, value=RigidTransform.Identity())
         self.assertEqual(obj.size(), 1)
-        self.assertIsInstance(obj.value(id=frame_id), Isometry3)
+        self.assertIsInstance(obj.value(id=frame_id), RigidTransform)
         self.assertTrue(obj.has_id(id=frame_id))
         self.assertIsInstance(obj.frame_ids(), list)
         self.assertIsInstance(obj.frame_ids()[0], mut.FrameId)
@@ -197,6 +196,7 @@ def test_render_label(self):
 
     @numpy_compare.check_nonsymbolic_types
     def test_query_object(self, T):
+        RigidTransform = RigidTransform_[float]
         SceneGraph = mut.SceneGraph_[T]
         QueryObject = mut.QueryObject_[T]
         SceneGraphInspector = mut.SceneGraphInspector_[T]

examples/contact_model/bowling_ball.cc

@@ -31,7 +31,6 @@
 #include "drake/common/eigen_types.h"
 #include "drake/common/find_resource.h"
 #include "drake/geometry/geometry_visualization.h"
-#include "drake/lcm/drake_lcm.h"
 #include "drake/multibody/parsers/urdf_parser.h"
 #include "drake/multibody/rigid_body_plant/rigid_body_plant.h"
 #include "drake/multibody/rigid_body_plant/rigid_body_plant_bridge.h"
@@ -43,9 +42,7 @@
 namespace drake {
 namespace systems {
 
-using drake::lcm::DrakeLcm;
 using multibody::joints::kQuaternion;
-using Eigen::VectorXd;
 using std::make_unique;
 
 // Simulation parameters.

examples/pendulum/pendulum_geometry.cc

@@ -103,7 +103,7 @@ void PendulumGeometry::OutputGeometryPose(
   const double theta = input.theta();
   const math::RigidTransformd pose(math::RotationMatrixd::MakeYRotation(theta));
 
-  *poses = {{frame_id_, pose.GetAsIsometry3()}};
+  *poses = {{frame_id_, pose}};
 }
 
 }  // namespace pendulum

examples/planar_gripper/gripper_brick.cc

@@ -90,7 +90,7 @@ GripperBrickHelper<T>::GripperBrickHelper() {
       inspector.GetShape(finger_tip_geometry_id);
   finger_tip_radius_ =
       dynamic_cast<const geometry::Sphere&>(fingertip_shape).get_radius();
-  p_L2Tip_ = inspector.X_FG(finger_tip_geometry_id).translation();
+  p_L2Tip_ = inspector.GetPoseInFrame(finger_tip_geometry_id).translation();
   const geometry::Shape& brick_shape =
       inspector.GetShape(inspector.GetGeometryIdByName(
           plant_->GetBodyFrameIdOrThrow(

examples/quadrotor/quadrotor_geometry.cc

@@ -69,7 +69,7 @@ void QuadrotorGeometry::OutputGeometryPose(
       math::RollPitchYawd(state.segment<3>(3)),
       state.head<3>());
 
-  *poses = {{frame_id_, pose.GetAsIsometry3()}};
+  *poses = {{frame_id_, pose}};
 }
 
 }  // namespace quadrotor

examples/scene_graph/bouncing_ball_plant.cc

@@ -27,6 +27,8 @@ using geometry::render::RenderLabel;
 using geometry::SceneGraph;
 using geometry::SourceId;
 using geometry::Sphere;
+using math::RigidTransform;
+using math::RigidTransformd;
 using std::make_unique;
 using systems::Context;
 
@@ -55,7 +57,7 @@ BouncingBallPlant<T>::BouncingBallPlant(SourceId source_id,
       source_id, GeometryFrame("ball_frame"));
   ball_id_ = scene_graph->RegisterGeometry(
       source_id, ball_frame_id_,
-      make_unique<GeometryInstance>(Isometry3<double>::Identity(), /*X_FG*/
+      make_unique<GeometryInstance>(RigidTransformd::Identity(), /*X_FG*/
                                     make_unique<Sphere>(diameter_ / 2.0),
                                     "ball"));
   // Use the default material.
@@ -107,9 +109,9 @@ void BouncingBallPlant<T>::CopyStateToOutput(
 template <typename T>
 void BouncingBallPlant<T>::CalcFramePoseOutput(
     const Context<T>& context, FramePoseVector<T>* poses) const {
-  Isometry3<T> pose = Isometry3<T>::Identity();
+  RigidTransform<T> pose = RigidTransform<T>::Identity();
   const BouncingBallVector<T>& state = get_state(context);
-  pose.translation() << p_WB_.x(), p_WB_.y(), state.z();
+  pose.set_translation({p_WB_.x(), p_WB_.y(), state.z()});
   *poses = {{ball_frame_id_, pose}};
 }
 

examples/scene_graph/solar_system.cc

@@ -17,6 +17,9 @@ namespace drake {
 namespace examples {
 namespace solar_system {
 
+using Eigen::AngleAxisd;
+using Eigen::Translation3d;
+using Eigen::Vector3d;
 using Eigen::Vector4d;
 using geometry::Box;
 using geometry::Convex;
@@ -31,6 +34,7 @@ using geometry::SceneGraph;
 using geometry::Mesh;
 using geometry::SourceId;
 using geometry::Sphere;
+using math::RigidTransformd;
 using systems::BasicVector;
 using systems::Context;
 using systems::ContinuousState;
@@ -123,15 +127,13 @@ void MakeArm(SourceId source_id, ParentId parent_id, double length,
              SceneGraph<double>* scene_graph) {
   // tilt it horizontally
   const math::RigidTransform<double> arm_pose(
-      Eigen::AngleAxis<double>(M_PI / 2, Vector3<double>::UnitY()),
-      Vector3<double>(length / 2, 0, 0));
+      AngleAxisd(M_PI / 2, Vector3d::UnitY()), Vector3d(length / 2, 0, 0));
   scene_graph->RegisterGeometry(
       source_id, parent_id,
       MakeShape<Cylinder>(arm_pose.GetAsIsometry3(), "HorzArm", material,
                           radius, length));
 
-  const math::RigidTransform<double> post_pose(
-      Vector3<double>(length, 0, height / 2));
+  const math::RigidTransform<double> post_pose(Vector3d(length, 0, height / 2));
   scene_graph->RegisterGeometry(
       source_id, parent_id,
       MakeShape<Cylinder>(post_pose.GetAsIsometry3(), "VertArm", material,
@@ -152,13 +154,13 @@ void SolarSystem<T>::AllocateGeometry(SceneGraph<T>* scene_graph) {
   // NOTE: we don't store the id of the sun geometry because we have no need
   // for subsequent access (the same is also true for dynamic geometries).
   scene_graph->RegisterAnchoredGeometry(
-      source_id_, MakeShape<Sphere>(Isometry3<double>::Identity(), "Sun",
+      source_id_, MakeShape<Sphere>(RigidTransformd::Identity(), "Sun",
                                     Vector4d(1, 1, 0, 1), 1.0 /* radius */));
 
   // The fixed post on which Sun sits and around which all planets rotate.
   const double post_height = 1;
-  Isometry3<double> post_pose = Isometry3<double>::Identity();
-  post_pose.translation() << 0, 0, (orrery_bottom + post_height / 2);
+  const RigidTransformd post_pose(
+      Translation3d{0, 0, orrery_bottom + post_height / 2});
   scene_graph->RegisterAnchoredGeometry(
       source_id_, MakeShape<Cylinder>(post_pose, "Post", post_material,
                                       pipe_radius, post_height));
@@ -171,18 +173,17 @@ void SolarSystem<T>::AllocateGeometry(SceneGraph<T>* scene_graph) {
   // Earth's orbital frame Oe lies directly *below* the sun (to account for the
   // orrery arm).
   const double kEarthBottom = orrery_bottom + 0.25;
-  Isometry3<double> X_SOe{Translation3<double>{0, 0, kEarthBottom}};
+  const RigidTransformd X_SOe{Translation3d{0, 0, kEarthBottom}};
   FrameId planet_id =
       scene_graph->RegisterFrame(source_id_, GeometryFrame("EarthOrbit"));
   body_ids_.push_back(planet_id);
   body_offset_.push_back(X_SOe);
-  axes_.push_back(Vector3<double>::UnitZ());
+  axes_.push_back(Vector3d::UnitZ());
 
   // The geometry is rigidly affixed to Earth's orbital frame so that it moves
   // in a circular path.
   const double kEarthOrbitRadius = 3.0;
-  Isometry3<double> X_OeE{
-      Translation3<double>{kEarthOrbitRadius, 0, -kEarthBottom}};
+  RigidTransformd X_OeE{Translation3d{kEarthOrbitRadius, 0, -kEarthBottom}};
   scene_graph->RegisterGeometry(
       source_id_, planet_id,
       MakeShape<Sphere>(X_OeE, "Earth", Vector4d(0, 0, 1, 1), 0.25));
@@ -192,12 +193,12 @@ void SolarSystem<T>::AllocateGeometry(SceneGraph<T>* scene_graph) {
 
   // Luna's orbital frame Ol is at the center of Earth's geometry (E).
   // So, X_OeOl = X_OeE.
-  const Isometry3<double>& X_OeOl = X_OeE;
-  FrameId luna_id = scene_graph->RegisterFrame(source_id_, planet_id,
-                                               GeometryFrame("LunaOrbit"));
+  const RigidTransformd& X_OeOl = X_OeE;
+  FrameId luna_id = scene_graph->RegisterFrame(
+      source_id_, planet_id, GeometryFrame("LunaOrbit"));
   body_ids_.push_back(luna_id);
   body_offset_.push_back(X_OeOl);
-  const Vector3<double> luna_axis_Oe{1, 1, 1};
+  const Vector3d luna_axis_Oe{1, 1, 1};
   axes_.push_back(luna_axis_Oe.normalized());
 
   // The geometry is rigidly affixed to Luna's orbital frame so that it moves
@@ -206,9 +207,9 @@ void SolarSystem<T>::AllocateGeometry(SceneGraph<T>* scene_graph) {
   // Pick a position at kLunaOrbitRadius distance from the Earth's origin on
   // the plane _perpendicular_ to the moon's normal (<1, 1, 1>).
   // luna_position.dot(luna_axis_Oe) will be zero.
-  Vector3<double> luna_position =
-      Vector3<double>(-1, 0.5, 0.5).normalized() * kLunaOrbitRadius;
-  Isometry3<double> X_OlL{Translation3<double>{luna_position}};
+  Vector3d luna_position =
+      Vector3d(-1, 0.5, 0.5).normalized() * kLunaOrbitRadius;
+  RigidTransformd X_OlL{Translation3d{luna_position}};
   scene_graph->RegisterGeometry(
       source_id_, luna_id,
       MakeShape<Sphere>(X_OlL, "Luna", Vector4d(0.5, 0.5, 0.35, 1.0), 0.075));
@@ -243,28 +244,28 @@ void SolarSystem<T>::AllocateGeometry(SceneGraph<T>* scene_graph) {
 
   // Mars's orbital frame Om lies directly *below* the sun (to account for the
   // orrery arm).
-  Isometry3<double> X_SOm{Translation3<double>{0, 0, orrery_bottom}};
+  RigidTransformd X_SOm{Translation3d{0, 0, orrery_bottom}};
   planet_id =
       scene_graph->RegisterFrame(source_id_, GeometryFrame("MarsOrbit"));
   body_ids_.push_back(planet_id);
   body_offset_.push_back(X_SOm);
-  Vector3<double> mars_axis_S{0, 0.1, 1};
+  Vector3d mars_axis_S{0, 0.1, 1};
   axes_.push_back(mars_axis_S.normalized());
 
   // The geometry is rigidly affixed to Mars's orbital frame so that it moves
   // in a circular path.
   const double kMarsOrbitRadius = 5.0;
   const double kMarsSize = 0.24;
-  Isometry3<double> X_OmM{
-      Translation3<double>{kMarsOrbitRadius, 0, -orrery_bottom}};
+  RigidTransformd X_OmM{
+      Translation3d{kMarsOrbitRadius, 0, -orrery_bottom}};
   GeometryId mars_geometry_id = scene_graph->RegisterGeometry(
       source_id_, planet_id,
       MakeShape<Sphere>(X_OmM, "Mars", Vector4d(0.9, 0.1, 0, 1), kMarsSize));
 
   std::string rings_absolute_path =
       FindResourceOrThrow("drake/examples/scene_graph/planet_rings.obj");
-  Vector3<double> axis = Vector3<double>(1, 1, 1).normalized();
-  Isometry3<double> X_MR(AngleAxis<double>(M_PI / 3, axis));
+  Vector3d axis = Vector3d(1, 1, 1).normalized();
+  RigidTransformd X_MR(AngleAxisd(M_PI / 3, axis), Vector3d{0, 0, 0});
   scene_graph->RegisterGeometry(
       source_id_, mars_geometry_id,
       MakeShape<Mesh>(X_MR, "MarsRings", Vector4d(0.45, 0.9, 0, 1),
@@ -277,7 +278,7 @@ void SolarSystem<T>::AllocateGeometry(SceneGraph<T>* scene_graph) {
   // Phobos's orbital frame Op is at the center of Mars (M).
   // So, X_OmOp = X_OmM. The normal of the plane is negated so it orbits in the
   // opposite direction.
-  const Isometry3<double>& X_OmOp = X_OmM;
+  const RigidTransformd& X_OmOp = X_OmM;
   FrameId phobos_id = scene_graph->RegisterFrame(source_id_, planet_id,
                                                  GeometryFrame("PhobosOrbit"));
   body_ids_.push_back(phobos_id);
@@ -287,7 +288,7 @@ void SolarSystem<T>::AllocateGeometry(SceneGraph<T>* scene_graph) {
 
   // The geometry is displaced from the Phobos's frame so that it orbits.
   const double kPhobosOrbitRadius = 0.34;
-  Isometry3<double> X_OpP{Translation3<double>{kPhobosOrbitRadius, 0, 0}};
+  const RigidTransformd X_OpP{Translation3d{kPhobosOrbitRadius, 0, 0}};
   scene_graph->RegisterGeometry(
       source_id_, phobos_id,
       MakeShape<Sphere>(X_OpP, "Phobos", Vector4d(0.65, 0.6, 0.8, 1), 0.06));
@@ -306,7 +307,7 @@ void SolarSystem<T>::CalcFramePoseOutput(const Context<T>& context,
     // rotation value.
     T rotation{state[i]};
     pose.set_rotation(AngleAxis<T>(rotation, axes_[i]));
-    poses->set_value(body_ids_[i], pose.GetAsIsometry3());
+    poses->set_value(body_ids_[i], pose);
   }
 }
 

examples/scene_graph/solar_system.h

@@ -4,6 +4,7 @@
 
 #include "drake/common/drake_copyable.h"
 #include "drake/geometry/scene_graph.h"
+#include "drake/math/rigid_transform.h"
 #include "drake/systems/framework/basic_vector.h"
 #include "drake/systems/framework/context.h"
 #include "drake/systems/framework/leaf_system.h"
@@ -151,7 +152,7 @@ class SolarSystem : public systems::LeafSystem<T> {
   // The axes around each body revolves (expressed in its parent's frame)
   std::vector<Vector3<double>> axes_;
   // The translational offset of each body from its parent frame
-  std::vector<Isometry3<double>> body_offset_;
+  std::vector<math::RigidTransformd> body_offset_;
 };
 
 }  // namespace solar_system

geometry/BUILD.bazel

@@ -227,6 +227,7 @@ drake_cc_library(
         ":shape_specification",
         "//common:copyable_unique_ptr",
         "//common:essential",
+        "//math:geometric_transform",
     ],
 )
 
@@ -244,7 +245,10 @@ drake_cc_library(
     name = "shape_specification",
     srcs = ["shape_specification.cc"],
     hdrs = ["shape_specification.h"],
-    deps = ["//common:essential"],
+    deps = [
+        "//common:essential",
+        "//math:geometric_transform",
+    ],
 )
 
 drake_cc_library(