Merge pull request #834 from ignitionrobotics/azeey/11_to_12_20220126

11 ➡️ 12

src/parser.cc

@@ -2534,7 +2534,10 @@ void checkJointParentChildNames(const sdf::Root *_root, Errors &_errors)
       auto joint = _model->JointByIndex(j);
 
       const std::string &parentName = joint->ParentLinkName();
-      if (parentName != "world" && !_model->LinkNameExists(parentName) &&
+      const std::string parentLocalName = sdf::SplitName(parentName).second;
+
+      if (parentName != "world" && parentLocalName != "__model__" &&
+          !_model->LinkNameExists(parentName) &&
           !_model->JointNameExists(parentName) &&
           !_model->FrameNameExists(parentName))
       {
@@ -2545,14 +2548,15 @@ void checkJointParentChildNames(const sdf::Root *_root, Errors &_errors)
       }
 
       const std::string &childName = joint->ChildLinkName();
+      const std::string childLocalName = sdf::SplitName(childName).second;
       if (childName == "world")
       {
         errors.push_back({ErrorCode::JOINT_CHILD_LINK_INVALID,
           "invalid child name[world] specified by joint with name[" +
           joint->Name() + "] in model with name[" + _model->Name() + "]."});
       }
 
-      if (!_model->LinkNameExists(childName) &&
+      if (childLocalName != "__model__" && !_model->LinkNameExists(childName) &&
           !_model->JointNameExists(childName) &&
           !_model->FrameNameExists(childName) &&
           !_model->ModelNameExists(childName))

test/integration/joint_dom.cc

@@ -230,6 +230,70 @@ TEST(DOMJoint, LoadInvalidJointChildWorld)
       "not found in model with name[joint_child_world]"));
 }
 
+/////////////////////////////////////////////////
+TEST(DOMJoint, LoadJointParentModelFrame)
+{
+  const std::string testFile =
+    sdf::testing::TestFile("sdf", "joint_parent_model_frame.sdf");
+
+  // Load the SDF file
+  sdf::Root root;
+  sdf::Errors errors = root.Load(testFile);
+  EXPECT_TRUE(errors.empty()) << errors;
+
+  // Get the first model
+  const sdf::Model *model = root.Model();
+  ASSERT_NE(nullptr, model);
+  EXPECT_EQ("joint_parent_model_frame", model->Name());
+  EXPECT_EQ(2u, model->LinkCount());
+
+  EXPECT_EQ(1u, model->JointCount());
+  auto *joint = model->JointByName("joint");
+  ASSERT_NE(nullptr, joint);
+  EXPECT_EQ("child_link", model->JointByName("joint")->ChildLinkName());
+  EXPECT_EQ("__model__", model->JointByName("joint")->ParentLinkName());
+  std::string resolvedLinkName;
+  EXPECT_TRUE(joint->ResolveParentLink(resolvedLinkName).empty());
+  EXPECT_EQ("base_link", resolvedLinkName);
+
+  ignition::math::Pose3d pose;
+  EXPECT_TRUE(
+    joint->SemanticPose().Resolve(pose, "__model__").empty());
+  EXPECT_EQ(ignition::math::Pose3d(1, 0, 0, 0, 0, 0), pose);
+}
+
+/////////////////////////////////////////////////
+TEST(DOMJoint, LoadJointChildModelFrame)
+{
+  const std::string testFile =
+    sdf::testing::TestFile("sdf", "joint_child_model_frame.sdf");
+
+  // Load the SDF file
+  sdf::Root root;
+  sdf::Errors errors = root.Load(testFile);
+  EXPECT_TRUE(errors.empty()) << errors;
+
+  // Get the first model
+  const sdf::Model *model = root.Model();
+  ASSERT_NE(nullptr, model);
+  EXPECT_EQ("joint_child_model_frame", model->Name());
+  EXPECT_EQ(2u, model->LinkCount());
+
+  EXPECT_EQ(1u, model->JointCount());
+  auto *joint = model->JointByName("joint");
+  ASSERT_NE(nullptr, joint);
+  EXPECT_EQ("__model__", model->JointByName("joint")->ChildLinkName());
+  EXPECT_EQ("parent_link", model->JointByName("joint")->ParentLinkName());
+  std::string resolvedLinkName;
+  EXPECT_TRUE(joint->ResolveChildLink(resolvedLinkName).empty());
+  EXPECT_EQ("base_link", resolvedLinkName);
+
+  ignition::math::Pose3d pose;
+  EXPECT_TRUE(
+    joint->SemanticPose().Resolve(pose, "__model__").empty());
+  EXPECT_EQ(ignition::math::Pose3d(0, 0, 0, 0, 0, 0), pose);
+}
+
 /////////////////////////////////////////////////
 TEST(DOMJoint, LoadJointParentFrame)
 {
@@ -857,6 +921,38 @@ TEST(DOMJoint, LoadJointNestedParentChild)
     EXPECT_TRUE(j5->SemanticPose().Resolve(pose, "__model__").empty());
     EXPECT_EQ(Pose(0, -1, 1, IGN_PI_2, 0, 0), pose);
   }
