Allow MBP to report the BOdy that is associated with a particular fra…

…me id

SceneGraph returns results based on geometry id. However, various
operations need to get *from* that geometry id back to the source's
representation. For MBP that's the body associated with the frame id.
This provides that look up method.

multibody/multibody_tree/multibody_plant/multibody_plant.cc

@@ -386,7 +386,7 @@ geometry::GeometryId MultibodyPlant<T>::RegisterGeometry(
   DRAKE_ASSERT(scene_graph == scene_graph_);
   // If not already done, register a frame for this body.
   if (!body_has_registered_frame(body)) {
-    body_index_to_frame_id_[body.index()] = scene_graph->RegisterFrame(
+    FrameId frame_id = scene_graph->RegisterFrame(
         source_id_.value(),
         GeometryFrame(
             body.name(),
@@ -395,6 +395,8 @@ geometry::GeometryId MultibodyPlant<T>::RegisterGeometry(
             /* TODO(@SeanCurtis-TRI): Add test coverage for this
              * model-instance support as requested in #9390. */
             body.model_instance()));
+    body_index_to_frame_id_[body.index()] = frame_id;
+    frame_id_to_body_index_[frame_id] = body.index();
   }
 
   // Register geometry in the body frame.

multibody/multibody_tree/multibody_plant/multibody_plant.h

@@ -209,6 +209,7 @@ class MultibodyPlant : public systems::LeafSystem<T> {
     // Copy of all members related with geometry registration.
     source_id_ = other.source_id_;
     body_index_to_frame_id_ = other.body_index_to_frame_id_;
+    frame_id_to_body_index_ = other.frame_id_to_body_index_;
     geometry_id_to_body_index_ = other.geometry_id_to_body_index_;
     geometry_id_to_visual_index_ = other.geometry_id_to_visual_index_;
     geometry_id_to_collision_index_ = other.geometry_id_to_collision_index_;
@@ -995,6 +996,14 @@ class MultibodyPlant : public systems::LeafSystem<T> {
     return !!source_id_;
   }
 
+  /// Given a geometry frame identifier, returns a pointer to the body
+  /// associated with that id (nullptr if there is no such body).
+  const Body<T>* GetBodyFromFrameId(geometry::FrameId frame_id) const {
+    const auto it = frame_id_to_body_index_.find(frame_id);
+    if (it == frame_id_to_body_index_.end()) return nullptr;
+    return &tree().get_body(it->second);
+  }
+
   /// If the body with `body_index` has geometry registered with it, it returns
   /// the geometry::FrameId associated with it. Otherwise, it returns nullopt.
   /// @throws if called pre-finalize.
@@ -1729,6 +1738,10 @@ class MultibodyPlant : public systems::LeafSystem<T> {
   // Iteration order on this map DOES matter, and therefore we use an std::map.
   std::map<BodyIndex, geometry::FrameId> body_index_to_frame_id_;
 
+  // Data to get back from a SceneGraph-reported frame id to its associated
+  // body.
+  std::unordered_map<geometry::FrameId, BodyIndex> frame_id_to_body_index_;
+
   // Map from GeometryId to BodyIndex. During contact queries, it allows to find
   // out to which body a given geometry corresponds to.
   std::unordered_map<geometry::GeometryId, BodyIndex>

multibody/multibody_tree/multibody_plant/test/multibody_plant_test.cc

@@ -585,6 +585,7 @@ TEST_F(AcrobotPlantTests, VisualGeometryRegistration) {
         plant_->GetBodyFrameIdIfExists(body_index);
     ASSERT_TRUE(optional_id.has_value());
     EXPECT_EQ(frame_id, *optional_id);
+    EXPECT_EQ(body_index, plant_->GetBodyFromFrameId(frame_id)->index());
     const Isometry3<double>& X_WB = poses.value(frame_id);
     const Isometry3<double>& X_WB_expected = X_WB_all[body_index];
     EXPECT_TRUE(CompareMatrices(X_WB.matrix(), X_WB_expected.matrix(),