Add test for DQ_SerialWholeBody::pose_jacobian (#10)

* [DQ_Kinematics_test.py] Added the test for the DQ_SerialWholeBody.pose_jacobian method

* [DQ_Kinematics_test.py] Added the test for the DQ_SerialWholeBody.pose_jacobian method

DQ_Kinematics_test.py

@@ -41,7 +41,7 @@
 serial_whole_body_q_list    = get_list_of_vector_from_mat('random_q_serial_whole_body', mat)
 serial_whole_body_pose_list = get_list_of_dq_from_mat('result_of_serial_whole_body_fkm', mat)
 serial_whole_body_raw_pose_list = get_list_of_dq_from_mat('result_of_serial_whole_body_raw_fkm', mat)
-
+serial_whole_body_pose_jacobian_list = get_list_of_matrices_from_mat('result_of_serial_whole_body_jacobian', mat)
 
 # A list of the result of fkm() for the list of random joint values
 serial_manipulator_pose_list = get_list_of_dq_from_mat('result_of_fkm', mat)
@@ -72,6 +72,7 @@ class DQTestCase(unittest.TestCase):
     global serial_whole_body_q_list
     global serial_whole_body_raw_pose_list
     global serial_whole_body_pose_list
+    global serial_whole_body_pose_jacobian_list
     global translation_jacobian_list
     global line_jacobian_list
     global plane_jacobian_list
@@ -122,6 +123,11 @@ def test_serial_whole_body_raw_fkm(self):
         for q, x in zip(serial_whole_body_q_list, serial_whole_body_raw_pose_list):
             self.assertEqual(serial_whole_body_robot.raw_fkm(q), x, "Error in DQ_SerialWholeBody.raw_fkm")
 
+    def test_serial_whole_body_pose_jacobian(self):
+        for q, J in zip(serial_whole_body_q_list, serial_whole_body_pose_jacobian_list):
+            numpy.testing.assert_almost_equal(serial_whole_body_robot.pose_jacobian(q), J, 12, "Error in DQ_SerialWholeBody.pose_jacobian")
+
+
     def test_distance_jacobian(self):
         distance_jacobian_list = get_list_of_matrices_from_mat('result_of_distance_jacobian', mat)
         for J, x, distance_jacobian in zip(serial_manipulator_pose_jacobian_list, serial_manipulator_pose_list, distance_jacobian_list):