Add quaternion derivative

examples/manifold_optimization.py

@@ -1,3 +1,9 @@
+# Please note that for running this example you need to install `matplotlib` and `scipy`.
+# You can do this by running the following command in your terminal:
+# pip install matplotlib scipy
+# If you are using anaconda, you can also run the following command:
+# conda install matplotlib scipy
+
 import casadi as cs
 import matplotlib.pyplot as plt
 import numpy as np

src/liecasadi/se3.py

@@ -19,6 +19,7 @@ class SE3:
     def __repr__(self) -> str:
         return f"Position: \t {self.pos} \nQuaternion: \t {self.xyzw}"
 
+    @staticmethod
     def from_matrix(H: Matrix) -> "SE3":
         assert H.shape == (4, 4)
         return SE3(pos=H[:3, 3], xyzw=SO3.from_matrix(H[:3, :3]).as_quat().coeffs())
@@ -37,11 +38,13 @@ def transform(self) -> Matrix:
             cs.horzcat([0, 0, 0, 1]).T,
         )
 
+    @staticmethod
     def from_position_quaternion(xyz: Vector, xyzw: Vector) -> "SE3":
         assert xyz.shape in [(3,), (3, 1)]
         assert xyzw.shape in [(4,), (4, 1)]
         return SE3(pos=xyz, xyzw=xyzw)
 
+    @staticmethod
     def from_translation_and_rotation(translation: Vector, rotation: SO3) -> "SE3":
         return SE3(pos=translation, xyzw=rotation.as_quat().coeffs())
 
@@ -107,7 +110,6 @@ def exp(self):
             + (theta_eps - cs.sin(theta_eps)) / theta_eps @ cs.skew(u) @ cs.skew(u)
         )
         trans = V @ vec[:3]
-        # trans = SO3.from_matrix(V).act(vec[:3])
         return SE3(pos=trans, xyzw=rot.as_quat().coeffs())
 
     def vector(self):

src/liecasadi/so3.py

@@ -109,35 +109,33 @@ def __sub__(self, other) -> "SO3Tangent":
         else:
             raise RuntimeError("[SO3: __add__] Hey! Someone is not a Lie element.")
 
-    @staticmethod
-    def _quat_dot_map(d_omega: Vector, representation: str):
-        # if d_omega in spatial representation
-        A = 0.5 * cs.vertcat(
-            cs.horzcat(0, -d_omega[0], -d_omega[1], -d_omega[2]),
-            cs.horzcat(d_omega[0], 0, d_omega[2], -d_omega[1]),
-            cs.horzcat(d_omega[1], -d_omega[2], 0, d_omega[0]),
-            cs.horzcat(d_omega[2], d_omega[1], -d_omega[0], 0),
-        )
-        if representation == "spatial":
-            A[1:, 1:] = A[1:, 1:].T
-        return A
-
-    def add_integrated_velocity(
-        self, d_omega: Vector, representation: str = ["spatial", "body"]
+    def quaternion_derivative(
+        self,
+        omega: Vector,
+        omega_in_body_fixed: bool = False,
+        baumgarte_coefficient: float = None,
     ):
-        repr_types = ["spatial", "body"]
-        if representation not in repr_types:
-            raise ValueError(f"Invalid representation. Expected are {repr_types}")
-        quat = self.as_quat().coeffs()
-        quat = cs.vertcat(quat[3], quat[0], quat[1], quat[2])
 
-        def _exp(o):
-            A = SO3._quat_dot_map(o, representation)
-            nor = cs.norm_2(o)
-            return cs.cos(nor / 2) * cs.DM.eye(4) + 2 * cs.sin(nor / 2) / nor * A
-
-        quat = _exp(d_omega) * quat
-        return SO3(xyzw=cs.vertcat(quat[1], quat[2], quat[3], quat[0]))
+        if baumgarte_coefficient is not None:
+            baumgarte_term = (
+                baumgarte_coefficient
+                * cs.norm_2(omega)
+                * (1 - cs.norm_2(self.as_quat().coeffs()))
+            )
+            _omega = Quaternion(
+                cs.vertcat(
+                    omega,
+                    baumgarte_term,
+                )
+            )
+        else:
+            _omega = Quaternion(cs.vertcat(omega, 0))
+        # using the quaternion product formula
+        return (
+            0.5 * self.as_quat() * _omega
+            if omega_in_body_fixed
+            else 0.5 * _omega * self.as_quat()
+        ).coeffs()
 
     @staticmethod
     def product(q1: Quaternion, q2: Quaternion) -> Quaternion:
@@ -151,7 +149,7 @@ class SO3Tangent:
     vec: TangentVector
 
     def __repr__(self) -> str:
-        return "SO3Tangent vector:" + str(self.vec)
+        return f"SO3Tangent vector:{str(self.vec)}"
 
     def exp(self) -> SO3:
         theta = cs.norm_2(self.vec + cs.np.finfo(np.float64).eps)