[sdf9] Support nested models in DOM and frame semantics

CMakeLists.txt

@@ -27,7 +27,7 @@ project (sdformat9)
 set (SDF_PROTOCOL_VERSION 1.7)
 
 set (SDF_MAJOR_VERSION 9)
-set (SDF_MINOR_VERSION 2)
+set (SDF_MINOR_VERSION 3)
 set (SDF_PATCH_VERSION 0)
 
 set (SDF_VERSION ${SDF_MAJOR_VERSION}.${SDF_MINOR_VERSION})

Changelog.md

@@ -2,6 +2,11 @@
 
 ### SDFormat 9.X.X (202X-XX-XX)
 
+### SDFormat 9.3.0 (2020-XX-XX)
+
+1. Support nested models in DOM and frame semantics.
+    * [Pull request 316](https://github.com/osrf/sdformat/pull/316)
+
 1. Find python3 in cmake, fix cmake warning.
     * [Pull request 328](https://github.com/osrf/sdformat/pull/328)
 

Migration.md

@@ -45,6 +45,10 @@ but with improved human-readability..
     + const Frame \*FrameByIndex(const uint64\_t) const
     + const Frame \*FrameByName(const std::string &) const
     + bool FrameNameExists(const std::string &) const
+    + uint64\_t ModelCount() const
+    + const Model \*ModelByIndex(const uint64\_t) const
+    + const Model \*ModelByName(const std::string &) const
+    + bool ModelNameExists(const std::string &) const
     + sdf::SemanticPose SemanticPose() const
 
 1. **sdf/SDFImpl.hh**
@@ -194,7 +198,7 @@ but with improved human-readability..
 1. **frame.sdf** `//frame/@attached_to` attribute
     + description: Name of the link or frame to which this frame is attached.
       If a frame is specified, recursively following the attached\_to attributes
-      of the specified frames must lead to the name of a link or the world frame.
+      of the specified frames must lead to the name of a link, a model, or the world frame.
     + type: string
     + default: ""
     + required: *

include/sdf/Model.hh

@@ -198,6 +198,29 @@ namespace sdf
     /// \return True if there exists an explicit frame with the given name.
     public: bool FrameNameExists(const std::string &_name) const;
 
+    /// \brief Get the number of nested models.
+    /// \return Number of nested models contained in this Model object.
+    public: uint64_t ModelCount() const;
+
+    /// \brief Get a nested model based on an index.
+    /// \param[in] _index Index of the nested model. The index should be in the
+    /// range [0..ModelCount()).
+    /// \return Pointer to the model. Nullptr if the index does not exist.
+    /// \sa uint64_t ModelCount() const
+    public: const Model *ModelByIndex(const uint64_t _index) const;
+
+    /// \brief Get whether a nested model name exists.
+    /// \param[in] _name Name of the nested model to check.
+    /// \return True if there exists a nested model with the given name.
+    public: bool ModelNameExists(const std::string &_name) const;
+
+    /// \brief Get a nested model based on a name.
+    /// \param[in] _name Name of the nested model.
+    /// \return Pointer to the model. Nullptr if a model with the given name
+    ///  does not exist.
+    /// \sa bool ModelNameExists(const std::string &_name) const
+    public: const Model *ModelByName(const std::string &_name) const;
+
     /// \brief Get the pose of the model. This is the pose of the model
     /// as specified in SDF (<model> <pose> ... </pose></model>), and is
     /// typically used to express the position and rotation of a model in a
@@ -281,9 +304,10 @@ namespace sdf
 
     /// \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.
+    /// World::Load and Model::Load if this is a nested model.
     /// \param[in] _graph Weak pointer to PoseRelativeToGraph.
-    private: void SetPoseRelativeToGraph(
+    /// \return Error if graph pointer is invalid.
+    private: sdf::Errors SetPoseRelativeToGraph(
         std::weak_ptr<const PoseRelativeToGraph> _graph);
 
     /// \brief Allow World::Load to call SetPoseRelativeToGraph.

sdf/1.7/frame.sdf

@@ -10,7 +10,7 @@
     <description>
       Name of the link or frame to which this frame is attached.
       If a frame is specified, recursively following the attached_to attributes
-      of the specified frames must lead to the name of a link or the world frame.
+      of the specified frames must lead to the name of a link, a model, or the world frame.
     </description>
   </attribute>
 

src/FrameSemantics.cc

@@ -285,6 +285,23 @@ Errors buildFrameAttachedToGraph(
     _out.map[frame->Name()] = frameId;
   }
 
+  // add nested model vertices
+  for (uint64_t m = 0; m < _model->ModelCount(); ++m)
+  {
+    auto nestedModel = _model->ModelByIndex(m);
+    if (_out.map.count(nestedModel->Name()) > 0)
+    {
+      errors.push_back({ErrorCode::DUPLICATE_NAME,
+          "Nested model with non-unique name [" + nestedModel->Name() +
+          "] detected in model with name [" + _model->Name() +
+          "]."});
+      continue;
+    }
+    auto nestedModelId =
+        _out.graph.AddVertex(nestedModel->Name(), sdf::FrameType::MODEL).Id();
+    _out.map[nestedModel->Name()] = nestedModelId;
+  }
+
   // add frame edges
   for (uint64_t f = 0; f < _model->FrameCount(); ++f)
   {
@@ -302,7 +319,7 @@ Errors buildFrameAttachedToGraph(
       errors.push_back({ErrorCode::FRAME_ATTACHED_TO_INVALID,
           "attached_to name[" + attachedTo +
           "] specified by frame with name[" + frame->Name() +
-          "] does not match a link, joint, or frame name "
+          "] does not match a nested model, link, joint, or frame name "
           "in model with name[" + _model->Name() + "]."});
       continue;
     }
@@ -352,9 +369,9 @@ Errors buildFrameAttachedToGraph(
   }
 
   // add model vertices
-  for (uint64_t l = 0; l < _world->ModelCount(); ++l)
+  for (uint64_t m = 0; m < _world->ModelCount(); ++m)
   {
-    auto model = _world->ModelByIndex(l);
+    auto model = _world->ModelByIndex(m);
     if (_out.map.count(model->Name()) > 0)
     {
       errors.push_back({ErrorCode::DUPLICATE_NAME,
@@ -537,6 +554,30 @@ Errors buildPoseRelativeToGraph(
     }
   }
 
+  // add nested model vertices and default edge if relative_to is empty
+  for (uint64_t m = 0; m < _model->ModelCount(); ++m)
+  {
+    auto nestedModel = _model->ModelByIndex(m);
+    if (_out.map.count(nestedModel->Name()) > 0)
+    {
+      errors.push_back({ErrorCode::DUPLICATE_NAME,
+          "Nested model with non-unique name [" + nestedModel->Name() +
+          "] detected in model with name [" + _model->Name() +
+          "]."});
+      continue;
+    }
+    auto nestedModelId =
+        _out.graph.AddVertex(nestedModel->Name(), sdf::FrameType::MODEL).Id();
+    _out.map[nestedModel->Name()] = nestedModelId;
+
+    if (nestedModel->PoseRelativeTo().empty())
+    {
+      // relative_to is empty, so add edge from implicit model frame
+      // to nestedModel
+      _out.graph.AddEdge({modelFrameId, nestedModelId}, nestedModel->RawPose());
+    }
+  }
+
   // now that all vertices have been added to the graph,
   // add the edges that reference other vertices
 
@@ -559,7 +600,7 @@ Errors buildPoseRelativeToGraph(
       errors.push_back({ErrorCode::POSE_RELATIVE_TO_INVALID,
           "relative_to name[" + relativeTo +
           "] specified by link with name[" + link->Name() +
-          "] does not match a link, joint, or frame name "
+          "] does not match a nested model, link, joint, or frame name "
           "in model with name[" + _model->Name() + "]."});
       continue;
     }
@@ -594,7 +635,7 @@ Errors buildPoseRelativeToGraph(
       errors.push_back({ErrorCode::POSE_RELATIVE_TO_INVALID,
           "relative_to name[" + relativeTo +
           "] specified by joint with name[" + joint->Name() +
-          "] does not match a link, joint, or frame name "
+          "] does not match a nested model, link, joint, or frame name "
           "in model with name[" + _model->Name() + "]."});
       continue;
     }
@@ -643,7 +684,7 @@ Errors buildPoseRelativeToGraph(
       errors.push_back({errorCode,
           typeForErrorMsg + " name[" + relativeTo +
           "] specified by frame with name[" + frame->Name() +
-          "] does not match a link, joint, or frame name "
+          "] does not match a nested model, link, joint, or frame name "
           "in model with name[" + _model->Name() + "]."});
       continue;
     }
@@ -659,6 +700,41 @@ Errors buildPoseRelativeToGraph(
     _out.graph.AddEdge({relativeToId, frameId}, frame->RawPose());
   }
 
+  for (uint64_t m = 0; m < _model->ModelCount(); ++m)
+  {
+    auto nestedModel = _model->ModelByIndex(m);
+
+    // check if we've already added a default edge
+    const std::string relativeTo = nestedModel->PoseRelativeTo();
+    if (relativeTo.empty())
+    {
+      continue;
+    }
+
+    auto nestedModelId = _out.map.at(nestedModel->Name());
+
+    // look for vertex in graph that matches relative_to value
+    if (_out.map.count(relativeTo) != 1)
+    {
+      errors.push_back({ErrorCode::POSE_RELATIVE_TO_INVALID,
+          "relative_to name[" + relativeTo +
+          "] specified by nested model with name[" + nestedModel->Name() +
+          "] does not match a nested model, link, joint, or frame name "
+          "in model with name[" + _model->Name() + "]."});
+      continue;
+    }
+    auto relativeToId = _out.map[relativeTo];
+    if (nestedModel->Name() == relativeTo)
+    {
+      errors.push_back({ErrorCode::POSE_RELATIVE_TO_CYCLE,
+          "relative_to name[" + relativeTo +
+          "] is identical to nested model name[" + nestedModel->Name() +
+          "], causing a graph cycle "
+          "in model with name[" + _model->Name() + "]."});
+    }
+    _out.graph.AddEdge({relativeToId, nestedModelId}, nestedModel->RawPose());
+  }
+
   return errors;
 }
 
@@ -916,6 +992,22 @@ Errors validateFrameAttachedToGraph(const FrameAttachedToGraph &_in)
                 "in MODEL attached_to graph."});
           }
           break;
+        case sdf::FrameType::MODEL:
+          if ("__model__" != vertexPair.second.get().Name())
+          {
+            if (outDegree != 0)
+            {
+              errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR,
+                  "FrameAttachedToGraph error, "
+                  "nested MODEL vertex with name [" +
+                  vertexPair.second.get().Name() +
+                  "] should have no outgoing edges "
+                  "in MODEL attached_to graph."});
+            }
+            break;
+          }
+          // fall through to default case for __model__
+          [[fallthrough]];
         default:
           if (outDegree == 0)
           {
@@ -1065,22 +1157,26 @@ Errors validatePoseRelativeToGraph(const PoseRelativeToGraph &_in)
               "should not have type WORLD in MODEL relative_to graph."});
           break;
         case sdf::FrameType::MODEL:
