Repository: bdaiinstitute/spatialmath-python

Issue:
Discrepancies in SE3.interp with respect to initial rotations and comparison to SciPy's SLERP implementation
I found using the interp function of the spatialmath toolbox that the interpolation results depend on the initial rotation frames. On further investigation I also found difference between the SE3.interp function and an implementation using 
scipy.spatial.transform.Slerp. 

## Steps to Reproduce

   - Create two SE3 frames with varying initial rotations.
   - Interpolate between these two frames using SE3.interp.
   - Compare the results by applying an additional rotation before interpolation and then unapplying it.
   - Observe that the outputs vary depending on the initial angles.

## Demo code

The code below provides both examples where SE3.interp is not working as expected, and an alternative implementation based using scipy.

```python
from spatialmath import SE3
import numpy as np
from scipy.spatial.transform import Slerp, Rotation

def sp_interp(start_frame: SE3, end_frame: SE3, num_steps: int) -> list[SE3]:
    """
    Interpolate between two SE3 frames using spherical linear interpolation (SLERP) for rotation
    and linear interpolation for translation.

    Args:
    start_frame (SE3): The starting transformation frame.
    end_frame (SE3): The ending transformation frame.
    num_steps (int): Number of interpolation steps including start and end.

    Returns:
    list[SE3]: List of interpolated SE3 frames.
    """
    interpolated_frames = []
    time_vector = np.linspace(0, 1, num_steps)
    rotation_slerp = Slerp([0, 1], Rotation.from_matrix([start_frame.R, end_frame.R]))
    for rot, fraction in zip(rotation_slerp(time_vector), time_vector):
        translation = end_frame.t * fraction + start_frame.t * (1 - fraction)
        interpolated_frames.append(SE3.Rt(rot.as_matrix(), translation))
    return interpolated_frames

# Testing the function with various orientations and rotations
test_angles = (0, 88, 100, -179, -181)
test_cases = [(i, j, k) for i in test_angles for j in test_angles for k in test_angles]

for i, j, k in test_cases:
    initial_frame = SE3(0, 2, 0) * SE3.Rz(j, unit="deg") * SE3.Rx(i, unit="deg")
    final_frame = SE3.Ry(j, unit="deg")
    rotation_frame = SE3.Rz(k, unit="deg")

    # Standard interpolation and rotation transformation
    direct_interp = initial_frame.interp(final_frame, 5)[1]
    rotated_interp = rotation_frame.inv() * (rotation_frame * initial_frame).interp(rotation_frame * final_frame, 5)[1]

    # Using the spherical interpolation function
    slerp_result = sp_interp(initial_frame, final_frame, 5)[1]
    rotated_slerp_result = rotation_frame.inv() * sp_interp(rotation_frame * initial_frame, rotation_frame * final_frame, 5)[1]

    # Check and output any inconsistencies
    if not np.isclose(direct_interp.data, rotated_interp.data, atol=1e-2).all():
        print(f"SE3.interp depends on initial rotation, i={i}, j={j}, k={k}")

    if not np.isclose(slerp_result.data, rotated_slerp_result.data, atol=1e-2).all():
        print(f"sp_slerp depends on initial rotation, i={i}, j={j}, k={k}")

    if not np.isclose(direct_interp.data, slerp_result.data, atol=1e-2).all():
        print(f"SE3.interp and sp_slerp differ, i={i}, j={j}, k={k}")
```
### Output / Reported inconsistencies
```text
SE3.interp depends on initial rotation, i=0, j=100, k=100
SE3.interp depends on initial rotation, i=88, j=100, k=100
SE3.interp depends on initial rotation, i=100, j=100, k=100
SE3.interp and sp_slerp differ, i=-179, j=0, k=0
SE3.interp and sp_slerp differ, i=-179, j=0, k=88
SE3.interp and sp_slerp differ, i=-179, j=0, k=100
SE3.interp and sp_slerp differ, i=-179, j=0, k=-179
SE3.interp and sp_slerp differ, i=-179, j=0, k=-181
SE3.interp depends on initial rotation, i=-179, j=100, k=100
SE3.interp depends on initial rotation, i=-181, j=100, k=100
```

