Get forward kinematics (w/o tf service call) from URDF & joint angles (Kinetic, Python)
Context:
I am writing a python ROS node that:
- Parses a URDF (from param server) and generates a trac_ik_python IK object.
- Generates an end-effector trajectory between the current location (xyz) and a target location (xyz).
- Then for each pt in the end-effector trajectory [(x0,y0,z0),(x1,y1,z1),...,(xn,yn,zn)].
- Gets joint angles from HEBI-X5 actuator.
- Calculates joint angles using trac_ik_python.
- Publishes joint angles to HEBI-X5.
Problem:
Inside the trajectory execution loop, I would like to know the actual end-effector position (based on some KDL tree and the HEBI actuator joint angle sensors).
If possible, I'd like to create the KDL object from the urdf - and just update it with the new joint angles.
Possible Solution:
This answer recommends using urdf_parser_py.urdf and pykdl_utils.kdl_kinematics
... unfortunately, pykdl_utils does not appear to be in ROS Kinetic.
Questions:
- What's the best way to obtain forward kinematics from a urdf and joint angles (w/o using a tf service call - too slow)?
- Is there a Kinetic substitute for pykdl_utils?