Merge pull request #13 from ami-iit/fix-slerp-step

[SLERP] Return the first quaternion when the two quaternion are equal

src/liecasadi/quaternion.py

@@ -122,6 +122,12 @@ def slerp_step(q1: Vector, q2: Vector, t: Scalar) -> Vector:
 
         dot = cs.dot(q1, q2)
         angle = cs.acos(dot)
+        # if the angle is small (meaning the quaternions are "equal") we return the first quaternion
         return Quaternion(
-            (cs.sin((1.0 - t) * angle) * q1 + cs.sin(t * angle) * q2) / cs.sin(angle)
+            cs.if_else(
+                angle < 1e-6,
+                q1,
+                (cs.sin((1.0 - t) * angle) * q1 + cs.sin(t * angle) * q2)
+                / cs.sin(angle),
+            )
         )