bkinnightskytw
/
kdl_parser
mirror of https://github.com/existedinnettw/kdl_parser.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

fixed a subtle bug in urdf interpretation of kdl_parser_py (#187) 

* fix for unspecified origin RPY and added test for omitted rpy
This commit is contained in:
eugene-katsevman 2017-04-25 20:33:45 +03:00 committed by Shane Loretz
parent c5915ec079
commit 90b3764e97
3 changed files with 14 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
kdl_parser_py/kdl_parser_py/urdf.py
View File

@ -1,4 +1,3 @@
from __future__ import print_function from __future__ import print_function
import urdf_parser_py.urdf as urdf import urdf_parser_py.urdf as urdf
@ -31,12 +30,14 @@ def treeFromString(xml):
return treeFromUrdfModel(urdf.URDF.from_xml_string(xml)) return treeFromUrdfModel(urdf.URDF.from_xml_string(xml))
def _toKdlPose(pose): def _toKdlPose(pose):
if pose and pose.rpy and len(pose.rpy) == 3 and pose.xyz and len(pose.xyz) == 3: # URDF might have RPY OR XYZ unspecified. Both default to zeros
return kdl.Frame( rpy = pose.rpy if pose and pose.rpy and len(pose.rpy) == 3 else [0, 0, 0]
kdl.Rotation.RPY(*pose.rpy), xyz = pose.xyz if pose and pose.xyz and len(pose.xyz) == 3 else [0, 0, 0]
kdl.Vector(*pose.xyz))
else: return kdl.Frame(
return kdl.Frame.Identity() kdl.Rotation.RPY(*rpy),
kdl.Vector(*xyz))
def _toKdlInertia(i): def _toKdlInertia(i):
# kdl specifies the inertia in the reference frame of the link, the urdf # kdl specifies the inertia in the reference frame of the link, the urdf

6
kdl_parser_py/test/test.urdf
View File

@ -169,7 +169,7 @@
<joint name="left_gripper_joint" type="revolute"> <joint name="left_gripper_joint" type="revolute">
<axis xyz="0 0 1"/> <axis xyz="0 0 1"/>
<limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/> <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"/> <parent link="gripper_pole"/>
<child link="left_gripper"/> <child link="left_gripper"/>
</joint> </joint>
@ -180,7 +180,7 @@
<geometry> <geometry>
<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger.dae"/> <mesh filename="package://pr2_description/meshes/gripper_v0/l_finger.dae"/>
</geometry> </geometry>
<origin rpy="0.0 0 0" xyz="0 0 0"/> <origin rpy="0.0 0 0"/>
</collision> </collision>
<inertial> <inertial>
<mass value="0.05"/> <mass value="0.05"/>
@ -208,6 +208,7 @@
<inertial> <inertial>
<mass value="0.05"/> <mass value="0.05"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/> <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> </inertial>
</link> </link>
@ -221,6 +222,7 @@
<inertial> <inertial>
<mass value="10"/> <mass value="10"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/> <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> </inertial>
</link> </link>

2
kdl_parser_py/test/test_kdl_parser.py
View File

@ -31,6 +31,8 @@ class TestKdlParser(unittest.TestCase):
self.assertEqual(chain.getSegment(1).getName(), "right_gripper") self.assertEqual(chain.getSegment(1).getName(), "right_gripper")
self.assertEqual(chain.getSegment(1).getJoint().getName(), "right_gripper_joint") 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__': if __name__ == '__main__':
rostest.run(PKG, NAME, TestKdlParser, sys.argv) rostest.run(PKG, NAME, TestKdlParser, sys.argv)