From af0cf950066d6b7b34eb5575fa0640bc43ce087e Mon Sep 17 00:00:00 2001
From: "Addisu Z. Taddese" <addisu@openrobotics.org>
Date: Wed, 5 Aug 2020 13:40:41 -0500
Subject: [PATCH] Reviewer feedback

Signed-off-by: Addisu Z. Taddese <addisu@openrobotics.org>
---
 src/Model.cc                     | 22 ++++++++++++++--------
 test/integration/nested_model.cc |  4 ++--
 test/sdf/placement_frame.sdf     |  2 +-
 3 files changed, 17 insertions(+), 11 deletions(-)

diff --git a/src/Model.cc b/src/Model.cc
index 456ff9fde..d319627f3 100644
--- a/src/Model.cc
+++ b/src/Model.cc
@@ -379,12 +379,15 @@ Errors Model::Load(ElementPtr _sdf)
     if (resolveErrors.empty())
     {
       // Before this update, i.e, as specified in the SDFormat, the model pose
-      // (X_MpPf) is the pose of the placement frame (Pf) relative to a frame in
-      // the parent scope of the model (Mp). However, when this model (M) is
-      // inserted into a pose graph of the parent scope, only the pose (X_MpM)
+      // (X_RPf) is the pose of the placement frame (Pf) relative to a frame (R)
+      // in the parent scope of the model. However, when this model (M) is
+      // inserted into a pose graph of the parent scope, only the pose (X_RM)
       // of the __model__ frame can be used. Thus, the model pose has to be
-      // updated to X_MpM. And this is done by:
-      // X_MpM = X_MpPf * inv(X_MPf)
+      // updated to X_RM. And this is done by:
+      // X_RM = X_RPf * inv(X_MPf)
+      //
+      // Note that X_RPf is the raw pose specified in //model/pose before this
+      // update.
       this->dataPtr->pose =
           this->dataPtr->pose * placementFrameRelPose.Inverse();
     }
@@ -417,8 +420,11 @@ Errors Model::Load(ElementPtr _sdf)
           this->FrameByName(childModelName + "::__model__");
       if (nullptr != childModelFrame)
       {
-        // We assume that all model frames are specified relative to their
-        // parent frame, so using the RawPose should be okay.
+        // The RawPose of the child model frame is the desired pose of the
+        // placement frame relative to a frame (R) in the scope of the parent
+        // model. We don't need to resolve the pose because the relative_to
+        // frame remains unchanged after the pose update here. i.e, the updated
+        // pose of the child model frame will still be relative to R.
         const auto &placementFramePose = childModelFrame->RawPose();
 
         ignition::math::Pose3d placementFrameRelPose;
@@ -430,7 +436,7 @@ Errors Model::Load(ElementPtr _sdf)
         // frame as well as the corresponding edge in the pose graph because
         // just updating childModelFrame doesn't update the pose graph.
         //
-        // X_MpM = X_MpPf * inv(X_MPf)
+        // X_RM = X_RPf * inv(X_MPf)
         auto newModelPose =
             placementFramePose * placementFrameRelPose.Inverse();
         frame.SetRawPose(newModelPose);
diff --git a/test/integration/nested_model.cc b/test/integration/nested_model.cc
index 93e63b101..72d961f21 100644
--- a/test/integration/nested_model.cc
+++ b/test/integration/nested_model.cc
@@ -989,7 +989,7 @@ TEST_F(PlacementFrame, ModelInclude)
   // Test that the pose of an included model with placement_frame can use the
   // relative_to attribute
   this->TestExpectedModelPose<sdf::Link>("parent_model_include",
-      "include_with_placement_frame_and_pose_relative_to::L4");
+      "nested_include_with_placement_frame_and_pose_relative_to::L4");
 }
 
 //////////////////////////////////////////////////
@@ -1007,7 +1007,7 @@ TEST_F(PlacementFrame, ModelPlacementFrameAttribute)
   this->TestExpectedWorldPose<sdf::Joint>(
       "model_with_joint_placement_frame", "J2");
 
-  // Test that the pose of an included model with placement_frame can use the
+  // Test that the pose of a model with a placement_frame attribute can use the
   // relative_to attribute
   this->TestExpectedWorldPose<sdf::Link>(
       "model_with_placement_frame_and_pose_relative_to", "L4");
diff --git a/test/sdf/placement_frame.sdf b/test/sdf/placement_frame.sdf
index f780f1379..e52014ac4 100644
--- a/test/sdf/placement_frame.sdf
+++ b/test/sdf/placement_frame.sdf
@@ -68,7 +68,7 @@
       <pose>-1 10 0 1.570796326794895 0 0</pose>
     </frame>
     <include>
-      <name>include_with_placement_frame_and_pose_relative_to</name>
+      <name>nested_include_with_placement_frame_and_pose_relative_to</name>
       <uri>test_model_with_frames</uri>
       <pose relative_to='frame_1'>1 0 0 0 0 0</pose>
       <placement_frame>L4</placement_frame>