Discussion:
@petercorke: Thanks for the detailed problem statement.  The discrepancy is in the rotational part of the transforms, and that is ultimately performed by `base/quaternions/qslerp()`.  For a rotation about an axis there are always two ways to rotate and typically one is longer than the other.  By default `qslerp()` doesn't choose the shortest, but it looks like whether it is shortest or longest depends on the initial rotation.  If I hack `qslerp()` to have `shortest=True` by default, then your code runs fine.  The implication is that the SciPy SLERP function does this by default, and it has no options to control its behaviour in this regard.

The issue is how to resolve this.  The `shortest` option is not passed down from the `.interp()` method you are calling but perhaps it should be.

@jcao-bdai we can change the default, would cause change in behaviour (but not wrong behaviour) for existing users or pass the option from `pose3d/interp()` through `base/transforms3d/trinterp()` to `base/quaternions/qslerp()`.  While we are it, I'd like to add an option to `base/quaternions/r2q()` to enforce a quaternion with a non-negative scalar part.@jcao-bdai: @tdamsma @petercorke the second option seems more user-friendly, along that line, i can put together a quick PR which
1. exposes this `shortest` argument to `pose3d.interp(..., shortest=True)` through `base.transform3d.trinterp(..., shortest=True)` to `base.quaternion.qslerp(..., shortest=False)` (not changing default in the last one);
2. similarly, passes that argument to `base.transform3d.trrinterp2(..., shortest=True)` which a shortest angle logic will be added, for 2D case N==2 in `pose3d.interp()`;
3. adds `shortest` argument to `base.quaternions.r2q()` which ensures returning q with non-negative scalar part.

please note that my response may be slow till next week (05/13) as i'm taking the upcoming week off.@jcao-bdai: ~a draft PR is created at https://github.com/bdaiinstitute/spatialmath-python/pull/123~
~i will also add some unit tests accordingly.~

please review https://github.com/bdaiinstitute/spatialmath-python/pull/123
(tests have been added)@jcao-bdai: updates have been merged. thank you all for looking into this issue!

Solution patch:
diff --git a/spatialmath/base/quaternions.py b/spatialmath/base/quaternions.py
index 77efe64..0adf77c 100755
--- a/spatialmath/base/quaternions.py
+++ b/spatialmath/base/quaternions.py
@@ -549,6 +549,7 @@ def r2q(
     check: Optional[bool] = False,
     tol: float = 20,
     order: Optional[str] = "sxyz",
+    shortest: bool = False,
 ) -> UnitQuaternionArray:
     """
     Convert SO(3) rotation matrix to unit-quaternion
@@ -562,6 +563,8 @@ def r2q(
     :param order: the order of the returned quaternion elements. Must be 'sxyz' or
         'xyzs'. Defaults to 'sxyz'.
     :type order: str
+    :param shortest: ensures the quaternion has non-negative scalar part.
+    :type shortest: bool, default to False
     :return: unit-quaternion as Euler parameters
     :rtype: ndarray(4)
     :raises ValueError: for non SO(3) argument
@@ -633,6 +636,9 @@ def r2q(
         e[1] = math.copysign(e[1], R[0, 2] + R[2, 0])
         e[2] = math.copysign(e[2], R[2, 1] + R[1, 2])
 
+    if shortest and e[0] < 0:
+        e = -e
+
     if order == "sxyz":
         return e
     elif order == "xyzs":
diff --git a/spatialmath/base/transforms2d.py b/spatialmath/base/transforms2d.py
index 669c8fd..c64ed03 100644
--- a/spatialmath/base/transforms2d.py
+++ b/spatialmath/base/transforms2d.py
@@ -853,16 +853,16 @@ def tr2jac2(T: SE2Array) -> R3x3:
 
 
 @overload
-def trinterp2(start: Optional[SO2Array], end: SO2Array, s: float) -> SO2Array:
+def trinterp2(start: Optional[SO2Array], end: SO2Array, s: float, shortest: bool = True) -> SO2Array:
     ...
 
 
 @overload
-def trinterp2(start: Optional[SE2Array], end: SE2Array, s: float) -> SE2Array:
+def trinterp2(start: Optional[SE2Array], end: SE2Array, s: float, shortest: bool = True) -> SE2Array:
     ...
 
 
-def trinterp2(start, end, s):
+def trinterp2(start, end, s, shortest: bool = True):
     """
     Interpolate SE(2) or SO(2) matrices
 
