urdf.py
Go to the documentation of this file.
00001 
00002 from __future__ import print_function
00003 
00004 import urdf_parser_py.urdf as urdf
00005 
00006 import PyKDL as kdl
00007 
00008 def treeFromFile(filename):
00009     """
00010     Construct a PyKDL.Tree from an URDF file.
00011     :param filename: URDF file path
00012     """
00013 
00014     with open(filename) as urdf_file:
00015         return treeFromUrdfModel(urdf.URDF.from_xml_string(urdf_file.read()))
00016 
00017 def treeFromParam(param):
00018     """
00019     Construct a PyKDL.Tree from an URDF in a ROS parameter.
00020     :param param: Parameter name, ``str``
00021     """
00022 
00023     return treeFromUrdfModel(urdf.URDF.from_parameter_server())
00024 
00025 def treeFromString(xml):
00026     """
00027     Construct a PyKDL.Tree from an URDF xml string.
00028     :param xml: URDF xml string, ``str``
00029     """
00030 
00031     return treeFromUrdfModel(urdf.URDF.from_xml_string(xml))
00032 
00033 def _toKdlPose(pose):
00034     if pose and pose.rpy and len(pose.rpy) == 3 and pose.xyz and len(pose.xyz) == 3:
00035         return kdl.Frame(
00036               kdl.Rotation.RPY(*pose.rpy),
00037               kdl.Vector(*pose.xyz))
00038     else:
00039         return kdl.Frame.Identity()
00040 
00041 def _toKdlInertia(i):
00042     # kdl specifies the inertia in the reference frame of the link, the urdf
00043     # specifies the inertia in the inertia reference frame
00044     origin = _toKdlPose(i.origin)
00045     inertia = i.inertia
00046     return origin.M * kdl.RigidBodyInertia(
00047             i.mass, origin.p,
00048             kdl.RotationalInertia(inertia.ixx, inertia.iyy, inertia.izz, inertia.ixy, inertia.ixz, inertia.iyz));
00049 
00050 def _toKdlJoint(jnt):
00051 
00052     fixed = lambda j,F: kdl.Joint(j.name, kdl.Joint.None)
00053     rotational = lambda j,F: kdl.Joint(j.name, F.p, F.M * kdl.Vector(*j.axis), kdl.Joint.RotAxis)
00054     translational = lambda j,F: kdl.Joint(j.name, F.p, F.M * kdl.Vector(*j.axis), kdl.Joint.TransAxis)
00055 
00056     type_map = {
00057             'fixed': fixed,
00058             'revolute': rotational,
00059             'continuous': rotational,
00060             'prismatic': translational,
00061             'floating': fixed,
00062             'planar': fixed,
00063             'unknown': fixed,
00064             }
00065 
00066     return type_map[jnt.type](jnt, _toKdlPose(jnt.origin))
00067 
00068 def _add_children_to_tree(robot_model, root, tree):
00069 
00070 
00071     # constructs the optional inertia
00072     inert = kdl.RigidBodyInertia(0)
00073     if root.inertial:
00074         inert = _toKdlInertia(root.inertial)
00075 
00076     # constructs the kdl joint
00077     (parent_joint_name, parent_link_name) = robot_model.parent_map[root.name]
00078     parent_joint = robot_model.joint_map[parent_joint_name]
00079 
00080     # construct the kdl segment
00081     sgm = kdl.Segment(
00082         root.name,
00083         _toKdlJoint(parent_joint),
00084         _toKdlPose(parent_joint.origin),
00085         inert)
00086 
00087     # add segment to tree
00088     if not tree.addSegment(sgm, parent_link_name):
00089         return False
00090 
00091     if root.name not in robot_model.child_map:
00092         return True
00093 
00094     children = [robot_model.link_map[l] for (j,l) in robot_model.child_map[root.name]]
00095 
00096     # recurslively add all children
00097     for child in children:
00098         if not _add_children_to_tree(robot_model, child, tree):
00099             return False
00100 
00101     return True;
00102 
00103 def treeFromUrdfModel(robot_model, quiet=False):
00104     """
00105     Construct a PyKDL.Tree from an URDF model from urdf_parser_python.
00106 
00107     :param robot_model: URDF xml string, ``str``
00108     :param quiet: If true suppress messages to stdout, ``bool``
00109     """
00110 
00111     root = robot_model.link_map[robot_model.get_root()]
00112 
00113     if root.inertial and not quiet:
00114         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);
00115 
00116     ok = True
00117     tree = kdl.Tree(root.name)
00118 
00119     #  add all children
00120     for (joint,child) in robot_model.child_map[root.name]:
00121         if not _add_children_to_tree(robot_model, robot_model.link_map[child], tree):
00122             ok = False
00123             break
00124   
00125     return (ok, tree)


kdl_parser_py
Author(s): Jonathan Bohren , Jackie Kay
autogenerated on Sat May 20 2017 03:02:57