Discrepancies in SE3.interp with respect to initial rotations and comparison to SciPy's SLERP implementation

[tdamsma]

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.

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

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

[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 #123
i will also add some unit tests accordingly.

please review #123
(tests have been added)

[jcao-bdai]

updates have been merged. thank you all for looking into this issue!