@@ -872,6 +872,8 @@ def trinterp2(start, end, s):
     :type end: ndarray(3,3) or ndarray(2,2)
     :param s: interpolation coefficient, range 0 to 1
     :type s: float
+    :param shortest: take the shortest path along the great circle for the rotation
+    :type shortest: bool, default to True
     :return: interpolated SE(2) or SO(2) matrix value
     :rtype: ndarray(3,3) or ndarray(2,2)
     :raises ValueError: bad arguments
@@ -917,6 +919,8 @@ def trinterp2(start, end, s):
 
             th0 = math.atan2(start[1, 0], start[0, 0])
             th1 = math.atan2(end[1, 0], end[0, 0])
+            if shortest:
+                th1 = th0 + smb.wrap_mpi_pi(th1 - th0)
 
             th = th0 * (1 - s) + s * th1
 
@@ -937,6 +941,8 @@ def trinterp2(start, end, s):
 
             th0 = math.atan2(start[1, 0], start[0, 0])
             th1 = math.atan2(end[1, 0], end[0, 0])
+            if shortest:
+                th1 = th0 + smb.wrap_mpi_pi(th1 - th0)
 
             p0 = transl2(start)
             p1 = transl2(end)
diff --git a/spatialmath/base/transforms3d.py b/spatialmath/base/transforms3d.py
index 376af9f..ceff873 100644
--- a/spatialmath/base/transforms3d.py
+++ b/spatialmath/base/transforms3d.py
@@ -1605,16 +1605,16 @@ def trnorm(T: SE3Array) -> SE3Array:
 
 
 @overload
-def trinterp(start: Optional[SO3Array], end: SO3Array, s: float) -> SO3Array:
+def trinterp(start: Optional[SO3Array], end: SO3Array, s: float, shortest: bool = True) -> SO3Array:
     ...
 
 
 @overload
-def trinterp(start: Optional[SE3Array], end: SE3Array, s: float) -> SE3Array:
+def trinterp(start: Optional[SE3Array], end: SE3Array, s: float, shortest: bool = True) -> SE3Array:
     ...
 
 
-def trinterp(start, end, s):
+def trinterp(start, end, s, shortest=True):
     """
     Interpolate SE(3) matrices
 
@@ -1624,6 +1624,8 @@ def trinterp(start, end, s):
     :type end: ndarray(4,4) or ndarray(3,3)
     :param s: interpolation coefficient, range 0 to 1
     :type s: float
+    :param shortest: take the shortest path along the great circle for the rotation
+    :type shortest: bool, default to True
     :return: interpolated SE(3) or SO(3) matrix value
     :rtype: ndarray(4,4) or ndarray(3,3)
     :raises ValueError: bad arguments
@@ -1663,12 +1665,12 @@ def trinterp(start, end, s):
         if start is None:
             # 	TRINTERP(T, s)
             q0 = r2q(end)
-            qr = qslerp(qeye(), q0, s)
+            qr = qslerp(qeye(), q0, s, shortest=shortest)
         else:
             # 	TRINTERP(T0, T1, s)
             q0 = r2q(start)
             q1 = r2q(end)
-            qr = qslerp(q0, q1, s)
+            qr = qslerp(q0, q1, s, shortest=shortest)
 
         return q2r(qr)
 
@@ -1679,7 +1681,7 @@ def trinterp(start, end, s):
             q0 = r2q(t2r(end))
             p0 = transl(end)
 
-            qr = qslerp(qeye(), q0, s)
+            qr = qslerp(qeye(), q0, s, shortest=shortest)
             pr = s * p0
         else:
             # 	TRINTERP(T0, T1, s)
@@ -1689,7 +1691,7 @@ def trinterp(start, end, s):
             p0 = transl(start)
             p1 = transl(end)
 
-            qr = qslerp(q0, q1, s)
+            qr = qslerp(q0, q1, s, shortest=shortest)
             pr = p0 * (1 - s) + s * p1
 
         return rt2tr(q2r(qr), pr)
diff --git a/spatialmath/baseposematrix.py b/spatialmath/baseposematrix.py
index 379a543..1a85060 100644
--- a/spatialmath/baseposematrix.py
+++ b/spatialmath/baseposematrix.py
@@ -377,7 +377,7 @@ class BasePoseMatrix(BasePoseList):
         else:
             return log
 
-    def interp(self, end: Optional[bool] = None, s: Union[int, float] = None) -> Self:
+    def interp(self, end: Optional[bool] = None, s: Union[int, float] = None, shortest: bool = True) -> Self:
         """
         Interpolate between poses (superclass method)
 
