diff --git a/src/liecasadi/quaternion.py b/src/liecasadi/quaternion.py
index ce5c352..44594de 100644
--- a/src/liecasadi/quaternion.py
+++ b/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),
+            )
         )