From 1d2e788df39b64f0c8ba8b81915d4dc5cd5d418f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juan=20Jos=C3=A9=20Quiroz=20Oma=C3=B1a?= Date: Sat, 1 Mar 2025 08:40:24 +0000 Subject: [PATCH] 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 | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) 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):