Merge pull request #1225 from pollen-robotics/fix/prevent-yaw-interpolation-through-the-back

Fix : Prevent yaw from being interpolated through 180° when using goto.

Author: scienceetonnante

src/reachy_mini/motion/goto.py

@@ -57,7 +57,10 @@ def evaluate(
         interp_time = time_trajectory(t / self.duration, method=self.method)
 
         interp_head_pose = linear_pose_interpolation(
-            self.start_head_pose, self.target_head_pose, interp_time
+            self.start_head_pose,
+            self.target_head_pose,
+            interp_time,
+            yaw_as_scalar=True,
         )
         interp_antennas_joint = (
             self.start_antennas

src/reachy_mini/utils/interpolation.py

@@ -56,19 +56,39 @@ def f(t: float) -> npt.NDArray[np.float64]:
 
 
 def linear_pose_interpolation(
-    start_pose: npt.NDArray[np.float64], target_pose: npt.NDArray[np.float64], t: float
+    start_pose: npt.NDArray[np.float64],
+    target_pose: npt.NDArray[np.float64],
+    t: float,
+    yaw_as_scalar: bool = False,
 ) -> npt.NDArray[np.float64]:
-    """Linearly interpolate between two poses in 6D space."""
+    """Linearly interpolate between two poses in 6D space.
+
+    Use `yaw_as_scalar` to interpolate yaw as a signed scalar Euler angle instead of along the SO(3) geodesic.
+    This keeps the path through the front rather than taking the shortest 3D rotation through +-180° (the back).
+    """
     # Extract rotations
     rot_start = R.from_matrix(start_pose[:3, :3])
     rot_end = R.from_matrix(target_pose[:3, :3])
 
-    # Compute relative rotation q_rel such that rot_start * q_rel = rot_end
-    q_rel = rot_start.inv() * rot_end
-    # Convert to rotation vector (axis-angle)
-    rotvec_rel = q_rel.as_rotvec()
-    # Scale the rotation vector by t (allows t<0 or >1 for overshoot)
-    rot_interp = (rot_start * R.from_rotvec(rotvec_rel * t)).as_matrix()
+    if yaw_as_scalar:
+        # Factor yaw out (outermost factor) and interpolate it as a signed scalar,
+        yaw_start = rot_start.as_euler("xyz")[2]
+        yaw_end = rot_end.as_euler("xyz")[2]
+        yaw_interp = yaw_start + (yaw_end - yaw_start) * t
+        res_start = R.from_euler("z", -yaw_start) * rot_start
+        res_end = R.from_euler("z", -yaw_end) * rot_end
+        # SLERPing only the pitch/roll residual.
+        rotvec_rel = (res_start.inv() * res_end).as_rotvec()
+        res_interp = res_start * R.from_rotvec(rotvec_rel * t)
+        rot_interp = (R.from_euler("z", yaw_interp) * res_interp).as_matrix()
+    else:
+        # Geodesic (shortest-path) SLERP via rotation vector.
+        # Compute relative rotation q_rel such that rot_start * q_rel = rot_end
+        q_rel = rot_start.inv() * rot_end
+        # Convert to rotation vector (axis-angle)
+        rotvec_rel = q_rel.as_rotvec()
+        # Scale the rotation vector by t (allows t<0 or >1 for overshoot)
+        rot_interp = (rot_start * R.from_rotvec(rotvec_rel * t)).as_matrix()
 
     # Extract translations
     pos_start = start_pose[:3, 3]

tests/unit_tests/test_interpolation.py

@@ -0,0 +1,63 @@
+"""Tests for pose interpolation, in particular the yaw_as_scalar routing fix."""
+
+import numpy as np
+from scipy.spatial.transform import Rotation as R
+
+from reachy_mini.utils import create_head_pose
+from reachy_mini.utils.interpolation import linear_pose_interpolation
+
+
+def _world_yaw_deg(pose):
+    return R.from_matrix(pose[:3, :3]).as_euler("xyz", degrees=True)[2]
+
+
+def test_yaw_as_scalar_routes_through_front_not_back():
+    """look-left(+120) -> look-right(-120) must route through 0, never the back (+-180).
+
+    Regression for the SLERP geodesic taking the short 3D path around +-180 deg, which
+    the bounded +-160 deg body yaw cannot follow (causing a discontinuous body_yaw flip).
+    """
+    start = create_head_pose(yaw=120, degrees=True)
+    end = create_head_pose(yaw=-120, degrees=True)
+
+    yaws = [
+        _world_yaw_deg(
+            linear_pose_interpolation(start, end, i / 60, yaw_as_scalar=True)
+        )
+        for i in range(61)
+    ]
+
+    # never swings out toward +-180 (the back)
+    assert max(abs(y) for y in yaws) <= 121.0
+    # passes through the front (~0 deg)
+    assert min(abs(y) for y in yaws) <= 5.0
+    # smooth: no large per-frame jump from a back crossover
+    assert max(abs(b - a) for a, b in zip(yaws, yaws[1:])) < 10.0
+
+
+def test_yaw_as_scalar_matches_slerp_without_back_crossing():
+    """Outside the cross-the-back case, yaw_as_scalar is identical to the geodesic SLERP."""
+    # same-side sweep with pitch/roll, body must move but no back crossing
+    start = create_head_pose(yaw=130, pitch=15, roll=10, degrees=True)
+    end = create_head_pose(yaw=70, pitch=15, roll=10, degrees=True)
+
+    for i in range(21):
+        t = i / 20
+        np.testing.assert_allclose(
+            linear_pose_interpolation(start, end, t, yaw_as_scalar=True),
+            linear_pose_interpolation(start, end, t),
+            atol=1e-9,
+        )
+
+
+def test_yaw_as_scalar_endpoints_exact():
+    """t=0 and t=1 reproduce the start and target poses exactly."""
+    start = create_head_pose(yaw=120, pitch=5, degrees=True)
+    end = create_head_pose(yaw=-120, roll=8, degrees=True)
+
+    np.testing.assert_allclose(
+        linear_pose_interpolation(start, end, 0.0, yaw_as_scalar=True), start, atol=1e-9
+    )
+    np.testing.assert_allclose(
+        linear_pose_interpolation(start, end, 1.0, yaw_as_scalar=True), end, atol=1e-9
+    )