diff --git a/DQ_Kinematics_test.py b/DQ_Kinematics_test.py index d57a10b..0733785 100644 --- a/DQ_Kinematics_test.py +++ b/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):