Merge pull request bdaiinstitute#82 from KPereida/master

myeatman-bdai · web-flow · commit 07c6f6246003 · 2023-07-20T11:24:45.000-04:00
Fix qangle function and add associated test
diff --git 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
@@ -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()
