Add convenience function to compute forward kinematics

[DavidB-CMU]

I can add a simple unit test that checks if a specific configuration returns the expected end effector transform?

[mkoval]

It would be better to only pass the LinkTransformations option here. Otherwise we'll save a bunch of extra state that we don't modify (e.g. joint limits, active DOFs, etc).

[DavidB-CMU]

GetForwardKinematics() returns the end-effector transform w.r.t. the "/world" frame.

I'm thinking about adding an optional argument to specify the desired reference frame,
for example to get the transform w.r.t. the base link of the WAM manipulator:

...

# Example:  frame = '/right/wam_base'
from tf import transformations
if frame != None:
    T_ref_frame = robot.GetLink(frame).GetTransform()
    T_ee = transformations.concatenate_matrices(numpy.linalg.inv(T_ref_frame), T_ee)

[mkoval]

I noticed that a more general version of this functionality exists in BodyPointsStatesFromJointState. That code optionally supports velocities (derivatives=[0,1]) and accelerations (derivatives=[0,1,2]). It also allows you to query the state of any point, instead of only the end-effector frame. See #166 for some discussion about this API.

Is this sufficient for your needs? Or, if not, it would be nice to implement the new function in terms of the existing ones. Thoughts?

[DavidB-CMU]

The GetForwardKinematics() function has 4 lines of code and is very easy to understand. I think we should add it.

I don't think GetForwardKinematics() should wrap a complicated function that is 53 lines and more difficult to understand.

[mkoval]

I'd prefer to not add a tf dependency for this. Could you use numpy.dot instead?

[mkoval]

I am fine with merging this if you: (1) remove the tf dependency, see above, and (2) fix the failed build on Travis.

[DavidB-CMU]

• Removed tf dependency
• Added an extra unit test
• Rebased the commits to make Travis happy