Merge pull request RobotLocomotion#11092 from SeanCurtis-TRI/PR_remov…

…e_geometry_frame_pose_requirement

Removing pose from GeometryFrame

attic/multibody/rigid_body_plant/rigid_body_plant_bridge.cc

@@ -142,8 +142,7 @@ void RigidBodyPlantBridge<T>::RegisterTree(SceneGraph<T>* scene_graph) {
       // All other bodies register a frame and (possibly) get a unique label.
       body_id = scene_graph->RegisterFrame(
           source_id_,
-          GeometryFrame(body.get_name(), Isometry3<double>::Identity(),
-                        body.get_model_instance_id()));
+          GeometryFrame(body.get_name(), body.get_model_instance_id()));
     }
     body_ids_.push_back(body_id);
 

automotive/automotive_simulator.cc

@@ -177,7 +177,7 @@ void AutomotiveSimulator<T>::ConnectCarOutputsAndPriusVis(
       "car_" + std::to_string(id));
   const geometry::FrameId frame_id = scene_graph_->RegisterFrame(
       source_id,
-      geometry::GeometryFrame("car_origin", Isometry3d::Identity()));
+      geometry::GeometryFrame("car_origin"));
 
   // Defines the distance between the SDF model's origin and the middle of the
   // rear axle.  This offset is necessary because the pose_output frame's

examples/pendulum/pendulum_plant.cc

