From 90b3764e97df43bcf59fd586bad90c1507a38830 Mon Sep 17 00:00:00 2001
From: eugene-katsevman <eugene.katsevman@gmail.com>
Date: Tue, 25 Apr 2017 20:33:45 +0300
Subject: [PATCH] fixed a subtle bug in urdf interpretation of kdl_parser_py
 (#187)

* fix for unspecified origin RPY and added test for omitted rpy
---
 kdl_parser_py/kdl_parser_py/urdf.py   | 15 ++++++++-------
 kdl_parser_py/test/test.urdf          |  6 ++++--
 kdl_parser_py/test/test_kdl_parser.py |  2 ++
 3 files changed, 14 insertions(+), 9 deletions(-)

diff --git a/kdl_parser_py/kdl_parser_py/urdf.py b/kdl_parser_py/kdl_parser_py/urdf.py
index 99f1c4c..0198856 100644
--- a/kdl_parser_py/kdl_parser_py/urdf.py
+++ b/kdl_parser_py/kdl_parser_py/urdf.py
@@ -1,4 +1,3 @@
-
 from __future__ import print_function
 
 import urdf_parser_py.urdf as urdf
@@ -31,12 +30,14 @@ def treeFromString(xml):
     return treeFromUrdfModel(urdf.URDF.from_xml_string(xml))
 
 def _toKdlPose(pose):
-    if pose and pose.rpy and len(pose.rpy) == 3 and pose.xyz and len(pose.xyz) == 3:
-        return kdl.Frame(
-              kdl.Rotation.RPY(*pose.rpy),
-              kdl.Vector(*pose.xyz))
-    else:
-        return kdl.Frame.Identity()
+    # URDF might have RPY OR XYZ unspecified. Both default to zeros
+    rpy = pose.rpy if pose and pose.rpy and len(pose.rpy) == 3 else [0, 0, 0]
+    xyz = pose.xyz if pose and pose.xyz and len(pose.xyz) == 3 else [0, 0, 0]
+
+    return kdl.Frame(
+          kdl.Rotation.RPY(*rpy),
+          kdl.Vector(*xyz))
+
 
 def _toKdlInertia(i):
     # kdl specifies the inertia in the reference frame of the link, the urdf
diff --git a/kdl_parser_py/test/test.urdf b/kdl_parser_py/test/test.urdf
index d36d6d9..1a3bd06 100644
--- a/kdl_parser_py/test/test.urdf
+++ b/kdl_parser_py/test/test.urdf
@@ -169,7 +169,7 @@
   <joint name="left_gripper_joint" type="revolute">
     <axis xyz="0 0 1"/>
     <limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
-    <origin rpy="0 0 0" xyz="0.2 0.01 0"/>
+    <origin xyz="0.2 0.01 0"/>
     <parent link="gripper_pole"/>
     <child link="left_gripper"/>
   </joint>
@@ -180,7 +180,7 @@
       <geometry>
         <mesh filename="package://pr2_description/meshes/gripper_v0/l_finger.dae"/>
       </geometry>
-      <origin rpy="0.0 0 0" xyz="0 0 0"/>
+      <origin rpy="0.0 0 0"/>
     </collision>
     <inertial>
       <mass value="0.05"/>
@@ -208,6 +208,7 @@
     <inertial>
       <mass value="0.05"/>
       <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+      <origin xyz="1 2 3" />
     </inertial>
   </link>
 
@@ -221,6 +222,7 @@
     <inertial>
       <mass value="10"/>
       <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+      <origin rpy="3 2 1" />
     </inertial>
   </link>
 
diff --git a/kdl_parser_py/test/test_kdl_parser.py b/kdl_parser_py/test/test_kdl_parser.py
index 5b7fbb7..f3e3066 100755
--- a/kdl_parser_py/test/test_kdl_parser.py
+++ b/kdl_parser_py/test/test_kdl_parser.py
@@ -31,6 +31,8 @@ class TestKdlParser(unittest.TestCase):
         self.assertEqual(chain.getSegment(1).getName(), "right_gripper")
         self.assertEqual(chain.getSegment(1).getJoint().getName(), "right_gripper_joint")
 
+        inertia = chain.getSegment(1).getInertia()
+        self.assertAlmostEqual(inertia.getCOG().z(), 3.0)
 
 if __name__ == '__main__':
     rostest.run(PKG, NAME, TestKdlParser, sys.argv)