diff --git a/Changelog.md b/Changelog.md
index 303600d9c..2b4ef908a 100644
--- a/Changelog.md
+++ b/Changelog.md
@@ -42,6 +42,11 @@
### libsdformat 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)
diff --git a/Migration.md b/Migration.md
index 0e3b60a18..5b1db586f 100644
--- a/Migration.md
+++ b/Migration.md
@@ -92,6 +92,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**
@@ -252,7 +256,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: *
diff --git a/include/sdf/Model.hh b/include/sdf/Model.hh
index 9a3599de8..f700cca84 100644
--- a/include/sdf/Model.hh
+++ b/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 ( ... ), and is
/// typically used to express the position and rotation of a model in a
@@ -258,9 +281,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 _graph);
/// \brief Allow World::Load to call SetPoseRelativeToGraph.
diff --git a/sdf/1.7/frame.sdf b/sdf/1.7/frame.sdf
index be28ffe9b..dcc1c3397 100644
--- a/sdf/1.7/frame.sdf
+++ b/sdf/1.7/frame.sdf
@@ -10,7 +10,7 @@
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.
diff --git a/src/FrameSemantics.cc b/src/FrameSemantics.cc
index 570ca196e..32d9f27a1 100644
--- a/src/FrameSemantics.cc
+++ b/src/FrameSemantics.cc
@@ -273,6 +273,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 edges from joint to child frames
for (uint64_t j = 0; j < _model->JointCount(); ++j)
{
@@ -308,7 +325,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;
}
@@ -358,9 +375,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,
@@ -527,6 +544,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
@@ -549,7 +590,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;
}
@@ -584,7 +625,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;
}
@@ -633,7 +674,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;
}
@@ -649,6 +690,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;
}
@@ -906,6 +982,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)
{
@@ -1055,22 +1147,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."});
@@ -1196,13 +1292,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();
diff --git a/src/FrameSemantics.hh b/src/FrameSemantics.hh
index 2e34201ff..80ebf84ff 100644
--- a/src/FrameSemantics.hh
+++ b/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.
diff --git a/src/Model.cc b/src/Model.cc
index 608b27033..62d578bac 100644
--- a/src/Model.cc
+++ b/src/Model.cc
@@ -70,6 +70,9 @@ class sdf::ModelPrivate
/// \brief The frames specified in this model.
public: std::vector frames;
+ /// \brief The nested models specified in this model.
+ public: std::vector models;
+
/// \brief The SDF element pointer used during load.
public: sdf::ElementPtr sdf;
@@ -79,8 +82,11 @@ class sdf::ModelPrivate
/// \brief Pose Relative-To Graph constructed during Load.
public: std::shared_ptr poseGraph;
- /// \brief Pose Relative-To Graph in parent (world) scope.
+ /// \brief Pose Relative-To Graph in parent (world or __model__) scope.
public: std::weak_ptr parentPoseGraph;
+
+ /// \brief Scope name of parent Pose Relative-To Graph (world or __model__).
+ public: std::string parentPoseGraphScopeName;
};
/////////////////////////////////////////////////
@@ -115,6 +121,10 @@ Model::Model(const Model &_model)
{
link.SetPoseRelativeToGraph(this->dataPtr->poseGraph);
}
+ for (auto &model : this->dataPtr->models)
+ {
+ model.SetPoseRelativeToGraph(this->dataPtr->poseGraph);
+ }
for (auto &joint : this->dataPtr->joints)
{
joint.SetFrameAttachedToGraph(this->dataPtr->frameAttachedToGraph);
@@ -204,14 +214,6 @@ Errors Model::Load(ElementPtr _sdf)
// Load the pose. Ignore the return value since the model pose is optional.
loadPose(_sdf, this->dataPtr->pose, this->dataPtr->poseRelativeTo);
- // Nested models are not yet supported.
- if (_sdf->HasElement("model"))
- {
- errors.push_back({ErrorCode::NESTED_MODELS_UNSUPPORTED,
- "Nested models are not yet supported by DOM objects, "
- "skipping model [" + this->dataPtr->name + "]."});
- }
-
if (!_sdf->HasUniqueChildNames())
{
sdfwarn << "Non-unique names detected in XML children of model with name["
@@ -222,17 +224,56 @@ Errors Model::Load(ElementPtr _sdf)
// name collisions
std::unordered_set frameNames;
+ // Load nested models.
+ Errors nestedModelLoadErrors = loadUniqueRepeated(_sdf, "model",
+ this->dataPtr->models);
+ errors.insert(errors.end(),
+ nestedModelLoadErrors.begin(),
+ nestedModelLoadErrors.end());
+
+ // Nested models are loaded first, and loadUniqueRepeated ensures there are no
+ // duplicate names, so these names can be added to frameNames without
+ // checking uniqueness.
+ for (const auto &model : this->dataPtr->models)
+ {
+ frameNames.insert(model.Name());
+ }
+
// Load all the links.
Errors linkLoadErrors = loadUniqueRepeated(_sdf, "link",
this->dataPtr->links);
errors.insert(errors.end(), linkLoadErrors.begin(), linkLoadErrors.end());
- // Links are loaded first, and loadUniqueRepeated ensures there are no
- // duplicate names, so these names can be added to frameNames without
- // checking uniqueness.
- for (const auto &link : this->dataPtr->links)
+ // Check links for name collisions and modify and warn if so.
+ for (auto &link : this->dataPtr->links)
{
- frameNames.insert(link.Name());
+ std::string linkName = link.Name();
+ if (frameNames.count(linkName) > 0)
+ {
+ // This link has a name collision
+ if (sdfVersion < ignition::math::SemanticVersion(1, 7))
+ {
+ // This came from an old file, so try to workaround by renaming link
+ linkName += "_link";
+ int i = 0;
+ while (frameNames.count(linkName) > 0)
+ {
+ linkName = link.Name() + "_link" + std::to_string(i++);
+ }
+ sdfwarn << "Link with name [" << link.Name() << "] "
+ << "in model with name [" << this->Name() << "] "
+ << "has a name collision, changing link name to ["
+ << linkName << "].\n";
+ link.SetName(linkName);
+ }
+ else
+ {
+ sdferr << "Link with name [" << link.Name() << "] "
+ << "in model with name [" << this->Name() << "] "
+ << "has a name collision. Please rename this link.\n";
+ }
+ }
+ frameNames.insert(linkName);
}
// If the model is not static:
@@ -359,6 +400,13 @@ Errors Model::Load(ElementPtr _sdf)
{
link.SetPoseRelativeToGraph(this->dataPtr->poseGraph);
}
+ for (auto &model : this->dataPtr->models)
+ {
+ Errors setPoseRelativeToGraphErrors =
+ model.SetPoseRelativeToGraph(this->dataPtr->poseGraph);
+ errors.insert(errors.end(), setPoseRelativeToGraphErrors.begin(),
+ setPoseRelativeToGraphErrors.end());
+ }
for (auto &joint : this->dataPtr->joints)
{
joint.SetPoseRelativeToGraph(this->dataPtr->poseGraph);
@@ -620,6 +668,46 @@ const Frame *Model::FrameByName(const std::string &_name) const
return nullptr;
}
+/////////////////////////////////////////////////
+uint64_t Model::ModelCount() const
+{
+ return this->dataPtr->models.size();
+}
+
+/////////////////////////////////////////////////
+const Model *Model::ModelByIndex(const uint64_t _index) const
+{
+ if (_index < this->dataPtr->models.size())
+ return &this->dataPtr->models[_index];
+ return nullptr;
+}
+
+/////////////////////////////////////////////////
+bool Model::ModelNameExists(const std::string &_name) const
+{
+ for (auto const &m : this->dataPtr->models)
+ {
+ if (m.Name() == _name)
+ {
+ return true;
+ }
+ }
+ return false;
+}
+
+/////////////////////////////////////////////////
+const Model *Model::ModelByName(const std::string &_name) const
+{
+ for (auto const &m : this->dataPtr->models)
+ {
+ if (m.Name() == _name)
+ {
+ return &m;
+ }
+ }
+ return nullptr;
+}
+
/////////////////////////////////////////////////
const Link *Model::CanonicalLink() const
{
@@ -682,10 +770,23 @@ void Model::SetPoseRelativeTo(const std::string &_frame)
}
/////////////////////////////////////////////////
-void Model::SetPoseRelativeToGraph(
+Errors Model::SetPoseRelativeToGraph(
std::weak_ptr _graph)
{
+ Errors errors;
+
+ auto graph = _graph.lock();
+ if (!graph)
+ {
+ errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR,
+ "Tried to set PoseRelativeToGraph with invalid pointer."});
+ return errors;
+ }
+
+ this->dataPtr->parentPoseGraphScopeName = graph->sourceName;
this->dataPtr->parentPoseGraph = _graph;
+
+ return errors;
}
/////////////////////////////////////////////////
@@ -694,7 +795,7 @@ sdf::SemanticPose Model::SemanticPose() const
return sdf::SemanticPose(
this->dataPtr->pose,
this->dataPtr->poseRelativeTo,
- "world",
+ this->dataPtr->parentPoseGraphScopeName,
this->dataPtr->parentPoseGraph);
}
diff --git a/src/Model_TEST.cc b/src/Model_TEST.cc
index b428efa67..290e9f9a9 100644
--- a/src/Model_TEST.cc
+++ b/src/Model_TEST.cc
@@ -47,21 +47,35 @@ TEST(DOMModel, Construction)
model.SetEnableWind(true);
EXPECT_TRUE(model.EnableWind());
+ EXPECT_EQ(0u, model.ModelCount());
+ EXPECT_EQ(nullptr, model.ModelByIndex(0));
+ EXPECT_EQ(nullptr, model.ModelByIndex(1));
+ EXPECT_EQ(nullptr, model.ModelByName(""));
+ EXPECT_EQ(nullptr, model.ModelByName("default"));
+ EXPECT_FALSE(model.ModelNameExists(""));
+ EXPECT_FALSE(model.ModelNameExists("default"));
+
EXPECT_EQ(0u, model.LinkCount());
EXPECT_EQ(nullptr, model.LinkByIndex(0));
EXPECT_EQ(nullptr, model.LinkByIndex(1));
+ EXPECT_EQ(nullptr, model.LinkByName(""));
+ EXPECT_EQ(nullptr, model.LinkByName("default"));
EXPECT_FALSE(model.LinkNameExists(""));
EXPECT_FALSE(model.LinkNameExists("default"));
EXPECT_EQ(0u, model.JointCount());
EXPECT_EQ(nullptr, model.JointByIndex(0));
EXPECT_EQ(nullptr, model.JointByIndex(1));
+ EXPECT_EQ(nullptr, model.JointByName(""));
+ EXPECT_EQ(nullptr, model.JointByName("default"));
EXPECT_FALSE(model.JointNameExists(""));
EXPECT_FALSE(model.JointNameExists("default"));
EXPECT_EQ(0u, model.FrameCount());
EXPECT_EQ(nullptr, model.FrameByIndex(0));
EXPECT_EQ(nullptr, model.FrameByIndex(1));
+ EXPECT_EQ(nullptr, model.FrameByName(""));
+ EXPECT_EQ(nullptr, model.FrameByName("default"));
EXPECT_FALSE(model.FrameNameExists(""));
EXPECT_FALSE(model.FrameNameExists("default"));
diff --git a/src/SemanticPose.cc b/src/SemanticPose.cc
index 01e7f904e..eefc7d51c 100644
--- a/src/SemanticPose.cc
+++ b/src/SemanticPose.cc
@@ -106,7 +106,7 @@ Errors SemanticPose::Resolve(
auto graph = this->dataPtr->poseRelativeToGraph.lock();
if (!graph)
{
- errors.push_back({ErrorCode::ELEMENT_INVALID,
+ errors.push_back({ErrorCode::POSE_RELATIVE_TO_GRAPH_ERROR,
"SemanticPose has invalid pointer to PoseRelativeToGraph."});
return errors;
}
diff --git a/src/World.cc b/src/World.cc
index c21646155..38ac9c1b3 100644
--- a/src/World.cc
+++ b/src/World.cc
@@ -372,7 +372,10 @@ Errors World::Load(sdf::ElementPtr _sdf)
}
for (auto &model : this->dataPtr->models)
{
- model.SetPoseRelativeToGraph(this->dataPtr->poseRelativeToGraph);
+ Errors setPoseRelativeToGraphErrors =
+ model.SetPoseRelativeToGraph(this->dataPtr->poseRelativeToGraph);
+ errors.insert(errors.end(), setPoseRelativeToGraphErrors.begin(),
+ setPoseRelativeToGraphErrors.end());
}
for (auto &light : this->dataPtr->lights)
{
diff --git a/src/ign_TEST.cc b/src/ign_TEST.cc
index ee85c9bc5..bfd92442c 100644
--- a/src/ign_TEST.cc
+++ b/src/ign_TEST.cc
@@ -337,6 +337,17 @@ TEST(check, SDF)
std::string::npos) << output;
}
+ // Check an SDF file with an invalid model without links.
+ {
+ std::string path = pathBase +"/model_without_links.sdf";
+
+ // Check model_without_links.sdf
+ std::string output =
+ custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion);
+ EXPECT_NE(output.find("Error: A model must have at least one link."),
+ std::string::npos) << output;
+ }
+
// Check an SDF file with a nested model.
{
std::string path = pathBase +"/nested_model.sdf";
@@ -344,9 +355,7 @@ TEST(check, SDF)
// Check nested_model.sdf
std::string output =
custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion);
- EXPECT_NE(output.find("Error: Nested models are not yet supported by DOM "
- "objects, skipping model [top_level_model]."),
- std::string::npos) << output;
+ EXPECT_EQ("Valid.\n", output) << output;
}
// Check an invalid SDF file that uses reserved names.
@@ -415,6 +424,17 @@ TEST(check, SDF)
EXPECT_EQ("Valid.\n", output) << output;
}
+ // Check an SDF file with model frames attached_to a nested model.
+ // This is a valid file.
+ {
+ std::string path = pathBase +"/model_frame_attached_to_nested_model.sdf";
+
+ // Check model_frame_attached_to_nested_model.sdf
+ std::string output =
+ custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion);
+ EXPECT_EQ("Valid.\n", output) << output;
+ }
+
// Check an SDF file with model frames with invalid attached_to attributes.
{
std::string path = pathBase +"/model_frame_invalid_attached_to.sdf";
@@ -423,8 +443,8 @@ TEST(check, SDF)
std::string output =
custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion);
EXPECT_NE(output.find("Error: attached_to name[A] specified by frame with "
- "name[F3] does not match a link, joint, or frame "
- "name in model with "
+ "name[F3] does not match a nested model, link, "
+ "joint, or frame name in model with "
"name[model_frame_invalid_attached_to]."),
std::string::npos) << output;
EXPECT_NE(output.find("Error: attached_to name[F4] is identical to frame "
@@ -497,8 +517,8 @@ TEST(check, SDF)
std::string output =
custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion);
EXPECT_NE(output.find("Error: relative_to name[A] specified by link with "
- "name[L] does not match a link, joint, or frame "
- "name in model with "
+ "name[L] does not match a nested model, link, "
+ "joint, or frame name in model with "
"name[model_invalid_link_relative_to]."),
std::string::npos) << output;
EXPECT_NE(output.find("Error: relative_to name[self_cycle] is identical to "
@@ -507,6 +527,31 @@ TEST(check, SDF)
std::string::npos) << output;
}
+ // Check an SDF file with nested_models using the relative_to attribute.
+ // This is a valid file.
+ {
+ std::string path = pathBase +"/model_nested_model_relative_to.sdf";
+
+ // Check model_nested_model_relative_to.sdf
+ std::string output =
+ custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion);
+ EXPECT_EQ("Valid.\n", output) << output;
+ }
+
+ // Check an invalid SDF file with a joint that specifies a child link
+ // within a sibling nested model using the unsupported :: syntax.
+ {
+ std::string path = pathBase +"/model_invalid_nested_joint_child.sdf";
+
+ // Check model_invalid_nested_joint_child.sdf
+ std::string output =
+ custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion);
+ EXPECT_NE(output.find("Error: Child frame with name[M::C] specified by "
+ "joint with name[J] not found in model with "
+ "name[model_invalid_nested_joint_child]."),
+ std::string::npos) << output;
+ }
+
// Check an SDF file with joints using the relative_to attribute.
// This is a valid file.
{
@@ -526,8 +571,8 @@ TEST(check, SDF)
std::string output =
custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion);
EXPECT_NE(output.find("Error: relative_to name[A] specified by joint with "
- "name[J] does not match a link, joint, or frame "
- "name in model with "
+ "name[J] does not match a nested model, link, "
+ "joint, or frame name in model with "
"name[model_invalid_joint_relative_to]."),
std::string::npos) << output;
EXPECT_NE(output.find("Error: relative_to name[Jcycle] is identical to "
@@ -566,8 +611,8 @@ TEST(check, SDF)
std::string output =
custom_exec_str(g_ignCommand + " sdf -k " + path + g_sdfVersion);
EXPECT_NE(output.find("Error: relative_to name[A] specified by frame with "
- "name[F] does not match a link, joint, or frame "
- "name in model with "
+ "name[F] does not match a nested model, link, "
+ "joint, or frame name in model with "
"name[model_invalid_frame_relative_to]."),
std::string::npos) << output;
EXPECT_NE(output.find("Error: relative_to name[cycle] is identical to "
diff --git a/src/parser.cc b/src/parser.cc
index d3c16dc4f..5fde25f7d 100644
--- a/src/parser.cc
+++ b/src/parser.cc
@@ -1570,13 +1570,14 @@ bool checkFrameAttachedToNames(const sdf::Root *_root)
modelResult = false;
}
else if (!_model->LinkNameExists(attachedTo) &&
+ !_model->ModelNameExists(attachedTo) &&
!_model->JointNameExists(attachedTo) &&
!_model->FrameNameExists(attachedTo))
{
std::cerr << "Error: attached_to name[" << attachedTo
<< "] specified by frame with name[" << frame->Name()
- << "] does not match a link, joint, or frame name "
- << "in model with name[" << _model->Name()
+ << "] does not match a nested model, link, joint, "
+ << "or frame name in model with name[" << _model->Name()
<< "]."
<< std::endl;
modelResult = false;
diff --git a/test/integration/frame.cc b/test/integration/frame.cc
index 6c9c89b12..02542f232 100644
--- a/test/integration/frame.cc
+++ b/test/integration/frame.cc
@@ -489,7 +489,7 @@ TEST(DOMFrame, LoadModelFramesInvalidAttachedTo)
EXPECT_NE(std::string::npos,
errors[0].Message().find(
"attached_to name[A] specified by frame with name[F3] does not match a "
- "link, joint, or frame name in model"));
+ "nested model, link, joint, or frame name in model"));
EXPECT_EQ(errors[1].Code(), sdf::ErrorCode::FRAME_ATTACHED_TO_CYCLE);
EXPECT_NE(std::string::npos,
errors[1].Message().find(
@@ -502,7 +502,7 @@ TEST(DOMFrame, LoadModelFramesInvalidAttachedTo)
EXPECT_NE(std::string::npos,
errors[5].Message().find(
"attached_to name[A] specified by frame with name[F3] does not match a "
- "link, joint, or frame name in model"));
+ "nested model, link, joint, or frame name in model"));
EXPECT_EQ(errors[6].Code(), sdf::ErrorCode::POSE_RELATIVE_TO_CYCLE);
EXPECT_NE(std::string::npos,
errors[6].Message().find(
@@ -578,6 +578,55 @@ TEST(DOMFrame, LoadModelFramesAttachedToJoint)
EXPECT_EQ("C", body);
}
+/////////////////////////////////////////////////
+TEST(DOMFrame, LoadModelFramesAttachedToNestedModel)
+{
+ const std::string testFile =
+ sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf",
+ "model_frame_attached_to_nested_model.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);
+ ASSERT_NE(nullptr, model);
+ EXPECT_EQ("model_frame_attached_to_nested_model", model->Name());
+ EXPECT_EQ(1u, model->LinkCount());
+ EXPECT_NE(nullptr, model->LinkByIndex(0));
+ EXPECT_EQ(nullptr, model->LinkByIndex(1));
+
+ EXPECT_TRUE(model->LinkNameExists("link"));
+
+ EXPECT_TRUE(model->CanonicalLinkName().empty());
+
+ EXPECT_EQ(0u, model->JointCount());
+ EXPECT_EQ(nullptr, model->JointByIndex(0));
+
+ EXPECT_EQ(1u, model->ModelCount());
+ EXPECT_NE(nullptr, model->ModelByIndex(0));
+ EXPECT_EQ(nullptr, model->ModelByIndex(1));
+
+ EXPECT_TRUE(model->ModelNameExists("nested_model"));
+
+ EXPECT_EQ(2u, model->FrameCount());
+ EXPECT_NE(nullptr, model->FrameByIndex(0));
+ EXPECT_NE(nullptr, model->FrameByIndex(1));
+ EXPECT_EQ(nullptr, model->FrameByIndex(2));
+ ASSERT_TRUE(model->FrameNameExists("F1"));
+ ASSERT_TRUE(model->FrameNameExists("F2"));
+
+ EXPECT_EQ("nested_model", model->FrameByName("F1")->AttachedTo());
+ EXPECT_EQ("F1", model->FrameByName("F2")->AttachedTo());
+
+ std::string body;
+ EXPECT_TRUE(model->FrameByName("F1")->ResolveAttachedToBody(body).empty());
+ EXPECT_EQ("nested_model", body);
+ EXPECT_TRUE(model->FrameByName("F2")->ResolveAttachedToBody(body).empty());
+ EXPECT_EQ("nested_model", body);
+}
+
/////////////////////////////////////////////////
TEST(DOMFrame, LoadWorldFramesAttachedTo)
{
@@ -837,7 +886,7 @@ TEST(DOMFrame, LoadModelFramesInvalidRelativeTo)
EXPECT_NE(std::string::npos,
errors[0].Message().find(
"relative_to name[A] specified by frame with name[F] does not match a "
- "link, joint, or frame name in model"));
+ "nested model, link, joint, or frame name in model"));
EXPECT_EQ(errors[1].Code(), sdf::ErrorCode::POSE_RELATIVE_TO_CYCLE);
EXPECT_NE(std::string::npos,
errors[1].Message().find(
diff --git a/test/integration/joint_dom.cc b/test/integration/joint_dom.cc
index c1c881c9c..cb711c1b1 100644
--- a/test/integration/joint_dom.cc
+++ b/test/integration/joint_dom.cc
@@ -537,7 +537,7 @@ TEST(DOMJoint, LoadInvalidJointPoseRelativeTo)
EXPECT_NE(std::string::npos,
errors[1].Message().find(
"relative_to name[A] specified by joint with name[J] does not match a "
- "link, joint, or frame name in model"));
+ "nested model, link, joint, or frame name in model"));
// errors[2]
// errors[3]
// errors[4]
diff --git a/test/integration/link_dom.cc b/test/integration/link_dom.cc
index 0633d8f22..19b018da5 100644
--- a/test/integration/link_dom.cc
+++ b/test/integration/link_dom.cc
@@ -672,7 +672,7 @@ TEST(DOMLink, LoadInvalidLinkPoseRelativeTo)
EXPECT_NE(std::string::npos,
errors[0].Message().find(
"relative_to name[A] specified by link with name[L] does not match a "
- "link, joint, or frame name in model"));
+ "nested model, link, joint, or frame name in model"));
EXPECT_EQ(errors[1].Code(), sdf::ErrorCode::POSE_RELATIVE_TO_CYCLE);
EXPECT_NE(std::string::npos,
errors[1].Message().find(
diff --git a/test/integration/model_dom.cc b/test/integration/model_dom.cc
index 171ba1edb..299100509 100644
--- a/test/integration/model_dom.cc
+++ b/test/integration/model_dom.cc
@@ -155,10 +155,7 @@ TEST(DOMRoot, NestedModel)
// Load the SDF file
sdf::Root root;
auto errors = root.Load(testFile);
-
- // it should complain because nested models aren't yet supported
- EXPECT_FALSE(errors.empty());
- EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::NESTED_MODELS_UNSUPPORTED);
+ EXPECT_TRUE(errors.empty());
EXPECT_EQ(1u, root.ModelCount());
@@ -181,6 +178,107 @@ TEST(DOMRoot, NestedModel)
EXPECT_EQ(nullptr, model->JointByIndex(1));
EXPECT_TRUE(model->JointNameExists("top_level_joint"));
+
+ ASSERT_EQ(1u, model->ModelCount());
+ const sdf::Model *nestedModel = model->ModelByIndex(0);
+ ASSERT_NE(nullptr, nestedModel);
+ EXPECT_EQ(nullptr, model->ModelByIndex(1));
+
+ EXPECT_TRUE(model->ModelNameExists("nested_model"));
+ EXPECT_EQ(nestedModel, model->ModelByName("nested_model"));
+ EXPECT_EQ("nested_model", nestedModel->Name());
+
+ EXPECT_EQ(1u, nestedModel->LinkCount());
+ EXPECT_NE(nullptr, nestedModel->LinkByIndex(0));
+ EXPECT_EQ(nullptr, nestedModel->LinkByIndex(1));
+
+ EXPECT_TRUE(nestedModel->LinkNameExists("nested_link01"));
+
+ EXPECT_EQ(0u, nestedModel->JointCount());
+ EXPECT_EQ(0u, nestedModel->FrameCount());
+}
+
+/////////////////////////////////////////////////
+TEST(DOMLink, NestedModelPoseRelativeTo)
+{
+ const std::string testFile =
+ sdf::filesystem::append(PROJECT_SOURCE_PATH, "test", "sdf",
+ "model_nested_model_relative_to.sdf");
+
+ // Load the SDF file
+ sdf::Root root;
+ EXPECT_TRUE(root.Load(testFile).empty());
+
+ using Pose = ignition::math::Pose3d;
+
+ // Get the first model
+ const sdf::Model *model = root.ModelByIndex(0);
+ ASSERT_NE(nullptr, model);
+ EXPECT_EQ("model_nested_model_relative_to", model->Name());
+ EXPECT_EQ(1u, model->LinkCount());
+ EXPECT_NE(nullptr, model->LinkByIndex(0));
+ EXPECT_EQ(nullptr, model->LinkByIndex(1));
+ EXPECT_EQ(Pose(0, 0, 0, 0, 0, 0), model->RawPose());
+ EXPECT_EQ("", model->PoseRelativeTo());
+
+ ASSERT_TRUE(model->LinkNameExists("L"));
+ EXPECT_TRUE(model->LinkByName("L")->PoseRelativeTo().empty());
+ EXPECT_EQ(Pose(0, 0, 0, 0, 0, 0), model->LinkByName("L")->RawPose());
+
+ ASSERT_TRUE(model->ModelNameExists("M1"));
+ ASSERT_TRUE(model->ModelNameExists("M2"));
+ ASSERT_TRUE(model->ModelNameExists("M3"));
+ EXPECT_TRUE(model->ModelByName("M1")->PoseRelativeTo().empty());
+ EXPECT_TRUE(model->ModelByName("M2")->PoseRelativeTo().empty());
+ EXPECT_EQ("M1", model->ModelByName("M3")->PoseRelativeTo());
+
+ EXPECT_EQ(Pose(1, 0, 0, 0, IGN_PI/2, 0), model->ModelByName("M1")->RawPose());
+ EXPECT_EQ(Pose(2, 0, 0, 0, 0, 0), model->ModelByName("M2")->RawPose());
+ EXPECT_EQ(Pose(3, 0, 0, 0, 0, 0), model->ModelByName("M3")->RawPose());
+
+ EXPECT_EQ(Pose(1, 0, 0, 0, IGN_PI / 2, 0),
+ model->ModelByName("M1")->SemanticPose().RawPose());
+ EXPECT_EQ(Pose(2, 0, 0, 0, 0, 0),
+ model->ModelByName("M2")->SemanticPose().RawPose());
+ EXPECT_EQ(Pose(3, 0, 0, 0, 0, 0),
+ model->ModelByName("M3")->SemanticPose().RawPose());
+
+ // Test SemanticPose().Resolve to get each nested model pose in the
+ // __model__ frame
+ Pose pose;
+ EXPECT_TRUE(
+ model->ModelByName("M1")->SemanticPose().Resolve(pose,
+ "__model__").empty());
+ EXPECT_EQ(Pose(1, 0, 0, 0, IGN_PI/2, 0), pose);
+ EXPECT_TRUE(
+ model->ModelByName("M2")->SemanticPose().Resolve(pose,
+ "__model__").empty());
+ EXPECT_EQ(Pose(2, 0, 0, 0, 0, 0), pose);
+ EXPECT_TRUE(
+ model->ModelByName("M3")->SemanticPose().Resolve(pose,
+ "__model__").empty());
+ EXPECT_EQ(Pose(1, 0, -3, 0, IGN_PI/2, 0), pose);
+ // test other API too
+ EXPECT_TRUE(model->ModelByName("M1")->SemanticPose().Resolve(pose).empty());
+ EXPECT_EQ(Pose(1, 0, 0, 0, IGN_PI/2, 0), pose);
+ EXPECT_TRUE(model->ModelByName("M2")->SemanticPose().Resolve(pose).empty());
+ EXPECT_EQ(Pose(2, 0, 0, 0, 0, 0), pose);
+ EXPECT_TRUE(model->ModelByName("M3")->SemanticPose().Resolve(pose).empty());
+ EXPECT_EQ(Pose(1, 0, -3, 0, IGN_PI/2, 0), pose);
+
+ // resolve pose of M1 relative to M3
+ // should be inverse of M3's Pose()
+ EXPECT_TRUE(
+ model->ModelByName("M1")->SemanticPose().Resolve(pose, "M3").empty());
+ EXPECT_EQ(Pose(-3, 0, 0, 0, 0, 0), pose);
+
+ EXPECT_TRUE(model->CanonicalLinkName().empty());
+
+ EXPECT_EQ(0u, model->JointCount());
+ EXPECT_EQ(nullptr, model->JointByIndex(0));
+
+ EXPECT_EQ(0u, model->FrameCount());
+ EXPECT_EQ(nullptr, model->FrameByIndex(0));
}
/////////////////////////////////////////////////
diff --git a/test/sdf/model_frame_attached_to_nested_model.sdf b/test/sdf/model_frame_attached_to_nested_model.sdf
new file mode 100644
index 000000000..995d828dd
--- /dev/null
+++ b/test/sdf/model_frame_attached_to_nested_model.sdf
@@ -0,0 +1,11 @@
+
+
+
+
+
+
+
+
+
+
+
diff --git a/test/sdf/model_invalid_nested_joint_child.sdf b/test/sdf/model_invalid_nested_joint_child.sdf
new file mode 100644
index 000000000..efde66366
--- /dev/null
+++ b/test/sdf/model_invalid_nested_joint_child.sdf
@@ -0,0 +1,16 @@
+
+
+
+
+
+ 1 0 0 0 1.5707963267948966 0
+
+
+
+ P
+
+
+ M::C
+
+
+
diff --git a/test/sdf/model_nested_model_relative_to.sdf b/test/sdf/model_nested_model_relative_to.sdf
new file mode 100644
index 000000000..fc3c4630b
--- /dev/null
+++ b/test/sdf/model_nested_model_relative_to.sdf
@@ -0,0 +1,18 @@
+
+
+
+
+
+ 1 0 0 0 1.5707963267948966 0
+
+
+
+ 2 0 0 0 0 0
+
+
+
+ 3 0 0 0 0 0
+
+
+
+