Merge sdf12 ➡️ sdf13

src/parser_urdf.cc

@@ -424,6 +424,39 @@ void ReduceFixedJoints(tinyxml2::XMLElement *_root, urdf::LinkSharedPtr _link)
     sdfdbg << "Fixed Joint Reduction: extension lumping from ["
            << _link->name << "] to [" << _link->getParent()->name << "]\n";
 
+    // Add //model/frame tag to memorialize reduced joint
+    sdf::Frame jointFrame;
+    jointFrame.SetName(_link->parent_joint->name);
+    jointFrame.SetAttachedTo(_link->getParent()->name);
+    jointFrame.SetRawPose(
+      CopyPose(_link->parent_joint->parent_to_joint_origin_transform));
+
+    // Add //model/frame tag to memorialize reduced link
+    sdf::Frame linkFrame;
+    linkFrame.SetName(_link->name);
+    linkFrame.SetAttachedTo(_link->parent_joint->name);
+
+    // Serialize sdf::Frame objects to xml and add to SDFExtension
+    SDFExtensionPtr sdfExt = std::make_shared<SDFExtension>();
+    auto sdfFrameToExtension = [&sdfExt](const sdf::Frame &_frame)
+    {
+      XMLDocumentPtr xmlNewDoc = std::make_shared<tinyxml2::XMLDocument>();
+      sdf::PrintConfig config;
+      config.SetOutPrecision(16);
+      xmlNewDoc->Parse(_frame.ToElement()->ToString("", config).c_str());
+      if (xmlNewDoc->Error())
+      {
+        sdferr << "Error while parsing serialized frames: "
+               << xmlNewDoc->ErrorStr() << '\n';
+      }
+      sdfExt->blobs.push_back(xmlNewDoc);
+    };
+    sdfFrameToExtension(jointFrame);
+    sdfFrameToExtension(linkFrame);
+
+    // Add //frame tags to model extension vector
+    g_extensions[""].push_back(sdfExt);
+
     // lump sdf extensions to parent, (give them new reference _link names)
     ReduceSDFExtensionToParent(_link);
 

test/integration/fixed_joint_example.urdf

@@ -0,0 +1,63 @@
+<?xml version="1.0" ?>
+<robot name="fixed_joint_example">
+
+  <link name="base">
+    <!-- base has inertial and visual geometry of a half-cube -->
+    <inertial>
+      <mass value="6"/>
+      <origin rpy="0 0 0" xyz="0 0 0.25"/>
+      <inertia ixx="0.625" ixy="0" ixz="0" iyy="0.625" iyz="0" izz="1.0"/>
+    </inertial>
+    <visual>
+      <geometry>
+        <box size="1.0 1.0 0.5"/>
+      </geometry>
+      <origin rpy="0 0 0" xyz="0 0 0.25"/>
+    </visual>
+  </link>
+
+  <!-- Intermediate link with fixed joint that will be reduced. -->
+  <joint name="intermediate_joint" type="fixed">
+    <origin rpy="1.57079632679 0 0" xyz="0 0 0.75"/>
+    <parent link="base"/>
+    <child link="intermediate_link"/>
+  </joint>
+  <link name="intermediate_link">
+    <!-- intermediate_link has inertial and visual geometry of a half-cube -->
+    <inertial>
+      <mass value="6"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <inertia ixx="0.625" ixy="0" ixz="0" iyy="1.0" iyz="0" izz="0.625"/>
+    </inertial>
+    <visual>
+      <geometry>
+        <box size="1.0 0.5 1.0"/>
+      </geometry>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+    </visual>
+  </link>
+
+  <!-- Continuous joint connected to intermediate_link -->
+  <joint name="rotary_joint" type="continuous">
+    <origin rpy="0 0 1.57079632679" xyz="0 0.75 0"/>
+    <axis xyz="1 0 0"/>
+    <parent link="intermediate_link"/>
+    <child link="rotary_link"/>
+  </joint>
+  <link name="rotary_link">
+    <!-- rotary_link has inertial and visual geometry of a cube equivalent to the
+         merged properties of base and intermediate_link. -->
+    <inertial>
+      <mass value="12"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <inertia ixx="2" ixy="0" ixz="0" iyy="2" iyz="0" izz="2"/>
+    </inertial>
+    <visual>
+      <geometry>
+        <box size="1.0 1.0 1.0"/>
+      </geometry>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+    </visual>
+  </link>
+
+</robot>

test/integration/fixed_joint_simple.urdf

@@ -0,0 +1,34 @@
+<?xml version="1.0" ?>
+<robot name="fixed_joint_simple">
+
+  <link name="base">
+    <inertial>
+      <mass value="1"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+    </inertial>
+  </link>
+
+  <!-- Intermediate fixed joint that will be reduced. -->
+  <joint name="fixed_joint" type="fixed">
+    <origin rpy="0 0 0" xyz="0 0 0"/>
+    <parent link="base"/>
+    <child link="child_link"/>
+  </joint>
+
+  <!-- Child link with visual that will be merged to base link -->
+  <link name="child_link">
+    <inertial>
+      <mass value="1"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <inertia ixx="1" ixy="0" ixz="0" iyy="1.0" iyz="0" izz="1"/>
+    </inertial>
+    <visual>
+      <geometry>
+        <box size="1.0 1.0 1.0"/>
+      </geometry>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+    </visual>
+  </link>
+
+</robot>

test/integration/urdf_gazebo_extensions.cc

@@ -29,11 +29,13 @@ TEST(SDFParser, UrdfGazeboExtensionURDFTest)
   const std::string urdfTestFile =
       sdf::testing::TestFile("integration", "urdf_gazebo_extensions.urdf");
 
