Merge pull request #738 from ignitionrobotics/chapulina/6_to_9

6 ➡️ 9

.github/CODEOWNERS

@@ -0,0 +1,4 @@
+# More info:
+# https://help.github.com/en/github/creating-cloning-and-archiving-repositories/about-code-owners
+
+*                     @azeey @scpeters

.github/workflows/ci.yml

@@ -23,4 +23,3 @@ jobs:
       - name: Compile and test
         id: ci
         uses: ignition-tooling/action-ignition-ci@focal
-

Changelog.md

@@ -749,7 +749,21 @@
 
 ## SDFormat 6.0
 
-### SDFormat 6.X.X (20XX-XX-XX)
+### SDFormat 6.3.1 (2021-07-06)
+
+1. Fix flattening logic for nested model names
+    * [Pull request 597](https://github.com/osrf/sdformat/pull/597)
+
+1. Translate poses of nested models inside other nested models
+    * [Pull request 596](https://github.com/osrf/sdformat/pull/596)
+
+### SDFormat 6.3.0 (2021-06-21)
+
+1. Move recursiveSameTypeUniqueNames from ign.cc to parser.cc and make public.
+    * [Pull request 580](https://github.com/osrf/sdformat/pull/580)
+
+1. Parse rpyOffset as radians
+    * [Pull request 497](https://github.com/osrf/sdformat/pull/497)
 
 1. Parse urdf files to sdf 1.5 instead of 1.4 to avoid `use_parent_model_frame`.
     * [BitBucket pull request 575](https://osrf-migration.github.io/sdformat-gh-pages/#!/osrf/sdformat/pull-requests/575)

Migration.md

@@ -145,6 +145,11 @@ but with improved human-readability..
 
 ## SDFormat 5.x to 6.x
 
+### Additions
+
+1. **sdf/parser.hh**
+   + bool recursiveSameTypeUniqueNames(sdf::ElementPtr)
+
 ### Deprecations
 
 1. **sdf/Types.hh**

doc/header.html

@@ -41,7 +41,7 @@ <h2 style="text-align:center;">
         <dd><a href="http://sdf.com/wiki/Tutorials">Tutorials</a></dd>
         <dd><a href="http://sdf.com/downloads.html">Download</a></dd>
         -->
-        <dd><a href="https://github.com/osrf/sdformat/issues/new">Report Documentation Issues</a></dd>
+        <dd><a href="https://github.com/ignitionrobotics/sdformat/issues/new">Report Documentation Issues</a></dd>
       </dl>
     </div>
     <div>

doc/mainpage.html

@@ -5,7 +5,7 @@
 Desctiption Format API. The code reference is divided into the groups below.
 Should you find problems with this documentation - typos, unclear phrases,
 or insufficient detail - please create a <a
-  href="https://github.com/osrf/sdformat/issues/new">new GitHub issue</a>.
+  href="https://github.com/ignitionrobotics/sdformat/issues/new">new GitHub issue</a>.
 Include sufficient detail to quickly locate the problematic documentation,
 and set the issue's fields accordingly: Assignee - blank; Kind - bug;
 Priority - minor; Version - blank.

src/parser.cc

@@ -1358,12 +1358,18 @@ void addNestedModel(ElementPtr _sdf, ElementPtr _includeSDF, Errors &_errors)
   for (std::map<std::string, std::string>::iterator iter = replace.begin();
        iter != replace.end(); ++iter)
   {
-    replace_all(str, std::string("\"") + iter->first + "\"",
-                std::string("\"") + iter->second + "\"");
-    replace_all(str, std::string("'") + iter->first + "'",
-                std::string("'") + iter->second + "'");
-    replace_all(str, std::string(">") + iter->first + "<",
-                std::string(">") + iter->second + "<");
+    std::string oldName(iter->first);
+    std::string nameWithNestedPrefix(iter->second);
+    replace_all(str, std::string("\"") + oldName + "\"",
+                     std::string("\"") + nameWithNestedPrefix + "\"");
+    replace_all(str, std::string("'") + oldName + "'",
+                     std::string("'") + nameWithNestedPrefix + "'");
+    replace_all(str, std::string(">") + oldName + "<",
+                     std::string(">") + nameWithNestedPrefix + "<");
+    // Deal with nested model inside other nested model and already with
+    // ::namespace:: entries in the name
+    replace_all(str, std::string(">") + oldName + "::",
+                     std::string(">") + nameWithNestedPrefix + "::");
   }
 
   _includeSDF->ClearElements();

src/parser_urdf.cc

@@ -221,10 +221,6 @@ std::string Values2str(unsigned int _count, const double *_values);
 
 void CreateGeometry(TiXmlElement *_elem, urdf::GeometrySharedPtr _geometry);
 
-ignition::math::Pose3d inverseTransformToParentFrame(
-    ignition::math::Pose3d _transformInLinkFrame,
-    urdf::Pose _parentToLinkTransform);
-
 /// reduced fixed joints: transform to parent frame
 urdf::Pose TransformToParentFrame(urdf::Pose _transformInLinkFrame,
     urdf::Pose _parentToLinkTransform);
@@ -2429,31 +2425,6 @@ ignition::math::Pose3d TransformToParentFrame(
   return transformInParentLinkFrame;
 }
 
-/////////////////////////////////////////////////
-/// reduced fixed joints: transform to parent frame
-ignition::math::Pose3d inverseTransformToParentFrame(
-    ignition::math::Pose3d _transformInLinkFrame,
-    urdf::Pose _parentToLinkTransform)
-{
-  ignition::math::Pose3d transformInParentLinkFrame;
-  //   rotate link pose to parentLink frame
-  urdf::Rotation ri = _parentToLinkTransform.rotation.GetInverse();
-  ignition::math::Quaterniond q1(ri.w, ri.x, ri.y, ri.z);
-  transformInParentLinkFrame.Pos() = q1 * _transformInLinkFrame.Pos();
-  urdf::Rotation r2 = _parentToLinkTransform.rotation.GetInverse();
-  ignition::math::Quaterniond q3(r2.w, r2.x, r2.y, r2.z);
-  transformInParentLinkFrame.Rot() = q3 * _transformInLinkFrame.Rot();
-  //   translate link to parentLink frame
-  transformInParentLinkFrame.Pos().X() = transformInParentLinkFrame.Pos().X()
-    - _parentToLinkTransform.position.x;
-  transformInParentLinkFrame.Pos().Y() = transformInParentLinkFrame.Pos().Y()
-    - _parentToLinkTransform.position.y;
-  transformInParentLinkFrame.Pos().Z() = transformInParentLinkFrame.Pos().Z()
-    - _parentToLinkTransform.position.z;
-
-  return transformInParentLinkFrame;
-}
-
 ////////////////////////////////////////////////////////////////////////////////
 void ReduceSDFExtensionToParent(urdf::LinkSharedPtr _link)
 {
@@ -3499,20 +3470,27 @@ void ReduceSDFExtensionPluginFrameReplace(
         TiXmlNode* rpyKey = (*_blobIt)->FirstChild("rpyOffset");
         if (rpyKey)
         {
-          urdf::Vector3 rpy = ParseVector3(rpyKey, M_PI/180.0);
+          urdf::Vector3 rpy = ParseVector3(rpyKey);
           _reductionTransform.Rot() =
             ignition::math::Quaterniond::EulerToQuaternion(rpy.x, rpy.y, rpy.z);
           // remove xyzOffset and rpyOffset
           (*_blobIt)->RemoveChild(rpyKey);
         }
+        TiXmlNode* correctedOffsetKey =
+            (*_blobIt)->FirstChild("ignition::corrected_offsets");
+        if (correctedOffsetKey)
+        {
+          (*_blobIt)->RemoveChild(correctedOffsetKey);
+        }
 
         // pass through the parent transform from fixed joint reduction
-        _reductionTransform = inverseTransformToParentFrame(_reductionTransform,
+        _reductionTransform = TransformToParentFrame(_reductionTransform,
             _link->parent_joint->parent_to_joint_origin_transform);
 
         // create new offset xml blocks
         xyzKey = new TiXmlElement("xyzOffset");
         rpyKey = new TiXmlElement("rpyOffset");
+        correctedOffsetKey = new TiXmlElement("ignition::corrected_offsets");
 
         // create new offset xml blocks
         urdf::Vector3 reductionXyz(_reductionTransform.Pos().X(),
@@ -3533,12 +3511,15 @@ void ReduceSDFExtensionPluginFrameReplace(
 
         TiXmlText* xyzTxt = new TiXmlText(xyzStream.str());
         TiXmlText* rpyTxt = new TiXmlText(rpyStream.str());
+        TiXmlText* correctedOffsetTxt = new TiXmlText("1");
 
         xyzKey->LinkEndChild(xyzTxt);
         rpyKey->LinkEndChild(rpyTxt);
+        correctedOffsetKey->LinkEndChild(correctedOffsetTxt);
 
         (*_blobIt)->LinkEndChild(xyzKey);
         (*_blobIt)->LinkEndChild(rpyKey);
+        (*_blobIt)->LinkEndChild(correctedOffsetKey);
       }
     }
   }

test/integration/fixed_joint_reduction.cc

@@ -36,6 +36,8 @@ const char SDF_TEST_FILE_COLLISION_VISUAL_EXTENSION_EMPTY_ROOT[] =
     "fixed_joint_reduction_collision_visual_empty_root.urdf";
 const char SDF_TEST_FILE_COLLISION_VISUAL_EXTENSION_EMPTY_ROOT_SDF[] =
     "fixed_joint_reduction_collision_visual_empty_root.sdf";
+const char SDF_TEST_FILE_PLUGIN_FRAME_EXTENSION[] =
+    "fixed_joint_reduction_plugin_frame_extension.urdf";
 
 static std::string GetFullTestFilePath(const char *_input)
 {
@@ -734,3 +736,31 @@ TEST(SDFParser, FixedJointReductionSimple)
     EXPECT_NEAR(iyz, mapIxyIxzIyz[linkName].Z(), gc_tolerance);
   }
 }
+
+/////////////////////////////////////////////////
+// This test uses a urdf that has chained fixed joints with plugin that
+// contains bodyName, xyzOffset and rpyOffset.
+// Test to make sure the offsets have the correct transfrom and frame of
+// reference
+TEST(SDFParser, FixedJointReductionPluginFrameExtensionTest)
+{
+  sdf::SDFPtr robot(new sdf::SDF());
+  sdf::init(robot);
+  ASSERT_TRUE(sdf::readFile(
+      GetFullTestFilePath(SDF_TEST_FILE_PLUGIN_FRAME_EXTENSION),
+      robot));
+
+  sdf::ElementPtr model = robot->Root()->GetElement("model");
+  sdf::ElementPtr plugin = model->GetElement("plugin");
+
+  auto xyzOffset = plugin->Get<ignition::math::Vector3d>("xyzOffset");
+  auto rpyOffset = plugin->Get<ignition::math::Vector3d>("rpyOffset");
+  auto bodyName = plugin->Get<std::string>("bodyName");
+  EXPECT_EQ("base_link", bodyName);
+  EXPECT_EQ(ignition::math::Vector3d(-0.707108, 1.70711, 0), xyzOffset);
+  EXPECT_EQ(ignition::math::Vector3d(0, 0, 1.5708), rpyOffset);
+
+  bool correctedOffset = plugin->Get<bool>("ignition::corrected_offsets");
+  EXPECT_TRUE(correctedOffset);
+}
+

test/integration/fixed_joint_reduction_plugin_frame_extension.urdf

@@ -0,0 +1,103 @@
+<?xml version="1.0" encoding="utf-8"?>
+<robot name="chained_fixed_joint_links">
+  <gazebo>
+    <plugin name='test_plugin' filename='libtest_plugin.so'>
+      <serviceName>/test/plugin/service</serviceName>
+      <topicName>/test/plugin/topic</topicName>
+      <bodyName>link2</bodyName>
+      <updateRate>100</updateRate>
+      <xyzOffset>0 0 0</xyzOffset>
+      <rpyOffset>0 0 0</rpyOffset>
+    </plugin>
+  </gazebo>
+
+  <!-- Base Link -->
+  <link name="base_link">
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0.0"/>
+      <geometry>
+        <box size="1.0 1.0 1"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 1.0"/>
+      <geometry>
+        <box size="0.1 0.1 2"/>
+      </geometry>
+    </visual>
+    <inertial>
+      <origin xyz="0 0 1" rpy="0 0 0"/>
+      <mass value="1"/>
+      <inertia
+        ixx="1.0" ixy="0.0" ixz="0.0"
+        iyy="1.0" iyz="0.0"
+        izz="1.0"/>
+    </inertial>
+  </link>
+
+  <!-- Link 1 -->
+  <link name="link1">
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0.0"/>
+      <geometry>
+        <box size="1.0 1.0 1"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0.0"/>
+      <geometry>
+        <box size="0.1 0.1 1"/>
+      </geometry>
+    </visual>
+    <inertial>
+      <origin xyz="0 0 1" rpy="0 0 0"/>
+      <mass value="1"/>
+      <inertia
+        ixx="1.0" ixy="0.0" ixz="0.0"
+        iyy="1.0" iyz="0.0"
+        izz="1.0"/>
+    </inertial>
+  </link>
+
+  <!-- Link 2 -->
+  <link name="link2">
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0.0"/>
+      <geometry>
+        <box size="1.0 1.0 1"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0.0"/>
+      <geometry>
+        <box size="0.1 0.1 1"/>
+      </geometry>
+    </visual>
+    <inertial>
+      <origin xyz="0 0 1" rpy="0 0 0"/>
+      <mass value="1"/>
+      <inertia
+        ixx="1.0" ixy="0.0" ixz="0.0"
+        iyy="1.0" iyz="0.0"
+        izz="1.0"/>
+    </inertial>
+  </link>
+
+  <!-- Joint 1 -->
+  <joint name="joint1" type="fixed">
+    <parent link="base_link"/>
+    <child link="link1"/>
+    <origin rpy="0 0 0.7854" xyz="0 1.0 0.0"/>
+    <dynamics damping="0.7"/>
+  </joint>
+
+  <!-- Joint 2 -->
+  <joint name="joint2" type="fixed">
+    <parent link="link1"/>
+    <child link="link2"/>
+    <origin rpy="0 0 0.7854" xyz="0 1.0 0.0"/>
+    <axis xyz="0 1 0"/>
+    <dynamics damping="0.7"/>
+  </joint>
+</robot>
+