@@ -137,7 +137,7 @@ void PendulumPlant<T>::RegisterGeometry(
       source_id_, id, MakeDrakeVisualizerProperties(Vector4d(.3, .6, .4, 1)));
 
   frame_id_ = scene_graph->RegisterFrame(
-      source_id_, GeometryFrame("arm", Isometry3d::Identity()));
+      source_id_, GeometryFrame("arm"));
 
   // The arm.
   id = scene_graph->RegisterGeometry(

examples/scene_graph/bouncing_ball_plant.cc

@@ -48,7 +48,7 @@ BouncingBallPlant<T>::BouncingBallPlant(SourceId source_id,
   static_assert(BouncingBallVectorIndices::kNumCoordinates == 1 + 1, "");
 
   ball_frame_id_ = scene_graph->RegisterFrame(
-      source_id, GeometryFrame("ball_frame", Isometry3<double>::Identity()));
+      source_id, GeometryFrame("ball_frame"));
   ball_id_ = scene_graph->RegisterGeometry(
       source_id, ball_frame_id_,
       make_unique<GeometryInstance>(Isometry3<double>::Identity(), /*X_FG*/

examples/scene_graph/dev/bouncing_ball_plant.cc

@@ -48,7 +48,7 @@ BouncingBallPlant<T>::BouncingBallPlant(SourceId source_id,
   static_assert(BouncingBallVectorIndices::kNumCoordinates == 1 + 1, "");
 
   ball_frame_id_ = scene_graph->RegisterFrame(
-      source_id, GeometryFrame("ball_frame", Isometry3<double>::Identity()));
+      source_id, GeometryFrame("ball_frame"));
   ball_id_ = scene_graph->RegisterGeometry(
       source_id, ball_frame_id_,
       make_unique<GeometryInstance>(Isometry3<double>::Identity(), /*X_FG*/

examples/scene_graph/dev/solar_system.cc

@@ -168,7 +168,7 @@ void SolarSystem<T>::AllocateGeometry(SceneGraph<T>* scene_graph) {
   const double kEarthBottom = orrery_bottom + 0.25;
   Isometry3<double> X_SE{Translation3<double>{0, 0, kEarthBottom}};
   FrameId planet_id =
-      scene_graph->RegisterFrame(source_id_, GeometryFrame("Earth", X_SE));
+      scene_graph->RegisterFrame(source_id_, GeometryFrame("Earth"));
   body_ids_.push_back(planet_id);
   body_offset_.push_back(X_SE);
   axes_.push_back(Vector3<double>::UnitZ());
@@ -187,7 +187,7 @@ void SolarSystem<T>::AllocateGeometry(SceneGraph<T>* scene_graph) {
   // Luna's frame L is at the center of Earth's geometry (Ge). So, X_EL = X_EGe.
   const Isometry3<double>& X_EL = X_EGe;
   FrameId luna_id = scene_graph->RegisterFrame(source_id_, planet_id,
-                                               GeometryFrame("Luna", X_EL));
+                                               GeometryFrame("Luna"));
   body_ids_.push_back(luna_id);
   body_offset_.push_back(X_EL);
   Vector3<double> plane_normal{1, 1, 1};
@@ -209,7 +209,7 @@ void SolarSystem<T>::AllocateGeometry(SceneGraph<T>* scene_graph) {
   // arm).
   Isometry3<double> X_SM{Translation3<double>{0, 0, orrery_bottom}};
   planet_id =
-      scene_graph->RegisterFrame(source_id_, GeometryFrame("Mars", X_SM));
+      scene_graph->RegisterFrame(source_id_, GeometryFrame("Mars"));
   body_ids_.push_back(planet_id);
   body_offset_.push_back(X_SM);
   plane_normal << 0, 0.1, 1;
@@ -242,7 +242,7 @@ void SolarSystem<T>::AllocateGeometry(SceneGraph<T>* scene_graph) {
   // opposite direction.
   const Isometry3<double>& X_MP = X_MGm;
   FrameId phobos_id = scene_graph->RegisterFrame(source_id_, planet_id,
-                                                 GeometryFrame("phobos", X_MP));
+                                                 GeometryFrame("phobos"));
   body_ids_.push_back(phobos_id);
   body_offset_.push_back(X_MP);
   plane_normal << 0, 0, -1;

examples/scene_graph/solar_system.cc

@@ -172,7 +172,7 @@ void SolarSystem<T>::AllocateGeometry(SceneGraph<T>* scene_graph) {
   const double kEarthBottom = orrery_bottom + 0.25;
   Isometry3<double> X_SOe{Translation3<double>{0, 0, kEarthBottom}};
   FrameId planet_id = scene_graph->RegisterFrame(
-      source_id_, GeometryFrame("EarthOrbit", X_SOe));
+      source_id_, GeometryFrame("EarthOrbit"));
   body_ids_.push_back(planet_id);
   body_offset_.push_back(X_SOe);
   axes_.push_back(Vector3<double>::UnitZ());
@@ -196,7 +196,7 @@ void SolarSystem<T>::AllocateGeometry(SceneGraph<T>* scene_graph) {
   // So, X_OeOl = X_OeE.
   const Isometry3<double>& X_OeOl = X_OeE;
   FrameId luna_id = scene_graph->RegisterFrame(
-      source_id_, planet_id, GeometryFrame("LunaOrbit", X_OeOl));
+      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};
@@ -221,7 +221,7 @@ void SolarSystem<T>::AllocateGeometry(SceneGraph<T>* scene_graph) {
   // Convex satellite orbits Earth in the same revolution as Luna but with
   // different initial position. See SetDefaultState().
   FrameId convexsat_id = scene_graph->RegisterFrame(source_id_, planet_id,
-                                          GeometryFrame("Convexsat", X_OeOl));
+                                          GeometryFrame("Convexsat"));
   body_ids_.push_back(convexsat_id);
   body_offset_.push_back(X_OeOl);
   axes_.push_back(luna_axis_Oe.normalized());
@@ -239,7 +239,7 @@ void SolarSystem<T>::AllocateGeometry(SceneGraph<T>* scene_graph) {
   // Box satellite orbits Earth in the same revolution as Luna but with
   // different initial position. See SetDefaultState().
   FrameId boxsat_id = scene_graph->RegisterFrame(
-      source_id_, planet_id, GeometryFrame("Boxsat", X_OeOl));
+      source_id_, planet_id, GeometryFrame("Boxsat"));
   body_ids_.push_back(boxsat_id);
   body_offset_.push_back(X_OeOl);
   axes_.push_back(luna_axis_Oe.normalized());
@@ -255,7 +255,7 @@ void SolarSystem<T>::AllocateGeometry(SceneGraph<T>* scene_graph) {
   // orrery arm).
   Isometry3<double> X_SOm{Translation3<double>{0, 0, orrery_bottom}};
   planet_id =
-      scene_graph->RegisterFrame(source_id_, GeometryFrame("MarsOrbit", X_SOm));
+      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};
@@ -296,7 +296,7 @@ void SolarSystem<T>::AllocateGeometry(SceneGraph<T>* scene_graph) {
   // opposite direction.
   const Isometry3<double>& X_OmOp = X_OmM;
   FrameId phobos_id = scene_graph->RegisterFrame(
-      source_id_, planet_id, GeometryFrame("PhobosOrbit", X_OmOp));
+      source_id_, planet_id, GeometryFrame("PhobosOrbit"));
   body_ids_.push_back(phobos_id);
   body_offset_.push_back(X_OmOp);
   mars_axis_S << 0, 0, -1;

geometry/dev/geometry_state.cc

@@ -264,7 +264,6 @@ GeometryState<T>& GeometryState<T>::operator=(
       const geometry::internal::InternalFrame& source_frame =
           tester.frames().at(frame_id);
       RegisterValidFrame(source_id, frame_id, source_frame.name(),
-                         Isometry3<double>::Identity(),
                          source_frame.frame_group(),
                          source_frame.parent_frame_id(), source_frame.clique(),
                          &frame_set);
@@ -587,8 +586,8 @@ FrameId GeometryState<T>::RegisterFrame(SourceId source_id, FrameId parent_id,
 
   int clique = GeometryStateCollisionFilterAttorney::get_next_clique(
       geometry_engine_.get_mutable());
-  RegisterValidFrame(source_id, frame_id, frame.name(), frame.pose(),
-                     frame.frame_group(), parent_id, clique, &f_set);
+  RegisterValidFrame(source_id, frame_id, frame.name(), frame.frame_group(),
+                     parent_id, clique, &f_set);
   return frame_id;
 }
 
@@ -1288,7 +1287,6 @@ void GeometryState<T>::RegisterValidSource(SourceId source_id,
 template <typename T>
 void GeometryState<T>::RegisterValidFrame(SourceId source_id, FrameId frame_id,
                                           const std::string& name,
-                                          const Isometry3<double>& X_PF,
                                           int frame_group, FrameId parent_id,
                                           int clique, FrameIdSet* frame_set) {
   if (parent_id != InternalFrame::world_frame_id()) {
@@ -1304,7 +1302,7 @@ void GeometryState<T>::RegisterValidFrame(SourceId source_id, FrameId frame_id,
 
   DRAKE_ASSERT(X_PF_.size() == frame_index_to_frame_map_.size());
   InternalIndex internal_index(X_PF_.size());
-  X_PF_.emplace_back(X_PF);
+  X_PF_.emplace_back(Isometry3<double>::Identity());
   X_WF_.emplace_back(Isometry3<double>::Identity());
   frame_index_to_frame_map_.push_back(frame_id);
   frame_set->insert(frame_id);

geometry/dev/geometry_state.h

@@ -861,8 +861,7 @@ class GeometryState {
   // version.
   void RegisterValidSource(SourceId source_id, const std::string& name);
   void RegisterValidFrame(SourceId source_id, FrameId frame_id,
-                          const std::string& name,
-                          const Isometry3<double>& X_PF, int frame_group,
+                          const std::string& name, int frame_group,
                           FrameId parent_id, int clique, FrameIdSet* frame_set);
   void RegisterValidGeometry(SourceId source_id, FrameId frame_id,
                              GeometryId geometry_id,

geometry/dev/test/geometry_state_test.cc

@@ -196,8 +196,7 @@ class DummyRenderEngine final : public render::RenderEngine {
 class GeometryStateTest : public ::testing::Test {
  protected:
   void SetUp() {
-    frame_ = make_unique<GeometryFrame>("ref_frame",
-                                        Isometry3<double>::Identity());
+    frame_ = make_unique<GeometryFrame>("ref_frame");
     instance_pose_.translation() << 10, 20, 30;
     instance_ = make_unique<GeometryInstance>(
         instance_pose_, make_unique<Sphere>(1.0), "instance");
@@ -263,22 +262,22 @@ class GeometryStateTest : public ::testing::Test {
     pose.translation() << 1, 2, 3;
     pose.linear() << 1, 0, 0, 0, 0, 1, 0, -1, 0;  // 90° around x-axis.
     frames_.push_back(geometry_state_.RegisterFrame(
-        source_id_, GeometryFrame("f0", pose)));
+        source_id_, GeometryFrame("f0")));
     X_WF_.push_back(pose);
     X_PF_.push_back(pose);
 
     // Create f1.
     pose.translation() << 10, 20, 30;
     pose.linear() << 0, 0, -1, 0, 1, 0, 1, 0, 0;  // 90° around y-axis.
     frames_.push_back(geometry_state_.RegisterFrame(
-        source_id_, GeometryFrame("f1", pose)));
+        source_id_, GeometryFrame("f1")));
     X_WF_.push_back(pose);
     X_PF_.push_back(pose);
 
     // Create f2.
     pose = pose.inverse();
     frames_.push_back(geometry_state_.RegisterFrame(
-        source_id_, frames_[1], GeometryFrame("f2", pose)));
+        source_id_, frames_[1], GeometryFrame("f2")));
     X_WF_.push_back(X_WF_[1] * pose);
     X_PF_.push_back(pose);
 
@@ -617,6 +616,25 @@ TEST_F(GeometryStateTest, ValidateSingleSourceTree) {
           CompareMatrices(frame_in_parent[frame.internal_index()].matrix(),
                           X_PF_[i].matrix()));
     };
+
+    // When added, all frames' poses w.r.t. their parents are the identity.
+    const auto& frame_in_parent = gs_tester_.get_frame_parent_poses();
+    for (FrameId frame_id : frames_) {
+      const auto& frame = internal_frames.at(frame_id);
+      EXPECT_TRUE(
+          CompareMatrices(frame_in_parent[frame.internal_index()].matrix(),
+                          Isometry3<double>::Identity().matrix()));
+    }
+
+    // Confirm posing positions the frames properly.
+    FramePoseVector<double> poses(source_id_, frames_);
+    poses.clear();
+    for (int f = 0; f < static_cast<int>(frames_.size()); ++f) {
+      poses.set_value(frames_[f], X_PF_[f]);
+    }
+    gs_tester_.SetFramePoses(poses);
+    gs_tester_.FinalizePoseUpdate();
+
     test_frame(0, gs_tester_.get_world_frame(), 0);
     test_frame(1, gs_tester_.get_world_frame(), 1);
     test_frame(2, frames_[1], 0);

geometry/dev/test/geometry_visualization_test.cc

@@ -37,7 +37,7 @@ GTEST_TEST(GeometryVisualization, SimpleScene) {
 
   const std::string frame_name("frame");
   FrameId frame_id = scene_graph.RegisterFrame(
-      source_id, GeometryFrame(frame_name, Isometry3d::Identity()));
+      source_id, GeometryFrame(frame_name));
 
   // Floats because the lcm messages store floats.
   const float radius = 1.25f;

geometry/dev/test/query_object_test.cc

@@ -149,7 +149,7 @@ GTEST_TEST(QueryObjectInspectTest, CreateValidInspector) {
   SourceId source_id = scene_graph.RegisterSource("source");
   auto identity = Isometry3<double>::Identity();
   FrameId frame_id =
-      scene_graph.RegisterFrame(source_id, GeometryFrame("frame", identity));
+      scene_graph.RegisterFrame(source_id, GeometryFrame("frame"));
   GeometryId geometry_id = scene_graph.RegisterGeometry(
       source_id, frame_id, make_unique<GeometryInstance>(
                                identity, make_unique<Sphere>(1.0), "sphere"));

geometry/dev/test/scene_graph_test.cc

@@ -218,17 +218,15 @@ TEST_F(SceneGraphTest, TopologyAfterAllocation) {
 
   // Attach frame to world.
   DRAKE_EXPECT_THROWS_MESSAGE(
-      scene_graph_.RegisterFrame(
-          id, GeometryFrame("frame", Isometry3<double>::Identity())),
+      scene_graph_.RegisterFrame(id, GeometryFrame("frame")),
       std::logic_error,
       "The call to RegisterFrame is invalid; a context has already been "
       "allocated.");
 
   // Attach frame to another frame.
   DRAKE_EXPECT_THROWS_MESSAGE(
-      scene_graph_.RegisterFrame(
-          id, FrameId::get_new_id(),
-          GeometryFrame("frame", Isometry3<double>::Identity())),
+      scene_graph_.RegisterFrame(id, FrameId::get_new_id(),
+          GeometryFrame("frame")),
       std::logic_error,
       "The call to RegisterFrame is invalid; a context has already been "
       "allocated.");
@@ -388,9 +386,9 @@ TEST_F(SceneGraphTest, ModelInspector) {
   ASSERT_TRUE(scene_graph_.SourceIsRegistered(source_id));
 
   FrameId frame_1 = scene_graph_.RegisterFrame(
-      source_id, GeometryFrame{"f1", Isometry3d::Identity()});
+      source_id, GeometryFrame{"f1"});
   FrameId frame_2 = scene_graph_.RegisterFrame(
-      source_id, GeometryFrame{"f2", Isometry3d::Identity()});
+      source_id, GeometryFrame{"f2"});
 
   // Note: all these geometries have the same *name* -- but because they are
   // affixed to different nodes, that should be alright.
@@ -426,7 +424,7 @@ class GeometrySourceSystem : public systems::LeafSystem<double> {
     // Register with SceneGraph.
     source_id_ = scene_graph->RegisterSource();
     FrameId f_id = scene_graph->RegisterFrame(
-        source_id_, GeometryFrame("frame", Isometry3<double>::Identity()));
+        source_id_, GeometryFrame("frame"));
     frame_ids_.push_back(f_id);
 
     // Set up output port now that the frame is registered.
@@ -446,7 +444,7 @@ class GeometrySourceSystem : public systems::LeafSystem<double> {
   // will *not* automatically get a pose.
   void add_extra_frame(bool add_to_output = true) {
     FrameId frame_id = scene_graph_->RegisterFrame(
-        source_id_, GeometryFrame("frame", Isometry3<double>::Identity()));
+        source_id_, GeometryFrame("frame"));
     if (add_to_output) extra_frame_ids_.push_back(frame_id);
   }
   // Method used to bring frame ids and poses out of sync. Adds a pose in
@@ -598,8 +596,6 @@ GTEST_TEST(SceneGraphVisualizationTest, NoWorldInPoseVector) {
                                                      &poses));
   }
 
-  const Isometry3<double> kIdentity = Isometry3<double>::Identity();
-
   // Case: Registered source with anchored geometry and frame but no dynamic
   // geometry --> empty pose vector; only frames with dynamic geometry with an
   // illustration role are included.
@@ -611,7 +607,7 @@ GTEST_TEST(SceneGraphVisualizationTest, NoWorldInPoseVector) {
         make_unique<GeometryInstance>(Isometry3<double>::Identity(),
                                       make_unique<Sphere>(1.0), "sphere"));
     FrameId f_id =
-        scene_graph.RegisterFrame(s_id, GeometryFrame("f", kIdentity));
+        scene_graph.RegisterFrame(s_id, GeometryFrame("f"));
     PoseBundle<double> poses = SceneGraphTester::MakePoseBundle(scene_graph);
     // The frame has no illustration geometry, so it is not part of the pose
     // bundle.