kdl_parser: Adding python kdl parser
This commit is contained in:
Jonathan Bohren
Jackie Kay
parent
a48aa284ff
commit
fedfd656fd
|
|
@ -0,0 +1,22 @@
|
|||
cmake_minimum_required(VERSION 2.8.3)
|
||||
|
||||
project(kdl_parser_py)
|
||||
|
||||
find_package(catkin REQUIRED
|
||||
COMPONENTS urdf cmake_modules
|
||||
)
|
||||
|
||||
catkin_package(
|
||||
LIBRARIES ${PROJECT_NAME}
|
||||
CATKIN_DEPENDS urdf
|
||||
)
|
||||
|
||||
catkin_python_setup()
|
||||
|
||||
catkin_install_python(PROGRAMS ${PROJECT_NAME}/urdf.py
|
||||
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
|
||||
|
||||
if(CATKIN_ENABLE_TESTING)
|
||||
find_package(catkin REQUIRED COMPONENTS rostest)
|
||||
add_rostest(test/test_kdl_parser.launch)
|
||||
endif()
|
||||
|
|
@ -0,0 +1,125 @@
|
|||
|
||||
from __future__ import print_function
|
||||
|
||||
import urdf_parser_py.urdf as urdf
|
||||
|
||||
import PyKDL as kdl
|
||||
|
||||
def treeFromFile(filename):
|
||||
"""
|
||||
Construct a PyKDL.Tree from an URDF file.
|
||||
:param filename: URDF file path
|
||||
"""
|
||||
|
||||
with open(filename) as urdf_file:
|
||||
return treeFromUrdfModel(urdf.URDF.from_xml_string(urdf_file.read()))
|
||||
|
||||
def treeFromParam(param):
|
||||
"""
|
||||
Construct a PyKDL.Tree from an URDF in a ROS parameter.
|
||||
:param param: Parameter name, ``str``
|
||||
"""
|
||||
|
||||
return treeFromUrdfModel(urdf.URDF.from_parameter_server())
|
||||
|
||||
def treeFromString(xml):
|
||||
"""
|
||||
Construct a PyKDL.Tree from an URDF xml string.
|
||||
:param xml: URDF xml string, ``str``
|
||||
"""
|
||||
|
||||
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()
|
||||
|
||||
def _toKdlInertia(i):
|
||||
# kdl specifies the inertia in the reference frame of the link, the urdf
|
||||
# specifies the inertia in the inertia reference frame
|
||||
origin = _toKdlPose(i.origin)
|
||||
inertia = i.inertia
|
||||
return origin.M * kdl.RigidBodyInertia(
|
||||
i.mass, origin.p,
|
||||
kdl.RotationalInertia(inertia.ixx, inertia.iyy, inertia.izz, inertia.ixy, inertia.ixz, inertia.iyz));
|
||||
|
||||
def _toKdlJoint(jnt):
|
||||
|
||||
fixed = lambda j,F: kdl.Joint(j.name, kdl.Joint.None)
|
||||
rotational = lambda j,F: kdl.Joint(j.name, F.p, F.M * kdl.Vector(*j.axis), kdl.Joint.RotAxis)
|
||||
translational = lambda j,F: kdl.Joint(j.name, F.p, F.M * kdl.Vector(*j.axis), kdl.Joint.TransAxis)
|
||||
|
||||
type_map = {
|
||||
'fixed': fixed,
|
||||
'revolute': rotational,
|
||||
'continuous': rotational,
|
||||
'prismatic': translational,
|
||||
'floating': fixed,
|
||||
'planar': fixed,
|
||||
'unknown': fixed,
|
||||
}
|
||||
|
||||
return type_map[jnt.type](jnt, _toKdlPose(jnt.origin))
|
||||
|
||||
def _add_children_to_tree(robot_model, root, tree):
|
||||
|
||||
|
||||
# constructs the optional inertia
|
||||
inert = kdl.RigidBodyInertia(0)
|
||||
if root.inertial:
|
||||
inert = _toKdlInertia(root.inertial)
|
||||
|
||||
# constructs the kdl joint
|
||||
(parent_joint_name, parent_link_name) = robot_model.parent_map[root.name]
|
||||
parent_joint = robot_model.joint_map[parent_joint_name]
|
||||
|
||||
# construct the kdl segment
|
||||
sgm = kdl.Segment(
|
||||
root.name,
|
||||
_toKdlJoint(parent_joint),
|
||||
_toKdlPose(parent_joint.origin),
|
||||
inert)
|
||||
|
||||
# add segment to tree
|
||||
if not tree.addSegment(sgm, parent_link_name):
|
||||
return False
|
||||
|
||||
if root.name not in robot_model.child_map:
|
||||
return True
|
||||
|
||||
children = [robot_model.link_map[l] for (j,l) in robot_model.child_map[root.name]]
|
||||
|
||||
# recurslively add all children
|
||||
for child in children:
|
||||
if not _add_children_to_tree(robot_model, child, tree):
|
||||
return False
|
||||
|
||||
return True;
|
||||
|
||||
def treeFromUrdfModel(robot_model, quiet=False):
|
||||
"""
|
||||
Construct a PyKDL.Tree from an URDF model from urdf_parser_python.
|
||||
|
||||
:param robot_model: URDF xml string, ``str``
|
||||
:param quiet: If true suppress messages to stdout, ``bool``
|
||||
"""
|
||||
|
||||
root = robot_model.link_map[robot_model.get_root()]
|
||||
|
||||
if root.inertial and not quiet:
|
||||
print("The root link %s has an inertia specified in the URDF, but KDL does not support a root link with an inertia. As a workaround, you can add an extra dummy link to your URDF." % root.name);
|
||||
|
||||
ok = True
|
||||
tree = kdl.Tree(root.name)
|
||||
|
||||
# add all children
|
||||
for (joint,child) in robot_model.child_map[root.name]:
|
||||
if not _add_children_to_tree(robot_model, robot_model.link_map[child], tree):
|
||||
ok = False
|
||||
break
|
||||
|
||||
return (ok, tree)
|
||||
|
|
@ -0,0 +1,31 @@
|
|||
<package>
|
||||
<name>kdl_parser_py</name>
|
||||
<version>1.11.8</version>
|
||||
<description>
|
||||
The Kinematics and Dynamics Library (KDL) defines a tree structure
|
||||
to represent the kinematic and dynamic parameters of a robot
|
||||
mechanism. <tt>kdl_parser_py</tt> provides Python tools to construct a KDL
|
||||
tree from an XML robot representation in URDF.
|
||||
</description>
|
||||
|
||||
<author email="jonathan.bohren@gmail.com">Jonathan Bohren</author>
|
||||
<maintainer email="jackie@osrfoundation.org">Jackie Kay</maintainer>
|
||||
|
||||
<license>BSD</license>
|
||||
|
||||
<url type="website">http://ros.org/wiki/kdl_parser_py</url>
|
||||
<url type="repository">https://github.com/ros/robot_model</url>
|
||||
<url type="bugtracker">https://github.com/ros/robot_model/issues</url>
|
||||
|
||||
<buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
|
||||
|
||||
<build_depend version_gte="1.3.0">orocos_kdl</build_depend>
|
||||
<build_depend>urdf</build_depend>
|
||||
<build_depend>rostest</build_depend>
|
||||
|
||||
<run_depend version_gte="1.3.0">orocos_kdl</run_depend>
|
||||
<run_depend>urdf</run_depend>
|
||||
<run_depend>urdf_parser_py</run_depend>
|
||||
<run_depend>python_orocos_kdl</run_depend>
|
||||
|
||||
</package>
|
||||
|
|
@ -0,0 +1,11 @@
|
|||
#!/usr/bin/env python
|
||||
|
||||
from distutils.core import setup
|
||||
from catkin_pkg.python_setup import generate_distutils_setup
|
||||
|
||||
d = generate_distutils_setup(
|
||||
packages=['kdl_parser_py'],
|
||||
package_dir={'': ''}
|
||||
)
|
||||
|
||||
setup(**d)
|
||||
File diff suppressed because it is too large
Load Diff
|
|
@ -0,0 +1,235 @@
|
|||
<?xml version="1.0"?>
|
||||
<!-- original: https://github.com/ros/urdf_tutorial/
|
||||
TODO(jacquelinekay): currently released version of urdf_parser_py doesn't
|
||||
allow joints without limits specified. once fixed, revert limitless joints
|
||||
TODO(jacquelinekay): currently released version of urdf_parser_py doesn't
|
||||
allow materials without color specified/aggregate materials. once fixed,
|
||||
revert materials
|
||||
-->
|
||||
<robot name="physics">
|
||||
<link name="base_link">
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length="0.6" radius="0.2"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
|
||||
<joint name="base_to_right_leg" type="fixed">
|
||||
<parent link="base_link"/>
|
||||
<child link="right_base"/>
|
||||
<origin xyz="0.22 0 .25"/>
|
||||
</joint>
|
||||
|
||||
<!-- link 1 -->
|
||||
<link name="right_base">
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size=".1 0.4 .1"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="10"/>
|
||||
<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="right_front_wheel">
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length=".1" radius="0.035"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<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="right_front_wheel_joint" type="continuous">
|
||||
<axis xyz="0 0 1"/>
|
||||
<parent link="right_base"/>
|
||||
<child link="right_front_wheel"/>
|
||||
<origin rpy="0 1.57075 0" xyz="0 0.133333333333 -0.085"/>
|
||||
<limit effort="100.0" lower="0" upper="0" velocity="0.5"/>
|
||||
</joint>
|
||||
|
||||
<!-- link 3 -->
|
||||
<link name="right_back_wheel">
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length=".1" radius="0.035"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<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 2 -->
|
||||
<joint name="right_back_wheel_joint" type="continuous">
|
||||
<axis xyz="0 0 1"/>
|
||||
<parent link="right_base"/>
|
||||
<child link="right_back_wheel"/>
|
||||
<origin rpy="0 1.57075 0" xyz="0 -0.133333333333 -0.085"/>
|
||||
<limit effort="100.0" lower="0" upper="0" velocity="0.5"/>
|
||||
</joint>
|
||||
|
||||
<joint name="base_to_left_leg" type="fixed">
|
||||
<parent link="base_link"/>
|
||||
<child link="left_base"/>
|
||||
<origin xyz="-0.22 0 .25"/>
|
||||
</joint>
|
||||
|
||||
<!-- link 4 -->
|
||||
<link name="left_base">
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size=".1 0.4 .1"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="10"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<!-- link 5 -->
|
||||
<link name="left_front_wheel">
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length=".1" radius="0.035"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<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 3 -->
|
||||
<joint name="left_front_wheel_joint" type="continuous">
|
||||
<axis xyz="0 0 1"/>
|
||||
<parent link="left_base"/>
|
||||
<child link="left_front_wheel"/>
|
||||
<origin rpy="0 1.57075 0" xyz="0 0.133333333333 -0.085"/>
|
||||
<limit effort="100.0" lower="0" upper="0" velocity="0.5"/>
|
||||
</joint>
|
||||
|
||||
<!-- link 6 -->
|
||||
<link name="left_back_wheel">
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length=".1" radius="0.035"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<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 4 -->
|
||||
<joint name="left_back_wheel_joint" type="continuous">
|
||||
<axis xyz="0 0 1"/>
|
||||
<parent link="left_base"/>
|
||||
<child link="left_back_wheel"/>
|
||||
<origin rpy="0 1.57075 0" xyz="0 -0.133333333333 -0.085"/>
|
||||
<limit effort="100.0" lower="0" upper="0" velocity="0.5"/>
|
||||
</joint>
|
||||
|
||||
<!-- joint 5 -->
|
||||
<joint name="gripper_extension" type="prismatic">
|
||||
<!-- TODO(jacquelinekay): axis default not used in urdf_parser_py, needed for prismatic links -->
|
||||
<axis xyz="1 0 0" />
|
||||
<parent link="base_link"/>
|
||||
<child link="gripper_pole"/>
|
||||
<limit effort="1000.0" lower="-0.38" upper="0" velocity="0.5"/>
|
||||
<origin rpy="0 0 1.57075" xyz="0 0.19 .2"/>
|
||||
</joint>
|
||||
|
||||
<!-- link 7 -->
|
||||
<link name="gripper_pole">
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length="0.2" radius=".01"/>
|
||||
</geometry>
|
||||
<origin rpy="0 1.57075 0 " xyz="0.1 0 0"/>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.05"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<!-- joint 6 -->
|
||||
<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"/>
|
||||
<parent link="gripper_pole"/>
|
||||
<child link="left_gripper"/>
|
||||
</joint>
|
||||
|
||||
<!-- link 8 -->
|
||||
<link name="left_gripper">
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger.dae"/>
|
||||
</geometry>
|
||||
<origin rpy="0.0 0 0" xyz="0 0 0"/>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.05"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<!-- joint 7 -->
|
||||
<joint name="right_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"/>
|
||||
<parent link="gripper_pole"/>
|
||||
<child link="right_gripper"/>
|
||||
</joint>
|
||||
|
||||
<!-- link 9 -->
|
||||
<link name="right_gripper">
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="package://pr2_description/meshes/gripper_v0/l_finger.dae"/>
|
||||
</geometry>
|
||||
<origin rpy="-3.1415 0 0" xyz="0 0 0"/>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.05"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<!-- link 10 -->
|
||||
<link name="head">
|
||||
<collision>
|
||||
<geometry>
|
||||
<sphere radius="0.2"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="10"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<!-- joint 8 -->
|
||||
<joint name="head_swivel" type="continuous">
|
||||
<parent link="base_link"/>
|
||||
<child link="head"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<origin xyz="0 0 0.3"/>
|
||||
</joint>
|
||||
|
||||
</robot>
|
||||
|
|
@ -0,0 +1,3 @@
|
|||
<launch>
|
||||
<test test-name="test_kdl_parser" pkg="kdl_parser_py" type="test_kdl_parser.py" args="$(find kdl_parser_py)/test/test.urdf"/>
|
||||
</launch>
|
||||
|
|
@ -0,0 +1,36 @@
|
|||
#!/usr/bin/env python
|
||||
|
||||
import sys
|
||||
|
||||
import kdl_parser_py.urdf
|
||||
import unittest
|
||||
|
||||
import rostest
|
||||
|
||||
PKG = "kdl_parser_py"
|
||||
NAME = "test_kdl_parser"
|
||||
|
||||
class TestKdlParser(unittest.TestCase):
|
||||
def runTest(self):
|
||||
filename = None
|
||||
if (sys.argv > 1):
|
||||
filename = sys.argv[1]
|
||||
else:
|
||||
self.fail("Expected filename!")
|
||||
(ok, tree) = kdl_parser_py.urdf.treeFromFile(filename)
|
||||
self.assertTrue(ok)
|
||||
# KDL doesn't count fixed joints (since they aren't kinematic)
|
||||
self.assertEqual(tree.getNrOfJoints(), 8)
|
||||
# KDL doesn't count base link (since it's attached by fixed links
|
||||
self.assertEqual(tree.getNrOfSegments(), 10)
|
||||
chain = tree.getChain("base_link", "right_gripper")
|
||||
self.assertEqual(chain.getNrOfSegments(), 2)
|
||||
self.assertEqual(chain.getNrOfJoints(), 2)
|
||||
self.assertEqual(chain.getSegment(0).getName(), "gripper_pole")
|
||||
self.assertEqual(chain.getSegment(0).getJoint().getName(), "gripper_extension")
|
||||
self.assertEqual(chain.getSegment(1).getName(), "right_gripper")
|
||||
self.assertEqual(chain.getSegment(1).getJoint().getName(), "right_gripper_joint")
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
rostest.run(PKG, NAME, TestKdlParser, sys.argv)
|
||||
Loading…
Reference in New Issue