From c38dd2f65e36904ad6c9ebc72cd9f2b3753f0c17 Mon Sep 17 00:00:00 2001 From: Karime Pereida Date: Tue, 20 Jun 2023 20:11:24 -0400 Subject: [PATCH] Fix qangle function and add associated test The fix consists of multiplying by a factor of 2. --- spatialmath/base/quaternions.py | 2 +- tests/base/test_quaternions.py | 10 ++++++++++ 2 files changed, 11 insertions(+), 1 deletion(-) diff --git a/spatialmath/base/quaternions.py b/spatialmath/base/quaternions.py index 3cac79e1..2b795173 100755 --- a/spatialmath/base/quaternions.py +++ b/spatialmath/base/quaternions.py @@ -985,7 +985,7 @@ def qangle(q1: ArrayLike4, q2: ArrayLike4) -> float: q1 = smb.getvector(q1, 4) q2 = smb.getvector(q2, 4) - return 2.0 * math.atan2(smb.norm(q1 - q2), smb.norm(q1 + q2)) + return 4.0 * math.atan2(smb.norm(q1 - q2), smb.norm(q1 + q2)) def qprint( diff --git a/tests/base/test_quaternions.py b/tests/base/test_quaternions.py index 3f202323..54977c20 100644 --- a/tests/base/test_quaternions.py +++ b/tests/base/test_quaternions.py @@ -218,6 +218,16 @@ def test_r2q(self): with self.assertRaises(ValueError): nt.assert_array_almost_equal(q1a, r2q(r1.R, order="aaa")) + def test_qangle(self): + # Test function that calculates angle between quaternions + q1 = [1., 0, 0, 0] + q2 = [1 / np.sqrt(2), 0, 1 / np.sqrt(2), 0] # 90deg rotation about y-axis + nt.assert_almost_equal(qangle(q1, q2), np.pi / 2) + + q1 = [1., 0, 0, 0] + q2 = [1 / np.sqrt(2), 1 / np.sqrt(2), 0, 0] # 90deg rotation about x-axis + nt.assert_almost_equal(qangle(q1, q2), np.pi / 2) + if __name__ == "__main__": unittest.main()