+  {
+    const sdf::Joint *j6 = model->JointByName("J6");
+    ASSERT_NE(nullptr, j6);
+    EXPECT_EQ("M1::__model__", j6->ParentLinkName());
+    EXPECT_EQ("L1", j6->ChildLinkName());
+
+    std::string resolvedLinkName;
+    EXPECT_TRUE(j6->ResolveParentLink(resolvedLinkName).empty());
+    EXPECT_EQ("M1::L1", resolvedLinkName);
+    EXPECT_TRUE(j6->ResolveChildLink(resolvedLinkName).empty());
+    EXPECT_EQ("L1", resolvedLinkName);
+
+    Pose pose;
+    EXPECT_TRUE(j6->SemanticPose().Resolve(pose, "__model__").empty());
+    EXPECT_EQ(Pose(1, 0, 10, 0, IGN_PI_2, 0), pose);
+  }
+  {
+    const sdf::Joint *j7 = model->JointByName("J7");
+    ASSERT_NE(nullptr, j7);
+    EXPECT_EQ("L1", j7->ParentLinkName());
+    EXPECT_EQ("M1::__model__", j7->ChildLinkName());
+
+    std::string resolvedLinkName;
+    EXPECT_TRUE(j7->ResolveParentLink(resolvedLinkName).empty());
+    EXPECT_EQ("L1", resolvedLinkName);
+    EXPECT_TRUE(j7->ResolveChildLink(resolvedLinkName).empty());
+    EXPECT_EQ("M1::L1", resolvedLinkName);
+
+    Pose pose;
+    EXPECT_TRUE(j7->SemanticPose().Resolve(pose, "__model__").empty());
+    EXPECT_EQ(Pose(0, 0, 1, 0, 0, 0), pose);
+  }
 }
 
 

test/sdf/joint_child_model_frame.sdf

@@ -0,0 +1,17 @@
+<?xml version="1.0" ?>
+<sdf version="1.8">
+  <model name="joint_child_model_frame">
+    <link name="base_link">
+      <pose>0 0 1 0 0 0</pose>
+    </link>
+
+    <link name="parent_link">
+      <pose>1 0 0 0 0 0</pose>
+    </link>
+
+    <joint name="joint" type="fixed">
+      <parent>parent_link</parent>
+      <child>__model__</child>
+    </joint>
+  </model>
+</sdf>

test/sdf/joint_nested_parent_child.sdf

@@ -57,5 +57,19 @@
       <parent>L1</parent>
       <child>M1::M2</child>
     </joint>
+
+    <!-- Joint with a nested model frame as a parent  -->
+    <joint name="J6" type="fixed">
+      <pose>0 0 1 0 0 0</pose>
+      <parent>M1::__model__</parent>
+      <child>L1</child>
+    </joint>
+
+    <!-- Joint with a nested model frame as a child  -->
+    <joint name="J7" type="fixed">
+      <pose>0 0 1 0 0 0</pose>
+      <parent>L1</parent>
+      <child>M1::__model__</child>
+    </joint>
   </model>
 </sdf>

test/sdf/joint_parent_model_frame.sdf

@@ -0,0 +1,17 @@
+<?xml version="1.0" ?>
+<sdf version="1.8">
+  <model name="joint_parent_model_frame">
+    <link name="base_link">
+      <pose>0 0 1 0 0 0</pose>
+    </link>
+
+    <link name="child_link">
+      <pose>1 0 0 0 0 0</pose>
+    </link>
+
+    <joint name="joint" type="fixed">
+      <parent>__model__</parent>
+      <child>child_link</child>
+    </joint>
+  </model>
+</sdf>