From a71da84ff3d047eeb66b53c9b9e8a271f909c2f9 Mon Sep 17 00:00:00 2001 From: David Butterworth Date: Tue, 19 Jan 2016 18:30:44 -0500 Subject: [PATCH] Add optional parameter to get F.K. relative to a specific frame --- src/prpy/util.py | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/src/prpy/util.py b/src/prpy/util.py index 12ad6ca..1d7806f 100644 --- a/src/prpy/util.py +++ b/src/prpy/util.py @@ -959,16 +959,21 @@ def CheckJointLimits(robot, q): description='position') -def GetForwardKinematics(robot, q, manipulator=None): +def GetForwardKinematics(robot, q, manipulator=None, frame=None): """ - Get the forward kinematics for a specific joint configuration. + Get the forward kinematics for a specific joint configuration, + relative to the OpenRAVE world frame. @param openravepy.robot robot: The robot object. @param list q: List or array of joint positions. - + @param manipulator + @param string frame: Get the end effector transform relative to a + specific frame e.g. '/right/wam_base' @returns T_ee: The pose of the end effector (or last link in the serial chain) as a 4x4 matrix. """ + from tf import transformations + if manipulator == None: manipulator = robot.GetActiveManipulator() @@ -983,6 +988,10 @@ def GetForwardKinematics(robot, q, manipulator=None): T_ee = manipulator.GetEndEffectorTransform() # Robot state is restored + if frame != None: + T_ref_frame = robot.GetLink(frame).GetTransform() + T_ee = transformations.concatenate_matrices(numpy.linalg.inv(T_ref_frame), T_ee) + return T_ee