Add magnitude function

spatial_casadi/spatial.py

@@ -138,6 +138,11 @@ def inv(self):
             normalize=not isinstance(self._quat, casadi.SX),
         )
 
+    def magnitude(self):
+        """! Get the magnitude of the rotation."""
+        quat = self._quat
+        return 2. * casadi.arctan2(casadi.norm_fro(quat[:3]), casadi.fabs(quat[3]))
+
     #
     # From methods
     #
@@ -597,6 +602,10 @@ def symbolic():
         t = casadi.SX.sym("t", 3)
         return Translation(t)
 
+    def magnitude(self):
+        """! Get the magnitude of the translation."""
+        return casadi.norm_fro(self._t)
+
     @staticmethod
     def from_vector(t):
         """! Initialize from translation vector.

test/test_rotation.py

@@ -87,7 +87,18 @@ def test_Rotation_inv():
     for _ in range(NUM_RANDOM):
         r = Rotation.random()
         R = Rot.from_quat(r.as_quat().toarray().flatten())
-        assert np.allclose(r.inv().as_quat().toarray().flatten(), R.inv().as_quat())
+        lhs = r.inv().as_quat().toarray().flatten()
+        rhs = R.inv().as_quat()
+        assert np.allclose(lhs, rhs) or np.allclose(lhs, -rhs)
+
+
+def test_Rotation_magnitiude():
+    for _ in range(NUM_RANDOM):
+        r = Rotation.random()
+        R = Rot.from_quat(r.as_quat().toarray().flatten())
+        lhs = r.magnitude()
+        rhs = R.magnitude()
+        assert np.allclose(lhs, rhs)
 
 
 def test_Rotation_from_matrix():

test/test_translation.py

@@ -69,3 +69,12 @@ def test_Translation_as_matrix():
         T[:3, :3] = Rotation.identity().as_matrix().toarray()
         t = Translation.from_matrix(T)
         assert np.allclose(t.as_matrix().toarray(), T)
+
+
+def test_Translation_magnitude():
+    for _ in range(NUM_RANDOM):
+        t = Translation.random()
+        t_np = t.as_vector().toarray().flatten()
+        magn_np = np.linalg.norm(t_np)
+        magn_sc = t.magnitude().toarray()
+        assert np.allclose(magn_np, magn_sc)