Merge pull request #114 from ros/kdl_parser_py
Create new package from kdl_parser_py
This commit is contained in:
Jackie Kay
commit
fbd03cc7e6
|
|
@ -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