[sdf9] Support nested models in DOM and frame semantics (gazebosim#316)

Nested model elements (`//model/model`) are currently supported in the
SDFormat 1.7 spec, but are not supported by the DOM API or frame
semantics operations in libsdformat 9.2.
Per Amendment 1 of the SDFormat 1.7 proposal
(sdformat.org/tutorials?tut=pose_frame_semantics_proposal#amendment-1-directly-nested-models),
this PR adds support for nested models in the DOM API and frame
semantics (fixing gazebosim#283) through three steps:

* adding `Model::Model*` methods for accessing nested models via
  the DOM API (047ec96)
* loading nested models in `Model::Load` (b57fea2, step 3 of model parsing
  stages (sdformat.org/tutorials?tut=pose_frame_semantics_proposal#1-model))
* supporting nested models (`//model/model`) in frame semantics as
  opaque frames to match how models in the world scope (`//world/model`)
  are treated (85e0b4f, steps 6-9 of model parsing stages
  (sdformat.org/tutorials?tut=pose_frame_semantics_proposal#1-model))

The first two steps are straightforward, while the 3rd step adds new behavior.
This behavior is added to `libsdformat9` because it is compatible with the
SDFormat 1.7 spec (which supports nested models) and makes the treatment
of models more consistent with regard to frame semantics.
In libsdformat 9.2.0, a `//world/model` supports frame semantics: they have
frames that can be referenced by name with `//pose/@relative_to` and
`//world/frame/@attached_to` values can resolve to a `//world/model`.
This extends that same behavior to nested `//model/model` elements;
they now have their own frames in the frame and pose graphs, and a
`//model/frame` is permitted to attach to a model.

This does not add support for referencing elements within a model via
the `::` syntax in the frame semantics or DOM APIs.
That will be added in SDFormat 1.8 (see gazebosim#293).

Signed-off-by: Steve Peters <[email protected]>

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:/osrf/sdformat/pull/316)
+
 1. Find python3 in cmake, fix cmake warning.
  * [Pull request 328](https:/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.