Usage
kdl_parser.kdl_tree_from_urdf_model
Used to convert URDF objects into PyKDL.Tree objects.
1 from urdf_parser_py.urdf import URDF
2 from pykdl_utils.kdl_parser import kdl_tree_from_urdf_model
3 robot = URDF.load_from_parameter_server(verbose=False)
4 tree = kdl_tree_from_urdf_model(robot)
5 print tree.getNrOfSegments()
6 chain = tree.getChain(base_link, end_link)
7 print chain.getNrOfJoints()
kdl_kinematics.KDLKinematics
Provides wrappers for performing KDL functions on a designated kinematic chain given a URDF representation of a robot.
1 from urdf_parser_py.urdf import URDF
2 from pykdl_utils.kdl_kinematics import KDLKinematics
3 robot = URDF.from_parameter_server()
4 kdl_kin = KDLKinematics(robot, base_link, end_link)
5 q = kdl_kin.random_joint_angles()
6 pose = kdl_kin.forward(q) # forward kinematics (returns homogeneous 4x4 numpy.mat)
7 q_ik = kdl_kin.inverse(pose, q+0.3) # inverse kinematics
8 if q_ik is not None:
9 pose_sol = kdl_kin.forward(q_ik) # should equal pose
10 J = kdl_kin.jacobian(q)
11 print 'q:', q
12 print 'q_ik:', q_ik
13 print 'pose:', pose
14 if q_ik is not None:
15 print 'pose_sol:', pose_sol
16 print 'J:', J
Unit Tests
For running all of the unit tests, you can either pass a URDF xml file to use or leave it blank and have the URDF pulled from the parameter /robot_description on the parameter server.
rosrun pykdl_utils kdl_parser.py [robot.xml] rosrun pykdl_utils kdl_kinematics.py [robot.xml] rosrun pykdl_utils joint_kinematics.py [robot.xml]