Upload folder using huggingface_hub

README.md

@@ -10,15 +10,15 @@ tags:
   - reachy_mini
   - reachy_mini_python_app
   - esp32
-  - websocket
+  - udp
   - imu
 ---
 
 # ESP32 Motion Controller
 
-WebSocket bridge (protocol v2) 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.
+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.
 
-See the [repository README](https://github.com/V4C38/esp32-reachy-mini-controller) and [`PROTOCOL.md`](../PROTOCOL.md). Firmware and app must both speak protocol v2.
+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.
 
 ## Local install
 
@@ -27,8 +27,7 @@ pip install -e .
 python -m esp32_motion_controller.main
 ```
 
-WebSocket: `ws://<host>:8766/ws`
-
+UDP: `<host>:8766`
 
 Live Space: https://huggingface.co/spaces/V4C38/esp32_motion_controller
 

esp32_motion_controller/__init__.py

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

esp32_motion_controller/behavior.py

@@ -1,100 +0,0 @@
-"""
-Antenna idle animation and threshold body-follow yaw.
-
-Body holds until |head_yaw - body_yaw| exceeds BODY_FOLLOW_THRESHOLD, then
-catches up on the excess only. IMU / clutch rotation always drives the head;
-body is irrelevant below that threshold. MAX_HEAD_YAW remains the hard neck
-limit used by the movement clamp.
-
-Antenna dualSine ported from lens-studio RobotDriver.ts.
-Owns body_yaw and antennas only — never head pose axes.
-"""
-
-from __future__ import annotations
-
-import math
-import time
-
-MAX_HEAD_YAW = 65.0 * math.pi / 180.0
-# Start body catch-up before the hard neck limit so torso rotates sooner.
-BODY_FOLLOW_THRESHOLD = 40.0 * math.pi / 180.0
-MAX_BODY_YAW = 160.0 * math.pi / 180.0
-MAX_HEAD_YAW_ABSOLUTE = math.pi
-
-# Puppeteer-like liveliness defaults
-DEFAULT_LIVELINESS = 1.25
-DEFAULT_GAZE_RESPONSIVENESS = 1.2
-DEFAULT_ANTENNA_ACTIVITY = 0.8
-HEAD_MOVE_SPEED = 0.06
-MAX_HEAD_DELTA_DEG = 2.0
-ANTENNA_AMPLITUDE_DEG = 15.0
-
-
-def dual_sine(t: float, freq_a: float, freq_b: float) -> float:
-    return math.sin(t * freq_a) * 0.6 + math.sin(t * freq_b) * 0.4
-
-
-def _clamp(value: float, lo: float, hi: float) -> float:
-    return max(lo, min(hi, value))
-
-
-def _dampen(delta: float, max_delta: float) -> float:
-    return _clamp(delta, -max_delta, max_delta)
-
-
-class Behavior:
-    """Derives body_yaw and antennas from head yaw + time."""
-
-    def __init__(
-        self,
-        liveliness: float = DEFAULT_LIVELINESS,
-        gaze_responsiveness: float = DEFAULT_GAZE_RESPONSIVENESS,
-        antenna_activity: float = DEFAULT_ANTENNA_ACTIVITY,
-    ) -> None:
-        self.liveliness = liveliness
-        self.gaze_responsiveness = gaze_responsiveness
-        self.antenna_activity = antenna_activity
-        self.body_yaw = 0.0
-        self.antenna_left = 0.0
-        self.antenna_right = 0.0
-        self._t0 = time.monotonic()
-
-    def reset(self) -> None:
-        self.body_yaw = 0.0
-        self.antenna_left = 0.0
-        self.antenna_right = 0.0
-        self._t0 = time.monotonic()
-
-    def update(self, head_yaw: float) -> tuple[float, list[float]]:
-        """Advance one tick. Returns (body_yaw, [left, right] antennas)."""
-        now = time.monotonic() - self._t0
-        deg = math.pi / 180.0
-
-        yaw_smoothing = HEAD_MOVE_SPEED * self.gaze_responsiveness
-        max_yaw_delta = MAX_HEAD_DELTA_DEG * self.gaze_responsiveness * deg
-        body_smoothing = yaw_smoothing * 0.7 * (0.3 + self.liveliness * 0.4)
-        antenna_smoothing = yaw_smoothing * 1.5
-        effective_ant_amp = ANTENNA_AMPLITUDE_DEG * self.antenna_activity * deg
-        ant_speed = 0.5 + self.antenna_activity * 0.5
-
-        # Body follows only when head exceeds the follow threshold
-        rel_yaw = head_yaw - self.body_yaw
-        if abs(rel_yaw) > BODY_FOLLOW_THRESHOLD:
-            excess = abs(rel_yaw) - BODY_FOLLOW_THRESHOLD
-            step = math.copysign(excess * body_smoothing * 8, rel_yaw)
-            self.body_yaw += _dampen(step, max_yaw_delta)
-            self.body_yaw = _clamp(self.body_yaw, -MAX_BODY_YAW, MAX_BODY_YAW)
-
-        # Antennas
-        desired_l = dual_sine(now * ant_speed, 1.3, 3.11) * effective_ant_amp
-        desired_r = dual_sine(now * ant_speed, 1.7, 2.73) * effective_ant_amp
-        self.antenna_left += (desired_l - self.antenna_left) * antenna_smoothing
-        self.antenna_right += (desired_r - self.antenna_right) * antenna_smoothing
-
-        return self.body_yaw, [self.antenna_left, self.antenna_right]
-
-
-def clamp_head_yaw_for_body(head_yaw: float, body_yaw: float) -> float:
-    """Optional helper: clamp absolute head yaw range after body follow."""
-    max_range = min(MAX_BODY_YAW + MAX_HEAD_YAW, MAX_HEAD_YAW_ABSOLUTE)
-    return _clamp(head_yaw, -max_range, max_range)

esp32_motion_controller/control.py

@@ -1,5 +1,5 @@
 """
-Pure control reducer for protocol v2.
+Pure control reducer for protocol v3.
 
 Owns clutch mapping, body follow, antenna phase, workspace projection, smoothing,
 stale release, and slew limiting. No I/O.
@@ -32,7 +32,16 @@ DEV_TO_HEAD = np.array(
 TRANSLATION_SCALE = 0.30
 TRANSLATION_GAIN_DEFAULT = 1.0
 
-STALE_PACKET_SEC = 0.300
+# 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
 
@@ -40,8 +49,9 @@ CONTROL_DT = 1.0 / CONTROL_HZ
 POSE_TAU_SEC = 0.255
 ANTENNA_TAU_SEC = 0.39
 
-MAX_ANGULAR_VEL = 1.5  # rad/s
-MAX_POS_VEL = 0.05  # m/s
+# Hard SDK-boundary speed lock. Per-axis rotation / wrap-safe body yaw / 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
 
 LIMIT_BODY_YAW_RAD = 160.0 * math.pi / 180.0
@@ -75,6 +85,13 @@ ANTENNA_AMPLITUDE_DEG = 15.0
 IK_FAIL_RETRACT_TARGET_ALPHA = 0.06
 IK_FAIL_CONSECUTIVE_THRESHOLD = 3
 
+# Disengaged rest: 5 cm down from reset, slight nod.
+DISENGAGED_Z = -0.050
+DISENGAGED_PITCH = math.radians(5.0)
+
+NEAR_POSE_EPS_M = 0.005
+NEAR_POSE_EPS_RAD = math.radians(3.0)
+
 Mode = str  # idle | engaged | resetting | fault
 
 
@@ -82,6 +99,13 @@ def zero_pose() -> dict[str, float]:
     return {k: 0.0 for k in POSE_AXES}
 
 
+def disengaged_rest_pose() -> dict[str, float]:
+    pose = zero_pose()
+    pose["z"] = DISENGAGED_Z
+    pose["pitch"] = DISENGAGED_PITCH
+    return pose
+
+
 def _clamp(value: float, lo: float, hi: float) -> float:
     return max(lo, min(hi, value))
 
@@ -104,16 +128,39 @@ def _wxyz_to_rotation(q: np.ndarray) -> R:
     return R.from_quat([q[1], q[2], q[3], q[0]])
 
 
-def quat_relative_rpy(q_ref: np.ndarray, q_device: np.ndarray) -> tuple[float, float, float]:
+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)
-    r_rel_dev = r_ref.inv() * r_dev
+    return r_ref.inv() * r_dev
+
+
+def _device_rotation_to_head_rpy(r_rel_dev: R) -> tuple[float, float, float]:
     m = R.from_matrix(DEV_TO_HEAD)
     r_head = m * r_rel_dev * m.inv()
     roll, pitch, yaw = r_head.as_euler("xyz", degrees=False)
     return float(roll), float(pitch), float(yaw)
 
 
+def quat_relative_rpy(q_ref: np.ndarray, q_device: np.ndarray) -> tuple[float, float, float]:
+    r_rel_dev = _relative_device_rotation(q_ref, q_device)
+    return _device_rotation_to_head_rpy(r_rel_dev)
+
+
+def scale_device_rotation(r_rel_dev: R) -> R:
+    """Scale clutch-relative rotation about each ESP32 body axis."""
+    rv = r_rel_dev.as_rotvec()
+    rv[0] *= FORWARD_GAIN
+    rv[1] *= HORIZONTAL_GAIN
+    rv[2] *= SIDEWAYS_GAIN
+    return R.from_rotvec(rv)
+
+
+def relative_head_rpy(q_ref: np.ndarray, q_device: np.ndarray) -> tuple[float, float, float]:
+    """Device-to-head rpy with per-axis physical-motion gains."""
+    r_rel_dev = scale_device_rotation(_relative_device_rotation(q_ref, q_device))
+    return _device_rotation_to_head_rpy(r_rel_dev)
+
+
 def remap_displacement(
     p_world_delta: np.ndarray,
     q_ref: np.ndarray,
@@ -163,17 +210,21 @@ def clamp_stewart_ellipsoid(
 
 
 def clamp_pose_to_daemon_limits(
-    pose: dict[str, float], body_yaw: float
+    pose: dict[str, float],
+    body_yaw: float,
+    *,
+    apply_ellipsoid: bool = True,
 ) -> tuple[dict[str, float], float]:
     out_pose = dict(pose)
-    cx, cy, cz, cr, cp = clamp_stewart_ellipsoid(
-        pose["x"], pose["y"], pose["z"], pose["roll"], pose["pitch"],
-    )
-    out_pose["x"] = cx
-    out_pose["y"] = cy
-    out_pose["z"] = cz
-    out_pose["roll"] = cr
-    out_pose["pitch"] = cp
+    if apply_ellipsoid:
+        cx, cy, cz, cr, cp = clamp_stewart_ellipsoid(
+            pose["x"], pose["y"], pose["z"], pose["roll"], pose["pitch"],
+        )
+        out_pose["x"] = cx
+        out_pose["y"] = cy
+        out_pose["z"] = cz
+        out_pose["roll"] = cr
+        out_pose["pitch"] = cp
 
     body_yaw_clamped = _clamp(body_yaw, -LIMIT_BODY_YAW_RAD, LIMIT_BODY_YAW_RAD)
     out_pose["yaw"] = _clamp(pose["yaw"], -LIMIT_HEAD_YAW_RAD, LIMIT_HEAD_YAW_RAD)
@@ -191,26 +242,110 @@ def _alpha(dt: float, tau: float) -> float:
     return 1.0 - math.exp(-max(dt, 0.0) / tau)
 
 
-def slew_limit(
+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
+    )
+
+
+def speed_lock(
     baseline: dict[str, float],
     baseline_body: float,
     desired: dict[str, float],
     desired_body: float,
     dt: float,
+    *,
+    apply_ellipsoid: bool = True,
 ) -> 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
+    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
-    send = {}
-    for axis in ANGULAR_AXES:
-        delta = desired[axis] - baseline[axis]
-        send[axis] = baseline[axis] + _clamp(delta, -max_d_ang, max_d_ang)
-    for axis in POSITIONAL_AXES:
+    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)
+    dist = float(np.linalg.norm(dp))
+    if dist > max_d_pos and dist > 0.0:
+        scale = max_d_pos / dist
+        for i, axis in enumerate(POSITIONAL_AXES):
+            send[axis] = baseline[axis] + float(dp[i]) * scale
+    else:
+        for axis in POSITIONAL_AXES:
+            send[axis] = desired[axis]
+
+    for axis in ("roll", "pitch"):
         delta = desired[axis] - baseline[axis]
-        send[axis] = baseline[axis] + _clamp(delta, -max_d_pos, max_d_pos)
-    body_delta = desired_body - baseline_body
-    send_body = baseline_body + _clamp(body_delta, -max_d_ang, max_d_ang)
-    return clamp_pose_to_daemon_limits(send, send_body)
+        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"])
+    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)
+    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)
+
+
+def slew_limit(
+    baseline: dict[str, float],
+    baseline_body: float,
+    desired: dict[str, float],
+    desired_body: float,
+    dt: float,
+    *,
+    apply_ellipsoid: bool = True,
+) -> tuple[dict[str, float], float]:
+    return speed_lock(
+        baseline,
+        baseline_body,
+        desired,
+        desired_body,
+        dt,
+        apply_ellipsoid=apply_ellipsoid,
+    )
+
+
+def pose_travel(
+    from_pose: dict[str, float],
+    to_pose: dict[str, float],
+    from_body: float,
+    to_body: float,
+) -> tuple[float, float]:
+    """Return (euclidean_m, geodesic_plus_body_rad) between two poses."""
+    dp = np.array(
+        [to_pose[k] - from_pose[k] for k in POSITIONAL_AXES], dtype=np.float64
+    )
+    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))
+    return dist, ang
+
+
+def pose_near(
+    from_pose: dict[str, float],
+    to_pose: dict[str, float],
+    from_body: float = 0.0,
+    to_body: float = 0.0,
+) -> bool:
+    dist, ang = pose_travel(from_pose, to_pose, from_body, to_body)
+    return dist < NEAR_POSE_EPS_M and ang < NEAR_POSE_EPS_RAD
 
 
 @dataclass
@@ -285,11 +420,14 @@ def seed_from_pose(
     body_yaw: float,
     *,
     antennas: Sequence[float] | None = None,
+    apply_ellipsoid: bool = True,
 ) -> ControlState:
     ants = list(antennas) if antennas is not None else [0.0, 0.0]
     ants = (ants + [0.0, 0.0])[:2]
     pose_c, body_c = clamp_pose_to_daemon_limits(
-        {k: float(pose.get(k, 0.0)) for k in POSE_AXES}, float(body_yaw)
+        {k: float(pose.get(k, 0.0)) for k in POSE_AXES},
+        float(body_yaw),
+        apply_ellipsoid=apply_ellipsoid,
     )
     return replace(
         state,
@@ -316,16 +454,38 @@ def seed_from_pose(
 
 def rebase_neutral(state: ControlState, *, measured_baseline: dict[str, float] | None,
                    measured_body: float | None) -> ControlState:
-    """After reset completion: targets at neutral; baseline from measured pose if provided."""
-    neutral = zero_pose()
+    """After appear / settings reset: targets at neutral; baseline from measured pose."""
+    return rebase_to_pose(
+        state,
+        target=zero_pose(),
+        target_body=0.0,
+        measured_baseline=measured_baseline,
+        measured_body=measured_body,
+    )
+
+
+def rebase_to_pose(
+    state: ControlState,
+    *,
+    target: dict[str, float],
+    target_body: float,
+    measured_baseline: dict[str, float] | None,
+    measured_body: float | None,
+) -> ControlState:
+    """After exclusive goto: hold `target`; baseline from measured pose if provided."""
+    target_c, body_c = clamp_pose_to_daemon_limits(
+        {k: float(target.get(k, 0.0)) for k in POSE_AXES},
+        float(target_body),
+        apply_ellipsoid=False,
+    )
     if measured_baseline is None or measured_body is None:
         return replace(
             state,
-            base_pose=dict(neutral),
-            desired_pose=dict(neutral),
-            smooth_pose=dict(neutral),
-            body_yaw=0.0,
-            smooth_body_yaw=0.0,
+            base_pose=dict(target_c),
+            desired_pose=dict(target_c),
+            smooth_pose=dict(target_c),
+            body_yaw=body_c,
+            smooth_body_yaw=body_c,
             antenna_left=0.0,
             antenna_right=0.0,
             smooth_antennas=[0.0, 0.0],
@@ -338,16 +498,18 @@ def rebase_neutral(state: ControlState, *, measured_baseline: dict[str, float] |
             error="robot pose unread after reset",
             behavior_t0=state.behavior_t0,
         )
-    pose_c, body_c = clamp_pose_to_daemon_limits(measured_baseline, measured_body)
+    pose_c, meas_body = clamp_pose_to_daemon_limits(
+        measured_baseline, measured_body, apply_ellipsoid=False
+    )
     return replace(
         state,
-        base_pose=dict(neutral),
-        desired_pose=dict(neutral),
-        smooth_pose=dict(neutral),
+        base_pose=dict(target_c),
+        desired_pose=dict(target_c),
+        smooth_pose=dict(target_c),
         baseline_pose=dict(pose_c),
-        body_yaw=0.0,
-        smooth_body_yaw=0.0,
-        baseline_body_yaw=body_c,
+        body_yaw=body_c,
+        smooth_body_yaw=body_c,
+        baseline_body_yaw=meas_body,
         antenna_left=0.0,
         antenna_right=0.0,
         smooth_antennas=[0.0, 0.0],
@@ -453,7 +615,7 @@ def _update_clutch(state: ControlState, sample: Sample, *, allow_engage: bool) -
         p_ref = p_dev.copy()
 
     if want:
-        roll, pitch, yaw = quat_relative_rpy(q_ref, q_dev)
+        roll, pitch, yaw = relative_head_rpy(q_ref, q_dev)
         disp = remap_displacement(
             p_dev - p_ref,
             q_ref,
@@ -524,11 +686,13 @@ def step(
     dt: float,
     sample: Sample | None,
     sample_is_fresh: bool,
+    controller_present: bool = True,
 ) -> StepResult:
     """Advance one control tick.
 
     `sample` is the latest validated sample (may be None before first packet).
     `sample_is_fresh` is True only when a new sample arrived since the previous tick.
+    Age-based stale release runs only when the controller socket is down.
     """
     prev_mode = state.mode
     st = state
@@ -537,10 +701,14 @@ def step(
         # No streaming commands while reset owns the robot.
         return StepResult(state=st, command=None, mode_changed=False)
 
-    # Stale detection uses host receipt time stamped on the sample mailbox.
-    if st.have_sample and st.last_sample_time > 0:
-        if now - st.last_sample_time > STALE_PACKET_SEC and st.engaged:
-            st = force_disengage(st)
+    if (
+        not controller_present
+        and st.have_sample
+        and st.last_sample_time > 0
+        and now - st.last_sample_time > STALE_PACKET_SEC
+        and st.engaged
+    ):
+        st = force_disengage(st)
 
     if sample is not None and sample_is_fresh and st.mode != "resetting":
         allow_engage = st.mode != "fault" and not st.sends_frozen
@@ -550,11 +718,16 @@ def step(
     if st.mode not in {"resetting"}:
         st = _advance_behavior(st, st.desired_pose["yaw"], now, dt)
 
-    target_pose, target_body = clamp_pose_to_daemon_limits(st.desired_pose, st.body_yaw)
+    apply_ellipsoid = bool(st.engaged)
+    target_pose, target_body = clamp_pose_to_daemon_limits(
+        st.desired_pose, st.body_yaw, apply_ellipsoid=apply_ellipsoid
+    )
     target_ants = [st.antenna_left, st.antenna_right]
 
     # Elapsed-time-normalized smoothing (replaces fixed 30 Hz POSE_ALPHA).
-    a_pose = _alpha(dt, POSE_TAU_SEC)
+    # Engaged streaming skips pose/body EMA so a new tilt is applied this tick;
+    # the per-axis speed lock is the snap guard.
+    a_pose = 1.0 if st.engaged else _alpha(dt, POSE_TAU_SEC)
     a_ant = _alpha(dt, ANTENNA_TAU_SEC)
     smooth = {
         k: st.smooth_pose[k] + a_pose * (target_pose[k] - st.smooth_pose[k])
@@ -587,12 +760,13 @@ def step(
             mode_changed=(st.mode != prev_mode),
         )
 
-    send_pose, send_body = slew_limit(
+    send_pose, send_body = speed_lock(
         st.baseline_pose,
         st.baseline_body_yaw,
         smooth,
         smooth_body,
         dt,
+        apply_ellipsoid=apply_ellipsoid,
     )
     command = Command(
         pose=send_pose,

esp32_motion_controller/controller_state.py

@@ -1,197 +0,0 @@
-"""
-Clutch mapping: relative orientation + displacement from the ESP32 controller.
-
-Owns the engage rising-edge reference and the desired head pose (x/y/z/rpy).
-Does not write body_yaw or antennas.
-
-Frames
-------
-Device (after IMU_MAP_*): X right, Y up, Z out of screen (screen = Reachy face).
-Head:                     x forward, y left, z up.
-
-DEV_TO_HEAD maps device vectors onto head vectors (screen↔face, up↔up):
-  head_x = device_z, head_y = device_x, head_z = device_y
-
-`p` arrives in the gravity-aligned world frame. On engage we rotate the
-world-frame delta into the clutch reference (device frame at engage), then
-apply DEV_TO_HEAD.
-"""
-
-from __future__ import annotations
-
-import math
-from typing import Sequence
-
-import numpy as np
-from scipy.spatial.transform import Rotation as R
-
-POSE_AXES = ("x", "y", "z", "roll", "pitch", "yaw")
-
-# Device → head: face with face, up with up (det = +1).
-# head_x = dev_z (face), head_y = dev_x (screen's left), head_z = dev_y (up)
-DEV_TO_HEAD = np.array(
-    [
-        [0.0, 0.0, 1.0],
-        [1.0, 0.0, 0.0],
-        [0.0, 1.0, 0.0],
-    ],
-    dtype=np.float64,
-)
-
-# Hand travel (metres) → head travel: ~50 mm of hand maps onto ~15 mm of head
-# at translation_gain = 1.0, so a comfortable push fills most of the workspace.
-TRANSLATION_SCALE = 0.30  # base m_head / m_hand before the UI gain
-TRANSLATION_GAIN_DEFAULT = 1.0
-
-
-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:
-        return np.array([1.0, 0.0, 0.0, 0.0], dtype=np.float64)
-    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:
-    # scipy uses [x, y, z, w]; wire format is [w, x, y, z]
-    return R.from_quat([q[1], q[2], q[3], q[0]])
-
-
-def quat_relative_rpy(q_ref: np.ndarray, q_device: np.ndarray) -> tuple[float, float, float]:
-    """Return head (roll, pitch, yaw) of the relative device rotation.
-
-    Applies the similarity transform M * R_rel_dev * M^{-1} so that a
-    physical tip/turn/roll of the board becomes the matching head RPY.
-    Euler order is extrinsic xyz, matching create_head_pose.
-    """
-    r_ref = _wxyz_to_rotation(q_ref)
-    r_dev = _wxyz_to_rotation(q_device)
-    r_rel_dev = r_ref.inv() * r_dev
-    m = R.from_matrix(DEV_TO_HEAD)
-    r_head = m * r_rel_dev * m.inv()
-    roll, pitch, yaw = r_head.as_euler("xyz", degrees=False)
-    return float(roll), float(pitch), float(yaw)
-
-
-def remap_displacement(
-    p_world_delta: np.ndarray,
-    q_ref: np.ndarray,
-    *,
-    translation_gain: float = TRANSLATION_GAIN_DEFAULT,
-) -> np.ndarray:
-    """Map world-frame displacement delta to head-frame metres.
-
-    Rotates into the engage reference (device frame at clutch), then applies
-    DEV_TO_HEAD, then scales by TRANSLATION_SCALE * translation_gain.
-    """
-    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)
-
-
-class ControllerState:
-    """Clutch state machine for one ESP32 controller."""
-
-    def __init__(
-        self,
-        *,
-        translation_gain: float = TRANSLATION_GAIN_DEFAULT,
-    ) -> None:
-        self.engaged = False
-        self.gain = 1.0
-        self.translation_gain = float(translation_gain)
-        self.ready = False
-        self._q_ref = np.array([1.0, 0.0, 0.0, 0.0], dtype=np.float64)
-        self._p_ref = np.zeros(3, dtype=np.float64)
-        self._base_pose = {k: 0.0 for k in POSE_AXES}
-        self._desired = {k: 0.0 for k in POSE_AXES}
-        self._was_engaged = False
-
-    @property
-    def desired_pose(self) -> dict[str, float]:
-        return dict(self._desired)
-
-    @property
-    def base_pose(self) -> dict[str, float]:
-        return dict(self._base_pose)
-
-    def set_base_pose(self, pose: dict[str, float]) -> None:
-        self._base_pose = {k: float(pose.get(k, 0.0)) for k in POSE_AXES}
-        if not self.engaged:
-            self._desired = dict(self._base_pose)
-
-    def rebase_neutral(self) -> None:
-        self._base_pose = {k: 0.0 for k in POSE_AXES}
-        self._desired = {k: 0.0 for k in POSE_AXES}
-        self.engaged = False
-        self._was_engaged = False
-        self._q_ref = np.array([1.0, 0.0, 0.0, 0.0], dtype=np.float64)
-        self._p_ref = np.zeros(3, dtype=np.float64)
-
-    def update(
-        self,
-        *,
-        q: Sequence[float],
-        p: Sequence[float],
-        engaged: bool,
-        gain: float,
-        ready: bool,
-        allow_engage: bool = True,
-    ) -> dict[str, float]:
-        """Ingest one controller_state packet. Returns desired head pose."""
-        self.ready = bool(ready)
-        self.gain = float(gain) if math.isfinite(float(gain)) else self.gain
-        self.gain = max(0.1, min(3.0, self.gain))
-
-        q_dev = _finite_quat(q)
-        p_dev = _finite_vec3(p)
-
-        want_engage = bool(engaged) and self.ready and allow_engage
-        rising = want_engage and not self._was_engaged
-        falling = (not want_engage) and self._was_engaged
-
-        if rising:
-            self._q_ref = q_dev.copy()
-            self._p_ref = p_dev.copy()
-            # base already holds the committed pose from last release / reset
-
-        self.engaged = want_engage
-        self._was_engaged = want_engage
-
-        if self.engaged:
-            roll, pitch, yaw = quat_relative_rpy(self._q_ref, q_dev)
-            # Translation uses its own gain (and TRANSLATION_SCALE); rotation
-            # uses the UI gain. Both multiply the UI gain so the slider still
-            # scales the whole motion feel.
-            disp = remap_displacement(
-                p_dev - self._p_ref,
-                self._q_ref,
-                translation_gain=self.translation_gain * self.gain,
-            )
-            self._desired = {
-                "x": self._base_pose["x"] + float(disp[0]),
-                "y": self._base_pose["y"] + float(disp[1]),
-                "z": self._base_pose["z"] + float(disp[2]),
-                "roll": self._base_pose["roll"] + self.gain * roll,
-                "pitch": self._base_pose["pitch"] + self.gain * pitch,
-                "yaw": self._base_pose["yaw"] + self.gain * yaw,
-            }
-        elif falling:
-            # Commit both rotation and translation into the base on release
-            self._base_pose = dict(self._desired)
-        # else idle: hold last desired / base
-
-        return dict(self._desired)
-
-    def force_disengage(self) -> None:
-        if self.engaged:
-            self._base_pose = dict(self._desired)
-        self.engaged = False
-        self._was_engaged = False

esp32_motion_controller/main.py

@@ -1,12 +1,14 @@
 """
-Motion Controller Reachy Mini app entry point (protocol v2).
+Motion Controller Reachy Mini app entry point (protocol v3).
 
-FastAPI/uvicorn on port 8766, mDNS advertise _reachyctl._tcp, clutch + safety bridge.
+FastAPI/uvicorn HTTP on TCP 8766, UDP datagrams on UDP 8766,
+mDNS advertise _reachyctl._tcp, clutch + safety bridge.
 """
 
 from __future__ import annotations
 
 import argparse
+import asyncio
 import logging
 import socket
 import sys
@@ -15,19 +17,22 @@ import time
 from pathlib import Path
 
 import uvicorn
-from fastapi import FastAPI, WebSocket, WebSocketDisconnect
+from fastapi import FastAPI
 from fastapi.responses import FileResponse, JSONResponse
 from fastapi.staticfiles import StaticFiles
 
+from esp32_motion_controller.protocol import PROTOCOL_VERSION
 from esp32_motion_controller.robot_control import RobotControl, RobotGateway
 from esp32_motion_controller.session import SessionHub
+from esp32_motion_controller import __version__
 
 logger = logging.getLogger(__name__)
 
-WS_PORT = 8766
+LINK_PORT = 8766
 STATIC_DIR = Path(__file__).parent / "static"
 MDNS_SERVICE_TYPE = "_reachyctl._tcp.local."
 SERVER_START_TIMEOUT_S = 10.0
+UDP_PROBE = b"RMC2?"
 
 
 def get_local_ips() -> list[str]:
@@ -49,8 +54,40 @@ def get_local_ips() -> list[str]:
     return list(dict.fromkeys(ips))
 
 
+class ControllerProtocol(asyncio.DatagramProtocol):
+    """One UDP socket: discovery probes + protocol v3 datagrams."""
+
+    def __init__(self, session: SessionHub) -> None:
+        self.session = session
+        self.transport: asyncio.DatagramTransport | None = None
+
+    def connection_made(self, transport: asyncio.BaseTransport) -> None:
+        self.transport = transport  # type: ignore[assignment]
+        self.session.bind_transport(transport)
+
+    def datagram_received(self, data: bytes, addr: tuple[str, int]) -> None:
+        if data.startswith(UDP_PROBE):
+            self._reply_probe(addr)
+            return
+        asyncio.create_task(self.session.handle_datagram(data, addr))
+
+    def error_received(self, exc: Exception) -> None:
+        logger.warning("UDP error: %s", exc)
+
+    def _reply_probe(self, addr: tuple[str, int]) -> None:
+        if self.transport is None:
+            return
+        ips = get_local_ips()
+        if not ips:
+            return
+        try:
+            self.transport.sendto(f"RMC2 {ips[0]} {LINK_PORT}".encode(), addr)
+        except OSError:
+            pass
+
+
 class MdnsAdvertiser:
-    def __init__(self, port: int = WS_PORT) -> None:
+    def __init__(self, port: int = LINK_PORT) -> None:
         self.port = port
         self._zc = None
         self._info = None
@@ -74,7 +111,7 @@ class MdnsAdvertiser:
                 f"esp32-motion-controller.{MDNS_SERVICE_TYPE}",
                 addresses=[socket.inet_aton(ip) for ip in ips],
                 port=self.port,
-                properties={"path": b"/ws", "protocol": b"2"},
+                properties={"protocol": b"3", "transport": b"udp"},
                 server="esp32-motion.local.",
             )
             zc = Zeroconf()
@@ -126,32 +163,27 @@ def create_app(
         robot_available=robot_available,
         log_only=log_only,
     )
+    session.on_hello = control.on_controller_hello
     app.state.session = session
     app.state.control = control
 
     @app.on_event("startup")
     async def _startup() -> None:
+        loop = asyncio.get_running_loop()
+        transport, _protocol = await loop.create_datagram_endpoint(
+            lambda: ControllerProtocol(session),
+            local_addr=("0.0.0.0", LINK_PORT),
+        )
+        app.state.udp_transport = transport
+        logger.info("UDP link listening on 0.0.0.0:%d", LINK_PORT)
         control.start()
 
     @app.on_event("shutdown")
     async def _shutdown() -> None:
         await control.stop()
-
-    @app.websocket("/ws")
-    async def websocket_endpoint(websocket: WebSocket) -> None:
-        generation = await session.on_connect(websocket)
-        await control.on_controller_connected()
-        try:
-            while not stop_event.is_set():
-                raw = await websocket.receive_text()
-                await session.handle_message(websocket, generation, raw)
-        except WebSocketDisconnect:
-            logger.info("WebSocket disconnect")
-        except Exception as exc:
-            logger.error("WebSocket error: %s", exc)
-        finally:
-            await session.cleanup(websocket, generation)
-            await control.on_controller_disconnected()
+        transport = getattr(app.state, "udp_transport", None)
+        if transport is not None:
+            transport.close()
 
     @app.get("/api/info")
     async def info() -> JSONResponse:
@@ -159,10 +191,10 @@ def create_app(
         return JSONResponse(
             {
                 "ips": ips,
-                "port": WS_PORT,
-                "ws_url": f"ws://{ips[0]}:{WS_PORT}/ws" if ips else None,
+                "port": LINK_PORT,
+                "udp": f"{ips[0]}:{LINK_PORT}" if ips else None,
                 "mdns": MDNS_SERVICE_TYPE,
-                "protocol_version": 2,
+                "protocol_version": PROTOCOL_VERSION,
             }
         )
 
@@ -177,7 +209,19 @@ def create_app(
                 "mode": snap["mode"],
                 "connected": snap["connected"],
                 "log_only": log_only,
-                "protocol_version": 2,
+                "protocol_version": PROTOCOL_VERSION,
+                "app_version": __version__,
+                "boot_id": snap["boot_id"],
+                "peer": snap["peer"],
+                "presents": snap["presents"],
+                "absents": snap["absents"],
+                "last_seq": snap["last_seq"],
+                "seq_skips": snap["seq_skips"],
+                "sample_gaps": snap["sample_gaps"],
+                "last_rx_age_ms": snap["last_rx_age_ms"],
+                "last_diag": snap["last_diag"],
+                "max_tick_lag_ms": snap["max_tick_lag_ms"],
+                "last_sdk_ms": snap["last_sdk_ms"],
             }
         )
 
@@ -192,16 +236,21 @@ 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 v2)")
+    logger.info("ESP32 Motion Controller (protocol v3) app_version=%s", __version__)
     logger.info("=" * 50)
     for ip in ips:
-        logger.info("  WebSocket: ws://%s:%d/ws", ip, WS_PORT)
+        logger.info("  UDP: %s:%d", ip, LINK_PORT)
     if log_only:
         logger.info("  Mode: --log-only (no robot SDK calls)")
     logger.info("=" * 50)
 
     app = create_app(reachy_mini, stop_event, log_only=log_only)
-    config = uvicorn.Config(app, host="0.0.0.0", port=WS_PORT, log_level="info")
+    config = uvicorn.Config(
+        app,
+        host="0.0.0.0",
+        port=LINK_PORT,
+        log_level="info",
+    )
     server = uvicorn.Server(config)
     thread = threading.Thread(target=server.run, daemon=True)
     thread.start()
@@ -211,15 +260,14 @@ def _run_server(reachy_mini, stop_event: threading.Event, *, log_only: bool) ->
         time.sleep(0.05)
     if not server.started:
         raise RuntimeError(
-            f"Could not serve on port {WS_PORT} — another Motion Controller "
+            f"Could not serve on port {LINK_PORT} — another Motion Controller "
             f"instance is probably already running"
         )
 
-    mdns = MdnsAdvertiser(WS_PORT)
+    mdns = MdnsAdvertiser(LINK_PORT)
     mdns.start()
 
     stop_event.wait()
-    # Shutdown path: uvicorn will fire FastAPI shutdown hooks.
     server.should_exit = True
     thread.join(timeout=5)
     mdns.stop()

esp32_motion_controller/movement_handler.py

@@ -1,596 +0,0 @@
-"""
-Movement state and robot commands: target/current LERP and set_target rate limiting.
-
-Ported from spectacles_reachy_mini.movement_handler with a five-axis Stewart
-ellipsoid that couples x/y translation into the workspace check.
-"""
-
-from __future__ import annotations
-
-import asyncio
-import logging
-import math
-import time
-import uuid as uuid_mod
-from typing import Any
-
-import numpy as np
-from reachy_mini import ReachyMini
-from reachy_mini.utils import create_head_pose
-from scipy.spatial.transform import Rotation
-
-logger = logging.getLogger(__name__)
-
-POSE_AXES = ("x", "y", "z", "roll", "pitch", "yaw")
-ANGULAR_AXES = ("roll", "pitch", "yaw")
-POSITIONAL_AXES = ("x", "y", "z")
-
-POSE_ALPHA = 0.12
-ANTENNA_ALPHA = 0.08
-# Hard safety gate (same numbers as spectacles_reachy_mini). set_target on the
-# daemon is instantaneous — these caps on consecutive sends are what keep the
-# head from whipping when bookkeeping and the robot disagree.
-MAX_ANGULAR_VEL = 1.5  # rad/s
-MAX_POS_VEL = 0.05  # m/s
-LOOP_INTERVAL = 0.033
-DEFAULT_SEND_RATE_HZ = 20.0
-SEND_RATE_HZ_MIN = 5.0
-SEND_RATE_HZ_MAX = 50.0
-MAX_DT_FOR_VEL_CLAMP = 0.05  # never allow a single step larger than 50 ms worth
-RESYNC_FROM_ROBOT_SEC = 2.0
-# Measured vs bookkeeping: within this, soft-refresh _prev_sent; beyond, hard seed.
-RESYNC_EPS_POS_M = 0.003
-RESYNC_EPS_ANG_RAD = 0.05
-
-LIMIT_BODY_YAW_RAD = 160.0 * math.pi / 180.0
-LIMIT_HEAD_YAW_RAD = math.pi
-LIMIT_HEAD_BODY_YAW_DELTA_RAD = 65.0 * math.pi / 180.0
-
-# Opened conservatively for controller translation (±20 mm).
-LIMIT_HEAD_X_MIN = -0.020
-LIMIT_HEAD_X_MAX = 0.020
-LIMIT_HEAD_Y_MIN = -0.020
-LIMIT_HEAD_Y_MAX = 0.020
-LIMIT_HEAD_Z_MIN = 0.0
-LIMIT_HEAD_Z_MAX = 0.025
-
-# Stewart workspace: motor_arm≈0.04 m, rod≈0.085 m → pitch/roll ≈ ±25°, Z ≈ ±0.03 m.
-# Rotation is clamped to its own radii first; translation then fits the remainder
-# so junk/large translation cannot scale a real tilt down.
-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
-
-IK_FAIL_RETRACT_TARGET_ALPHA = 0.06
-IK_FAIL_CONSECUTIVE_THRESHOLD = 3
-
-
-def _zero_pose() -> dict[str, float]:
-    return {k: 0.0 for k in POSE_AXES}
-
-
-def _clamp(value: float, lo: float, hi: float) -> float:
-    return max(lo, min(hi, value))
-
-
-def _parse_send_rate_hz(value: float | None) -> float:
-    if value is None:
-        return 1.0 / DEFAULT_SEND_RATE_HZ
-    rate = max(SEND_RATE_HZ_MIN, min(SEND_RATE_HZ_MAX, value))
-    return 1.0 / rate
-
-
-def _clamp_stewart_ellipsoid(
-    x: float, y: float, z: float, roll: float, pitch: float,
-) -> tuple[float, float, float, float, float]:
-    """Clamp rotation first, then fit translation into the remaining budget.
-
-    Budget: (x/X)^2 + (y/Y)^2 + (z/Z)^2 + (roll/R)^2 + (pitch/P)^2 <= 1.
-    Roll/pitch are hard-clamped to their radii (never scaled by translation).
-    Translation is then projected onto whatever budget remains.
-    """
-    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
-    rot_budget = nr * nr + np_ * np_
-    # Tiny epsilon so a full-scale tilt leaves a sliver for translation=0.
-    remaining = max(0.0, 1.0 - rot_budget)
-
-    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,
-    )
-
-
-def _clamp_pose_to_daemon_limits(
-    pose: dict[str, float], body_yaw: float
-) -> tuple[dict[str, float], float]:
-    out_pose = dict(pose)
-    cx, cy, cz, cr, cp = _clamp_stewart_ellipsoid(
-        pose["x"], pose["y"], pose["z"], pose["roll"], pose["pitch"],
-    )
-    out_pose["x"] = cx
-    out_pose["y"] = cy
-    out_pose["z"] = cz
-    out_pose["roll"] = cr
-    out_pose["pitch"] = cp
-
-    body_yaw_clamped = _clamp(body_yaw, -LIMIT_BODY_YAW_RAD, LIMIT_BODY_YAW_RAD)
-    out_pose["yaw"] = _clamp(pose["yaw"], -LIMIT_HEAD_YAW_RAD, LIMIT_HEAD_YAW_RAD)
-    delta = out_pose["yaw"] - body_yaw_clamped
-    if delta > LIMIT_HEAD_BODY_YAW_DELTA_RAD:
-        out_pose["yaw"] = body_yaw_clamped + LIMIT_HEAD_BODY_YAW_DELTA_RAD
-    elif delta < -LIMIT_HEAD_BODY_YAW_DELTA_RAD:
-        out_pose["yaw"] = body_yaw_clamped - LIMIT_HEAD_BODY_YAW_DELTA_RAD
-    return (out_pose, body_yaw_clamped)
-
-
-def _lerp(a: float, b: float, t: float) -> float:
-    return a + (b - a) * t
-
-
-class MovementHandler:
-    """Owns all movement state and SDK interaction."""
-
-    def __init__(
-        self,
-        reachy_mini: ReachyMini | None,
-        send_rate_hz: float | None = None,
-    ) -> None:
-        self.mini = reachy_mini
-        self._send_min_interval = _parse_send_rate_hz(send_rate_hz)
-        logger.info(
-            "MovementHandler send rate: %.1f Hz (interval %.3f s)",
-            1.0 / self._send_min_interval,
-            self._send_min_interval,
-        )
-
-        self._target_pose: dict[str, float] = _zero_pose()
-        self._target_body_yaw: float = 0.0
-        self._target_antennas: list[float] = [0.0, 0.0]
-
-        self._current_pose: dict[str, float] = _zero_pose()
-        self._current_body_yaw: float = 0.0
-        self._current_antennas: list[float] = [0.0, 0.0]
-
-        self._prev_sent_pose: dict[str, float] = _zero_pose()
-        self._prev_sent_body_yaw: float = 0.0
-
-        self._active_gotos: dict[str, bool] = {}
-        self._apply_task: asyncio.Task[None] | None = None
-        self._send_future: asyncio.Future[Any] | None = None
-        self._last_send_time: float = 0.0
-        self._send_count: int = 0
-        self._send_seq: int = 0
-        self._last_applied_seq: int = 0
-        self._consecutive_ik_failures: int = 0
-        self._last_resync_time: float = 0.0
-        self._seeded: bool = False
-        self._sends_frozen: bool = False
-
-    @property
-    def current_pose(self) -> dict[str, float]:
-        return dict(self._current_pose)
-
-    @property
-    def target_pose(self) -> dict[str, float]:
-        return dict(self._target_pose)
-
-    def set_target(
-        self,
-        pose: dict[str, float],
-        body_yaw: float | None = None,
-        antennas: list[float] | None = None,
-    ) -> None:
-        merged = {}
-        for k in POSE_AXES:
-            v = pose.get(k, self._target_pose[k])
-            merged[k] = (
-                v
-                if isinstance(v, (int, float)) and math.isfinite(v)
-                else self._target_pose[k]
-            )
-        by = (
-            body_yaw
-            if body_yaw is not None
-            and isinstance(body_yaw, (int, float))
-            and math.isfinite(body_yaw)
-            else self._target_body_yaw
-        )
-        self._target_pose, self._target_body_yaw = _clamp_pose_to_daemon_limits(
-            merged, by
-        )
-        if antennas is not None:
-            self._target_antennas = [
-                a
-                if isinstance(a, (int, float)) and math.isfinite(a)
-                else (self._target_antennas[i] if i < len(self._target_antennas) else 0.0)
-                for i, a in enumerate(antennas)
-            ]
-            self._target_antennas = (self._target_antennas + [0.0, 0.0])[:2]
-
-    def goto(
-        self,
-        pose: dict[str, float],
-        body_yaw: float = 0.0,
-        antennas: list[float] | None = None,
-        duration: float = 0.5,
-        interpolation: str = "minjerk",
-    ) -> str:
-        move_uuid = str(uuid_mod.uuid4())
-        self._active_gotos[move_uuid] = True
-
-        start_pose = dict(self._current_pose)
-        start_body_yaw = self._current_body_yaw
-        start_antennas = list(self._current_antennas)
-        end_pose = {k: pose.get(k, 0.0) for k in POSE_AXES}
-        end_antennas = list(antennas) if antennas else [0.0, 0.0]
-
-        asyncio.create_task(
-            self._run_goto(
-                move_uuid,
-                start_pose, end_pose,
-                start_body_yaw, body_yaw,
-                start_antennas, end_antennas,
-                duration, interpolation,
-            )
-        )
-        return move_uuid
-
-    def stop_move(self, move_uuid: str) -> bool:
-        if move_uuid in self._active_gotos:
-            self._active_gotos[move_uuid] = False
-            return True
-        return False
-
-    def rebase_to_neutral(self) -> None:
-        """Re-sync clamp reference after a reset goto; keep targets at neutral.
-
-        Refresh `_prev_sent` from the robot so the velocity gate is honest, but
-        do not copy the measured pose into targets — that would undo the goto.
-        On read failure freeze sends; never invent a zero clamp reference.
-        """
-        if not self._seed_from_robot(update_target=False):
-            logger.error("rebase_to_neutral: robot pose unread — freezing sends")
-            self._sends_frozen = True
-            return
-        for axis in POSE_AXES:
-            self._target_pose[axis] = 0.0
-            self._current_pose[axis] = 0.0
-        self._target_body_yaw = 0.0
-        self._current_body_yaw = 0.0
-        self._target_antennas = [0.0, 0.0]
-        self._current_antennas = [0.0, 0.0]
-
-    def _read_robot_pose(self) -> tuple[dict[str, float], float] | None:
-        if self.mini is None:
-            return None
-        try:
-            head = np.asarray(self.mini.get_current_head_pose(), dtype=np.float64)
-            joints, _ = self.mini.get_current_joint_positions()
-            body_yaw = float(joints[0])
-        except Exception as exc:
-            logger.warning("Could not read robot pose: %s", exc)
-            return None
-        rpy = Rotation.from_matrix(head[:3, :3]).as_euler("xyz")
-        pose = {
-            "x": float(head[0, 3]),
-            "y": float(head[1, 3]),
-            "z": float(head[2, 3]),
-            "roll": float(rpy[0]),
-            "pitch": float(rpy[1]),
-            "yaw": float(rpy[2]),
-        }
-        return pose, body_yaw
-
-    @staticmethod
-    def _pose_gap(
-        a: dict[str, float], a_yaw: float, b: dict[str, float], b_yaw: float
-    ) -> tuple[float, float]:
-        pos = math.sqrt(
-            (a["x"] - b["x"]) ** 2 + (a["y"] - b["y"]) ** 2 + (a["z"] - b["z"]) ** 2
-        )
-        ang = max(
-            abs(a["roll"] - b["roll"]),
-            abs(a["pitch"] - b["pitch"]),
-            abs(a["yaw"] - b["yaw"]),
-            abs(a_yaw - b_yaw),
-        )
-        return pos, ang
-
-    def _apply_seed_read(
-        self,
-        read: tuple[dict[str, float], float] | None,
-        *,
-        update_target: bool = True,
-    ) -> bool:
-        """Apply a pose read to bookkeeping. Returns False when read is None."""
-        if read is None:
-            self._sends_frozen = True
-            return False
-        pose, body_yaw = read
-        self._prev_sent_pose = dict(pose)
-        self._prev_sent_body_yaw = body_yaw
-        if update_target:
-            self._target_pose = dict(pose)
-            self._current_pose = dict(pose)
-            self._target_body_yaw = body_yaw
-            self._current_body_yaw = body_yaw
-            logger.info(
-                "Seeded movement state from robot: pose=%s body_yaw=%.3f",
-                {k: round(v, 4) for k, v in pose.items()},
-                body_yaw,
-            )
-        self._last_resync_time = time.monotonic()
-        self._seeded = True
-        self._sends_frozen = False
-        return True
-
-    def _seed_from_robot(self, *, update_target: bool = True) -> bool:
-        """Initialize pose bookkeeping from the robot's measured pose.
-
-        Returns False when no robot is attached or the readback failed.
-        On failure, bookkeeping is left untouched and sends freeze.
-        """
-        if self.mini is None:
-            # log-only / no robot: treat as seeded at current bookkeeping
-            self._seeded = True
-            self._sends_frozen = False
-            return True
-        return self._apply_seed_read(self._read_robot_pose(), update_target=update_target)
-
-    async def _periodic_resync(self) -> None:
-        """Refresh clamp reference only when the robot is near bookkeeping.
-
-        Pose reads go through the default executor so a USB round trip cannot
-        block the asyncio event loop (and therefore WebSocket pongs).
-        """
-        if self.mini is None:
-            self._last_resync_time = time.monotonic()
-            return
-        loop = asyncio.get_running_loop()
-        read = await loop.run_in_executor(None, self._read_robot_pose)
-        if read is None:
-            self._sends_frozen = True
-            return
-        pose, body_yaw = read
-        pos_gap, ang_gap = self._pose_gap(
-            pose, body_yaw, self._prev_sent_pose, self._prev_sent_body_yaw
-        )
-        self._last_resync_time = time.monotonic()
-        if pos_gap <= RESYNC_EPS_POS_M and ang_gap <= RESYNC_EPS_ANG_RAD:
-            self._prev_sent_pose = dict(pose)
-            self._prev_sent_body_yaw = body_yaw
-            self._sends_frozen = False
-            return
-        # Hard desync: full seed, then resume under the velocity gate.
-        logger.warning(
-            "Pose desync pos=%.4f m ang=%.3f rad — full resync",
-            pos_gap,
-            ang_gap,
-        )
-        self._prev_sent_pose = dict(pose)
-        self._prev_sent_body_yaw = body_yaw
-        self._target_pose = dict(pose)
-        self._current_pose = dict(pose)
-        self._target_body_yaw = body_yaw
-        self._current_body_yaw = body_yaw
-        self._sends_frozen = False
-
-    def resync_from_robot(self) -> bool:
-        """Public: re-align velocity clamp with the robot after a reconnect."""
-        return self._seed_from_robot(update_target=True)
-
-    async def resync_from_robot_async(self, *, update_target: bool = True) -> bool:
-        """Async resync — pose read runs in the executor so the event loop stays free."""
-        if self.mini is None:
-            self._seeded = True
-            self._sends_frozen = False
-            return True
-        loop = asyncio.get_running_loop()
-        read = await loop.run_in_executor(None, self._read_robot_pose)
-        return self._apply_seed_read(read, update_target=update_target)
-
-    def start(self) -> None:
-        if self._apply_task is None or self._apply_task.done():
-            if not self._seed_from_robot():
-                logger.error("start: robot pose unread — sends frozen until resync")
-            self._apply_task = asyncio.ensure_future(self._apply_loop())
-
-    def stop(self) -> None:
-        if self._apply_task is not None and not self._apply_task.done():
-            self._apply_task.cancel()
-            self._apply_task = None
-        for uid in list(self._active_gotos):
-            self._active_gotos[uid] = False
-        self._active_gotos.clear()
-
-    async def _apply_loop(self) -> None:
-        loop = asyncio.get_running_loop()
-        try:
-            while True:
-                now = time.monotonic()
-
-                for axis in POSE_AXES:
-                    self._current_pose[axis] += POSE_ALPHA * (
-                        self._target_pose[axis] - self._current_pose[axis]
-                    )
-                self._current_body_yaw += POSE_ALPHA * (
-                    self._target_body_yaw - self._current_body_yaw
-                )
-                for i in range(min(len(self._current_antennas), len(self._target_antennas))):
-                    self._current_antennas[i] += ANTENNA_ALPHA * (
-                        self._target_antennas[i] - self._current_antennas[i]
-                    )
-
-                interval_ok = (
-                    self._last_send_time == 0
-                    or (now - self._last_send_time) >= self._send_min_interval
-                )
-                # Serialize set_target: never dual in-flight (out-of-order
-                # done-callbacks corrupt the velocity-clamp reference).
-                previous_done = self._send_future is None or self._send_future.done()
-                can_send = (
-                    interval_ok
-                    and previous_done
-                    and not self._sends_frozen
-                    and (self._seeded or self.mini is None)
-                )
-
-                if can_send and self.mini is not None:
-                    # Hard resync overwrites targets — never fight an active goto
-                    # (reset minjerk owns the trajectory exclusively).
-                    if not self._active_gotos and (
-                        self._last_resync_time == 0.0
-                        or (now - self._last_resync_time) >= RESYNC_FROM_ROBOT_SEC
-                    ):
-                        await self._periodic_resync()
-                        if self._sends_frozen:
-                            await asyncio.sleep(LOOP_INTERVAL)
-                            continue
-
-                    dt_since_send = (
-                        now - self._last_send_time
-                        if self._last_send_time > 0
-                        else LOOP_INTERVAL
-                    )
-                    dt_clamped = min(dt_since_send, MAX_DT_FOR_VEL_CLAMP)
-                    max_d_ang = MAX_ANGULAR_VEL * dt_clamped
-                    max_d_pos = MAX_POS_VEL * dt_clamped
-
-                    send_pose: dict[str, float] = {}
-                    for axis in ANGULAR_AXES:
-                        delta = self._current_pose[axis] - self._prev_sent_pose[axis]
-                        send_pose[axis] = self._prev_sent_pose[axis] + _clamp(
-                            delta, -max_d_ang, max_d_ang
-                        )
-                    for axis in POSITIONAL_AXES:
-                        delta = self._current_pose[axis] - self._prev_sent_pose[axis]
-                        send_pose[axis] = self._prev_sent_pose[axis] + _clamp(
-                            delta, -max_d_pos, max_d_pos
-                        )
-                    body_yaw_delta = self._current_body_yaw - self._prev_sent_body_yaw
-                    send_body_yaw = self._prev_sent_body_yaw + _clamp(
-                        body_yaw_delta, -max_d_ang, max_d_ang
-                    )
-
-                    head = create_head_pose(
-                        x=send_pose["x"],
-                        y=send_pose["y"],
-                        z=send_pose["z"],
-                        roll=send_pose["roll"],
-                        pitch=send_pose["pitch"],
-                        yaw=send_pose["yaw"],
-                        degrees=False,
-                    )
-                    antennas_arr = np.array(self._current_antennas, dtype=np.float64)
-                    self._send_seq += 1
-                    send_seq = self._send_seq
-                    sent_pose = dict(send_pose)
-                    sent_body_yaw = send_body_yaw
-
-                    def _do_set_target(
-                        h=head, b=send_body_yaw, a=antennas_arr.copy()
-                    ) -> None:
-                        try:
-                            self.mini.set_target(head=h, body_yaw=b, antennas=a)
-                        except Exception as exc:
-                            logger.warning("set_target failed: %s", exc)
-                            raise
-
-                    self._send_future = loop.run_in_executor(None, _do_set_target)
-
-                    def _on_send_done(fut: asyncio.Future[Any]) -> None:
-                        if send_seq < self._last_applied_seq:
-                            return
-                        if fut.exception() is None:
-                            self._prev_sent_pose = sent_pose
-                            self._prev_sent_body_yaw = sent_body_yaw
-                            self._last_applied_seq = send_seq
-                            self._consecutive_ik_failures = 0
-                        else:
-                            self._consecutive_ik_failures += 1
-                            if self._consecutive_ik_failures >= IK_FAIL_CONSECUTIVE_THRESHOLD:
-                                # Pull only the target back toward neutral. Never
-                                # move _prev_sent — that is the clamp reference.
-                                alpha_t = IK_FAIL_RETRACT_TARGET_ALPHA
-                                for ax in POSE_AXES:
-                                    self._target_pose[ax] *= 1.0 - alpha_t
-                                self._target_body_yaw *= 1.0 - alpha_t
-
-                    self._send_future.add_done_callback(_on_send_done)
-                    self._last_send_time = now
-                    self._send_count += 1
-                elif can_send and self.mini is None:
-                    self._prev_sent_pose = dict(self._current_pose)
-                    self._prev_sent_body_yaw = self._current_body_yaw
-                    self._last_send_time = now
-
-                await asyncio.sleep(LOOP_INTERVAL)
-        except asyncio.CancelledError:
-            pass
-
-    async def _run_goto(
-        self,
-        move_uuid: str,
-        start_pose: dict[str, float],
-        end_pose: dict[str, float],
-        start_body_yaw: float,
-        end_body_yaw: float,
-        start_antennas: list[float],
-        end_antennas: list[float],
-        duration: float,
-        interpolation: str,
-    ) -> None:
-        t0 = time.monotonic()
-        while self._active_gotos.get(move_uuid, False):
-            elapsed = time.monotonic() - t0
-            t = min(elapsed / max(duration, 0.001), 1.0)
-            s = self._ease(t, interpolation)
-            lerped = {k: _lerp(start_pose[k], end_pose[k], s) for k in POSE_AXES}
-            by = _lerp(start_body_yaw, end_body_yaw, s)
-            self._target_pose, self._target_body_yaw = _clamp_pose_to_daemon_limits(
-                lerped, by
-            )
-            self._target_antennas = [
-                _lerp(start_antennas[i], end_antennas[i], s)
-                for i in range(min(len(start_antennas), len(end_antennas)))
-            ]
-            if t >= 1.0:
-                break
-            await asyncio.sleep(LOOP_INTERVAL)
-        self._active_gotos.pop(move_uuid, None)
-
-    @staticmethod
-    def _ease(t: float, mode: str) -> float:
-        if mode == "minjerk":
-            return t * t * t * (10 + t * (-15 + t * 6))
-        if mode == "ease":
-            if t < 0.5:
-                return 4 * t * t * t
-            return 1 - ((-2 * t + 2) ** 3) / 2
-        if mode == "cartoon":
-            c = 1.70158
-            c3 = c + 1
-            return 1 + c3 * ((t - 1) ** 3) + c * ((t - 1) ** 2)
-        return t

esp32_motion_controller/protocol.py

@@ -1,7 +1,8 @@
 """
-Protocol v2 parsing — dependency-light, no SDK imports.
+Protocol v3 parsing — dependency-light, no SDK imports.
 
-Rejects oversize frames, wrong types, non-finite numbers, and unsupported versions.
+UDP datagrams. Rejects oversize frames, wrong types, non-finite numbers,
+and unsupported versions.
 """
 
 from __future__ import annotations
@@ -11,10 +12,12 @@ import math
 from dataclasses import dataclass
 from typing import Any
 
-PROTOCOL_VERSION = 2
+PROTOCOL_VERSION = 3
 MAX_FRAME_BYTES = 512
 GAIN_MIN = 0.1
 GAIN_MAX = 3.0
+LINK_STALE_SEC = 1.0
+HELLO_PERIOD_SEC = 2.0
 
 
 class ProtocolError(ValueError):
@@ -23,11 +26,38 @@ class ProtocolError(ValueError):
         self.request_type = request_type
 
 
+@dataclass(frozen=True, slots=True)
+class LinkDiag:
+    rst: int = 0
+    wifi_n: int = 0
+    wifi_r: int = 0
+    rssi: int = 0
+    wifi_up: int = 0
+    down_ms: int = 0
+    send_ok: int = 0
+    send_fail: int = 0
+    send_ms: int = 0
+
+    def as_dict(self) -> dict[str, int]:
+        return {
+            "rst": self.rst,
+            "wifi_n": self.wifi_n,
+            "wifi_r": self.wifi_r,
+            "rssi": self.rssi,
+            "wifi_up": self.wifi_up,
+            "down_ms": self.down_ms,
+            "send_ok": self.send_ok,
+            "send_fail": self.send_fail,
+            "send_ms": self.send_ms,
+        }
+
+
 @dataclass(frozen=True, slots=True)
 class Hello:
     protocol_version: int
     boot_id: str
     device: str
+    diag: LinkDiag | None = None
 
 
 @dataclass(frozen=True, slots=True)
@@ -39,6 +69,7 @@ class Sample:
     engaged: bool
     gain: float
     ready: bool
+    op: int | None = None
 
 
 @dataclass(frozen=True, slots=True)
@@ -99,7 +130,7 @@ def _require_vec(msg: dict[str, Any], key: str, n: int, request_type: str) -> tu
     return tuple(out)
 
 
-def parse_frame(raw: str | bytes) -> Hello | Sample | Reset:
+def parse_frame(raw: str | bytes) -> Hello | Sample:
     if isinstance(raw, bytes):
         if len(raw) > MAX_FRAME_BYTES:
             raise ProtocolError("frame exceeds size limit", request_type="parse")
@@ -118,31 +149,50 @@ def parse_frame(raw: str | bytes) -> Hello | Sample | Reset:
         raise ProtocolError(f"invalid JSON: {exc}", request_type="parse") from exc
 
     msg = _require_dict(msg, "parse")
-    msg_type = msg.get("type")
-    if not isinstance(msg_type, str):
-        raise ProtocolError("missing type", request_type="parse")
+    version = msg.get("pv")
+    if not isinstance(version, int) or isinstance(version, bool):
+        raise ProtocolError("missing pv", request_type="parse")
+    if version != PROTOCOL_VERSION:
+        raise ProtocolError(
+            f"unsupported protocol_version: {version}",
+            request_type="hello" if msg.get("type") == "hello" else "parse",
+        )
 
-    if msg_type == "hello":
+    if msg.get("type") == "hello":
         return parse_hello(msg)
-    if msg_type == "sample":
-        return parse_sample(msg)
-    if msg_type == "reset":
-        return parse_reset(msg)
-    raise ProtocolError(f"unknown message type: {msg_type}", request_type=msg_type)
+    return parse_sample(msg)
+
+
+def _optional_int(msg: dict[str, Any], key: str) -> int:
+    val = msg.get(key)
+    if isinstance(val, bool) or not isinstance(val, int):
+        return 0
+    return int(val)
+
+
+def parse_link_diag(raw: Any) -> LinkDiag | None:
+    if not isinstance(raw, dict):
+        return None
+    return LinkDiag(
+        rst=_optional_int(raw, "rst"),
+        wifi_n=_optional_int(raw, "wifi_n"),
+        wifi_r=_optional_int(raw, "wifi_r"),
+        rssi=_optional_int(raw, "rssi"),
+        wifi_up=_optional_int(raw, "wifi_up"),
+        down_ms=_optional_int(raw, "down_ms"),
+        send_ok=_optional_int(raw, "send_ok"),
+        send_fail=_optional_int(raw, "send_fail"),
+        send_ms=_optional_int(raw, "send_ms"),
+    )
 
 
 def parse_hello(msg: dict[str, Any]) -> Hello:
     request_type = "hello"
-    version = _require_int(msg, "protocol_version", request_type)
-    if version != PROTOCOL_VERSION:
-        raise ProtocolError(
-            f"unsupported protocol_version: {version}",
-            request_type=request_type,
-        )
     return Hello(
-        protocol_version=version,
+        protocol_version=PROTOCOL_VERSION,
         boot_id=_require_str(msg, "boot_id", request_type),
         device=_require_str(msg, "device", request_type) if "device" in msg else "esp32",
+        diag=parse_link_diag(msg.get("diag")),
     )
 
 
@@ -159,6 +209,13 @@ def parse_sample(msg: dict[str, Any]) -> Sample:
     seq = _require_int(msg, "seq", request_type)
     if seq < 0:
         raise ProtocolError("seq must be non-negative", request_type=request_type)
+    op = None
+    if "op" in msg:
+        op_val = _require_int(msg, "op", request_type)
+        if op_val < 0:
+            raise ProtocolError("op must be non-negative", request_type=request_type)
+        if op_val > 0:
+            op = op_val
     return Sample(
         boot_id=_require_str(msg, "boot_id", request_type),
         seq=seq,
@@ -167,63 +224,25 @@ def parse_sample(msg: dict[str, Any]) -> Sample:
         engaged=_require_bool(msg, "engaged", request_type),
         gain=gain,
         ready=_require_bool(msg, "ready", request_type),
+        op=op,
     )
 
 
-def parse_reset(msg: dict[str, Any]) -> Reset:
-    request_type = "reset"
-    op_id = _require_int(msg, "op_id", request_type)
-    if op_id < 0:
-        raise ProtocolError("op_id must be non-negative", request_type=request_type)
-    return Reset(
-        boot_id=_require_str(msg, "boot_id", request_type),
-        op_id=op_id,
-    )
-
-
-def encode_hello_response(session_id: int) -> dict[str, Any]:
-    return {
-        "type": "hello",
-        "protocol_version": PROTOCOL_VERSION,
-        "session_id": int(session_id),
-    }
-
-
-def encode_host_state(
+def encode_state_reply(
     *,
     robot: bool,
     mode: str,
     error: str | None = None,
+    op_ack: int | None = None,
+    op_status: str | None = None,
 ) -> dict[str, Any]:
-    return {
-        "type": "host_state",
+    out: dict[str, Any] = {
+        "pv": PROTOCOL_VERSION,
         "robot": bool(robot),
         "mode": mode,
         "error": error,
     }
-
-
-def encode_reset_result(
-    *,
-    boot_id: str,
-    op_id: int,
-    status: str,
-    message: str | None = None,
-) -> dict[str, Any]:
-    out: dict[str, Any] = {
-        "type": "reset_result",
-        "boot_id": boot_id,
-        "op_id": int(op_id),
-        "status": status,
-    }
-    if message is not None:
-        out["message"] = message
+    if op_ack is not None:
+        out["op_ack"] = int(op_ack)
+        out["op_status"] = op_status
     return out
-
-
-def encode_error(request_type: str, message: str) -> dict[str, Any]:
-    return {
-        "type": "error",
-        "request_type": request_type,
-        "message": message,
-    }

esp32_motion_controller/robot_control.py

@@ -1,34 +1,39 @@
 """
-Single fixed-rate robot command owner for protocol v2.
+Single fixed-rate robot command owner for protocol v3.
 
 Consumes the latest sample from SessionHub, runs the pure reducer, and performs
-at most one SDK call in flight. Owns reset goto completion and pose seeding.
+at most one SDK call in flight. Every outbound pose is capped per tick.
 """
 
 from __future__ import annotations
 
 import asyncio
 import logging
+import math
 import time
+from dataclasses import replace
 from typing import Any
 
 import numpy as np
 from scipy.spatial.transform import Rotation
 
 from esp32_motion_controller.control import (
-    CONTROL_DT,
     CONTROL_HZ,
+    STALE_PACKET_SEC,
     Command,
     ControlState,
     begin_reset,
+    disengaged_rest_pose,
     force_disengage,
     initial_state,
     mark_pose_unread,
     mark_sdk_failure,
     mark_sdk_success,
     note_sample_receipt,
-    rebase_neutral,
+    pose_near,
+    pose_travel,
     seed_from_pose,
+    speed_lock,
     step,
     zero_pose,
 )
@@ -36,10 +41,13 @@ from esp32_motion_controller.session import SessionHub
 
 logger = logging.getLogger(__name__)
 
-RESET_DURATION_SEC = 1.5
 IDLE_RECONCILE_SEC = 2.0
 RESYNC_EPS_POS_M = 0.003
 RESYNC_EPS_ANG_RAD = 0.05
+# Incoming jumps larger than this are dropped (hold last sent). Smaller
+# deltas are speed-locked to one tick of MAX_*_VEL.
+CULL_POS_M = 0.020
+CULL_ANG_RAD = math.radians(20.0)
 
 
 class RobotGateway:
@@ -87,23 +95,6 @@ class RobotGateway:
         antennas = np.array(command.antennas, dtype=np.float64)
         self.mini.set_target(head=head, body_yaw=command.body_yaw, antennas=antennas)
 
-    def goto_neutral(self, duration: float = RESET_DURATION_SEC) -> None:
-        if self.mini is None or self.log_only:
-            time.sleep(min(duration, 0.05))
-            return
-        from reachy_mini.utils import create_head_pose
-
-        head = create_head_pose(
-            x=0.0, y=0.0, z=0.0, roll=0.0, pitch=0.0, yaw=0.0, degrees=False
-        )
-        self.mini.goto_target(
-            head=head,
-            body_yaw=0.0,
-            antennas=[0.0, 0.0],
-            duration=duration,
-            method="minjerk",
-        )
-
 
 class RobotControl:
     def __init__(
@@ -126,6 +117,159 @@ class RobotControl:
         self._last_reconcile = 0.0
         self._sdk_lock = asyncio.Lock()
         self._started = False
+        self._sent_pose: dict[str, float] | None = None
+        self._sent_body: float = 0.0
+        self._sent_at: float = 0.0
+        self._posture: str = "unknown"
+        self._boot_id: str | None = None
+        self._button_engaged: bool = False
+        self._suppress_button_edge: bool = False
+        self._anim: str | None = None
+        self._anim_pose = zero_pose()
+        self._anim_body = 0.0
+        self._pending_reset = None
+
+    def _remember_sent(self, pose: dict[str, float], body: float, now: float) -> None:
+        self._sent_pose = dict(pose)
+        self._sent_body = float(body)
+        self._sent_at = now
+
+    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.
+        """
+        del now
+        ref_pose = (
+            dict(self._sent_pose)
+            if self._sent_pose is not None
+            else dict(self._state.baseline_pose)
+        )
+        ref_body = (
+            float(self._sent_body)
+            if self._sent_pose is not None
+            else float(self._state.baseline_body_yaw)
+        )
+        if self._anim is None:
+            dist, ang = pose_travel(
+                ref_pose, command.pose, ref_body, command.body_yaw
+            )
+            if dist > CULL_POS_M or ang > CULL_ANG_RAD:
+                logger.warning(
+                    "cull jump pos=%.3f m ang=%.1f deg (hold last)",
+                    dist,
+                    math.degrees(ang),
+                )
+                return Command(
+                    pose=ref_pose,
+                    body_yaw=ref_body,
+                    antennas=command.antennas,
+                )
+        pose, body = speed_lock(
+            ref_pose,
+            ref_body,
+            command.pose,
+            command.body_yaw,
+            self.dt,
+            apply_ellipsoid=self._state.engaged and self._anim is None,
+        )
+        return Command(pose=pose, body_yaw=body, antennas=command.antennas)
+
+    def _begin_anim(self, kind: str, pose: dict[str, float], body: float) -> None:
+        self._anim = kind
+        self._anim_pose = dict(pose)
+        self._anim_body = float(body)
+        if kind in {"disappear", "reset"}:
+            self._state = force_disengage(self._state)
+
+    async def _tick_anim(
+        self, loop: asyncio.AbstractEventLoop, now: float
+    ) -> None:
+        command = Command(
+            pose=dict(self._anim_pose),
+            body_yaw=self._anim_body,
+            antennas=(0.0, 0.0),
+        )
+        sent = await self._send_command(loop, now, command, log_sample=False)
+        if sent is None:
+            return
+        if not pose_near(sent.pose, self._anim_pose, sent.body_yaw, self._anim_body):
+            return
+        kind = self._anim
+        self._anim = None
+        if kind == "appear":
+            self._posture = "neutral"
+            self._state = replace(
+                self._state,
+                desired_pose=zero_pose(),
+                base_pose=zero_pose(),
+                body_yaw=0.0,
+            )
+        elif kind == "disappear":
+            self._posture = "ducked"
+            rest = disengaged_rest_pose()
+            self._state = replace(
+                self._state,
+                desired_pose=dict(rest),
+                base_pose=dict(rest),
+                body_yaw=0.0,
+                engaged=False,
+            )
+        elif kind == "reset":
+            self._posture = "neutral"
+            self._state = replace(self._state, mode="idle")
+            reset = self._pending_reset
+            self._pending_reset = None
+            if reset is not None:
+                await self.session.complete_reset(
+                    boot_id=reset.boot_id,
+                    op_id=reset.op_id,
+                    status="completed",
+                )
+            self._begin_anim("disappear", disengaged_rest_pose(), 0.0)
+            await self.session.push_host_state(mode="idle", clear_error=True)
+
+    async def _send_command(
+        self,
+        loop: asyncio.AbstractEventLoop,
+        now: float,
+        command: Command,
+        *,
+        log_sample: bool,
+    ) -> Command | None:
+        command = self._guard_command(command, now)
+        if self.log_only:
+            if log_sample:
+                logger.info(
+                    "command engaged=%s pose=%s body=%.3f",
+                    self._state.engaged,
+                    {k: round(command.pose[k], 4) for k in command.pose},
+                    command.body_yaw,
+                )
+            self._state = mark_sdk_success(self._state, command)
+            self._remember_sent(command.pose, command.body_yaw, now)
+            return command
+
+        async with self._sdk_lock:
+            t_sdk = time.monotonic()
+            try:
+                await loop.run_in_executor(None, self.gateway.set_target, command)
+            except Exception as exc:
+                logger.warning("set_target failed: %s", exc)
+                self._state = mark_sdk_failure(self._state)
+                await self.session.push_host_state(
+                    mode=self._state.mode,
+                    error=self._state.error,
+                )
+                return None
+            sdk_ms = (time.monotonic() - t_sdk) * 1000.0
+            self.session.note_sdk_duration(sdk_ms)
+            if sdk_ms > 50.0:
+                logger.warning("set_target slow %.0f ms", sdk_ms)
+            self._state = mark_sdk_success(self._state, command)
+            self._remember_sent(command.pose, command.body_yaw, now)
+            return command
 
     @property
     def state(self) -> ControlState:
@@ -152,11 +296,16 @@ class RobotControl:
         try:
             # Initial seed attempt.
             await self._seed(update_host=True)
+            last = time.monotonic()
             while True:
                 t0 = time.monotonic()
                 dt = min(t0 - last, 0.05)
                 if dt <= 0:
                     dt = self.dt
+                lag_ms = (t0 - last) * 1000.0
+                if last > 0.0 and lag_ms > 80.0:
+                    logger.warning("control tick lag %.0f ms", lag_ms)
+                    self.session.note_tick_lag(lag_ms)
                 last = t0
 
                 await self._tick(loop, t0, dt)
@@ -166,12 +315,24 @@ class RobotControl:
         except asyncio.CancelledError:
             pass
 
+    def _want_engaged(self, sample) -> bool:
+        if sample is None:
+            return False
+        if self._state.mode == "fault" or self._state.sends_frozen:
+            return False
+        return bool(sample.engaged) and bool(sample.ready)
+
     async def _tick(self, loop: asyncio.AbstractEventLoop, now: float, dt: float) -> None:
-        # Handle pending reset first — exclusive robot ownership.
         pending = await self.session.take_pending_reset()
         if pending is not None:
-            await self._run_reset(loop, pending, now)
-            return
+            self._pending_reset = pending
+            self._state = begin_reset(self._state, now)
+            self._begin_anim("reset", zero_pose(), 0.0)
+            await self.session.push_host_state(mode="resetting", clear_error=True)
+
+        edge = self.session.poll_presence_edge()
+        if edge == "absent":
+            await self.on_controller_absent()
 
         latest = await self.session.take_latest_sample()
         sample = None
@@ -183,10 +344,46 @@ class RobotControl:
                 self._last_seen_seq = sample.seq
             self._state = note_sample_receipt(self._state, latest.receipt_time)
 
+        currently = self._state.engaged
+        stale = (
+            currently
+            and not self.session.controller_present
+            and self._state.have_sample
+            and self._state.last_sample_time > 0.0
+            and (now - self._state.last_sample_time) > STALE_PACKET_SEC
+        )
+        if stale:
+            logger.warning(
+                "stale disengage controller_absent age=%.0f ms (limit=%.0f ms)",
+                (now - self._state.last_sample_time) * 1000.0,
+                STALE_PACKET_SEC * 1000.0,
+            )
+            self._state = force_disengage(self._state)
+
+        if sample_is_fresh:
+            want = self._want_engaged(sample)
+            if self._state.mode == "resetting" or self._anim == "reset":
+                self._button_engaged = False
+            elif self._suppress_button_edge:
+                self._button_engaged = want
+                self._suppress_button_edge = False
+            elif want and not self._button_engaged:
+                self._button_engaged = True
+                if self._posture == "ducked":
+                    self._begin_anim("appear", zero_pose(), 0.0)
+            elif not want and self._button_engaged:
+                self._button_engaged = False
+                self._begin_anim("disappear", disengaged_rest_pose(), 0.0)
+
+        if self._anim is not None:
+            await self._tick_anim(loop, now)
+            return
+
         # Idle reconciliation when disengaged and not frozen.
         if (
             self._state.mode == "idle"
             and not self._state.engaged
+            and not self._button_engaged
             and not self._state.sends_frozen
             and (now - self._last_reconcile) >= IDLE_RECONCILE_SEC
         ):
@@ -199,6 +396,7 @@ class RobotControl:
             dt=dt,
             sample=sample,
             sample_is_fresh=sample_is_fresh,
+            controller_present=self.session.controller_present,
         )
         self._state = result.state
 
