Merge pull request #2 from cmower/fix-1

Fix issue #1: use alternate formula. Removed wide tols in unit test.

spatial_casadi/spatial.py

@@ -391,14 +391,13 @@ def as_rotvec(self, degrees: bool = False) -> ArrayType:
 @param degrees If True, then the given magnitudes are assumed to be in degrees. Default is False.
 @return A 3-dimensional rotation vector
         """
+
+        # w > 0 to ensure 0 <= angle <= pi
         quat = casadi.if_else(self._quat[3] < 0, -self._quat, self._quat)
-        angle = 2.0 * casadi.arctan2(casadi.norm_fro(quat), quat[3])
-        # angle2 = angle * angle
-        scale = casadi.if_else(
-            angle <= 1e-3,
-            2.0 + angle**2 / 12.0 + 7.0 * angle**2 * angle**2 / 2880.0,
-            angle / casadi.sin(angle * 0.5),
-        )
+
+        # Use formula: https://uk.mathworks.com/help/fusion/ref/quaternion.rotvec.html
+        theta = 2.0 * casadi.acos(quat[3])
+        scale = theta / casadi.sin(theta * 0.5)
 
         rotvec = casadi.vertcat(
             scale * quat[0],

test/test_rotation.py

@@ -3,6 +3,7 @@
 from spatial_casadi import Rotation
 from scipy.spatial.transform import Rotation as Rot
 
+
 NUM_RANDOM = 100
 
 EULER_SEQS = (
@@ -219,7 +220,7 @@ def test_Rotation_as_rotvec():
         print("  Scipy:", RV)
         print("  Lib:  ", rv)
 
-        assert np.allclose(rv, RV, atol=1e-1, rtol=1e-1)
+        assert np.allclose(rv, RV)
 
 
 def test_Rotation_as_mrp():