Ship clutch-relative UDP control (app 3.1.6).

Engage snapshots IMU zero; yaw is 1:1 with hard stops; appear/disappear
slews are faster. Protocol v4.

README.md

@@ -16,9 +16,9 @@ tags:
 
 # ESP32 Motion Controller
 
-UDP bridge (protocol v3) for an ESP32 handheld IMU controller that drives Reachy Mini's head with clutch-style relative motion, IK-safe clamping, antenna idle animation, and body follow only when head exceeds the neck yaw threshold.
+Reachy Mini app that receives protocol v4 UDP samples from the handheld ESP32. Engage snapshots IMU zero; roll/pitch/yaw stay clutch-relative (rotation only). The app owns IK-safe clamping, hard yaw stops, antenna idle, and body follow.
 
-See the [repository README](https://github.com/V4C38/esp32-reachy-mini-controller) and [`PROTOCOL.md`](../PROTOCOL.md). Firmware and app must both speak protocol v3.
+See the [repository README](https://github.com/V4C38/esp32-reachy-mini-controller) and [`PROTOCOL.md`](../PROTOCOL.md). Firmware and app must both speak v4.
 
 ## Local install
 
@@ -49,8 +49,4 @@ hf upload YOUR_USER/esp32_motion_controller . --repo-type space \
   --exclude ".DS_Store"
 ```
 
-Ensure README frontmatter includes those tags before upload.
-
-The excludes matter: `packages.find` would otherwise pick up a stray `build/lib/`
-tree as a second copy of the package, and `.pyc` files built by a different
-Python end up installed into the daemon's 3.12 venv.
+Keep those tags in the README frontmatter. The excludes matter: `packages.find` would otherwise pick up a stray `build/lib/` as a second copy of the package, and `.pyc` files from another Python end up in the daemon's 3.12 venv.

esp32_motion_controller/__init__.py

@@ -1,3 +1,3 @@
-"""ESP32 Motion Controller Reachy Mini app (protocol v3)."""
+"""ESP32 Motion Controller Reachy Mini app (protocol v4)."""
 
-__version__ = "3.0.0"
+__version__ = "3.1.6"

esp32_motion_controller/control.py

@@ -1,5 +1,5 @@
 """
-Pure control reducer for protocol v3.
+Pure control reducer for protocol v4.
 
 Owns clutch mapping, body follow, antenna phase, workspace projection, smoothing,
 stale release, and slew limiting. No I/O.
@@ -14,7 +14,7 @@ from typing import Sequence
 import numpy as np
 from scipy.spatial.transform import Rotation as R
 
-from esp32_motion_controller.protocol import Sample
+from esp32_motion_controller.protocol import GAIN_MAX, GAIN_MIN, Sample
 
 POSE_AXES = ("x", "y", "z", "roll", "pitch", "yaw")
 ANGULAR_AXES = ("roll", "pitch", "yaw")
@@ -29,18 +29,6 @@ DEV_TO_HEAD = np.array(
     dtype=np.float64,
 )
 
-TRANSLATION_SCALE = 0.30
-TRANSLATION_GAIN_DEFAULT = 1.0
-
-# Device-frame rotation gains, applied to the clutch-relative rotation vector
-# *before* DEV_TO_HEAD. Body axes after IMU_MAP / firmware ui.c:
-#   X = tip top toward user (forward tilt)
-#   Y = USB-down in-place turn (horizontal pan)
-#   Z = raise right edge (sideways roll)
-FORWARD_GAIN = 1.0      # device X → head pitch
-HORIZONTAL_GAIN = 1.75  # device Y → head yaw
-SIDEWAYS_GAIN = 0.75    # device Z → head roll
-
 STALE_PACKET_SEC = 0.600
 CONTROL_HZ = 20.0
 CONTROL_DT = 1.0 / CONTROL_HZ
@@ -49,13 +37,15 @@ CONTROL_DT = 1.0 / CONTROL_HZ
 POSE_TAU_SEC = 0.255
 ANTENNA_TAU_SEC = 0.39
 
-# Hard SDK-boundary speed lock. Per-axis rotation / wrap-safe body yaw / Euclidean xyz.
+# Hard SDK-boundary speed lock. Per-axis linear rotation / Euclidean xyz.
 MAX_ANGULAR_VEL = 1.5  # rad/s (~86 deg/s), matching Spectacles
 MAX_POS_VEL = 0.030  # 30 mm/s
 MAX_DT_FOR_VEL_CLAMP = 0.05
+# Appear/disappear slews (BOOT engage/clutch) run faster than streaming.
+ANIM_VEL_MULT = 2.5
 
 LIMIT_BODY_YAW_RAD = 160.0 * math.pi / 180.0
-LIMIT_HEAD_YAW_RAD = math.pi
+LIMIT_HEAD_YAW_RAD = math.radians(150.0)  # 30° of margin from the ±180° IK wrap
 LIMIT_HEAD_BODY_YAW_DELTA_RAD = 65.0 * math.pi / 180.0
 
 LIMIT_HEAD_X_MIN = -0.020
@@ -65,12 +55,30 @@ LIMIT_HEAD_Y_MAX = 0.020
 LIMIT_HEAD_Z_MIN = 0.0
 LIMIT_HEAD_Z_MAX = 0.025
 
-ELLIPSOID_X_MAX = 0.015
-ELLIPSOID_Y_MAX = 0.015
-ELLIPSOID_Z_MAX = 0.018
 ELLIPSOID_ROLL_MAX_RAD = 25.0 * math.pi / 180.0
 ELLIPSOID_PITCH_MAX_RAD = 25.0 * math.pi / 180.0
 
+# Nominal hold pose: screen toward user, USB + buttons down → device +Y is up.
+# Matches IMU_MAP_* and imu_gravity_sane() in firmware/main/config.h.
+HOLD_TILT = R.from_euler("x", math.pi / 2.0)
+
+# Pitch/roll: this much board tilt from the engage snapshot spans the head's ±25°.
+PITCH_ROLL_WINDOW_RAD = math.radians(40.0)
+PITCH_ROLL_SCALE = ELLIPSOID_PITCH_MAX_RAD / PITCH_ROLL_WINDOW_RAD  # 25/40 = 0.625
+
+# Device-frame rotation gains, applied to the clutch-relative rotation vector
+# *before* DEV_TO_HEAD. Body axes after IMU_MAP / firmware ui.c:
+#   X = tip top toward user (forward tilt)
+#   Y = USB-down in-place turn (horizontal pan) — 1:1
+#   Z = raise right edge (sideways roll)
+FORWARD_GAIN = PITCH_ROLL_SCALE  # device X → head pitch (engage-relative)
+SIDEWAYS_GAIN = PITCH_ROLL_SCALE  # device Z → head roll (engage-relative)
+
+HEADING_MIN_HORIZ = 0.20  # device +X too vertical (board on its side) to give a heading
+YAW_STOP_EPS = 1e-4
+# Sign-cross hold: only when already past the neck stop, not near centre.
+YAW_SIGN_HOLD_ABS = LIMIT_HEAD_BODY_YAW_DELTA_RAD
+
 MAX_HEAD_YAW = 65.0 * math.pi / 180.0
 BODY_FOLLOW_THRESHOLD = 40.0 * math.pi / 180.0
 MAX_BODY_YAW = 160.0 * math.pi / 180.0
@@ -81,6 +89,7 @@ DEFAULT_ANTENNA_ACTIVITY = 0.8
 HEAD_MOVE_SPEED = 0.06
 MAX_HEAD_DELTA_DEG = 2.0
 ANTENNA_AMPLITUDE_DEG = 15.0
+APPEAR_ANTENNA_SPEED_MULT = 2.0
 
 IK_FAIL_RETRACT_TARGET_ALPHA = 0.06
 IK_FAIL_CONSECUTIVE_THRESHOLD = 3
@@ -110,6 +119,12 @@ def _clamp(value: float, lo: float, hi: float) -> float:
     return max(lo, min(hi, value))
 
 
+def _wrap_delta(from_ang: float, to_ang: float) -> float:
+    """Shortest signed delta from `from_ang` to `to_ang`, wrapping at ±pi."""
+    d = (to_ang - from_ang + math.pi) % (2.0 * math.pi) - math.pi
+    return d
+
+
 def _finite_quat(q: Sequence[float]) -> np.ndarray:
     arr = np.asarray(q, dtype=np.float64).reshape(4)
     if not np.all(np.isfinite(arr)) or np.linalg.norm(arr) < 1e-9:
@@ -117,17 +132,51 @@ def _finite_quat(q: Sequence[float]) -> np.ndarray:
     return arr / np.linalg.norm(arr)
 
 
-def _finite_vec3(v: Sequence[float]) -> np.ndarray:
-    arr = np.asarray(v, dtype=np.float64).reshape(3)
-    if not np.all(np.isfinite(arr)):
-        return np.zeros(3, dtype=np.float64)
-    return arr
-
-
 def _wxyz_to_rotation(q: np.ndarray) -> R:
     return R.from_quat([q[1], q[2], q[3], q[0]])
 
 
+def _rotation_to_wxyz(r: R) -> np.ndarray:
+    x, y, z, w = r.as_quat()
+    return np.array([w, x, y, z], dtype=np.float64)
+
+
+def _right_edge_xy(r: R) -> tuple[float, float]:
+    """World-XY of device +X (right edge). Nod about +X leaves this vector still."""
+    v = r.apply(np.array([1.0, 0.0, 0.0]))
+    return float(v[0]), float(v[1])
+
+
+def _heading(r: R) -> float:
+    """Rotation about world up from the board's right-edge azimuth.
+
+    Nod (device +X) does not move this vector, so pitch cannot leak into yaw.
+    """
+    x, y = _right_edge_xy(r)
+    return math.atan2(y, x)
+
+
+def _heading_stable(r: R, heading_last: float | None) -> float:
+    """Heading from the right edge; freeze if the board is rolled onto its side."""
+    x, y = _right_edge_xy(r)
+    if math.hypot(x, y) < HEADING_MIN_HORIZ:
+        if heading_last is not None:
+            return float(heading_last)
+        return math.atan2(y, x)
+    return math.atan2(y, x)
+
+
+def hold_reference(q_device: np.ndarray) -> np.ndarray:
+    """Clutch reference with the nominal hold tilt at the device's heading.
+
+    Gravity pins tilt absolutely; heading is unobservable without a
+    magnetometer, so it is latched from the device at engage.
+    """
+    r_dev = _wxyz_to_rotation(q_device)
+    dh = _wrap_delta(_heading(HOLD_TILT), _heading(r_dev))
+    return _rotation_to_wxyz(R.from_euler("z", dh) * HOLD_TILT)
+
+
 def _relative_device_rotation(q_ref: np.ndarray, q_device: np.ndarray) -> R:
     r_ref = _wxyz_to_rotation(q_ref)
     r_dev = _wxyz_to_rotation(q_device)
@@ -147,10 +196,9 @@ def quat_relative_rpy(q_ref: np.ndarray, q_device: np.ndarray) -> tuple[float, f
 
 
 def scale_device_rotation(r_rel_dev: R) -> R:
-    """Scale clutch-relative rotation about each ESP32 body axis."""
+    """Scale rotation about each ESP32 body axis (tilt window; yaw is 1:1)."""
     rv = r_rel_dev.as_rotvec()
     rv[0] *= FORWARD_GAIN
-    rv[1] *= HORIZONTAL_GAIN
     rv[2] *= SIDEWAYS_GAIN
     return R.from_rotvec(rv)
 
@@ -161,16 +209,19 @@ def relative_head_rpy(q_ref: np.ndarray, q_device: np.ndarray) -> tuple[float, f
     return _device_rotation_to_head_rpy(r_rel_dev)
 
 
-def remap_displacement(
-    p_world_delta: np.ndarray,
+def tilt_head_rp(
     q_ref: np.ndarray,
-    *,
-    translation_gain: float = TRANSLATION_GAIN_DEFAULT,
-) -> np.ndarray:
+    q_device: np.ndarray,
+    heading_dev: float,
+) -> tuple[float, float]:
+    """Roll/pitch from relative rotation after stripping world-up heading."""
     r_ref = _wxyz_to_rotation(q_ref)
-    disp_ref = r_ref.inv().apply(p_world_delta)
-    disp_head = DEV_TO_HEAD @ disp_ref
-    return disp_head * TRANSLATION_SCALE * float(translation_gain)
+    r_dev = _wxyz_to_rotation(q_device)
+    r_ref_unyaw = R.from_euler("z", -_heading(r_ref)) * r_ref
+    r_dev_unyaw = R.from_euler("z", -heading_dev) * r_dev
+    r_tilt = r_ref_unyaw.inv() * r_dev_unyaw
+    roll, pitch, _ = _device_rotation_to_head_rpy(scale_device_rotation(r_tilt))
+    return float(roll), float(pitch)
 
 
 def dual_sine(t: float, freq_a: float, freq_b: float) -> float:
@@ -180,32 +231,12 @@ def dual_sine(t: float, freq_a: float, freq_b: float) -> float:
 def clamp_stewart_ellipsoid(
     x: float, y: float, z: float, roll: float, pitch: float,
 ) -> tuple[float, float, float, float, float]:
-    z_clamped = _clamp(z, LIMIT_HEAD_Z_MIN, LIMIT_HEAD_Z_MAX)
-    x_clamped = _clamp(x, LIMIT_HEAD_X_MIN, LIMIT_HEAD_X_MAX)
-    y_clamped = _clamp(y, LIMIT_HEAD_Y_MIN, LIMIT_HEAD_Y_MAX)
-
-    roll_c = _clamp(roll, -ELLIPSOID_ROLL_MAX_RAD, ELLIPSOID_ROLL_MAX_RAD)
-    pitch_c = _clamp(pitch, -ELLIPSOID_PITCH_MAX_RAD, ELLIPSOID_PITCH_MAX_RAD)
-
-    nr = roll_c / ELLIPSOID_ROLL_MAX_RAD if ELLIPSOID_ROLL_MAX_RAD > 0 else 0.0
-    np_ = pitch_c / ELLIPSOID_PITCH_MAX_RAD if ELLIPSOID_PITCH_MAX_RAD > 0 else 0.0
-    remaining = max(0.0, 1.0 - (nr * nr + np_ * np_))
-
-    nx = x_clamped / ELLIPSOID_X_MAX if ELLIPSOID_X_MAX > 0 else 0.0
-    ny = y_clamped / ELLIPSOID_Y_MAX if ELLIPSOID_Y_MAX > 0 else 0.0
-    nz = z_clamped / ELLIPSOID_Z_MAX if ELLIPSOID_Z_MAX > 0 else 0.0
-    trans_sq = nx * nx + ny * ny + nz * nz
-
-    if trans_sq <= remaining or trans_sq <= 1e-12:
-        return (x_clamped, y_clamped, z_clamped, roll_c, pitch_c)
-
-    scale = math.sqrt(remaining / trans_sq)
     return (
-        x_clamped * scale,
-        y_clamped * scale,
-        z_clamped * scale,
-        roll_c,
-        pitch_c,
+        _clamp(x, LIMIT_HEAD_X_MIN, LIMIT_HEAD_X_MAX),
+        _clamp(y, LIMIT_HEAD_Y_MIN, LIMIT_HEAD_Y_MAX),
+        _clamp(z, LIMIT_HEAD_Z_MIN, LIMIT_HEAD_Z_MAX),
+        _clamp(roll, -ELLIPSOID_ROLL_MAX_RAD, ELLIPSOID_ROLL_MAX_RAD),
+        _clamp(pitch, -ELLIPSOID_PITCH_MAX_RAD, ELLIPSOID_PITCH_MAX_RAD),
     )
 
 
@@ -236,18 +267,37 @@ def clamp_pose_to_daemon_limits(
     return out_pose, body_yaw_clamped
 
 
+def _yaw_positive_stop(body_yaw: float) -> float:
+    body_c = _clamp(body_yaw, -LIMIT_BODY_YAW_RAD, LIMIT_BODY_YAW_RAD)
+    return min(LIMIT_HEAD_YAW_RAD, body_c + LIMIT_HEAD_BODY_YAW_DELTA_RAD)
+
+
+def _yaw_negative_stop(body_yaw: float) -> float:
+    body_c = _clamp(body_yaw, -LIMIT_BODY_YAW_RAD, LIMIT_BODY_YAW_RAD)
+    return max(-LIMIT_HEAD_YAW_RAD, body_c - LIMIT_HEAD_BODY_YAW_DELTA_RAD)
+
+
+def _yaw_at_positive_stop(yaw: float, body_yaw: float) -> bool:
+    return yaw >= _yaw_positive_stop(body_yaw) - YAW_STOP_EPS
+
+
+def _yaw_at_negative_stop(yaw: float, body_yaw: float) -> bool:
+    return yaw <= _yaw_negative_stop(body_yaw) + YAW_STOP_EPS
+
+
+def _hold_sign_cross(baseline: float, desired: float, zone: float) -> float:
+    """Hold `baseline` if a limit-zone command would chase the opposite sign."""
+    if abs(baseline) >= zone - YAW_STOP_EPS and baseline * desired < 0.0:
+        return baseline
+    return desired
+
+
 def _alpha(dt: float, tau: float) -> float:
     if tau <= 0.0:
         return 1.0
     return 1.0 - math.exp(-max(dt, 0.0) / tau)
 
 
-def _wrap_delta(from_ang: float, to_ang: float) -> float:
-    """Shortest signed delta from `from_ang` to `to_ang`, wrapping at ±pi."""
-    d = (to_ang - from_ang + math.pi) % (2.0 * math.pi) - math.pi
-    return d
-
-
 def _pose_rotation(pose: dict[str, float]) -> R:
     return R.from_euler(
         "xyz", [pose["roll"], pose["pitch"], pose["yaw"]], degrees=False
@@ -262,17 +312,19 @@ def speed_lock(
     dt: float,
     *,
     apply_ellipsoid: bool = True,
+    max_ang_vel: float = MAX_ANGULAR_VEL,
+    max_pos_vel: float = MAX_POS_VEL,
 ) -> tuple[dict[str, float], float]:
     """Cap one streaming command against the last delivered pose.
 
-    Roll, pitch, and yaw each get an independent MAX_ANGULAR_VEL budget so a
-    new tilt is not starved by leftover pan. Yaw and body yaw use wrap-safe
-    shortest-arc. Translation uses Euclidean distance. `dt` is capped so
+    Roll, pitch, yaw, and body yaw each get an independent angular-velocity
+    budget (linear deltas — yaw is a bounded axis, not a wrap). Positional
+    slews (appear/disappear/reset) use Euclidean distance. `dt` is capped so
     stalls/reconnects cannot accumulate permission for a snap.
     """
     dt_c = min(max(dt, 0.0), MAX_DT_FOR_VEL_CLAMP)
-    max_d_ang = MAX_ANGULAR_VEL * dt_c
-    max_d_pos = MAX_POS_VEL * dt_c
+    max_d_ang = max_ang_vel * dt_c
+    max_d_pos = max_pos_vel * dt_c
     send = {k: float(desired[k]) for k in POSE_AXES}
 
     dp = np.array([desired[k] - baseline[k] for k in POSITIONAL_AXES], dtype=np.float64)
@@ -285,22 +337,27 @@ def speed_lock(
         for axis in POSITIONAL_AXES:
             send[axis] = desired[axis]
 
+    hold_yaw = _hold_sign_cross(baseline["yaw"], desired["yaw"], YAW_SIGN_HOLD_ABS)
+    hold_body = _hold_sign_cross(baseline_body, desired_body, LIMIT_BODY_YAW_RAD)
+
     for axis in ("roll", "pitch"):
         delta = desired[axis] - baseline[axis]
         if abs(delta) > max_d_ang:
             send[axis] = baseline[axis] + math.copysign(max_d_ang, delta)
         else:
             send[axis] = desired[axis]
-    yaw_delta = _wrap_delta(baseline["yaw"], desired["yaw"])
+    yaw_delta = hold_yaw - baseline["yaw"]
     if abs(yaw_delta) > max_d_ang:
-        yaw_delta = math.copysign(max_d_ang, yaw_delta)
-    send["yaw"] = baseline["yaw"] + yaw_delta
-
-    body_delta = _wrap_delta(baseline_body, desired_body)
+        send["yaw"] = baseline["yaw"] + math.copysign(max_d_ang, yaw_delta)
+    else:
+        send["yaw"] = hold_yaw
+    body_delta = hold_body - baseline_body
     if abs(body_delta) > max_d_ang:
         body_delta = math.copysign(max_d_ang, body_delta)
     send_body = baseline_body + body_delta
-    return clamp_pose_to_daemon_limits(send, send_body, apply_ellipsoid=apply_ellipsoid)
+    return clamp_pose_to_daemon_limits(
+        send, send_body, apply_ellipsoid=apply_ellipsoid
+    )
 
 
 def slew_limit(
@@ -334,7 +391,9 @@ def pose_travel(
     )
     dist = float(np.linalg.norm(dp))
     rel = _pose_rotation(from_pose).inv() * _pose_rotation(to_pose)
-    ang = float(rel.magnitude()) + abs(_wrap_delta(from_body, to_body))
+    # Geodesic treats +π and −π as identical; linear yaw catches a wrap attempt.
+    ang = max(float(rel.magnitude()), abs(to_pose["yaw"] - from_pose["yaw"]))
+    ang += abs(_wrap_delta(from_body, to_body))
     return dist, ang
 
 
@@ -358,10 +417,11 @@ class ControlState:
     was_engaged: bool = False
     gain: float = 1.0
     ready: bool = False
-    translation_gain: float = TRANSLATION_GAIN_DEFAULT
 
     q_ref: np.ndarray = field(default_factory=lambda: np.array([1.0, 0.0, 0.0, 0.0]))
-    p_ref: np.ndarray = field(default_factory=lambda: np.zeros(3))
+    heading_last: float | None = None
+    yaw_unwrapped: float = 0.0
+    heading_unwrapped: float = 0.0
     base_pose: dict[str, float] = field(default_factory=zero_pose)
     desired_pose: dict[str, float] = field(default_factory=zero_pose)
 
@@ -447,6 +507,9 @@ def seed_from_pose(
         consecutive_ik_failures=0,
         engaged=False,
         was_engaged=False,
+        heading_last=None,
+        yaw_unwrapped=0.0,
+        heading_unwrapped=0.0,
         mode="idle" if state.mode != "resetting" else state.mode,
         error=None,
     )
@@ -491,8 +554,10 @@ def rebase_to_pose(
             smooth_antennas=[0.0, 0.0],
             engaged=False,
             was_engaged=False,
+            heading_last=None,
+            yaw_unwrapped=0.0,
+            heading_unwrapped=0.0,
             q_ref=np.array([1.0, 0.0, 0.0, 0.0]),
-            p_ref=np.zeros(3),
             sends_frozen=True,
             mode="fault",
             error="robot pose unread after reset",
@@ -516,8 +581,10 @@ def rebase_to_pose(
         baseline_antennas=[0.0, 0.0],
         engaged=False,
         was_engaged=False,
+        heading_last=None,
+        yaw_unwrapped=0.0,
+        heading_unwrapped=0.0,
         q_ref=np.array([1.0, 0.0, 0.0, 0.0]),
-        p_ref=np.zeros(3),
         sends_frozen=False,
         seeded=True,
         mode="idle",
@@ -539,8 +606,10 @@ def begin_reset(state: ControlState, now: float) -> ControlState:
         antenna_right=0.0,
         was_engaged=False,
         engaged=False,
+        heading_last=None,
+        yaw_unwrapped=0.0,
+        heading_unwrapped=0.0,
         q_ref=np.array([1.0, 0.0, 0.0, 0.0]),
-        p_ref=np.zeros(3),
         behavior_t0=now,
     )
 
@@ -598,37 +667,53 @@ def mark_pose_unread(state: ControlState) -> ControlState:
 
 
 def _update_clutch(state: ControlState, sample: Sample, *, allow_engage: bool) -> ControlState:
-    gain = _clamp(float(sample.gain), 0.1, 3.0)
+    gain = _clamp(float(sample.gain), GAIN_MIN, GAIN_MAX)
     q_dev = _finite_quat(sample.q)
-    p_dev = _finite_vec3(sample.p)
     want = bool(sample.engaged) and bool(sample.ready) and allow_engage
     rising = want and not state.was_engaged
     falling = (not want) and state.was_engaged
 
     q_ref = state.q_ref
-    p_ref = state.p_ref
+    heading_last = state.heading_last
+    yaw_unwrapped = state.yaw_unwrapped
+    heading_unwrapped = state.heading_unwrapped
     base = dict(state.base_pose)
     desired = dict(state.desired_pose)
 
     if rising:
+        # Snapshot the device attitude as IMU zero: roll/pitch/yaw are
+        # relative to this grab. Prior head pose stays in base_pose; x/y/z
+        # hold at that base while engaged.
         q_ref = q_dev.copy()
-        p_ref = p_dev.copy()
+        r_dev = _wxyz_to_rotation(q_dev)
+        heading_last = _heading_stable(r_dev, None)
+        yaw_unwrapped = 0.0
+        heading_unwrapped = 0.0
 
     if want:
-        roll, pitch, yaw = relative_head_rpy(q_ref, q_dev)
-        disp = remap_displacement(
-            p_dev - p_ref,
-            q_ref,
-            translation_gain=state.translation_gain * gain,
-        )
+        r_dev = _wxyz_to_rotation(q_dev)
+        h = _heading_stable(r_dev, heading_last)
+        d = _wrap_delta(heading_last, h) if heading_last is not None else 0.0
+        heading_unwrapped = heading_unwrapped + d
+        heading_last = h
+        yaw = heading_unwrapped
+        roll, pitch = tilt_head_rp(q_ref, q_dev, h)
         desired = {
-            "x": base["x"] + float(disp[0]),
-            "y": base["y"] + float(disp[1]),
-            "z": base["z"] + float(disp[2]),
+            "x": base["x"],
+            "y": base["y"],
+            "z": base["z"],
             "roll": base["roll"] + gain * roll,
             "pitch": base["pitch"] + gain * pitch,
             "yaw": base["yaw"] + gain * yaw,
         }
+        prev_at_pos = _yaw_at_positive_stop(state.desired_pose["yaw"], state.body_yaw)
+        prev_at_neg = _yaw_at_negative_stop(state.desired_pose["yaw"], state.body_yaw)
+        if heading_unwrapped > yaw_unwrapped and prev_at_pos:
+            desired["yaw"] = base["yaw"] + gain * yaw_unwrapped
+        elif heading_unwrapped < yaw_unwrapped and prev_at_neg:
+            desired["yaw"] = base["yaw"] + gain * yaw_unwrapped
+        else:
+            yaw_unwrapped = heading_unwrapped
     elif falling:
         base = dict(desired)
 
@@ -637,7 +722,9 @@ def _update_clutch(state: ControlState, sample: Sample, *, allow_engage: bool) -
         ready=bool(sample.ready),
         gain=gain,
         q_ref=q_ref,
-        p_ref=p_ref,
+        heading_last=heading_last,
+        yaw_unwrapped=yaw_unwrapped,
+        heading_unwrapped=heading_unwrapped,
         base_pose=base,
         desired_pose=desired,
         engaged=want,
@@ -645,18 +732,39 @@ def _update_clutch(state: ControlState, sample: Sample, *, allow_engage: bool) -
     )
 
 
+def advance_antennas(
+    state: ControlState,
+    now: float,
+    dt: float,
+    *,
+    speed_mult: float = 1.0,
+) -> ControlState:
+    """Advance the idle antenna wiggle toward its dual-sine target."""
+    tick_scale = dt / 0.033 if dt > 0 else 1.0
+    t = now - state.behavior_t0
+    deg = math.pi / 180.0
+
+    yaw_smoothing = HEAD_MOVE_SPEED * state.gaze_responsiveness * tick_scale
+    antenna_smoothing = yaw_smoothing * 1.5
+    effective_ant_amp = ANTENNA_AMPLITUDE_DEG * state.antenna_activity * deg
+    ant_speed = (0.5 + state.antenna_activity * 0.5) * speed_mult
+
+    desired_l = dual_sine(t * ant_speed, 1.3, 3.11) * effective_ant_amp
+    desired_r = dual_sine(t * ant_speed, 1.7, 2.73) * effective_ant_amp
+    ant_l = state.antenna_left + (desired_l - state.antenna_left) * antenna_smoothing
+    ant_r = state.antenna_right + (desired_r - state.antenna_right) * antenna_smoothing
+
+    return replace(state, antenna_left=ant_l, antenna_right=ant_r)
+
+
 def _advance_behavior(state: ControlState, head_yaw: float, now: float, dt: float) -> ControlState:
     # Normalize behavior update against the legacy ~33 ms tick using dt scaling.
     tick_scale = dt / 0.033 if dt > 0 else 1.0
-    t = now - state.behavior_t0
     deg = math.pi / 180.0
 
     yaw_smoothing = HEAD_MOVE_SPEED * state.gaze_responsiveness * tick_scale
     max_yaw_delta = MAX_HEAD_DELTA_DEG * state.gaze_responsiveness * deg * tick_scale
     body_smoothing = yaw_smoothing * 0.7 * (0.3 + state.liveliness * 0.4)
-    antenna_smoothing = yaw_smoothing * 1.5
-    effective_ant_amp = ANTENNA_AMPLITUDE_DEG * state.antenna_activity * deg
-    ant_speed = 0.5 + state.antenna_activity * 0.5
 
     body_yaw = state.body_yaw
     rel_yaw = head_yaw - body_yaw
@@ -666,17 +774,8 @@ def _advance_behavior(state: ControlState, head_yaw: float, now: float, dt: floa
         body_yaw += _clamp(step, -max_yaw_delta, max_yaw_delta)
         body_yaw = _clamp(body_yaw, -MAX_BODY_YAW, MAX_BODY_YAW)
 
-    desired_l = dual_sine(t * ant_speed, 1.3, 3.11) * effective_ant_amp
-    desired_r = dual_sine(t * ant_speed, 1.7, 2.73) * effective_ant_amp
-    ant_l = state.antenna_left + (desired_l - state.antenna_left) * antenna_smoothing
-    ant_r = state.antenna_right + (desired_r - state.antenna_right) * antenna_smoothing
-
-    return replace(
-        state,
-        body_yaw=body_yaw,
-        antenna_left=ant_l,
-        antenna_right=ant_r,
-    )
+    st = advance_antennas(state, now, dt)
+    return replace(st, body_yaw=body_yaw)
 
 
 def step(
@@ -715,10 +814,13 @@ def step(
         st = _update_clutch(st, sample, allow_engage=allow_engage)
         st = replace(st, have_sample=True)
 
-    if st.mode not in {"resetting"}:
-        st = _advance_behavior(st, st.desired_pose["yaw"], now, dt)
-
     apply_ellipsoid = bool(st.engaged)
+    # Clamp before body follow so the neck never chases unclamped clutch yaw.
+    target_pose, _ = clamp_pose_to_daemon_limits(
+        st.desired_pose, st.body_yaw, apply_ellipsoid=apply_ellipsoid
+    )
+    if st.mode not in {"resetting"}:
+        st = _advance_behavior(st, target_pose["yaw"], now, dt)
     target_pose, target_body = clamp_pose_to_daemon_limits(
         st.desired_pose, st.body_yaw, apply_ellipsoid=apply_ellipsoid
     )

esp32_motion_controller/main.py

@@ -1,5 +1,5 @@
 """
-Motion Controller Reachy Mini app entry point (protocol v3).
+Motion Controller Reachy Mini app entry point (protocol v4).
 
 FastAPI/uvicorn HTTP on TCP 8766, UDP datagrams on UDP 8766,
 mDNS advertise _reachyctl._tcp, clutch + safety bridge.
@@ -55,7 +55,7 @@ def get_local_ips() -> list[str]:
 
 
 class ControllerProtocol(asyncio.DatagramProtocol):
-    """One UDP socket: discovery probes + protocol v3 datagrams."""
+    """One UDP socket: discovery probes + protocol v4 datagrams."""
 
     def __init__(self, session: SessionHub) -> None:
         self.session = session
@@ -236,7 +236,7 @@ def create_app(
 def _run_server(reachy_mini, stop_event: threading.Event, *, log_only: bool) -> None:
     ips = get_local_ips()
     logger.info("=" * 50)
-    logger.info("ESP32 Motion Controller (protocol v3) app_version=%s", __version__)
+    logger.info("ESP32 Motion Controller (protocol v4) app_version=%s", __version__)
     logger.info("=" * 50)
     for ip in ips:
         logger.info("  UDP: %s:%d", ip, LINK_PORT)

esp32_motion_controller/protocol.py

@@ -1,5 +1,5 @@
 """
-Protocol v3 parsing — dependency-light, no SDK imports.
+Protocol v4 parsing — dependency-light, no SDK imports.
 
 UDP datagrams. Rejects oversize frames, wrong types, non-finite numbers,
 and unsupported versions.
@@ -12,10 +12,10 @@ import math
 from dataclasses import dataclass
 from typing import Any
 
-PROTOCOL_VERSION = 3
+PROTOCOL_VERSION = 4
 MAX_FRAME_BYTES = 512
 GAIN_MIN = 0.1
-GAIN_MAX = 3.0
+GAIN_MAX = 2.0
 LINK_STALE_SEC = 1.0
 HELLO_PERIOD_SEC = 2.0
 
@@ -65,7 +65,6 @@ class Sample:
     boot_id: str
     seq: int
     q: tuple[float, float, float, float]
-    p: tuple[float, float, float]
     engaged: bool
     gain: float
     ready: bool
@@ -199,7 +198,6 @@ def parse_hello(msg: dict[str, Any]) -> Hello:
 def parse_sample(msg: dict[str, Any]) -> Sample:
     request_type = "sample"
     q = _require_vec(msg, "q", 4, request_type)
-    p = _require_vec(msg, "p", 3, request_type)
     gain = _require_finite_float(msg, "gain", request_type)
     if gain < GAIN_MIN or gain > GAIN_MAX:
         raise ProtocolError(
@@ -220,7 +218,6 @@ def parse_sample(msg: dict[str, Any]) -> Sample:
         boot_id=_require_str(msg, "boot_id", request_type),
         seq=seq,
         q=(q[0], q[1], q[2], q[3]),
-        p=(p[0], p[1], p[2]),
         engaged=_require_bool(msg, "engaged", request_type),
         gain=gain,
         ready=_require_bool(msg, "ready", request_type),

esp32_motion_controller/robot_control.py

@@ -1,5 +1,5 @@
 """
-Single fixed-rate robot command owner for protocol v3.
+Single fixed-rate robot command owner for protocol v4.
 
 Consumes the latest sample from SessionHub, runs the pure reducer, and performs
 at most one SDK call in flight. Every outbound pose is capped per tick.
@@ -18,10 +18,15 @@ import numpy as np
 from scipy.spatial.transform import Rotation
 
 from esp32_motion_controller.control import (
+    ANIM_VEL_MULT,
+    APPEAR_ANTENNA_SPEED_MULT,
     CONTROL_HZ,
+    MAX_ANGULAR_VEL,
+    MAX_POS_VEL,
     STALE_PACKET_SEC,
     Command,
     ControlState,
+    advance_antennas,
     begin_reset,
     disengaged_rest_pose,
     force_disengage,
@@ -137,8 +142,9 @@ class RobotControl:
     def _guard_command(self, command: Command, now: float) -> Command:
         """Cap (or drop) an incoming pose before it reaches the robot.
 
-        Appear/disappear slews are speed-locked. Streaming jumps larger than
-        CULL_* are discarded so a stall cannot accumulate into a snap.
+        Appear/disappear slews are speed-locked (faster than streaming).
+        Streaming jumps larger than CULL_* are discarded so a stall cannot
+        accumulate into a snap.
         """
         del now
         ref_pose = (
@@ -166,6 +172,7 @@ class RobotControl:
                     body_yaw=ref_body,
                     antennas=command.antennas,
                 )
+        anim_slew = self._anim in {"appear", "disappear"}
         pose, body = speed_lock(
             ref_pose,
             ref_body,
@@ -173,6 +180,8 @@ class RobotControl:
             command.body_yaw,
             self.dt,
             apply_ellipsoid=self._state.engaged and self._anim is None,
+            max_ang_vel=MAX_ANGULAR_VEL * ANIM_VEL_MULT if anim_slew else MAX_ANGULAR_VEL,
+            max_pos_vel=MAX_POS_VEL * ANIM_VEL_MULT if anim_slew else MAX_POS_VEL,
         )
         return Command(pose=pose, body_yaw=body, antennas=command.antennas)
 
@@ -182,14 +191,30 @@ class RobotControl:
         self._anim_body = float(body)
         if kind in {"disappear", "reset"}:
             self._state = force_disengage(self._state)
+        elif kind == "appear":
+            now = time.monotonic()
+            self._state = replace(
+                self._state,
+                behavior_t0=now,
+                antenna_left=0.0,
+                antenna_right=0.0,
+                smooth_antennas=[0.0, 0.0],
+            )
 
     async def _tick_anim(
-        self, loop: asyncio.AbstractEventLoop, now: float
+        self, loop: asyncio.AbstractEventLoop, now: float, dt: float
     ) -> None:
+        antennas = (0.0, 0.0)
+        if self._anim == "appear":
+            self._state = advance_antennas(
+                self._state, now, dt, speed_mult=APPEAR_ANTENNA_SPEED_MULT
+            )
+            antennas = (self._state.antenna_left, self._state.antenna_right)
+
         command = Command(
             pose=dict(self._anim_pose),
             body_yaw=self._anim_body,
-            antennas=(0.0, 0.0),
+            antennas=antennas,
         )
         sent = await self._send_command(loop, now, command, log_sample=False)
         if sent is None:
@@ -205,6 +230,10 @@ class RobotControl:
                 desired_pose=zero_pose(),
                 base_pose=zero_pose(),
                 body_yaw=0.0,
+                smooth_antennas=[
+                    self._state.antenna_left,
+                    self._state.antenna_right,
+                ],
             )
         elif kind == "disappear":
             self._posture = "ducked"
@@ -376,7 +405,7 @@ class RobotControl:
                 self._begin_anim("disappear", disengaged_rest_pose(), 0.0)
 
         if self._anim is not None:
-            await self._tick_anim(loop, now)
+            await self._tick_anim(loop, now, dt)
             return
 
         # Idle reconciliation when disengaged and not frozen.

esp32_motion_controller/session.py

@@ -1,5 +1,5 @@
 """
-UDP session ingress for protocol v3.
+UDP session ingress for protocol v4.
 
 Validates datagrams, keeps only the latest sample, tracks the peer by
 address + last_rx. Never calls the robot SDK.
@@ -16,6 +16,7 @@ from typing import Any, Awaitable, Callable
 
 from esp32_motion_controller.protocol import (
     LINK_STALE_SEC,
+    PROTOCOL_VERSION,
     Hello,
     LinkDiag,
     ProtocolError,
@@ -139,7 +140,7 @@ class SessionHub:
     async def _maybe_reseed(self, boot_id: str, hello: Hello | None) -> None:
         if self.on_hello is None:
             return
-        payload = hello or Hello(protocol_version=3, boot_id=boot_id, device="esp32")
+        payload = hello or Hello(protocol_version=PROTOCOL_VERSION, boot_id=boot_id, device="esp32")
         await self.on_hello(payload)
 
     def _reset_fields(self, boot_id: str, op: int | None) -> tuple[int | None, str | None]:

esp32_motion_controller/static/index.html

@@ -12,7 +12,7 @@
 </head>
 <body>
   <h1>🕹️ ESP32 Motion Controller</h1>
-  <p>ESP32 handheld IMU controller bridge for Reachy Mini (protocol v3).</p>
+  <p>ESP32 handheld IMU controller bridge for Reachy Mini (protocol v4).</p>
   <p>UDP: <code id="udp">…:8766</code></p>
   <p><a href="/api/info">/api/info</a> · <a href="/api/status">/api/status</a></p>
   <script>

pyproject.toml

@@ -4,7 +4,7 @@ build-backend = "setuptools.build_meta"
 
 [project]
 name = "esp32_motion_controller"
-version = "3.0.1"
+version = "3.1.6"
 description = "ESP32 Motion Controller — handheld IMU controller for Reachy Mini"
 readme = "README.md"
 requires-python = ">=3.10"