@@ -212,30 +410,7 @@ class RobotControl:
 
         if result.command is None:
             return
-
-        if self.log_only:
-            if sample_is_fresh:
-                logger.info(
-                    "command engaged=%s pose=%s body=%.3f",
-                    self._state.engaged,
-                    {k: round(result.command.pose[k], 4) for k in result.command.pose},
-                    result.command.body_yaw,
-                )
-            self._state = mark_sdk_success(self._state, result.command)
-            return
-
-        async with self._sdk_lock:
-            try:
-                await loop.run_in_executor(None, self.gateway.set_target, result.command)
-            except Exception as exc:
-                logger.warning("set_target failed: %s", exc)
-                self._state = mark_sdk_failure(self._state)
-                await self.session.push_host_state(
-                    mode=self._state.mode,
-                    error=self._state.error,
-                )
-                return
-            self._state = mark_sdk_success(self._state, result.command)
+        await self._send_command(loop, now, result.command, log_sample=sample_is_fresh)
 
     async def _seed(self, *, update_host: bool) -> bool:
         loop = asyncio.get_running_loop()
@@ -246,7 +421,8 @@ class RobotControl:
                 await self.session.push_host_state(mode="fault", error=self._state.error)
             return False
         pose, body = read
-        self._state = seed_from_pose(self._state, pose, body)
+        self._state = seed_from_pose(self._state, pose, body, apply_ellipsoid=False)
+        self._remember_sent(self._state.baseline_pose, self._state.baseline_body_yaw, time.monotonic())
         self._last_reconcile = time.monotonic()
         if update_host:
             await self.session.push_host_state(mode="idle", clear_error=True)