-  sdf::SDFPtr robot(new sdf::SDF());
-  sdf::init(robot);
-  ASSERT_TRUE(sdf::readFile(urdfTestFile, robot));
+  sdf::Root root;
+  auto errors = root.Load(urdfTestFile);
+  EXPECT_TRUE(errors.empty()) << errors;
 
-  sdf::ElementPtr model = robot->Root()->GetElement("model");
+  auto modelDom = root.Model();
+  ASSERT_NE(nullptr, modelDom);
+  sdf::ElementPtr model = modelDom->Element();
   for (sdf::ElementPtr joint = model->GetElement("joint"); joint;
        joint = joint->GetNextElement("joint"))
   {
@@ -299,4 +301,97 @@ TEST(SDFParser, UrdfGazeboExtensionURDFTest)
   checkElementPoses("light");
   checkElementPoses("projector");
   checkElementPoses("sensor");
+
+  // Check that //model/frame elements are added for reduced joints
+  EXPECT_EQ(14u, modelDom->FrameCount());
+
+  EXPECT_TRUE(modelDom->FrameNameExists("issue378_link_joint"));
+  EXPECT_TRUE(modelDom->FrameNameExists("jCamera"));
+  EXPECT_TRUE(modelDom->FrameNameExists("jointSensorNoPose"));
+  EXPECT_TRUE(modelDom->FrameNameExists("jointSensorPose"));
+  EXPECT_TRUE(modelDom->FrameNameExists("jointSensorPoseRelative"));
+  EXPECT_TRUE(modelDom->FrameNameExists("jointSensorPoseTwoLevel"));
+  EXPECT_TRUE(modelDom->FrameNameExists("jointSensorPoseTwoLevel2"));
+
+  EXPECT_TRUE(modelDom->FrameNameExists("issue378_link"));
+  EXPECT_TRUE(modelDom->FrameNameExists("Camera"));
+  EXPECT_TRUE(modelDom->FrameNameExists("linkSensorNoPose"));
+  EXPECT_TRUE(modelDom->FrameNameExists("linkSensorPose"));
+  EXPECT_TRUE(modelDom->FrameNameExists("linkSensorPoseRelative"));
+  EXPECT_TRUE(modelDom->FrameNameExists("linkSensorPoseTwoLevel"));
+  EXPECT_TRUE(modelDom->FrameNameExists("linkSensorPoseTwoLevel2"));
+}
+
+/////////////////////////////////////////////////
+TEST(SDFParser, FixedJointExample)
+{
+  const std::string urdfTestFile =
+      sdf::testing::TestFile("integration", "fixed_joint_example.urdf");
+
+  sdf::Root root;
+  auto errors = root.Load(urdfTestFile);
+  EXPECT_TRUE(errors.empty()) << errors;
+
+  auto model = root.Model();
+  ASSERT_NE(nullptr, model);
+  EXPECT_EQ("fixed_joint_example", model->Name());
+
+  EXPECT_EQ(2u, model->LinkCount());
+  EXPECT_TRUE(model->LinkNameExists("base"));
+  EXPECT_TRUE(model->LinkNameExists("rotary_link"));
+
+  // Expect MassMatrix3 values to match for links
+  auto link1 = model->LinkByName("base");
+  auto link2 = model->LinkByName("rotary_link");
+  ASSERT_NE(nullptr, link1);
+  ASSERT_NE(nullptr, link2);
+  auto massMatrix1 = link1->Inertial().MassMatrix();
+  auto massMatrix2 = link2->Inertial().MassMatrix();
+  EXPECT_DOUBLE_EQ(massMatrix1.Mass(), massMatrix2.Mass());
+  EXPECT_EQ(massMatrix1.Moi(), massMatrix2.Moi());
+
+  EXPECT_EQ(1u, model->JointCount());
+  EXPECT_TRUE(model->JointNameExists("rotary_joint"));
+
+  EXPECT_EQ(2u, model->FrameCount());
+  ASSERT_TRUE(model->FrameNameExists("intermediate_joint"));
+  ASSERT_TRUE(model->FrameNameExists("intermediate_link"));
+
+  const std::string j = "intermediate_joint";
+  const std::string l = "intermediate_link";
+  std::string body;
+  EXPECT_TRUE(model->FrameByName(j)->ResolveAttachedToBody(body).empty());
+  EXPECT_EQ("base", body);
+  EXPECT_TRUE(model->FrameByName(l)->ResolveAttachedToBody(body).empty());
+  EXPECT_EQ("base", body);
+}
+
+/////////////////////////////////////////////////
+TEST(SDFParser, FixedJointSimple)
+{
+  const std::string urdfTestFile =
+      sdf::testing::TestFile("integration", "fixed_joint_simple.urdf");
+
+  sdf::Root root;
+  auto errors = root.Load(urdfTestFile);
+  EXPECT_TRUE(errors.empty()) << errors;
+
+  auto model = root.Model();
+  ASSERT_NE(nullptr, model);
+  EXPECT_EQ("fixed_joint_simple", model->Name());
+
+  EXPECT_EQ(1u, model->LinkCount());
+  EXPECT_TRUE(model->LinkNameExists("base"));
+
+  auto link = model->LinkByName("base");
+  ASSERT_NE(nullptr, link);
+  auto massMatrix = link->Inertial().MassMatrix();
+  EXPECT_DOUBLE_EQ(2.0, massMatrix.Mass());
+  EXPECT_EQ(2.0 * gz::math::Matrix3d::Identity, massMatrix.Moi());
+
+  EXPECT_EQ(0u, model->JointCount());
+
+  EXPECT_EQ(2u, model->FrameCount());
+  ASSERT_TRUE(model->FrameNameExists("fixed_joint"));
+  ASSERT_TRUE(model->FrameNameExists("child_link"));
 }