Add support for //include/placement_frame and //model/@placement_frame

include/sdf/Error.hh

@@ -131,6 +131,9 @@ namespace sdf
 
     /// \brief Indicates that reading an SDF string failed.
     STRING_READ,
+
+    /// \brief The specified placement frame is invalid
+    MODEL_PLACEMENT_FRAME_INVALID,
   };
 
   class SDFORMAT_VISIBLE Error

include/sdf/Model.hh

@@ -247,6 +247,15 @@ namespace sdf
     /// \return SemanticPose object for this link.
     public: sdf::SemanticPose SemanticPose() const;
 
+    /// \brief Get the name of the placement frame of the model.
+    /// \return Name of the placement frame attribute of the model.
+    public: const std::string &PlacementFrameName() const;
+
+    /// \brief Set the name of the placement frame of the model.
+    /// The specified placement frame must exist within the model.
+    /// \param[in] _name Name of the placement frame.
+    public: void SetPlacementFrameName(const std::string &_name);
+
     /// \brief Give a weak pointer to the PoseRelativeToGraph to be used
     /// for resolving poses. This is private and is intended to be called by
     /// World::Load.

sdf/1.8/model.sdf

@@ -14,6 +14,9 @@
       as the canonical link.
     </description>
   </attribute>
+  <attribute name="placement_frame" type="string" default="" required="0">
+    <description>The frame inside this model whose pose will be set by the pose element of the model. i.e, the pose element specifies the pose of this frame instead of the model frame.</description>
+  </attribute>
 
   <element name="static" type="bool" default="false" required="0">
     <description>If set to true, the model is immovable. Otherwise the model is simulated in the dynamics engine.</description>
@@ -49,6 +52,10 @@
     <element name="static" type="bool" default="false" required="0">
       <description>Override the static value of the included model.</description>
     </element>
+
+    <element name="placement_frame" type="string" default="" required="0">
+      <description>The frame inside the included model whose pose will be set by the specified pose element. If this element is specified, the pose must be specified.</description>
+    </element>
   </element>
 
   <element name="model" ref="model" required="*">

sdf/1.8/world.sdf

@@ -36,6 +36,10 @@
     </element>
 
     <include filename="pose.sdf" required="0"/>
+
+    <element name="placement_frame" type="string" default="" required="0">
+      <description>The frame inside the included entity whose pose will be set by the specified pose element. If this element is specified, the pose must be specified.</description>
+    </element>
   </element>
 
   <element name="gravity" type="vector3" default="0 0 -9.8" required="1">

src/FrameSemantics.cc

@@ -1284,5 +1284,46 @@ Errors resolvePose(
 
   return errors;
 }
+
+/////////////////////////////////////////////////
+Errors updateGraphPose(
+    PoseRelativeToGraph &_graph,
+    const std::string &_frameName,
+    const ignition::math::Pose3d &_pose)
+{
+  Errors errors;
+
+  if (_graph.map.count(_frameName) != 1)
+  {
+    errors.push_back({ErrorCode::POSE_RELATIVE_TO_INVALID,
+        "PoseRelativeToGraph unable to find unique frame with name [" +
+        _frameName+ "] in graph."});
+    return errors;
+  }
+  auto vertexId = _graph.map.at(_frameName);
+  auto incidentsTo = _graph.graph.IncidentsTo(vertexId);
+  if (incidentsTo.size() == 1)
+  {
+    // There's no API to update the data of an edge, so we remove the edge and
+    // insert a new one with the new pose.
+    auto &edge = incidentsTo.begin()->second;
+    _graph.graph.AddEdge({edge.get().Tail(), edge.get().Head()}, _pose);
+    _graph.graph.RemoveEdge(edge.get().Id());
+  }
+  else if (incidentsTo.empty())
+  {
+    errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR,
+        "PoseRelativeToGraph error: no incoming edge to "
+        "vertex [" + _frameName + "]."});
+  }
+  else
+  {
+    errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR,
+        "PoseRelativeToGraph error: multiple incoming edges to "
+        "vertex [" + _frameName + "]."});
+  }
+
+  return errors;
+}
 }
 }

src/FrameSemantics.hh

@@ -178,6 +178,18 @@ namespace sdf
       const PoseRelativeToGraph &_graph,
       const std::string &_frameName,
       const std::string &_resolveTo);
+
+  /// \brief Update the pose of a frame in the pose graph. This updates the
+  /// content of the edge incident to the vertex identified by the given
+  /// _frameName.
+  /// \param[in] _graph PoseRelativeToGraph to update.
+  /// \param[in] _frameName Name of frame whose pose is to be updated.
+  /// \param[in] _pose New pose.
+  /// \return Errors.
+  Errors updateGraphPose(
+      PoseRelativeToGraph &_graph,
+      const std::string &_frameName,
+      const ignition::math::Pose3d &_pose);
   }
 }
 #endif

src/FrameSemantics_TEST.cc