@@ -281,92 +457,67 @@ class RobotControl:
                 self._state,
                 Command(pose=pose, body_yaw=body, antennas=tuple(self._state.baseline_antennas[:2])),
             )
+            self._remember_sent(pose, body, now)
             return
         logger.warning("Pose desync pos=%.4f m ang=%.3f rad — idle resync", pos, ang)
-        self._state = seed_from_pose(self._state, pose, body)
-
-    async def _run_reset(
-        self, loop: asyncio.AbstractEventLoop, reset, now: float
-    ) -> None:
-        if not self.robot_available and not self.log_only:
-            await self.session.complete_reset(
-                boot_id=reset.boot_id,
-                op_id=reset.op_id,
-                status="failed",
-                message="Robot not available",
-            )
-            return
+        self._state = seed_from_pose(
+            self._state, pose, body, apply_ellipsoid=False
+        )
+        self._remember_sent(pose, body, now)
 
-        self._state = begin_reset(self._state, now)
-        await self.session.push_host_state(mode="resetting", clear_error=True)
+    async def on_controller_connected(self) -> None:
+        """Reseed clutch from the measured pose. Tests and first-hello use this."""
+        await self._reseed_controller()
 
-        # Seed from measured pose before goto.
-        read = await loop.run_in_executor(None, self.gateway.read_pose)
-        if read is None and not self.log_only:
-            self._state = mark_pose_unread(self._state)
-            await self.session.complete_reset(
-                boot_id=reset.boot_id,
-                op_id=reset.op_id,
-                status="failed",
-                message="Robot pose unread",
-            )
-            return
-        if read is not None:
-            pose, body = read
-            self._state = seed_from_pose(self._state, pose, body)
-            self._state = begin_reset(self._state, now)
+    async def on_controller_hello(self, hello) -> None:
+        """Keep clutch on first hello and any same-boot reconnect.
 
-        async with self._sdk_lock:
-            try:
-                await loop.run_in_executor(None, self.gateway.goto_neutral, RESET_DURATION_SEC)
-            except Exception as exc:
-                logger.error("goto_target failed: %s", exc)
-                self._state = mark_pose_unread(self._state)
-                await self.session.complete_reset(
-                    boot_id=reset.boot_id,
-                    op_id=reset.op_id,
-                    status="failed",
-                    message=str(exc),
-                )
-                return
-
-        measured = await loop.run_in_executor(None, self.gateway.read_pose)
-        if measured is None and not self.log_only:
-            self._state = rebase_neutral(self._state, measured_baseline=None, measured_body=None)
-            await self.session.complete_reset(
-                boot_id=reset.boot_id,
-                op_id=reset.op_id,
-                status="failed",
-                message="Robot pose unread after reset",
+        Appear/disappear are button edges only — a socket gap must not duck
+        or rise. Reseed only when the device actually rebooted.
+        """
+        prev = self._boot_id
+        first = prev is None
+        same = prev is not None and hello.boot_id == prev
+        self._boot_id = hello.boot_id
+        keep = first or (
+            same
+            and not self._state.sends_frozen
+            and self._state.mode not in {"fault"}
+        )
+        if keep:
+            logger.info(
+                "hello keep clutch boot_id=%s first=%s same_boot=%s engaged=%s button=%s posture=%s",
+                hello.boot_id,
+                first,
+                same,
+                self._state.engaged,
+                self._button_engaged,
+                self._posture,
             )
             return
