From 261a4c8a09fa8b0503259b7251b333ec07439f51 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Mon, 20 Jan 2025 09:05:07 +0100 Subject: [PATCH] Check quaternions for equal dot_product instead of comparing their components individually (#1238) The lookup can result in quaternions that look different component-wise while still being very similar. Instead of comparing them component-wise to decide whether they are similar, we check whether their dot-product is equal to 1.0. That should be more robust. (cherry picked from commit fa82013dd789e7569725c5c8e243bdcc4e643124) --- .../test/integration_test_force_mode.py | 28 ++++++------------- 1 file changed, 9 insertions(+), 19 deletions(-) diff --git a/ur_robot_driver/test/integration_test_force_mode.py b/ur_robot_driver/test/integration_test_force_mode.py index 5c793081b..a1dc89f23 100644 --- a/ur_robot_driver/test/integration_test_force_mode.py +++ b/ur_robot_driver/test/integration_test_force_mode.py @@ -67,6 +67,11 @@ TIMEOUT_EXECUTE_TRAJECTORY = 30 +def are_quaternions_same(q1, q2, tolerance): + dot_product = q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w + return (abs(dot_product) - 1.0) < tolerance + + @pytest.mark.launch_test @launch_testing.parametrize( "tf_prefix", @@ -205,25 +210,10 @@ def test_force_mode_controller(self, tf_prefix): trans_before.transform.translation.z, delta=0.001, ) - self.assertAlmostEqual( - trans_after.transform.rotation.x, - trans_before.transform.rotation.x, - delta=0.01, - ) - self.assertAlmostEqual( - trans_after.transform.rotation.y, - trans_before.transform.rotation.y, - delta=0.01, - ) - self.assertAlmostEqual( - trans_after.transform.rotation.z, - trans_before.transform.rotation.z, - delta=0.01, - ) - self.assertAlmostEqual( - trans_after.transform.rotation.w, - trans_before.transform.rotation.w, - delta=0.01, + self.assertTrue( + are_quaternions_same( + trans_after.transform.rotation, trans_before.transform.rotation, 0.001 + ) ) res = self._force_mode_controller_interface.stop_force_mode()