Add support for the S1 (circle) manifold and its tangent space

README.md

@@ -46,14 +46,16 @@ pip install "liecasadi @ git+https://github.com/ami-iit/lie-casadi.git"
 | --------- | ------------------ |
 | SO3       | 3D Rotations       |
 | SE3       | 3D Rigid Transform |
+| S1        | Circle (1D angle)  |
 
 ### 🚀 Operations
 
+
 Being:
 
-- $X, Y \in SO3, \ SE3$
+- $X, Y \in SO3, \ SE3, \ S1$
 
-- $w \in \text{SO3Tangent}, \ \text{SE3Tangent}$
+- $w \in \text{SO3Tangent}, \ \text{SE3Tangent}, \ \text{S1Tangent}$
 
 - $v \in \mathbb{R}^3$
 

src/liecasadi/__init__.py

@@ -2,4 +2,5 @@
 from .quaternion import Quaternion
 from .so3 import SO3, SO3Tangent
 from .se3 import SE3, SE3Tangent
+from .s1 import S1, S1Tangent
 from .dualquaternion import DualQuaternion

src/liecasadi/s1.py

@@ -0,0 +1,83 @@
+import dataclasses
+import casadi as cs
+
+from liecasadi.hints import Angle, Scalar
+
+def _wrap_to_pi(angle: Scalar) -> Scalar:
+    """Wrap an angle to the range [-pi, pi]."""
+    s = cs.sin(angle)
+    c = cs.cos(angle)
+    return cs.atan2(s, c)
+
+@dataclasses.dataclass
+class S1:
+    """Class representing the circle manifold S1."""
+
+    angle: Angle
+
+    def __repr__(self) -> str:
+        return f"S1 angle: {self.angle}"
+
+    @staticmethod
+    def Identity() -> "S1":
+        """Return the identity element (0 radians)."""
+        return S1(0.0)
+
+    def as_angle(self) -> Angle:
+        """Return the angle value."""
+        return self.angle
+
+    def inverse(self) -> "S1":
+        """Return the inverse rotation."""
+        return S1(-self.angle)
+
+    def __mul__(self, other: "S1") -> "S1":
+        if type(self) is type(other):
+            return S1(_wrap_to_pi(self.angle + other.angle))
+        raise RuntimeError("[S1: __mul__] Please multiply with an S1 object.")
+
+    def __rmul__(self, other: "S1") -> "S1":
+        return self.__mul__(other)
+
+    def log(self) -> "S1Tangent":
+        """Return the tangent vector via the logarithm map."""
+        return S1Tangent(_wrap_to_pi(self.angle))
+
+    def __sub__(self, other: "S1") -> "S1Tangent":
+        if type(self) is type(other):
+            return S1Tangent(_wrap_to_pi(self.angle - other.angle))
+        raise RuntimeError("[S1: __sub__] Please subtract an S1 object.")
+
+
+@dataclasses.dataclass
+class S1Tangent:
+    """Tangent space element of S1."""
+
+    val: Scalar
+
+    def __repr__(self) -> str:
+        return f"S1Tangent: {self.val}"
+
+    def exp(self) -> S1:
+        """Exponential map returning an S1 element."""
+        return S1(_wrap_to_pi(self.val))
+
+    def __add__(self, other: S1) -> S1:
+        if type(other) is S1:
+            return S1(_wrap_to_pi(other.angle + self.val))
+        raise RuntimeError("[S1Tangent: __add__] Please add an S1 object.")
+
+    def __radd__(self, other: S1) -> S1:
+        return self.__add__(other)
+
+    def __mul__(self, scalar: float) -> "S1Tangent":
+        if isinstance(scalar, (int, float)):
+            return S1Tangent(self.val * scalar)
+        raise RuntimeError("[S1Tangent: __mul__] Please multiply with a scalar.")
+
+    def __rmul__(self, scalar: float) -> "S1Tangent":
+        return self.__mul__(scalar)
+
+    def value(self) -> Scalar:
+        """Return the underlying scalar value."""
+        return self.val

tests/test_liecasadi.py

@@ -2,8 +2,8 @@
 import numpy as np
 import pytest
 from scipy.spatial.transform import Rotation
-
-from liecasadi import SE3, SO3, SE3Tangent, SO3Tangent
+import casadi as cs
+from liecasadi import SE3, SO3, S1, SE3Tangent, SO3Tangent, S1Tangent
 
 # quat generation
 quat = (np.random.rand(4) - 0.5) * 5
@@ -110,10 +110,10 @@ def test_act():
 mySE3_2 = SE3(pos=pos2, xyzw=quat2)
 
 
-# def test_inverse():
-assert (mySE3.inverse().transform() - manifSE3.inverse().transform()) == pytest.approx(
-    0.0, abs=1e-4
-)
+def test_inverse():
+    assert (mySE3.inverse().transform() - manifSE3.inverse().transform()) == pytest.approx(
+        0.0, abs=1e-4
+    )
 
 
 def test_mul():
@@ -148,3 +148,23 @@ def test_sub_SE3():
     assert (mySE3 - mySE3_2).exp().transform() - (
         manifSE3 - manifSE3_2
     ).exp().transform() == pytest.approx(0.0, abs=1e-4)
+
+
+def test_identity():
+    assert S1.Identity().as_angle() == 0.0
+
+def test_mul():
+    a = S1(cs.pi / 4)
+    b = S1(cs.pi / 4)
+    c = a * b
+    assert float(c.as_angle()) == pytest.approx(float(cs.pi / 2))
+
+def test_sub_and_log_exp():
+    a = S1(cs.pi / 3)
+    b = S1(cs.pi / 6)
+    diff = a - b
+    rebuilt = diff.exp() * b
+    assert float(rebuilt.as_angle()) == pytest.approx(float(cs.pi / 3))
+
+    tangent = a.log()
+    assert float(tangent.exp().as_angle()) == pytest.approx(float(cs.pi / 3))