@@ -286,3 +286,86 @@ TEST(FrameSemantics, buildPoseRelativeToGraph)
         "PoseRelativeToGraph unable to find unique frame with name ["
         "invalid] in graph."));
 }
+
+/////////////////////////////////////////////////
+TEST(FrameSemantics, updateGraphPose)
+{
+  const std::string testFile = sdf::filesystem::append(
+      PROJECT_SOURCE_PATH, "test", "sdf", "model_link_relative_to.sdf");
+
+  // Load the SDF file
+  sdf::Root root;
+  EXPECT_TRUE(root.Load(testFile).empty());
+
+  // Get the first model
+  const sdf::Model *model = root.ModelByIndex(0);
+
+  sdf::PoseRelativeToGraph graph;
+  {
+    sdf::Errors errors = sdf::buildPoseRelativeToGraph(graph, model);
+    EXPECT_TRUE(errors.empty());
+    EXPECT_TRUE(sdf::validatePoseRelativeToGraph(graph).empty());
+  }
+
+  using ignition::math::Pose3d;
+
+  EXPECT_EQ(Pose3d::Zero, model->RawPose());
+
+  {
+    Pose3d pose;
+    EXPECT_TRUE(sdf::resolvePose(pose, graph, "L1", "__model__").empty());
+    EXPECT_EQ(Pose3d(1, 0, 0, 0, IGN_PI_2, 0), pose);
+  }
+  {
+    Pose3d pose;
+    EXPECT_TRUE(sdf::resolvePose(pose, graph, "L3", "__model__").empty());
+    EXPECT_EQ(Pose3d(1, 0, -3, 0, IGN_PI_2, 0), pose);
+  }
+  // Try updating the pose of a frame that doesn't exist
+  {
+    sdf::Errors errors = sdf::updateGraphPose(graph, "nonexistent", {});
+    ASSERT_FALSE(errors.empty());
+    EXPECT_EQ(sdf::ErrorCode::POSE_RELATIVE_TO_INVALID, errors[0].Code());
+  }
+
+  // Update L1's pose in the graph. This will be relative to __model__
+  Pose3d l1NewPose(0, 5, 0, 0, 0, 0);
+  EXPECT_TRUE(sdf::updateGraphPose(graph, "L1", l1NewPose).empty());
+
+  {
+    // L1 relative to __model__ is l1NewPose
+    Pose3d pose;
+    EXPECT_TRUE(sdf::resolvePose(pose, graph, "L1", "__model__").empty());
+    EXPECT_EQ(l1NewPose, pose);
+  }
+  {
+    // Since L3's pose is given relative to L1, updating L1 affects L3
+    Pose3d pose;
+    EXPECT_TRUE(sdf::resolvePose(pose, graph, "L3", "__model__").empty());
+    EXPECT_EQ(l1NewPose * Pose3d(3, 0, 0, 0, 0, 0), pose);
+  }
+
+  // Update L3's pose. This will be relative to L1
+  Pose3d l3NewPose(0, 0, 2, 0, 0, 0);
+  EXPECT_TRUE(sdf::updateGraphPose(graph, "L3", l3NewPose).empty());
+
+  {
+    // L3 relative to L1 is l3NewPose
+    Pose3d pose;
+    EXPECT_TRUE(sdf::resolvePose(pose, graph, "L3", "L1").empty());
+    EXPECT_EQ(l3NewPose, pose);
+  }
+  {
+    // L1's pose shouldn't have changed
+    Pose3d pose;
+    EXPECT_TRUE(sdf::resolvePose(pose, graph, "L1", "__model__").empty());
+    EXPECT_EQ(l1NewPose, pose);
+  }
+  {
+    // The new pose of L3 relative to __model__ is affected by the new relative
+    // poses of L1 and L3
+    Pose3d pose;
+    EXPECT_TRUE(sdf::resolvePose(pose, graph, "L3", "__model__").empty());
+    EXPECT_EQ(l1NewPose * l3NewPose, pose);
+  }
+}

src/Model.cc

@@ -52,6 +52,9 @@ class sdf::ModelPrivate
   /// \brief Name of the canonical link.
   public: std::string canonicalLink = "";
 
+  /// \brief Name of the placement frame
+  public: std::string placementFrameName = "";
+
   /// \brief Pose of the model
   public: ignition::math::Pose3d pose = ignition::math::Pose3d::Zero;
 
@@ -186,6 +189,9 @@ Errors Model::Load(ElementPtr _sdf)
     }
   }
 
+  this->dataPtr->placementFrameName = _sdf->Get<std::string>("placement_frame",
+                             this->dataPtr->placementFrameName).first;
+
   this->dataPtr->isStatic = _sdf->Get<bool>("static", false).first;
 
   this->dataPtr->selfCollide = _sdf->Get<bool>("self_collide", false).first;
@@ -362,6 +368,91 @@ Errors Model::Load(ElementPtr _sdf)
     frame.SetPoseRelativeToGraph(this->dataPtr->poseGraph);
   }
 