@@ -385,6 +385,8 @@ class BasePoseMatrix(BasePoseList):
         :type end: same as ``self``
         :param s: interpolation coefficient, range 0 to 1, or number of steps
         :type s: array_like or int
+        :param shortest: take the shortest path along the great circle for the rotation
+        :type shortest: bool, default to True
         :return: interpolated pose
         :rtype: same as ``self``
 
@@ -432,13 +434,13 @@ class BasePoseMatrix(BasePoseList):
         if self.N == 2:
             # SO(2) or SE(2)
             return self.__class__(
-                [smb.trinterp2(start=self.A, end=end, s=_s) for _s in s]
+                [smb.trinterp2(start=self.A, end=end, s=_s, shortest=shortest) for _s in s]
             )
 
         elif self.N == 3:
             # SO(3) or SE(3)
             return self.__class__(
-                [smb.trinterp(start=self.A, end=end, s=_s) for _s in s]
+                [smb.trinterp(start=self.A, end=end, s=_s, shortest=shortest) for _s in s]
             )
 
     def interp1(self, s: float = None) -> Self:


Test patch:
diff --git a/tests/base/test_quaternions.py b/tests/base/test_quaternions.py
index 54977c2..c512c6d 100644
--- a/tests/base/test_quaternions.py
+++ b/tests/base/test_quaternions.py
@@ -131,6 +131,13 @@ class TestQuaternion(unittest.TestCase):
         )
         nt.assert_array_almost_equal(qvmul([0, 1, 0, 0], [0, 0, 1]), np.r_[0, 0, -1])
 
+        large_rotation = math.pi + 0.01
+        q1 = r2q(tr.rotx(large_rotation), shortest=False)
+        q2 = r2q(tr.rotx(large_rotation), shortest=True)
+        self.assertLess(q1[0], 0)
+        self.assertGreater(q2[0], 0)
+        self.assertTrue(qisequal(q1=q1, q2=q2, unitq=True))
+
     def test_slerp(self):
         q1 = np.r_[0, 1, 0, 0]
         q2 = np.r_[0, 0, 1, 0]
diff --git a/tests/test_pose2d.py b/tests/test_pose2d.py
index e45cc91..d6d9681 100755
--- a/tests/test_pose2d.py
+++ b/tests/test_pose2d.py
@@ -456,6 +456,16 @@ class TestSE2(unittest.TestCase):
         array_compare(I.interp(TT, s=1), TT)
         array_compare(I.interp(TT, s=0.5), SE2(1, -2, 0.3))
 
+        R1 = SO2(math.pi - 0.1)
+        R2 = SO2(-math.pi + 0.2)
+        array_compare(R1.interp(R2, s=0.5, shortest=False), SO2(0.05))
+        array_compare(R1.interp(R2, s=0.5, shortest=True), SO2(-math.pi + 0.05))
+
+        T1 = SE2(0, 0, math.pi - 0.1)
+        T2 = SE2(0, 0, -math.pi + 0.2)
+        array_compare(T1.interp(T2, s=0.5, shortest=False), SE2(0, 0, 0.05))
+        array_compare(T1.interp(T2, s=0.5, shortest=True), SE2(0, 0, -math.pi + 0.05))
+
     def test_miscellany(self):
         TT = SE2(1, 2, 0.3)