-
-        if measured is None:
-            measured = (zero_pose(), 0.0)
-        pose, body = measured
-        self._state = rebase_neutral(
-            self._state, measured_baseline=pose, measured_body=body
+        logger.info(
+            "hello reseed boot_id=%s prev=%s mode=%s",
+            hello.boot_id,
+            prev,
+            self._state.mode,
         )
-        self._last_reconcile = time.monotonic()
-        await self.session.complete_reset(
-            boot_id=reset.boot_id,
-            op_id=reset.op_id,
-            status="completed",
-        )
-
-    async def on_controller_connected(self) -> None:
-        """Reseed when a new controller session is admitted.
+        await self._reseed_controller()
 
-        Do not push host_state here — the hello handler sends the first snapshot
-        so the device always sees hello before host_state.
-        """
+    async def _reseed_controller(self) -> None:
+        """Do not push host_state here — hello already sent the snapshot."""
         self._state = force_disengage(self._state)
         self._last_seen_seq = None
+        self._posture = "unknown"
+        self._button_engaged = False
+        self._suppress_button_edge = False
         ok = await self._seed(update_host=False)
         if not ok and self.log_only:
             self._state = seed_from_pose(self._state, zero_pose(), 0.0)
+            self._remember_sent(zero_pose(), 0.0, time.monotonic())
 
-    async def on_controller_disconnected(self) -> None:
-        self._state = force_disengage(self._state)
-        if self._state.mode not in {"resetting"}:
-            await self.session.push_host_state(mode="idle")
+    async def on_controller_absent(self) -> None:
+        logger.info(
+            "controller absent engaged=%s mode=%s",
+            self._state.engaged,
+            self._state.mode,
+        )

esp32_motion_controller/session.py

@@ -1,43 +1,39 @@
 """
-WebSocket session ingress for protocol v2.
+UDP session ingress for protocol v3.
 
-Validates frames, keeps only the latest sample, manages session generation and
-reset operation mailbox. Never calls the robot SDK.
+Validates datagrams, keeps only the latest sample, tracks the peer by
+address + last_rx. Never calls the robot SDK.
 """
 
 from __future__ import annotations
 
 import asyncio
+import json
 import logging
 import time
 from dataclasses import dataclass, field
 from typing import Any, Awaitable, Callable
 
-from fastapi import WebSocket
-from starlette.websockets import WebSocketState
-
 from esp32_motion_controller.protocol import (
+    LINK_STALE_SEC,
     Hello,
+    LinkDiag,
     ProtocolError,
     Reset,
     Sample,
-    encode_error,
-    encode_hello_response,
-    encode_host_state,
-    encode_reset_result,
+    encode_state_reply,
     parse_frame,
 )
 
 logger = logging.getLogger(__name__)
 
-SendFn = Callable[[dict[str, Any]], Awaitable[None]]
+OnBootFn = Callable[[Hello], Awaitable[None]]
 
 
 @dataclass
 class LatestSample:
     sample: Sample
     receipt_time: float
-    generation: int
 
 
 @dataclass
@@ -50,12 +46,12 @@ class ResetRecord:
 
 @dataclass
 class SessionMailbox:
-    """Shared state between the WS receiver and the control loop."""
+    """Shared state between the UDP receiver and the control loop."""
 
     lock: asyncio.Lock = field(default_factory=asyncio.Lock)
-    generation: int = 0
     boot_id: str | None = None
-    active_ws: WebSocket | None = None
+    peer: tuple[str, int] | None = None
+    last_rx: float = 0.0
     latest: LatestSample | None = None
     last_seq: int | None = None
     pending_reset: Reset | None = None
@@ -63,175 +59,178 @@ class SessionMailbox:
     host_mode: str = "idle"
     host_robot: bool = True
     host_error: str | None = None
-    controller_present: bool = False
+    last_diag: LinkDiag | None = None
+    seq_skips: int = 0
+    sample_gaps: int = 0
+    presents: int = 0
+    absents: int = 0
 
 
 class SessionHub:
     def __init__(self, *, robot_available: bool) -> None:
         self.mailbox = SessionMailbox(host_robot=robot_available)
-        self._send_lock = asyncio.Lock()
+        self._transport: Any = None
+        self._last_sample_receipt: float = 0.0
+        self._present: bool = False
+        self.max_tick_lag_ms: float = 0.0
+        self.last_sdk_ms: float = 0.0
+        self.on_hello: OnBootFn | None = None
 
-    @property
-    def generation(self) -> int:
-        return self.mailbox.generation
+    def bind_transport(self, transport: Any) -> None:
+        self._transport = transport
 
-    async def on_connect(self, websocket: WebSocket) -> int:
-        mb = self.mailbox
-        async with mb.lock:
-            old = mb.active_ws
-            mb.generation += 1
-            gen = mb.generation
-            mb.active_ws = websocket
-            mb.latest = None
-            mb.last_seq = None
-            mb.pending_reset = None
-            mb.boot_id = None
-            mb.controller_present = True
-            mb.host_mode = "idle"
-            mb.host_error = None
-
-        if old is not None and old is not websocket:
-            alive = old.client_state == WebSocketState.CONNECTED
-            if alive:
-                logger.warning("Replacing active controller connection")
-                try:
-                    await old.close(code=1000)
-                except Exception:
-                    pass
-
-        await websocket.accept()
-        logger.info("Controller socket accepted generation=%d", gen)
-        return gen
-
-    async def cleanup(self, websocket: WebSocket, generation: int) -> None:
-        mb = self.mailbox
-        async with mb.lock:
-            if mb.active_ws is not websocket or mb.generation != generation:
-                return
-            mb.active_ws = None
-            mb.controller_present = False
-            mb.latest = None
-            # Keep reset_cache and boot_id so reconnect can retrieve outcomes.
-        logger.info("Controller disconnected generation=%d", generation)
-
-    async def handle_message(self, websocket: WebSocket, generation: int, raw: str) -> None:
+    @property
+    def controller_present(self) -> bool:
+        last = self.mailbox.last_rx
+        if last <= 0.0:
+            return False
+        return (time.monotonic() - last) < LINK_STALE_SEC
+
+    def note_tick_lag(self, lag_ms: float) -> None:
+        if lag_ms > self.max_tick_lag_ms:
+            self.max_tick_lag_ms = lag_ms
+
+    def note_sdk_duration(self, duration_ms: float) -> None:
+        self.last_sdk_ms = duration_ms
+
+    def poll_presence_edge(self) -> str | None:
+        """Return 'present' / 'absent' on a liveness edge, else None."""
+        now = self.controller_present
+        prev = self._present
+        if now == prev:
+            return None
+        self._present = now
+        if now:
+            self.mailbox.presents += 1
+            return "present"
+        self.mailbox.absents += 1
+        return "absent"
+
+    async def handle_datagram(self, data: bytes, addr: tuple[str, int]) -> None:
         try:
-            msg = parse_frame(raw)
+            msg = parse_frame(data)
         except ProtocolError as exc:
-            await self._send(websocket, generation, encode_error(exc.request_type, str(exc)))
-            if exc.request_type == "hello":
-                try:
-                    await websocket.close(code=1002)
-                except Exception:
-                    pass
+            logger.warning("drop datagram from %s: %s", addr, exc)
             return
 
         if isinstance(msg, Hello):
-            await self._handle_hello(websocket, generation, msg)
+            await self._handle_hello(msg, addr)
             return
         if isinstance(msg, Sample):
-            await self._handle_sample(websocket, generation, msg)
+            await self._handle_sample(msg, addr)
             return
-        if isinstance(msg, Reset):
-            await self._handle_reset(websocket, generation, msg)
+
+    def _touch_peer(
+        self, boot_id: str, addr: tuple[str, int], *, hello: Hello | None
+    ) -> bool:
+        """Update peer/boot under lock. Returns True when boot_id changed."""
+        mb = self.mailbox
+        prev_boot = mb.boot_id
+        mb.peer = addr
+        mb.last_rx = time.monotonic()
+        if prev_boot is not None and boot_id != prev_boot:
+            mb.last_seq = None
+            mb.latest = None
+        mb.boot_id = boot_id
+        if hello is not None and hello.diag is not None:
+            mb.last_diag = hello.diag
+        return prev_boot != boot_id
+
+    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")
+        await self.on_hello(payload)
 
-    async def _handle_hello(self, websocket: WebSocket, generation: int, hello: Hello) -> None:
+    def _reset_fields(self, boot_id: str, op: int | None) -> tuple[int | None, str | None]:
+        mb = self.mailbox
+        if op is None:
+            return None, None
+        cached = mb.reset_cache.get((boot_id, op))
+        if cached is not None:
+            return cached.op_id, cached.status
+        if mb.host_mode == "resetting":
+            pending = mb.pending_reset
+            if pending is not None and pending.op_id == op and pending.boot_id == boot_id:
+                return op, "accepted"
+            mb.reset_cache[(boot_id, op)] = ResetRecord(
+                boot_id=boot_id,
+                op_id=op,
+                status="failed",
+                message="Reset already in progress",
+            )
+            return op, "failed"
+        mb.pending_reset = Reset(boot_id=boot_id, op_id=op)
+        mb.reset_cache[(boot_id, op)] = ResetRecord(
+            boot_id=boot_id, op_id=op, status="accepted"
+        )
+        mb.host_mode = "resetting"
+        return op, "accepted"
+
+    async def _handle_hello(self, hello: Hello, addr: tuple[str, int]) -> None:
         mb = self.mailbox
         async with mb.lock:
-            if mb.generation != generation or mb.active_ws is not websocket:
-                return
-            mb.boot_id = hello.boot_id
+            changed = self._touch_peer(hello.boot_id, addr, hello=hello)
             robot = mb.host_robot
             mode = mb.host_mode
             err = mb.host_error
-        await self._send(websocket, generation, encode_hello_response(generation))
-        await self._send(
-            websocket,
-            generation,
-            encode_host_state(robot=robot, mode=mode, error=err),
+        diag = hello.diag.as_dict() if hello.diag is not None else None
+        logger.info("hello boot_id=%s diag=%s", hello.boot_id, diag)
+        self._send(
+            addr,
+            encode_state_reply(robot=robot, mode=mode, error=err),
         )
-        logger.info("Hello ok boot_id=%s generation=%d", hello.boot_id, generation)
+        if changed:
+            await self._maybe_reseed(hello.boot_id, hello)
 
-    async def _handle_sample(self, websocket: WebSocket, generation: int, sample: Sample) -> None:
+    async def _handle_sample(self, sample: Sample, addr: tuple[str, int]) -> None:
         mb = self.mailbox
         now = time.monotonic()
         async with mb.lock:
-            if mb.generation != generation or mb.active_ws is not websocket:
-                return
-            if mb.boot_id is None:
-                # Implicit bind if device skipped hello (should not happen).
-                mb.boot_id = sample.boot_id
-            if sample.boot_id != mb.boot_id:
-                logger.warning("sample boot_id mismatch; dropping")
-                return
+            changed = self._touch_peer(sample.boot_id, addr, hello=None)
             if mb.last_seq is not None:
-                # Duplicate
                 if sample.seq == mb.last_seq:
+                    op_ack, op_status = self._reset_fields(sample.boot_id, sample.op)
+                    payload = encode_state_reply(
+                        robot=mb.host_robot,
+                        mode=mb.host_mode,
+                        error=mb.host_error,
+                        op_ack=op_ack,
+                        op_status=op_status,
+                    )
+                    self._send(addr, payload)
                     return
-                # Out-of-order (small backward jump) — drop unless wrap.
                 if sample.seq < mb.last_seq:
-                    # uint32 wrap: large backward jump
                     if mb.last_seq - sample.seq < 2**31:
                         return
+                elif sample.seq > mb.last_seq + 1:
+                    mb.seq_skips += 1
+                    logger.warning(
+                        "seq skip last=%s now=%s gap=%s",
+                        mb.last_seq,
+                        sample.seq,
+                        sample.seq - mb.last_seq,
+                    )
             mb.last_seq = sample.seq
-            mb.latest = LatestSample(sample=sample, receipt_time=now, generation=generation)
+            mb.latest = LatestSample(sample=sample, receipt_time=now)
+            op_ack, op_status = self._reset_fields(sample.boot_id, sample.op)
+            payload = encode_state_reply(
+                robot=mb.host_robot,
+                mode=mb.host_mode,
+                error=mb.host_error,
+                op_ack=op_ack,
+                op_status=op_status,
+            )
 
-    async def _handle_reset(self, websocket: WebSocket, generation: int, reset: Reset) -> None:
-        mb = self.mailbox
-        async with mb.lock:
-            if mb.generation != generation or mb.active_ws is not websocket:
-                return
-            if mb.boot_id is None:
-                mb.boot_id = reset.boot_id
-            if reset.boot_id != mb.boot_id:
-                await self._send(
-                    websocket,
-                    generation,
-                    encode_reset_result(
-                        boot_id=reset.boot_id,
-                        op_id=reset.op_id,
-                        status="failed",
-                        message="boot_id mismatch",
-                    ),
-                )
-                return
-            cached = mb.reset_cache.get((reset.boot_id, reset.op_id))
-            if cached is not None:
-                result = encode_reset_result(
-                    boot_id=cached.boot_id,
-                    op_id=cached.op_id,
-                    status=cached.status,
-                    message=cached.message,
-                )
-                pending = None
-            elif mb.host_mode == "resetting":
-                # Another reset already running with a different op_id.
-                result = encode_reset_result(
-                    boot_id=reset.boot_id,
-                    op_id=reset.op_id,
-                    status="failed",
-                    message="Reset already in progress",
-                )
-                pending = None
-            else:
-                mb.pending_reset = reset
-                mb.reset_cache[(reset.boot_id, reset.op_id)] = ResetRecord(
-                    boot_id=reset.boot_id,
-                    op_id=reset.op_id,
-                    status="accepted",
-                )
-                mb.host_mode = "resetting"
-                result = encode_reset_result(
-                    boot_id=reset.boot_id,
-                    op_id=reset.op_id,
-                    status="accepted",
-                )
-                pending = reset
-
-        await self._send(websocket, generation, result)
-        if pending is not None:
-            await self.push_host_state(mode="resetting")
+        if self._last_sample_receipt > 0.0:
+            gap = now - self._last_sample_receipt
+            if gap > 0.250:
+                mb.sample_gaps += 1
+                logger.warning("sample gap %.0f ms seq=%s", gap * 1000.0, sample.seq)
+        self._last_sample_receipt = now
+        self._send(addr, payload)
+        if changed:
+            await self._maybe_reseed(sample.boot_id, None)
 
     async def take_latest_sample(self) -> LatestSample | None:
         mb = self.mailbox
@@ -261,24 +260,6 @@ class SessionHub:
             if status in {"completed", "failed"} and mb.host_mode == "resetting":
                 mb.host_mode = "idle" if status == "completed" else "fault"
                 mb.host_error = message if status == "failed" else None
-            ws = mb.active_ws
-            gen = mb.generation
-            mode = mb.host_mode
-            robot = mb.host_robot
-            err = mb.host_error
-        if ws is not None:
-            await self._send(
-                ws,
-                gen,
-                encode_reset_result(
-                    boot_id=boot_id, op_id=op_id, status=status, message=message
-                ),
-            )
-            await self._send(
-                ws,
-                gen,
-                encode_host_state(robot=robot, mode=mode, error=err),
-            )
 
     async def push_host_state(
         self,
@@ -298,36 +279,37 @@ class SessionHub:
                 mb.host_error = None
             elif error is not None:
                 mb.host_error = error
-            ws = mb.active_ws
-            gen = mb.generation
-            payload = encode_host_state(
-                robot=mb.host_robot, mode=mb.host_mode, error=mb.host_error
-            )
-        if ws is not None:
-            await self._send(ws, gen, payload)
 
     async def snapshot_status(self) -> dict[str, Any]:
         mb = self.mailbox
         async with mb.lock:
+            last_rx = mb.last_rx
+            present = last_rx > 0.0 and (time.monotonic() - last_rx) < LINK_STALE_SEC
+            age_ms = (time.monotonic() - last_rx) * 1000.0 if last_rx > 0.0 else None
             return {
                 "robot": mb.host_robot,
                 "busy": mb.host_mode == "resetting",
                 "mode": mb.host_mode,
-                "connected": mb.controller_present,
+                "connected": present,
                 "error": mb.host_error,
+                "boot_id": mb.boot_id,
+                "peer": f"{mb.peer[0]}:{mb.peer[1]}" if mb.peer else None,
+                "presents": mb.presents,
+                "absents": mb.absents,
+                "last_seq": mb.last_seq,
+                "seq_skips": mb.seq_skips,
+                "sample_gaps": mb.sample_gaps,
+                "last_rx_age_ms": None if age_ms is None else round(age_ms, 1),
+                "last_diag": mb.last_diag.as_dict() if mb.last_diag is not None else None,
+                "max_tick_lag_ms": round(self.max_tick_lag_ms, 1),
+                "last_sdk_ms": round(self.last_sdk_ms, 1),
             }
 
-    async def _send(
-        self, websocket: WebSocket, generation: int, payload: dict[str, Any]
-    ) -> None:
-        async with self._send_lock:
-            mb = self.mailbox
-            async with mb.lock:
-                if mb.generation != generation or mb.active_ws is not websocket:
-                    return
-            if websocket.client_state != WebSocketState.CONNECTED:
-                return
-            try:
-                await websocket.send_json(payload)
-            except Exception as exc:
-                logger.warning("WS send failed: %s", exc)
+    def _send(self, addr: tuple[str, int], payload: dict[str, Any]) -> None:
+        transport = self._transport
+        if transport is None:
+            return
+        try:
+            transport.sendto(json.dumps(payload, separators=(",", ":")).encode(), addr)
+        except Exception as exc:
+            logger.warning("UDP send failed: %s", exc)

esp32_motion_controller/static/index.html

@@ -12,12 +12,12 @@
 </head>
 <body>
   <h1>🕹️ ESP32 Motion Controller</h1>
-  <p>ESP32 handheld IMU controller bridge for Reachy Mini (protocol v2).</p>
-  <p>WebSocket: <code id="ws">ws://…:8766/ws</code></p>
+  <p>ESP32 handheld IMU controller bridge for Reachy Mini (protocol v3).</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>
     fetch('/api/info').then(r => r.json()).then(j => {
-      if (j.ws_url) document.getElementById('ws').textContent = j.ws_url;
+      if (j.udp) document.getElementById('udp').textContent = j.udp;
     }).catch(() => {});
   </script>
 </body>

esp32_motion_controller/ws_handler.py

@@ -1,296 +0,0 @@
-"""
-WebSocket router for the ESP32 motion controller.
-
-Handles controller_state / reset / status, the 300 ms stale-packet watchdog,
-reset interlock (busy ignores engage; rebases clutch on goto completion),
-and single-controller admission.
-"""
-
-from __future__ import annotations
-
-import asyncio
-import json
-import logging
-import time
-from typing import Any
-
-from fastapi import WebSocket
-from starlette.websockets import WebSocketState
-
-from esp32_motion_controller.behavior import Behavior
-from esp32_motion_controller.controller_state import ControllerState
-from esp32_motion_controller.movement_handler import MovementHandler
-
-logger = logging.getLogger(__name__)
-
-STALE_PACKET_SEC = 0.300
-BEHAVIOR_TICK_SEC = 0.033
-RESET_DURATION_SEC = 1.5
-# Keep the apply loop across short reconnect blips; stop after this idle.
-IDLE_STOP_SEC = 5.0
-
-
-class WebSocketHandler:
-    def __init__(
-        self,
-        movement: MovementHandler,
-        controller: ControllerState,
-        behavior: Behavior,
-        *,
-        robot_available: bool = True,
-        log_only: bool = False,
-    ) -> None:
-        self.movement = movement
-        self.controller = controller
-        self.behavior = behavior
-        self.robot_available = robot_available
-        self.log_only = log_only
-        self.busy = False
-        self._last_packet_time: float = 0.0
-        self._watchdog_task: asyncio.Task[None] | None = None
-        self._behavior_task: asyncio.Task[None] | None = None
-        self._idle_stop_task: asyncio.Task[None] | None = None
-        self._active_ws: WebSocket | None = None
-
-    async def on_connect(self, websocket: WebSocket) -> bool:
-        """Admit a single controller. Stale sockets are replaced, not rejected.
-
-        The ESP client auto-reconnects after WiFi blips. If we still hold the
-        previous WebSocket object, a hard reject (1008) makes the board flap
-        forever and the face stays closed.
-
-        Movement keeps running across reconnects — stop/start would re-zero
-        velocity-clamp bookkeeping and let the next set_target snap the head.
-        """
-        if self._idle_stop_task is not None and not self._idle_stop_task.done():
-            self._idle_stop_task.cancel()
-            self._idle_stop_task = None
-
-        if self._active_ws is not None:
-            old = self._active_ws
-            alive = old.client_state == WebSocketState.CONNECTED
-            if alive:
-                logger.warning("Replacing active controller connection")
-            self._detach_controller(stop_movement=False)
-            if alive:
-                try:
-                    await old.close(code=1000)
-                except Exception:
-                    pass
-
-        await websocket.accept()
-        self._active_ws = websocket
-        self.movement.start()
-        if self.movement.resync_from_robot():
-            # Clutch base matches the physical head so idle/engage cannot yank home.
-            self.controller.set_base_pose(self.movement.current_pose)
-        self._watchdog_task = asyncio.create_task(self._watchdog_loop())
-        self._behavior_task = asyncio.create_task(self._behavior_loop())
-        logger.info("Controller connected")
-        return True
-
-    def _detach_controller(self, *, stop_movement: bool) -> None:
-        self.controller.force_disengage()
-        if stop_movement:
-            self.movement.stop()
-        for task in (self._watchdog_task, self._behavior_task):
-            if task is not None and not task.done():
-                task.cancel()
-        self._watchdog_task = None
-        self._behavior_task = None
-        self._active_ws = None
-
-    def cleanup(self, websocket: WebSocket | None = None) -> None:
-        """Drop the active controller. Ignore stale sockets that already lost the slot."""
-        if websocket is not None and self._active_ws is not websocket:
-            return
-        if self._active_ws is None and websocket is None:
-            return
-        # Keep the apply loop alive on a blip so velocity clamping stays
-        # continuous; schedule a stop if nothing reconnects.
-        self._detach_controller(stop_movement=False)
-        if self._idle_stop_task is None or self._idle_stop_task.done():
-            self._idle_stop_task = asyncio.create_task(self._idle_stop_after_grace())
-        logger.info("Controller disconnected")
-
-    async def _idle_stop_after_grace(self) -> None:
-        try:
-            await asyncio.sleep(IDLE_STOP_SEC)
-            if self._active_ws is None:
-                logger.info("No controller for %.1fs — stopping movement loop", IDLE_STOP_SEC)
-                self.movement.stop()
-        except asyncio.CancelledError:
-            pass
-
-    def shutdown(self) -> None:
-        """App teardown: always stop movement."""
-        if self._idle_stop_task is not None and not self._idle_stop_task.done():
-            self._idle_stop_task.cancel()
-            self._idle_stop_task = None
-        self._detach_controller(stop_movement=True)
-
-    async def handle_message(self, websocket: WebSocket, raw: str) -> None:
-        try:
-            msg: dict[str, Any] = json.loads(raw)
-        except json.JSONDecodeError as exc:
-            await self._send_error(websocket, "parse", f"Invalid JSON: {exc}")
-            return
-
-        msg_type = msg.get("type")
-        request_id = msg.get("_id")
-        handler = {
-            "controller_state": self._handle_controller_state,
-            "reset": self._handle_reset,
-            "status": self._handle_status,
-        }.get(msg_type)
-
-        if handler is None:
-            await self._send_error(
-                websocket,
-                msg_type or "unknown",
-                f"Unknown message type: {msg_type}",
-                request_id,
-            )
-            return
-
-        try:
-            response = await handler(msg)
-        except Exception as exc:
-            logger.error("Handler %s failed: %s", msg_type, exc)
-            await self._send_error(websocket, msg_type, str(exc), request_id)
-            return
-
-        if request_id is None or response is None:
-            return
-        response["_id"] = request_id
-        await websocket.send_json(response)
-
-    async def _handle_controller_state(self, msg: dict[str, Any]) -> dict[str, Any] | None:
-        self._last_packet_time = time.monotonic()
-        q = msg.get("q", [1, 0, 0, 0])
-        p = msg.get("p", [0, 0, 0])
-        engaged = bool(msg.get("engaged", False))
-        gain = float(msg.get("gain", 1.0))
-        ready = bool(msg.get("ready", False))
-
-        desired = self.controller.update(
-            q=q,
-            p=p,
-            engaged=engaged,
-            gain=gain,
-            ready=ready,
-            allow_engage=not self.busy,
-        )
-
-        # Reset goto owns the robot target exclusively — streaming set_target
-        # here fights the minjerk path and looks like a whip when rebase snaps.
-        if self.busy:
-            return None
-
-        # Behavior owns body_yaw + antennas from head yaw
-        body_yaw, antennas = self.behavior.update(desired["yaw"])
-        self.movement.set_target(desired, body_yaw=body_yaw, antennas=antennas)
-
-        if self.log_only:
-            logger.info(
-                "controller engaged=%s gain=%.2f ready=%s pose=%s body_yaw=%.3f",
-                self.controller.engaged,
-                self.controller.gain,
-                ready,
-                {k: round(desired[k], 4) for k in desired},
-                body_yaw,
-            )
-        return None
-
-    async def _handle_reset(self, _msg: dict[str, Any]) -> dict[str, Any]:
-        if self.busy:
-            return {"type": "reset_result", "success": False, "message": "Reset already in progress"}
-        if not self.robot_available and not self.log_only:
-            return {"type": "reset_result", "success": False, "message": "Robot not available"}
-
-        self.busy = True
-        self.controller.force_disengage()
-        # Zero clutch immediately so idle/behavior cannot re-target the old pose
-        # while the goto is running.
-        self.controller.rebase_neutral()
-        self.behavior.reset()
-
-        # Seed from the measured pose so goto starts where the head actually is
-        # (and clears a prior send-freeze). Fail closed when the robot is real.
-        if not await self.movement.resync_from_robot_async(update_target=True):
-            if not self.log_only:
-                self.busy = False
-                return {
-                    "type": "reset_result",
-                    "success": False,
-                    "message": "Robot pose unread",
-                }
-
-        neutral = {k: 0.0 for k in ("x", "y", "z", "roll", "pitch", "yaw")}
-        move_uuid = self.movement.goto(
-            neutral, body_yaw=0.0, antennas=[0.0, 0.0],
-            duration=RESET_DURATION_SEC, interpolation="minjerk",
-        )
-        asyncio.create_task(self._finish_reset(move_uuid))
-        return {"type": "reset_result", "success": True, "uuid": move_uuid}
-
-    async def _finish_reset(self, move_uuid: str) -> None:
-        try:
-            await asyncio.sleep(RESET_DURATION_SEC + 0.05)
-        finally:
-            self.movement.rebase_to_neutral()
-            self.controller.rebase_neutral()
-            self.behavior.reset()
-            self.busy = False
-            logger.info("Reset complete; clutch rebased to neutral (uuid=%s)", move_uuid)
-
-    async def _handle_status(self, _msg: dict[str, Any]) -> dict[str, Any]:
-        return {
-            "type": "status_result",
-            "connected": True,
-            "robot": self.robot_available,
-            "busy": self.busy,
-        }
-
-    async def _watchdog_loop(self) -> None:
-        try:
-            while True:
-                await asyncio.sleep(0.05)
-                if self._last_packet_time <= 0:
-                    continue
-                if time.monotonic() - self._last_packet_time > STALE_PACKET_SEC:
-                    if self.controller.engaged:
-                        logger.warning("Stale controller_state; forcing disengage")
-                        self.controller.force_disengage()
-                        # freeze: keep last desired as movement target, no further updates
-        except asyncio.CancelledError:
-            pass
-
-    async def _behavior_loop(self) -> None:
-        """Keep antennas / body-follow alive while idle (not engaged)."""
-        try:
-            while True:
-                await asyncio.sleep(BEHAVIOR_TICK_SEC)
-                if self.controller.engaged or self.busy:
-                    continue
-                pose = self.controller.desired_pose
-                body_yaw, antennas = self.behavior.update(pose["yaw"])
-                self.movement.set_target(pose, body_yaw=body_yaw, antennas=antennas)
-        except asyncio.CancelledError:
-            pass
-
-    async def _send_error(
-        self,
-        websocket: WebSocket,
-        request_type: str,
-        message: str,
-        request_id: Any = None,
-    ) -> None:
-        response: dict[str, Any] = {
-            "type": "error",
-            "request_type": request_type,
-            "message": message,
-        }
-        if request_id is not None:
-            response["_id"] = request_id
-        await websocket.send_json(response)

index.html

@@ -13,7 +13,7 @@
         Install this Space from the Reachy Mini Control desktop app, then hold
         the ESP32 display to clutch-drive Reachy Mini's head.
       </p>
-      <p>WebSocket once running: <code>ws://&lt;host&gt;:8766/ws</code></p>
+      <p>UDP once running: <code>&lt;host&gt;:8766</code></p>
     </main>
   </body>
 </html>

pyproject.toml

@@ -4,7 +4,7 @@ build-backend = "setuptools.build_meta"
 
 [project]
 name = "esp32_motion_controller"
-version = "2.0.0"
+version = "3.0.1"
 description = "ESP32 Motion Controller — handheld IMU controller for Reachy Mini"
 readme = "README.md"
 requires-python = ">=3.10"
@@ -19,7 +19,7 @@ dependencies = [
 keywords = ["reachy-mini-app"]
 
 [project.optional-dependencies]
-dev = ["pytest", "pytest-asyncio", "httpx", "websockets"]
+dev = ["pytest", "pytest-asyncio", "httpx"]
 
 [project.entry-points."reachy_mini_apps"]
 esp32_motion_controller = "esp32_motion_controller.main:Esp32MotionController"