parser_urdf: add //frame for reduced links/joints (#1148)

Currently the location of some links and joints is lost
when fixed joints are reduced and links lumped together.
This adds //model/frame tags with the same name and
pose of the reduced links and joints.

Signed-off-by: Steve Peters <scpeters@openrobotics.org>

src/parser_urdf.cc

@@ -412,6 +412,42 @@ void ReduceFixedJoints(TiXmlElement *_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
+    std::stringstream ssj;
+    ssj << "<frame name='" << _link->parent_joint->name << "'"
+        << " attached_to='" << _link->getParent()->name << "'>\n";
+    ssj << "  <pose>"
+        << CopyPose(_link->parent_joint->parent_to_joint_origin_transform)
+        << "</pose>\n";
+    ssj << "</frame>\n";
+
+    // Add //model/frame tag to memorialize reduced link
+    std::stringstream ssl;
+    ssl << "<frame name='" << _link->name + "'"
+        << " attached_to='" << _link->parent_joint->name << "'/>\n";
+
+    // Serialize sdf::Frame objects to xml and add to SDFExtension
+    SDFExtensionPtr sdfExt = std::make_shared<SDFExtension>();
+    auto stringToExtension = [&sdfExt](const std::string &_frame)
+    {
+      TiXmlDocument xmlNewDoc;
+      xmlNewDoc.Parse(_frame.c_str());
+      if (xmlNewDoc.Error())
+      {
+        sdferr << "Error while parsing serialized frames: "
+               << xmlNewDoc.ErrorDesc() << '\n';
+      }
+
+      TiXmlElementPtr blob =
+          std::make_shared<TiXmlElement>(*xmlNewDoc.FirstChildElement());
+      sdfExt->blobs.push_back(blob);
+    };
+    stringToExtension(ssj.str());
+    stringToExtension(ssl.str());
+
+    // 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,17 @@ 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());
+  for (auto e : errors)
+  {
+    std::cerr << e << std::endl;
+  }
 
-  sdf::ElementPtr model = robot->Root()->GetElement("model");
+  auto modelDom = root.ModelByIndex(0);
+  ASSERT_NE(nullptr, modelDom);
+  sdf::ElementPtr model = modelDom->Element();
   for (sdf::ElementPtr joint = model->GetElement("joint"); joint;
        joint = joint->GetNextElement("joint"))
   {
@@ -306,4 +312,105 @@ 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());
+  for (auto e : errors)
+  {
+    std::cerr << e << std::endl;
+  }
+
+  auto model = root.ModelByIndex(0);
+  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());
+  for (auto e : errors)
+  {
+    std::cerr << e << std::endl;
+  }
+
+  auto model = root.ModelByIndex(0);
+  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 * ignition::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"));
 }