Get forward kinematics (w/o tf service call) from URDF & joint angles (Kinetic, Python)

Question

Context:
I am writing a python ROS node that:

1. Parses a URDF (from param server) and generates a trac_ik_python IK object.
2. Generates an end-effector trajectory between the current location (xyz) and a target location (xyz).
3. Then for each pt in the end-effector trajectory [(x0,y0,z0),(x1,y1,z1),...,(xn,yn,zn)].
4. Gets joint angles from HEBI-X5 actuator.
5. Calculates joint angles using trac_ik_python.
6. 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?

---

Originally posted by josephcoombe on ROS Answers with karma: 697 on 2018-01-30

Post score: 1

Answer 1

I asked a similar question a couple of days ago that went unanswered. I've since decided to dig into the source for the package python_orocos_kdl that I think will address both of our needs (though the documentation is definitely lacking). I'll update my question/the docs once I'm able to get something working.

---

Originally posted by rkeatin3 with karma: 156 on 2018-01-31

This answer was NOT ACCEPTED on the original site

Post score: 2

---

Original comments

Comment by josephcoombe on 2018-01-31:
Hmm, thanks for pointer - right now looking at using kdl_parser_py and python_orocos_kdl.

Comment by josephcoombe on 2018-01-31:
^ The two python modules I linked (kdl_parser_py.urdf and python_orocos_kdl.kinfamtest) seem to provide the most promising implementation. You are correct. Neither package has a well-documented Python interface.

Answer 2

The easy workaround is to simply copy the hrl-kdl repository into your workspace: https://github.com/gt-ros-pkg/hrl-kdl So far, the forward kinematics (from urdf and sensor joint angles) have been correct - which is all that I want right now.

from urdf_parser_py.urdf import URDF
from pykdl_utils.kdl_kinematics import KDLKinematics

# pykdl_utils setup
robot_urdf = URDF.from_xml_string(urdf_str)
kdl_kin = KDLKinematics(robot_urdf, base_link, end_link)

test_jt_angles = [0, 0, 0]
pose = kdl_kin.forward(test_jt_angles)

---

Alternatively, you could re-implement pykdl_utils using http://wiki.ros.org/urdfdom_py and http://wiki.ros.org/python_orocos_kdl

---

Originally posted by josephcoombe with karma: 697 on 2018-01-31

This answer was ACCEPTED on the original site

Post score: 1