+  // Update the model pose to account for the placement frame.
+  if (!this->dataPtr->placementFrameName.empty())
+  {
+    ignition::math::Pose3d placementFrameRelPose;
+
+    sdf::Errors resolveErrors =
+        sdf::resolvePose(placementFrameRelPose, *this->dataPtr->poseGraph,
+                    this->dataPtr->placementFrameName, "__model__");
+    if (resolveErrors.empty())
+    {
+      // Before this update, i.e, as specified in the SDFormat, the model pose
+      // (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_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();
+    }
+    else
+    {
+      errors.insert(errors.end(), resolveErrors.begin(), resolveErrors.end());
+    }
+  }
+
+  // The placement_frame attributes of included child models are currently lost
+  // during parsing because the parser expands the included models into the
+  // parent model. As a temporary workardound to preserve this information, the
+  // parser injects <model name>::__placement_frame__ for every included child
+  // model. This serves as an identifier that the model frame of the child model
+  // should be treated as if it has its placement_frame attribute set.
+  // This will not be necessary when included nested models work as directly
+  // nested models. See
+  // https://github.com/osrf/sdformat/issues/319#issuecomment-665214004
+  // TODO (addisu) Remove placementFrameIdentifier once PR addressing
+  // https://github.com/osrf/sdformat/issues/284 lands
+  for (auto &frame : this->dataPtr->frames)
+  {
+    auto placementSubstrInd = frame.Name().rfind("::__placement_frame__");
+    if (placementSubstrInd != std::string::npos)
+    {
+      const std::string childModelName =
+          frame.Name().substr(0, placementSubstrInd);
+      // Find the model frame associated with this placement frame
+      const Frame *childModelFrame =
+          this->FrameByName(childModelName + "::__model__");
+      if (nullptr != childModelFrame)
+      {
+        // 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;
+        sdf::Errors resolveErrors = frame.SemanticPose().Resolve(
+            placementFrameRelPose, childModelFrame->Name());
+        errors.insert(errors.end(), resolveErrors.begin(), resolveErrors.end());
+
+        // We need to update childModelFrame's pose relative to the parent
+        // frame as well as the corresponding edge in the pose graph because
+        // just updating childModelFrame doesn't update the pose graph.
+        //
+        // X_RM = X_RPf * inv(X_MPf)
+        auto newModelPose =
+            placementFramePose * placementFrameRelPose.Inverse();
+        frame.SetRawPose(newModelPose);
+        sdf::updateGraphPose(
+            *this->dataPtr->poseGraph, childModelFrame->Name(), newModelPose);
+      }
+      else
+      {
+        errors.push_back({ErrorCode::MODEL_PLACEMENT_FRAME_INVALID,
+            "Found a __placement_frame__ for child model with name [" +
+            childModelName + "], but the model does not exist in the" +
+            " parent model with name [" + this->Name() + "]"});
+      }
+    }
+  }
+
   return errors;
 }
 
@@ -557,6 +648,18 @@ void Model::SetCanonicalLinkName(const std::string &_canonicalLink)
   this->dataPtr->canonicalLink = _canonicalLink;
 }
 
+/////////////////////////////////////////////////
+const std::string &Model::PlacementFrameName() const
+{
+  return this->dataPtr->placementFrameName;
+}
+
+/////////////////////////////////////////////////
+void Model::SetPlacementFrameName(const std::string &_placementFrame)
+{
+  this->dataPtr->placementFrameName = _placementFrame;
+}
+
 /////////////////////////////////////////////////
 const ignition::math::Pose3d &Model::RawPose() const
 {

src/Model_TEST.cc

@@ -71,6 +71,10 @@ TEST(DOMModel, Construction)
   EXPECT_EQ("link", model.CanonicalLinkName());
   EXPECT_EQ(nullptr, model.CanonicalLink());
 
+  EXPECT_TRUE(model.PlacementFrameName().empty());
+  model.SetPlacementFrameName("test_frame");
+  EXPECT_EQ("test_frame", model.PlacementFrameName());
+
   EXPECT_EQ(ignition::math::Pose3d::Zero, model.RawPose());
   EXPECT_TRUE(model.PoseRelativeTo().empty());
   {

src/ign_TEST.cc

@@ -630,6 +630,19 @@ TEST(check, SDF)
                           "name[world_frame_invalid_relative_to]."),
               std::string::npos) << output;
   }
+  // Check an SDF file with an invalid frame specified as the placement_frame
+  // attribute.
+  {
+    std::string path = pathBase + "/model_invalid_placement_frame.sdf";
+
+    // Check model_invalid_placement_frame.sdf
+    std::string output =
+      custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion);
+    EXPECT_NE(
+        output.find("unable to find unique frame with name [link3] in graph"),
+        std::string::npos)
+        << output;
+  }
 }
 
 /////////////////////////////////////////////////