Merged in urdf_convert_1.5 (pull request #575)

parse urdf files to sdf 1.5 instead of 1.4 to avoid use_parent_model_frame Approved-by: Eric Cousineau <eacousineau@gmail.com> Approved-by: Addisu Z. Taddese <addisu@openrobotics.org>
gazebosim · Oct 7, 2019 · 8a0a8d7 · 8a0a8d7
2 parents 0135330 + 172276b
commit 8a0a8d7
Show file tree

Hide file tree

Showing 4 changed files with 31 additions and 16 deletions.
diff --git a/Changelog.md b/Changelog.md
@@ -2,6 +2,9 @@
 
 ### SDFormat 6.X.X (20XX-XX-XX)
 
+1. Parse urdf files to sdf 1.5 instead of 1.4 to avoid `use_parent_model_frame`.
+    * [Pull request 575](https://bitbucket.org/osrf/sdformat/pull-requests/575)
+
 1. Set camera intrinsics axis skew (s) default value to 0
     * [Pull request 504](https://bitbucket.org/osrf/sdformat/pull-requests/504)
 

diff --git a/src/parser_urdf.cc b/src/parser_urdf.cc
@@ -2894,7 +2894,7 @@ void CreateInertial(TiXmlElement *_elem,
 ////////////////////////////////////////////////////////////////////////////////
 void CreateJoint(TiXmlElement *_root,
                  urdf::LinkConstSharedPtr _link,
-                 ignition::math::Pose3d &_currentTransform)
+                 ignition::math::Pose3d &/*_currentTransform*/)
 {
   // compute the joint tag
   std::string jtype;
@@ -2974,15 +2974,12 @@ void CreateJoint(TiXmlElement *_root,
     }
     else if (jtype != "fixed")
     {
-      ignition::math::Vector3d rotatedJointAxis =
-        _currentTransform.Rot().RotateVector(
-            ignition::math::Vector3d(_link->parent_joint->axis.x,
-              _link->parent_joint->axis.y,
-              _link->parent_joint->axis.z));
-      double rotatedJointAxisArray[3] =
-      { rotatedJointAxis.X(), rotatedJointAxis.Y(), rotatedJointAxis.Z() };
+      double jointAxisXyzArray[3] =
+      { _link->parent_joint->axis.x,
+        _link->parent_joint->axis.y,
+        _link->parent_joint->axis.z};
       AddKeyValue(jointAxis, "xyz",
-                  Values2str(3, rotatedJointAxisArray));
+                  Values2str(3, jointAxisXyzArray));
       if (_link->parent_joint->dynamics)
       {
         AddKeyValue(jointAxisDynamics, "damping",
@@ -3230,9 +3227,9 @@ TiXmlDocument URDF2SDF::InitModelString(const std::string &_urdfStr,
   // add robot to sdfXmlOut
   TiXmlElement *sdf = new TiXmlElement("sdf");
 
-  // URDF is compatible with version 1.4. The automatic conversion script
+  // URDF is compatible with version 1.5. The automatic conversion script
   // will up-convert URDF to SDF.
-  sdf->SetAttribute("version", "1.4");
+  sdf->SetAttribute("version", "1.5");
 
   sdf->LinkEndChild(robot);
   sdfXmlOut.LinkEndChild(sdf);

diff --git a/src/parser_urdf_TEST.cc b/src/parser_urdf_TEST.cc
@@ -78,11 +78,8 @@ TEST(URDFParser, ParseResults_BasicModel_ParseEqualToModel)
 
   // SDF -> SDF
   std::ostringstream stream;
-  // Use hard-coded "1.4" for version string
-  // until parser_urdf.cc exports version "1.5"
-  // see `sdf->SetAttribute("version", "1.4");`
-  // in URDF2SDF::InitModelString()
-  stream << "<sdf version='" << "1.4" << "'>"
+  // parser_urdf.cc exports version "1.5"
+  stream << "<sdf version='" << "1.5" << "'>"
          << "  <model name='test_robot' />"
          << "</sdf>";
   TiXmlDocument sdf_doc;

diff --git a/test/integration/urdf_joint_parameters.cc b/test/integration/urdf_joint_parameters.cc
@@ -68,4 +68,22 @@ TEST(SDFParser, JointAxisParameters)
     EXPECT_DOUBLE_EQ(value, dynamics->Get<double>("friction"));
   }
   EXPECT_EQ(bitmask, 0x3u);
+
+  // check joint axis values
+  for (sdf::ElementPtr joint = model->GetElement("joint"); joint;
+       joint = joint->GetNextElement("joint"))
+  {
+    std::string jointName = joint->Get<std::string>("name");
+
+    ignition::math::Vector3d xyz(1, 0, 0);
+    if (jointName == "joint12" || jointName == "joint23")
+    {
+      xyz.Set(0, 1, 0);
+    }
+
+    ASSERT_TRUE(joint->HasElement("axis"));
+    sdf::ElementPtr axis = joint->GetElement("axis");
+    EXPECT_EQ(xyz, axis->Get<ignition::math::Vector3d>("xyz"));
+    EXPECT_FALSE(axis->Get<bool>("use_parent_model_frame"));
+  }
 }