diff --git a/src/FrameSemantics_TEST.cc b/src/FrameSemantics_TEST.cc
index 27fad211e..5f38c4df0 100644
--- a/src/FrameSemantics_TEST.cc
+++ b/src/FrameSemantics_TEST.cc
@@ -54,9 +54,9 @@ TEST(FrameSemantics, buildFrameAttachedToGraph_Model)
EXPECT_TRUE(sdf::checkFrameAttachedToGraph(&root));
EXPECT_TRUE(sdf::checkFrameAttachedToNames(&root));
- EXPECT_EQ(6u, graph.map.size());
- EXPECT_EQ(6u, graph.graph.Vertices().size());
- EXPECT_EQ(5u, graph.graph.Edges().size());
+ EXPECT_EQ(7u, graph.map.size());
+ EXPECT_EQ(7u, graph.graph.Vertices().size());
+ EXPECT_EQ(6u, graph.graph.Edges().size());
EXPECT_EQ(1u, graph.map.count("__model__"));
EXPECT_EQ(1u, graph.map.count("L"));
@@ -118,9 +118,9 @@ TEST(FrameSemantics, buildFrameAttachedToGraph_World)
EXPECT_TRUE(sdf::checkFrameAttachedToGraph(&root));
EXPECT_TRUE(sdf::checkFrameAttachedToNames(&root));
- EXPECT_EQ(6u, graph.map.size());
- EXPECT_EQ(6u, graph.graph.Vertices().size());
- EXPECT_EQ(4u, graph.graph.Edges().size());
+ EXPECT_EQ(7u, graph.map.size());
+ EXPECT_EQ(7u, graph.graph.Vertices().size());
+ EXPECT_EQ(5u, graph.graph.Edges().size());
EXPECT_EQ(1u, graph.map.count("world"));
EXPECT_EQ(1u, graph.map.count("world_frame"));
diff --git a/src/Model.cc b/src/Model.cc
index e08ec8e24..7c88c2094 100644
--- a/src/Model.cc
+++ b/src/Model.cc
@@ -333,7 +333,7 @@ Errors Model::Load(ElementPtr _sdf)
std::string frameName = frame.Name();
if (frameNames.count(frameName) > 0)
{
- // This joint has a name collision
+ // This frame has a name collision
if (sdfVersion < ignition::math::SemanticVersion(1, 7))
{
// This came from an old file, so try to workaround by renaming frame
diff --git a/src/parser.cc b/src/parser.cc
index afb781704..4b108fc04 100644
--- a/src/parser.cc
+++ b/src/parser.cc
@@ -1554,8 +1554,8 @@ bool checkFrameAttachedToNames(const sdf::Root *_root)
const std::string &attachedTo = frame->AttachedTo();
- // the attached_to attribute is always permitted to be empty
- if (attachedTo.empty())
+ // the attached_to attribute is always permitted to be empty or __model__
+ if (attachedTo.empty() || "__model__" == attachedTo)
{
continue;
}
@@ -1597,8 +1597,8 @@ bool checkFrameAttachedToNames(const sdf::Root *_root)
const std::string &attachedTo = frame->AttachedTo();
- // the attached_to attribute is always permitted to be empty
- if (attachedTo.empty())
+ // the attached_to attribute is always permitted to be empty or world
+ if (attachedTo.empty() || "world" == attachedTo)
{
continue;
}
diff --git a/test/integration/frame.cc b/test/integration/frame.cc
index fd1b89080..1cb6c0772 100644
--- a/test/integration/frame.cc
+++ b/test/integration/frame.cc
@@ -436,28 +436,34 @@ TEST(DOMFrame, LoadModelFramesAttachedTo)
EXPECT_EQ(0u, model->JointCount());
EXPECT_EQ(nullptr, model->JointByIndex(0));
- EXPECT_EQ(4u, model->FrameCount());
+ EXPECT_EQ(5u, model->FrameCount());
EXPECT_NE(nullptr, model->FrameByIndex(0));
EXPECT_NE(nullptr, model->FrameByIndex(1));
EXPECT_NE(nullptr, model->FrameByIndex(2));
EXPECT_NE(nullptr, model->FrameByIndex(3));
- EXPECT_EQ(nullptr, model->FrameByIndex(4));
+ EXPECT_NE(nullptr, model->FrameByIndex(4));
+ EXPECT_EQ(nullptr, model->FrameByIndex(5));
+ ASSERT_TRUE(model->FrameNameExists("F000"));
ASSERT_TRUE(model->FrameNameExists("F00"));
ASSERT_TRUE(model->FrameNameExists("F0"));
ASSERT_TRUE(model->FrameNameExists("F1"));
ASSERT_TRUE(model->FrameNameExists("F2"));
+ EXPECT_EQ("__model__", model->FrameByName("F000")->AttachedTo());
EXPECT_TRUE(model->FrameByName("F00")->AttachedTo().empty());
EXPECT_TRUE(model->FrameByName("F0")->AttachedTo().empty());
EXPECT_EQ("L", model->FrameByName("F1")->AttachedTo());
EXPECT_EQ("F1", model->FrameByName("F2")->AttachedTo());
+ EXPECT_TRUE(model->FrameByName("F000")->PoseRelativeTo().empty());
EXPECT_TRUE(model->FrameByName("F00")->PoseRelativeTo().empty());
EXPECT_TRUE(model->FrameByName("F0")->PoseRelativeTo().empty());
EXPECT_TRUE(model->FrameByName("F1")->PoseRelativeTo().empty());
EXPECT_TRUE(model->FrameByName("F2")->PoseRelativeTo().empty());
std::string body;
+ EXPECT_TRUE(model->FrameByName("F000")->ResolveAttachedToBody(body).empty());
+ EXPECT_EQ("L", body);
EXPECT_TRUE(model->FrameByName("F00")->ResolveAttachedToBody(body).empty());
EXPECT_EQ("L", body);
EXPECT_TRUE(model->FrameByName("F0")->ResolveAttachedToBody(body).empty());
@@ -718,23 +724,27 @@ TEST(DOMFrame, LoadWorldFramesAttachedTo)
ASSERT_TRUE(model->FrameNameExists("F0"));
EXPECT_EQ("L", model->FrameByName("F0")->AttachedTo());
- EXPECT_EQ(4u, world->FrameCount());
+ EXPECT_EQ(5u, world->FrameCount());
EXPECT_NE(nullptr, world->FrameByIndex(0));
EXPECT_NE(nullptr, world->FrameByIndex(1));
EXPECT_NE(nullptr, world->FrameByIndex(2));
EXPECT_NE(nullptr, world->FrameByIndex(3));
- EXPECT_EQ(nullptr, world->FrameByIndex(4));
+ EXPECT_NE(nullptr, world->FrameByIndex(4));
+ EXPECT_EQ(nullptr, world->FrameByIndex(5));
ASSERT_TRUE(world->FrameNameExists("world_frame"));
+ ASSERT_TRUE(world->FrameNameExists("F00"));
ASSERT_TRUE(world->FrameNameExists("F0"));
ASSERT_TRUE(world->FrameNameExists("F1"));
ASSERT_TRUE(world->FrameNameExists("F2"));
EXPECT_TRUE(world->FrameByName("world_frame")->AttachedTo().empty());
EXPECT_TRUE(world->FrameByName("F0")->AttachedTo().empty());
+ EXPECT_EQ("world", world->FrameByName("F00")->AttachedTo());
EXPECT_EQ("F0", world->FrameByName("F1")->AttachedTo());
EXPECT_EQ("M1", world->FrameByName("F2")->AttachedTo());
EXPECT_TRUE(world->FrameByName("world_frame")->PoseRelativeTo().empty());
+ EXPECT_TRUE(world->FrameByName("F00")->PoseRelativeTo().empty());
EXPECT_TRUE(world->FrameByName("F0")->PoseRelativeTo().empty());
EXPECT_TRUE(world->FrameByName("F1")->PoseRelativeTo().empty());
EXPECT_TRUE(world->FrameByName("F2")->PoseRelativeTo().empty());
@@ -743,6 +753,8 @@ TEST(DOMFrame, LoadWorldFramesAttachedTo)
EXPECT_TRUE(
world->FrameByName("world_frame")->ResolveAttachedToBody(body).empty());
EXPECT_EQ("world", body);
+ EXPECT_TRUE(world->FrameByName("F00")->ResolveAttachedToBody(body).empty());
+ EXPECT_EQ("world", body);
EXPECT_TRUE(world->FrameByName("F0")->ResolveAttachedToBody(body).empty());
EXPECT_EQ("world", body);
EXPECT_TRUE(world->FrameByName("F1")->ResolveAttachedToBody(body).empty());
@@ -1194,26 +1206,53 @@ TEST(DOMFrame, LoadWorldFramesRelativeTo)
EXPECT_EQ("M2", world->ModelByName("M3")->PoseRelativeTo());
EXPECT_EQ("F1", world->ModelByName("M4")->PoseRelativeTo());
- EXPECT_EQ(4u, world->FrameCount());
+ EXPECT_EQ(5u, world->FrameCount());
EXPECT_NE(nullptr, world->FrameByIndex(0));
EXPECT_NE(nullptr, world->FrameByIndex(1));
EXPECT_NE(nullptr, world->FrameByIndex(2));
EXPECT_NE(nullptr, world->FrameByIndex(3));
- EXPECT_EQ(nullptr, world->FrameByIndex(4));
+ EXPECT_NE(nullptr, world->FrameByIndex(4));
+ EXPECT_EQ(nullptr, world->FrameByIndex(5));
ASSERT_TRUE(world->FrameNameExists("world_frame"));
ASSERT_TRUE(world->FrameNameExists("F0"));
ASSERT_TRUE(world->FrameNameExists("F1"));
+ ASSERT_TRUE(world->FrameNameExists("F1a"));
ASSERT_TRUE(world->FrameNameExists("F2"));
EXPECT_TRUE(world->FrameByName("world_frame")->PoseRelativeTo().empty());
EXPECT_TRUE(world->FrameByName("F0")->PoseRelativeTo().empty());
EXPECT_EQ("F0", world->FrameByName("F1")->PoseRelativeTo());
+ EXPECT_EQ("world", world->FrameByName("F1a")->PoseRelativeTo());
EXPECT_EQ("M1", world->FrameByName("F2")->PoseRelativeTo());
EXPECT_TRUE(world->FrameByName("world_frame")->AttachedTo().empty());
EXPECT_TRUE(world->FrameByName("F0")->AttachedTo().empty());
EXPECT_TRUE(world->FrameByName("F1")->AttachedTo().empty());
+ EXPECT_EQ("F1", world->FrameByName("F1a")->AttachedTo());
EXPECT_TRUE(world->FrameByName("F2")->AttachedTo().empty());
+
+ using Pose = ignition::math::Pose3d;
+ Pose pose;
+ EXPECT_TRUE(
+ world->FrameByName("world_frame")->
+ SemanticPose().Resolve(pose, "world").empty());
+ EXPECT_EQ(Pose(0, 0, 0, 0, 0, 0), pose);
+ EXPECT_TRUE(
+ world->FrameByName("F0")->
+ SemanticPose().Resolve(pose, "world").empty());
+ EXPECT_EQ(Pose(1, 0, 0, 0, 0, 0), pose);
+ EXPECT_TRUE(
+ world->FrameByName("F1")->
+ SemanticPose().Resolve(pose, "world").empty());
+ EXPECT_EQ(Pose(3, 0, 0, 0, 0, 0), pose);
+ EXPECT_TRUE(
+ world->FrameByName("F1a")->
+ SemanticPose().Resolve(pose, "world").empty());
+ EXPECT_EQ(Pose(4, 0, 0, 0, 0, 0), pose);
+ EXPECT_TRUE(
+ world->FrameByName("F2")->
+ SemanticPose().Resolve(pose, "world").empty());
+ EXPECT_EQ(Pose(3, 0, 0, 0, 0, 0), pose);
}
/////////////////////////////////////////////////
diff --git a/test/integration/link_dom.cc b/test/integration/link_dom.cc
index 506c868a6..aeb507449 100644
--- a/test/integration/link_dom.cc
+++ b/test/integration/link_dom.cc
@@ -622,29 +622,35 @@ TEST(DOMLink, LoadLinkPoseRelativeTo)
const sdf::Model *model = root.ModelByIndex(0);
ASSERT_NE(nullptr, model);
EXPECT_EQ("model_link_relative_to", model->Name());
- EXPECT_EQ(3u, model->LinkCount());
+ EXPECT_EQ(4u, model->LinkCount());
EXPECT_NE(nullptr, model->LinkByIndex(0));
EXPECT_NE(nullptr, model->LinkByIndex(1));
EXPECT_NE(nullptr, model->LinkByIndex(2));
- EXPECT_EQ(nullptr, model->LinkByIndex(3));
+ EXPECT_NE(nullptr, model->LinkByIndex(3));
+ EXPECT_EQ(nullptr, model->LinkByIndex(4));
EXPECT_EQ(Pose(0, 0, 0, 0, 0, 0), model->RawPose());
EXPECT_EQ("", model->PoseRelativeTo());
ASSERT_TRUE(model->LinkNameExists("L1"));
ASSERT_TRUE(model->LinkNameExists("L2"));
+ ASSERT_TRUE(model->LinkNameExists("L2a"));
ASSERT_TRUE(model->LinkNameExists("L3"));
EXPECT_TRUE(model->LinkByName("L1")->PoseRelativeTo().empty());
EXPECT_TRUE(model->LinkByName("L2")->PoseRelativeTo().empty());
+ EXPECT_EQ("__model__", model->LinkByName("L2a")->PoseRelativeTo());
EXPECT_EQ("L1", model->LinkByName("L3")->PoseRelativeTo());
EXPECT_EQ(Pose(1, 0, 0, 0, IGN_PI/2, 0), model->LinkByName("L1")->RawPose());
EXPECT_EQ(Pose(2, 0, 0, 0, 0, 0), model->LinkByName("L2")->RawPose());
+ EXPECT_EQ(Pose(12, 0, 0, 0, 0, 0), model->LinkByName("L2a")->RawPose());
EXPECT_EQ(Pose(3, 0, 0, 0, 0, 0), model->LinkByName("L3")->RawPose());
EXPECT_EQ(Pose(1, 0, 0, 0, IGN_PI / 2, 0),
model->LinkByName("L1")->SemanticPose().RawPose());
EXPECT_EQ(Pose(2, 0, 0, 0, 0, 0),
model->LinkByName("L2")->SemanticPose().RawPose());
+ EXPECT_EQ(Pose(12, 0, 0, 0, 0, 0),
+ model->LinkByName("L2a")->SemanticPose().RawPose());
EXPECT_EQ(Pose(3, 0, 0, 0, 0, 0),
model->LinkByName("L3")->SemanticPose().RawPose());
@@ -656,6 +662,10 @@ TEST(DOMLink, LoadLinkPoseRelativeTo)
EXPECT_TRUE(
model->LinkByName("L2")->SemanticPose().Resolve(pose, "__model__").empty());
EXPECT_EQ(Pose(2, 0, 0, 0, 0, 0), pose);
+ EXPECT_TRUE(
+ model->LinkByName("L2a")->SemanticPose().Resolve(pose,
+ "__model__").empty());
+ EXPECT_EQ(Pose(12, 0, 0, 0, 0, 0), pose);
EXPECT_TRUE(
model->LinkByName("L3")->SemanticPose().Resolve(pose, "__model__").empty());
EXPECT_EQ(Pose(1, 0, -3, 0, IGN_PI/2, 0), pose);
@@ -664,6 +674,8 @@ TEST(DOMLink, LoadLinkPoseRelativeTo)
EXPECT_EQ(Pose(1, 0, 0, 0, IGN_PI/2, 0), pose);
EXPECT_TRUE(model->LinkByName("L2")->SemanticPose().Resolve(pose).empty());
EXPECT_EQ(Pose(2, 0, 0, 0, 0, 0), pose);
+ EXPECT_TRUE(model->LinkByName("L2a")->SemanticPose().Resolve(pose).empty());
+ EXPECT_EQ(Pose(12, 0, 0, 0, 0, 0), pose);
EXPECT_TRUE(model->LinkByName("L3")->SemanticPose().Resolve(pose).empty());
EXPECT_EQ(Pose(1, 0, -3, 0, IGN_PI/2, 0), pose);
diff --git a/test/sdf/model_frame_attached_to.sdf b/test/sdf/model_frame_attached_to.sdf
index 51230db48..b8434e565 100644
--- a/test/sdf/model_frame_attached_to.sdf
+++ b/test/sdf/model_frame_attached_to.sdf
@@ -4,6 +4,7 @@
+
diff --git a/test/sdf/model_link_relative_to.sdf b/test/sdf/model_link_relative_to.sdf
index a3a48eac7..c02770982 100644
--- a/test/sdf/model_link_relative_to.sdf
+++ b/test/sdf/model_link_relative_to.sdf
@@ -7,6 +7,9 @@
2 0 0 0 0 0
+
+ 12 0 0 0 0 0
+
3 0 0 0 0 0
diff --git a/test/sdf/world_frame_attached_to.sdf b/test/sdf/world_frame_attached_to.sdf
index a9a027a4b..d5db45a28 100644
--- a/test/sdf/world_frame_attached_to.sdf
+++ b/test/sdf/world_frame_attached_to.sdf
@@ -3,6 +3,7 @@
+
diff --git a/test/sdf/world_frame_relative_to.sdf b/test/sdf/world_frame_relative_to.sdf
index 713e00f4f..bcc224829 100644
--- a/test/sdf/world_frame_relative_to.sdf
+++ b/test/sdf/world_frame_relative_to.sdf
@@ -9,6 +9,9 @@
2 0 0 0 0 0
+
+ 4 0 0 0 0 0
+