Merge pull request #251 from DavidB-CMU/master

Add convenience function to compute forward kinematics

Author: mkoval

src/prpy/util.py

@@ -1027,6 +1027,44 @@ def CheckJointLimits(robot, q):
             description='position')
 
 
+def GetForwardKinematics(robot, q, manipulator=None, frame=None):
+    """
+    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.
+    """
+    if manipulator == None:
+        manipulator = robot.GetActiveManipulator()
+
+    T_ee = None
+
+    # Save the robot state
+    sp = openravepy.Robot.SaveParameters
+    robot_saver = robot.CreateRobotStateSaver(sp.LinkTransformation)
+
+    with robot_saver:
+        robot.SetActiveDOFValues(q)
+        T_ee = manipulator.GetEndEffectorTransform()
+    # Robot state is restored
+
+    if frame != None:
+        link = robot.GetLink(frame)
+        if link == None:
+            raise ValueError('Failed to get link \'{:s}\''.format(frame))
+
+        T_ref_frame = link.GetTransform()
+        T_ee = numpy.dot(numpy.linalg.inv(T_ref_frame), T_ee)
+
+    return T_ee
+
+
 def ConvertIntToBinaryString(x, reverse=False):
     """
     Convert an integer to a binary string.

tests/test_util.py

@@ -565,7 +565,7 @@ def test_SampleTimeGenerator_StepOfTwo(self):
                                                 decimal=7, err_msg=error, \
                                                 verbose=True)
 
-    def test_SampleTimeGenerator_(self):
+    def test_SampleTimeGenerator_EndPointMoreThanHalfStepSize(self):
         # Check that the sequence includes the end-point when it's
         # distance from the previous point is more than half the
         # step-size. This is required for collision-checking
@@ -581,7 +581,7 @@ def test_SampleTimeGenerator_(self):
                                                 decimal=7, err_msg=error, \
                                                 verbose=True)
 
-    def test_SampleTimeGenerator_(self):
+    def test_SampleTimeGenerator_EndPointLessThanHalfStepSize(self):
         # Check that the end-point is excluded when it's less than
         # half the step-size from the previous value.
         expected_sequence = [0.0, 2.0, 4.0]
@@ -595,7 +595,70 @@ def test_SampleTimeGenerator_(self):
                                                 decimal=7, err_msg=error, \
                                                 verbose=True)
 
+
+    # GetForwardKinematics()
+
+    def test_GetForwardKinematics_UseActiveManipulator(self):
+        # Zero configuration, uses the the active manipulator by default
+        q0 = numpy.array([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
+        T_ee = prpy.util.GetForwardKinematics(self.robot, q0)
+
+        expected_T_ee0 = numpy.array([[1.0, 0.0, 0.0, 0.07 ],
+                                      [0.0, 1.0, 0.0, 0.0  ],
+                                      [0.0, 0.0, 1.0, 2.168],
+                                      [0.0, 0.0, 0.0, 1.0  ]])
+        error = 'The end-effector transform ' + str(T_ee) + \
+                ' does not equal the expected end-effector' + \
+                ' transform ' + str(expected_T_ee0)
+        numpy.testing.assert_array_almost_equal(T_ee, expected_T_ee0, decimal=7, \
+                                                err_msg=error, verbose=True)
+
+    def test_GetForwardKinematics_SpecifyManipulator(self):
+        # A different configuration, specifying the manipulator to use
+        q1 = numpy.array([-1.8, 0.0, -0.7, 2.0, 0.5, 0.2, 0.0])
+        manip = self.robot.GetActiveManipulator()
+        T_ee = prpy.util.GetForwardKinematics(self.robot, q1, manip)
+
+        expected_T_ee1 = numpy.array([[ 0.7126777,  0.3653714, -0.5988272, -0.3313395],
+                                      [-0.0541115, -0.8224716, -0.5662263, -0.3259651],
+                                      [-0.6994014,  0.4359404, -0.5663864,  1.4394693],
+                                      [ 0.0,        0.0,        0.0,        1.0      ]])
+        error = 'The end-effector transform ' + str(T_ee) + \
+                ' does not equal the expected end-effector' + \
+                ' transform ' + str(expected_T_ee1)
+        numpy.testing.assert_array_almost_equal(T_ee, expected_T_ee1, decimal=7, \
+                                                err_msg=error, verbose=True)
+
+    def test_GetForwardKinematics_SpecifyFrame(self):
+        # Get the FK with respect to the base frame ('segway') of the
+        # manipulator chain. This should give the same result as not
+        # specifying the reference frame at all.
+        q1 = numpy.array([-1.8, 0.0, -0.7, 2.0, 0.5, 0.2, 0.0])
+        manip = self.robot.GetActiveManipulator()
+        frame = 'segway'
+        T_ee = prpy.util.GetForwardKinematics(self.robot, q1, manip, frame)
+
+        expected_T_ee2 = numpy.array([[ 0.7126777,  0.3653714, -0.5988272, -0.3313395],
+                                      [-0.0541115, -0.8224716, -0.5662263, -0.3259651],
+                                      [-0.6994014,  0.4359404, -0.5663864,  1.4394693],
+                                      [ 0.0,        0.0,        0.0,        1.0      ]])
+
+        # We need to transform the expected result along the z-axis
+        # because the Segway-WAM model is strange
+        T_segway = self.robot.GetLink('segway').GetTransform()
+        expected_T_ee2 = numpy.dot(numpy.linalg.inv(T_segway), expected_T_ee2)
+
+        error = 'The end-effector transform ' + str(T_ee) + \
+                ' does not equal the expected end-effector' + \
+                ' transform ' + str(expected_T_ee2)
+        numpy.testing.assert_array_almost_equal(T_ee, expected_T_ee2, decimal=7, \
+                                                err_msg=error, verbose=True)
+
+
 class Test_GetPointFrom(unittest.TestCase):
+    """
+    Unit Tests for GetPointFrom()
+    """
     def setUp(self):
         self.env = openravepy.Environment()
 
@@ -641,5 +704,6 @@ def test_GetPointFrom_List(self):
         list_result = prpy.util.GetPointFrom(list_coord)
         numpy.testing.assert_array_almost_equal(list_result, expected_coord)
 
+
 if __name__ == '__main__':
     unittest.main()