-          if (inDegree != 0)
+          if ("__model__" == vertexPair.second.get().Name())
           {
-            errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR,
-                "PoseRelativeToGraph error, "
-                "MODEL vertex with name [" +
-                vertexPair.second.get().Name() +
-                "] should have no incoming edges "
-                "in MODEL relative_to graph."});
+            if (inDegree != 0)
+            {
+              errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR,
+                  "PoseRelativeToGraph error, "
+                  "MODEL vertex with name [__model__"
+                  "] should have no incoming edges "
+                  "in MODEL relative_to graph."});
+            }
+            break;
           }
-          break;
+          // fall through to default case for nested models
+          [[fallthrough]];
         default:
           if (inDegree == 0)
           {
             errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR,
                 "PoseRelativeToGraph error, "
-                "Non-MODEL vertex with name [" +
+                "Vertex with name [" +
                 vertexPair.second.get().Name() +
                 "] is disconnected; it should have 1 incoming edge " +
                 "in MODEL relative_to graph."});
@@ -1206,13 +1302,26 @@ Errors resolveFrameAttachedToBody(
     return errors;
   }
 
-  if (_in.scopeName == "__model__" && sinkVertex.Data() != FrameType::LINK)
+  if (_in.scopeName == "__model__")
   {
-    errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR,
-        "Graph has __model__ scope but sink vertex named [" +
-        sinkVertex.Name() + "] does not have FrameType LINK "
-        "when starting from vertex with name [" + _vertexName + "]."});
-    return errors;
+    if (sinkVertex.Data() == FrameType::MODEL &&
+        sinkVertex.Name() == "__model__")
+    {
+      errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR,
+          "Graph with __model__ scope has sink vertex named [__model__] "
+          "when starting from vertex with name [" + _vertexName + "], "
+          "which is not permitted."});
+      return errors;
+    }
+    else if (sinkVertex.Data() != FrameType::LINK &&
+             sinkVertex.Data() != FrameType::MODEL)
+    {
+      errors.push_back({ErrorCode::FRAME_ATTACHED_TO_GRAPH_ERROR,
+          "Graph has __model__ scope but sink vertex named [" +
+          sinkVertex.Name() + "] does not have FrameType LINK OR MODEL "
+          "when starting from vertex with name [" + _vertexName + "]."});
+      return errors;
+    }
   }
 
   _attachedToBody = sinkVertex.Name();

src/FrameSemantics.hh

@@ -32,6 +32,11 @@
 /// The Frame Semantics Utilities construct and operate on graphs representing
 /// the kinematics, frame attached_to, and pose relative_to relationships
 /// defined within models and world.
+///
+/// Note that all graphs should only contain relative names (e.g. "my_link"),
+/// not absolute names ("top_model::nested_model::my_link").
+/// Graphs inside nested models (currently via directly nested models in
+/// //model or //world elements) will be explicitly separate graphs.
 namespace sdf
 {
   // Inline bracket to help doxygen filtering.