v2: protocol rewrite with single host control owner

README.md

@@ -16,9 +16,9 @@ tags:
 
 # ESP32 Motion Controller
 
-WebSocket bridge 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.
+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.
 
-See the [repository README](https://github.com/V4C38/esp32-reachy-mini-controller) and [`PROTOCOL.md`](../PROTOCOL.md).
+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.
 
 ## Local install
 

esp32_motion_controller/__init__.py

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

esp32_motion_controller/control.py

@@ -0,0 +1,610 @@
+"""
+Pure control reducer for protocol v2.
+
+Owns clutch mapping, body follow, antenna phase, workspace projection, smoothing,
+stale release, and slew limiting. No I/O.
+"""
+
+from __future__ import annotations
+
+import math
+from dataclasses import dataclass, field, replace
+from typing import Sequence
+
+import numpy as np
+from scipy.spatial.transform import Rotation as R
+
+from esp32_motion_controller.protocol import Sample
+
+POSE_AXES = ("x", "y", "z", "roll", "pitch", "yaw")
+ANGULAR_AXES = ("roll", "pitch", "yaw")
+POSITIONAL_AXES = ("x", "y", "z")
+
+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,
+)
+
+TRANSLATION_SCALE = 0.30
+TRANSLATION_GAIN_DEFAULT = 1.0
+
+STALE_PACKET_SEC = 0.300
+CONTROL_HZ = 20.0
+CONTROL_DT = 1.0 / CONTROL_HZ
+
+# Match v1 feel: POSE_ALPHA=0.12 at 33 ms ≈ tau ~0.26 s
+POSE_TAU_SEC = 0.255
+ANTENNA_TAU_SEC = 0.39
+
+MAX_ANGULAR_VEL = 1.5  # rad/s
+MAX_POS_VEL = 0.05  # m/s
+MAX_DT_FOR_VEL_CLAMP = 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
+
+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
+
+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
+
+MAX_HEAD_YAW = 65.0 * math.pi / 180.0
+BODY_FOLLOW_THRESHOLD = 40.0 * math.pi / 180.0
+MAX_BODY_YAW = 160.0 * math.pi / 180.0
+
+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
+
+IK_FAIL_RETRACT_TARGET_ALPHA = 0.06
+IK_FAIL_CONSECUTIVE_THRESHOLD = 3
+
+Mode = str  # idle | engaged | resetting | fault
+
+
+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 _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:
+    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]:
+    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:
+    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)
+
+
+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_stewart_ellipsoid(
+    x: float, y: float, z: float, roll: float, pitch: float,
+) -> tuple[float, float, float, float, float]:
+    z_clamped = _clamp(z, LIMIT_HEAD_Z_MIN, LIMIT_HEAD_Z_MAX)
+    x_clamped = _clamp(x, LIMIT_HEAD_X_MIN, LIMIT_HEAD_X_MAX)
+    y_clamped = _clamp(y, LIMIT_HEAD_Y_MIN, LIMIT_HEAD_Y_MAX)
+
+    roll_c = _clamp(roll, -ELLIPSOID_ROLL_MAX_RAD, ELLIPSOID_ROLL_MAX_RAD)
+    pitch_c = _clamp(pitch, -ELLIPSOID_PITCH_MAX_RAD, ELLIPSOID_PITCH_MAX_RAD)
+
+    nr = roll_c / ELLIPSOID_ROLL_MAX_RAD if ELLIPSOID_ROLL_MAX_RAD > 0 else 0.0
+    np_ = pitch_c / ELLIPSOID_PITCH_MAX_RAD if ELLIPSOID_PITCH_MAX_RAD > 0 else 0.0
+    remaining = max(0.0, 1.0 - (nr * nr + np_ * np_))
+
+    nx = x_clamped / ELLIPSOID_X_MAX if ELLIPSOID_X_MAX > 0 else 0.0
+    ny = y_clamped / ELLIPSOID_Y_MAX if ELLIPSOID_Y_MAX > 0 else 0.0
+    nz = z_clamped / ELLIPSOID_Z_MAX if ELLIPSOID_Z_MAX > 0 else 0.0
+    trans_sq = nx * nx + ny * ny + nz * nz
+
+    if trans_sq <= remaining or trans_sq <= 1e-12:
+        return (x_clamped, y_clamped, z_clamped, roll_c, pitch_c)
+
+    scale = math.sqrt(remaining / trans_sq)
+    return (
+        x_clamped * scale,
+        y_clamped * scale,
+        z_clamped * scale,
+        roll_c,
+        pitch_c,
+    )
+
+
+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 _alpha(dt: float, tau: float) -> float:
+    if tau <= 0.0:
+        return 1.0
+    return 1.0 - math.exp(-max(dt, 0.0) / tau)
+
+
+def slew_limit(
+    baseline: dict[str, float],
+    baseline_body: float,
+    desired: dict[str, float],
+    desired_body: float,
+    dt: float,
+) -> tuple[dict[str, float], float]:
+    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:
+        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)
+
+
+@dataclass
+class ControlState:
+    mode: Mode = "idle"
+    robot_available: bool = True
+    error: str | None = None
+
+    engaged: bool = False
+    was_engaged: bool = False
+    gain: float = 1.0
+    ready: bool = False
+    translation_gain: float = TRANSLATION_GAIN_DEFAULT
+
+    q_ref: np.ndarray = field(default_factory=lambda: np.array([1.0, 0.0, 0.0, 0.0]))
+    p_ref: np.ndarray = field(default_factory=lambda: np.zeros(3))
+    base_pose: dict[str, float] = field(default_factory=zero_pose)
+    desired_pose: dict[str, float] = field(default_factory=zero_pose)
+
+    body_yaw: float = 0.0
+    antenna_left: float = 0.0
+    antenna_right: float = 0.0
+    behavior_t0: float = 0.0
+
+    smooth_pose: dict[str, float] = field(default_factory=zero_pose)
+    smooth_body_yaw: float = 0.0
+    smooth_antennas: list[float] = field(default_factory=lambda: [0.0, 0.0])
+
+    baseline_pose: dict[str, float] = field(default_factory=zero_pose)
+    baseline_body_yaw: float = 0.0
+    baseline_antennas: list[float] = field(default_factory=lambda: [0.0, 0.0])
+
+    last_sample_time: float = 0.0
+    have_sample: bool = False
+    sends_frozen: bool = True
+    seeded: bool = False
+    consecutive_ik_failures: int = 0
+
+    liveliness: float = DEFAULT_LIVELINESS
+    gaze_responsiveness: float = DEFAULT_GAZE_RESPONSIVENESS
+    antenna_activity: float = DEFAULT_ANTENNA_ACTIVITY
+
+
+@dataclass(frozen=True, slots=True)
+class Command:
+    pose: dict[str, float]
+    body_yaw: float
+    antennas: tuple[float, float]
+
+
+@dataclass(frozen=True, slots=True)
+class StepResult:
+    state: ControlState
+    command: Command | None
+    mode_changed: bool = False
+    host_error: str | None = None
+
+
+def initial_state(*, robot_available: bool, now: float) -> ControlState:
+    return ControlState(
+        robot_available=robot_available,
+        behavior_t0=now,
+        sends_frozen=not robot_available,
+        seeded=not robot_available,
+        mode="idle",
+    )
+
+
+def seed_from_pose(
+    state: ControlState,
+    pose: dict[str, float],
+    body_yaw: float,
+    *,
+    antennas: Sequence[float] | None = None,
+) -> 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)
+    )
+    return replace(
+        state,
+        base_pose=dict(pose_c),
+        desired_pose=dict(pose_c),
+        smooth_pose=dict(pose_c),
+        baseline_pose=dict(pose_c),
+        body_yaw=body_c,
+        smooth_body_yaw=body_c,
+        baseline_body_yaw=body_c,
+        antenna_left=float(ants[0]),
+        antenna_right=float(ants[1]),
+        smooth_antennas=[float(ants[0]), float(ants[1])],
+        baseline_antennas=[float(ants[0]), float(ants[1])],
+        seeded=True,
+        sends_frozen=False,
+        consecutive_ik_failures=0,
+        engaged=False,
+        was_engaged=False,
+        mode="idle" if state.mode != "resetting" else state.mode,
+        error=None,
+    )
+
+
+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()
+    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,
+            antenna_left=0.0,
+            antenna_right=0.0,
+            smooth_antennas=[0.0, 0.0],
+            engaged=False,
+            was_engaged=False,
+            q_ref=np.array([1.0, 0.0, 0.0, 0.0]),
+            p_ref=np.zeros(3),
+            sends_frozen=True,
+            mode="fault",
+            error="robot pose unread after reset",
+            behavior_t0=state.behavior_t0,
+        )
+    pose_c, body_c = clamp_pose_to_daemon_limits(measured_baseline, measured_body)
+    return replace(
+        state,
+        base_pose=dict(neutral),
+        desired_pose=dict(neutral),
+        smooth_pose=dict(neutral),
+        baseline_pose=dict(pose_c),
+        body_yaw=0.0,
+        smooth_body_yaw=0.0,
+        baseline_body_yaw=body_c,
+        antenna_left=0.0,
+        antenna_right=0.0,
+        smooth_antennas=[0.0, 0.0],
+        baseline_antennas=[0.0, 0.0],
+        engaged=False,
+        was_engaged=False,
+        q_ref=np.array([1.0, 0.0, 0.0, 0.0]),
+        p_ref=np.zeros(3),
+        sends_frozen=False,
+        seeded=True,
+        mode="idle",
+        error=None,
+        consecutive_ik_failures=0,
+    )
+
+
+def begin_reset(state: ControlState, now: float) -> ControlState:
+    st = force_disengage(state)
+    return replace(
+        st,
+        mode="resetting",
+        error=None,
+        base_pose=zero_pose(),
+        desired_pose=zero_pose(),
+        body_yaw=0.0,
+        antenna_left=0.0,
+        antenna_right=0.0,
+        was_engaged=False,
+        engaged=False,
+        q_ref=np.array([1.0, 0.0, 0.0, 0.0]),
+        p_ref=np.zeros(3),
+        behavior_t0=now,
+    )
+
+
+def force_disengage(state: ControlState) -> ControlState:
+    if state.engaged:
+        return replace(
+            state,
+            base_pose=dict(state.desired_pose),
+            engaged=False,
+            was_engaged=False,
+            mode="idle" if state.mode == "engaged" else state.mode,
+        )
+    return replace(state, engaged=False, was_engaged=False)
+
+
+def mark_sdk_success(state: ControlState, command: Command) -> ControlState:
+    return replace(
+        state,
+        baseline_pose=dict(command.pose),
+        baseline_body_yaw=command.body_yaw,
+        baseline_antennas=list(command.antennas),
+        consecutive_ik_failures=0,
+        sends_frozen=False,
+    )
+
+
+def mark_sdk_failure(state: ControlState) -> ControlState:
+    fails = state.consecutive_ik_failures + 1
+    desired = dict(state.desired_pose)
+    body = state.body_yaw
+    if fails >= IK_FAIL_CONSECUTIVE_THRESHOLD:
+        alpha = IK_FAIL_RETRACT_TARGET_ALPHA
+        for ax in POSE_AXES:
+            desired[ax] *= 1.0 - alpha
+        body *= 1.0 - alpha
+    return replace(
+        state,
+        consecutive_ik_failures=fails,
+        desired_pose=desired,
+        body_yaw=body,
+        sends_frozen=True,
+        mode="fault",
+        error="set_target failed",
+    )
+
+
+def mark_pose_unread(state: ControlState) -> ControlState:
+    return replace(
+        state,
+        sends_frozen=True,
+        mode="fault",
+        error="robot pose unread",
+    )
+
+
+def _update_clutch(state: ControlState, sample: Sample, *, allow_engage: bool) -> ControlState:
+    gain = _clamp(float(sample.gain), 0.1, 3.0)
+    q_dev = _finite_quat(sample.q)
+    p_dev = _finite_vec3(sample.p)
+    want = bool(sample.engaged) and bool(sample.ready) and allow_engage
+    rising = want and not state.was_engaged
+    falling = (not want) and state.was_engaged
+
+    q_ref = state.q_ref
+    p_ref = state.p_ref
+    base = dict(state.base_pose)
+    desired = dict(state.desired_pose)
+
+    if rising:
+        q_ref = q_dev.copy()
+        p_ref = p_dev.copy()
+
+    if want:
+        roll, pitch, yaw = quat_relative_rpy(q_ref, q_dev)
+        disp = remap_displacement(
+            p_dev - p_ref,
+            q_ref,
+            translation_gain=state.translation_gain * gain,
+        )
+        desired = {
+            "x": base["x"] + float(disp[0]),
+            "y": base["y"] + float(disp[1]),
+            "z": base["z"] + float(disp[2]),
+            "roll": base["roll"] + gain * roll,
+            "pitch": base["pitch"] + gain * pitch,
+            "yaw": base["yaw"] + gain * yaw,
+        }
+    elif falling:
+        base = dict(desired)
+
+    return replace(
+        state,
+        ready=bool(sample.ready),
+        gain=gain,
+        q_ref=q_ref,
+        p_ref=p_ref,
+        base_pose=base,
+        desired_pose=desired,
+        engaged=want,
+        was_engaged=want,
+    )
+
+
+def _advance_behavior(state: ControlState, head_yaw: float, now: float, dt: float) -> ControlState:
+    # Normalize behavior update against the legacy ~33 ms tick using dt scaling.
+    tick_scale = dt / 0.033 if dt > 0 else 1.0
+    t = now - state.behavior_t0
+    deg = math.pi / 180.0
+
+    yaw_smoothing = HEAD_MOVE_SPEED * state.gaze_responsiveness * tick_scale
+    max_yaw_delta = MAX_HEAD_DELTA_DEG * state.gaze_responsiveness * deg * tick_scale
+    body_smoothing = yaw_smoothing * 0.7 * (0.3 + state.liveliness * 0.4)
+    antenna_smoothing = yaw_smoothing * 1.5
+    effective_ant_amp = ANTENNA_AMPLITUDE_DEG * state.antenna_activity * deg
+    ant_speed = 0.5 + state.antenna_activity * 0.5
+
+    body_yaw = state.body_yaw
+    rel_yaw = head_yaw - body_yaw
+    if abs(rel_yaw) > BODY_FOLLOW_THRESHOLD:
+        excess = abs(rel_yaw) - BODY_FOLLOW_THRESHOLD
+        step = math.copysign(excess * body_smoothing * 8, rel_yaw)
+        body_yaw += _clamp(step, -max_yaw_delta, max_yaw_delta)
+        body_yaw = _clamp(body_yaw, -MAX_BODY_YAW, MAX_BODY_YAW)
+
+    desired_l = dual_sine(t * ant_speed, 1.3, 3.11) * effective_ant_amp
+    desired_r = dual_sine(t * ant_speed, 1.7, 2.73) * effective_ant_amp
+    ant_l = state.antenna_left + (desired_l - state.antenna_left) * antenna_smoothing
+    ant_r = state.antenna_right + (desired_r - state.antenna_right) * antenna_smoothing
+
+    return replace(
+        state,
+        body_yaw=body_yaw,
+        antenna_left=ant_l,
+        antenna_right=ant_r,
+    )
+
+
+def step(
+    state: ControlState,
+    *,
+    now: float,
+    dt: float,
+    sample: Sample | None,
+    sample_is_fresh: bool,
+) -> 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.
+    """
+    prev_mode = state.mode
+    st = state
+
+    if st.mode == "resetting":
+        # 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 sample is not None and sample_is_fresh and st.mode != "resetting":
+        allow_engage = st.mode != "fault" and not st.sends_frozen
+        st = _update_clutch(st, sample, allow_engage=allow_engage)
+        st = replace(st, have_sample=True)
+
+    if st.mode not in {"resetting"}:
+        st = _advance_behavior(st, st.desired_pose["yaw"], now, dt)
+
+    target_pose, target_body = clamp_pose_to_daemon_limits(st.desired_pose, st.body_yaw)
+    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)
+    a_ant = _alpha(dt, ANTENNA_TAU_SEC)
+    smooth = {
+        k: st.smooth_pose[k] + a_pose * (target_pose[k] - st.smooth_pose[k])
+        for k in POSE_AXES
+    }
+    smooth_body = st.smooth_body_yaw + a_pose * (target_body - st.smooth_body_yaw)
+    smooth_ants = [
+        st.smooth_antennas[i] + a_ant * (target_ants[i] - st.smooth_antennas[i])
+        for i in range(2)
+    ]
+
+    new_mode: Mode = st.mode
+    if st.mode not in {"resetting", "fault"}:
+        new_mode = "engaged" if st.engaged else "idle"
+
+    st = replace(
+        st,
+        desired_pose=dict(target_pose),
+        body_yaw=target_body,
+        smooth_pose=smooth,
+        smooth_body_yaw=smooth_body,
+        smooth_antennas=smooth_ants,
+        mode=new_mode,
+    )
+
+    if st.sends_frozen or not st.seeded:
+        return StepResult(
+            state=st,
+            command=None,
+            mode_changed=(st.mode != prev_mode),
+        )
+
+    send_pose, send_body = slew_limit(
+        st.baseline_pose,
+        st.baseline_body_yaw,
+        smooth,
+        smooth_body,
+        dt,
+    )
+    command = Command(
+        pose=send_pose,
+        body_yaw=send_body,
+        antennas=(smooth_ants[0], smooth_ants[1]),
+    )
+    return StepResult(
+        state=st,
+        command=command,
+        mode_changed=(st.mode != prev_mode),
+    )
+
+
+def note_sample_receipt(state: ControlState, receipt_time: float) -> ControlState:
+    return replace(state, last_sample_time=receipt_time, have_sample=True)

esp32_motion_controller/main.py

@@ -1,14 +1,13 @@
 """
-Motion Controller Reachy Mini app entry point.
+Motion Controller Reachy Mini app entry point (protocol v2).
 
-FastAPI/uvicorn on port 8766, mDNS advertise _reachyctl._tcp, clutch + behavior bridge.
+FastAPI/uvicorn on port 8766, mDNS advertise _reachyctl._tcp, clutch + safety bridge.
 """
 
 from __future__ import annotations
 
 import argparse
 import logging
-import os
 import socket
 import sys
 import threading
@@ -20,31 +19,17 @@ from fastapi import FastAPI, WebSocket, WebSocketDisconnect
 from fastapi.responses import FileResponse, JSONResponse
 from fastapi.staticfiles import StaticFiles
 
-from esp32_motion_controller.behavior import Behavior
-from esp32_motion_controller.controller_state import ControllerState
-from esp32_motion_controller.movement_handler import MovementHandler
-from esp32_motion_controller.ws_handler import WebSocketHandler
+from esp32_motion_controller.robot_control import RobotControl, RobotGateway
+from esp32_motion_controller.session import SessionHub
 
 logger = logging.getLogger(__name__)
 
 WS_PORT = 8766
 STATIC_DIR = Path(__file__).parent / "static"
 MDNS_SERVICE_TYPE = "_reachyctl._tcp.local."
-ENV_SEND_RATE_HZ = "REACHY_MOTION_SEND_RATE_HZ"
-DEFAULT_SEND_RATE_HZ = 20.0
 SERVER_START_TIMEOUT_S = 10.0
 
 
-def _get_send_rate_hz() -> float:
-    raw = os.environ.get(ENV_SEND_RATE_HZ)
-    if raw is None or raw.strip() == "":
-        return DEFAULT_SEND_RATE_HZ
-    try:
-        return max(5.0, min(50.0, float(raw.strip())))
-    except ValueError:
-        return DEFAULT_SEND_RATE_HZ
-
-
 def get_local_ips() -> list[str]:
     ips: list[str] = []
     try:
@@ -89,14 +74,10 @@ 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"},
+                properties={"path": b"/ws", "protocol": b"2"},
                 server="esp32-motion.local.",
             )
             zc = Zeroconf()
-            # An advertisement from a previous run lingers in the mDNS cache for
-            # the record TTL (75 min), so a restart collides with itself.
-            # Renaming is safe: the controller browses the service type, never
-            # the instance name.
             zc.register_service(info, allow_name_change=True)
         except Exception:
             logger.warning(
@@ -136,35 +117,41 @@ def create_app(
     log_only: bool = False,
 ) -> FastAPI:
     app = FastAPI(title="ESP32 Motion Controller")
-    send_rate = _get_send_rate_hz()
     robot_available = reachy_mini is not None and not log_only
-    movement = MovementHandler(reachy_mini, send_rate_hz=send_rate)
-    controller = ControllerState()
-    behavior = Behavior()
-    ws_handler = WebSocketHandler(
-        movement,
-        controller,
-        behavior,
-        robot_available=robot_available or log_only,
+    session = SessionHub(robot_available=robot_available or log_only)
+    gateway = RobotGateway(reachy_mini, log_only=log_only)
+    control = RobotControl(
+        session,
+        gateway,
+        robot_available=robot_available,
         log_only=log_only,
     )
-    app.state.ws_handler = ws_handler
+    app.state.session = session
+    app.state.control = control
+
+    @app.on_event("startup")
+    async def _startup() -> None:
+        control.start()
+
+    @app.on_event("shutdown")
+    async def _shutdown() -> None:
+        await control.stop()
 
     @app.websocket("/ws")
     async def websocket_endpoint(websocket: WebSocket) -> None:
-        admitted = await ws_handler.on_connect(websocket)
-        if not admitted:
-            return
+        generation = await session.on_connect(websocket)
+        await control.on_controller_connected()
         try:
             while not stop_event.is_set():
                 raw = await websocket.receive_text()
-                await ws_handler.handle_message(websocket, raw)
+                await session.handle_message(websocket, generation, raw)
         except WebSocketDisconnect:
             logger.info("WebSocket disconnect")
         except Exception as exc:
             logger.error("WebSocket error: %s", exc)
         finally:
-            ws_handler.cleanup(websocket)
+            await session.cleanup(websocket, generation)
+            await control.on_controller_disconnected()
 
     @app.get("/api/info")
     async def info() -> JSONResponse:
@@ -175,17 +162,22 @@ def create_app(
                 "port": WS_PORT,
                 "ws_url": f"ws://{ips[0]}:{WS_PORT}/ws" if ips else None,
                 "mdns": MDNS_SERVICE_TYPE,
+                "protocol_version": 2,
             }
         )
 
     @app.get("/api/status")
     async def status() -> JSONResponse:
+        snap = await session.snapshot_status()
         return JSONResponse(
             {
                 "status": "ok",
                 "robot": robot_available or log_only,
-                "busy": ws_handler.busy,
+                "busy": snap["busy"],
+                "mode": snap["mode"],
+                "connected": snap["connected"],
                 "log_only": log_only,
+                "protocol_version": 2,
             }
         )
 
@@ -200,7 +192,7 @@ def create_app(
 def _run_server(reachy_mini, stop_event: threading.Event, *, log_only: bool) -> None:
     ips = get_local_ips()
     logger.info("=" * 50)
-    logger.info("ESP32 Motion Controller")
+    logger.info("ESP32 Motion Controller (protocol v2)")
     logger.info("=" * 50)
     for ip in ips:
         logger.info("  WebSocket: ws://%s:%d/ws", ip, WS_PORT)
@@ -209,15 +201,11 @@ def _run_server(reachy_mini, stop_event: threading.Event, *, log_only: bool) ->
     logger.info("=" * 50)
 
     app = create_app(reachy_mini, stop_event, log_only=log_only)
-    # Stash for shutdown — create_app closes over the handler.
-    ws_handler: WebSocketHandler = app.state.ws_handler  # type: ignore[attr-defined]
     config = uvicorn.Config(app, host="0.0.0.0", port=WS_PORT, log_level="info")
     server = uvicorn.Server(config)
     thread = threading.Thread(target=server.run, daemon=True)
     thread.start()
 
-    # A failed bind only kills the server thread, so without this the app would
-    # sit here advertising a port nothing listens on.
     deadline = time.monotonic() + SERVER_START_TIMEOUT_S
     while not server.started and thread.is_alive() and time.monotonic() < deadline:
         time.sleep(0.05)
@@ -227,12 +215,11 @@ def _run_server(reachy_mini, stop_event: threading.Event, *, log_only: bool) ->
             f"instance is probably already running"
         )
 
-    # Advertise only once we are actually listening.
     mdns = MdnsAdvertiser(WS_PORT)
     mdns.start()
 
     stop_event.wait()
-    ws_handler.shutdown()
+    # Shutdown path: uvicorn will fire FastAPI shutdown hooks.
     server.should_exit = True
     thread.join(timeout=5)
     mdns.stop()

esp32_motion_controller/protocol.py

@@ -0,0 +1,229 @@
+"""
+Protocol v2 parsing — dependency-light, no SDK imports.
+
+Rejects oversize frames, wrong types, non-finite numbers, and unsupported versions.
+"""
+
+from __future__ import annotations
+
+import json
+import math
+from dataclasses import dataclass
+from typing import Any
+
+PROTOCOL_VERSION = 2
+MAX_FRAME_BYTES = 512
+GAIN_MIN = 0.1
+GAIN_MAX = 3.0
+
+
+class ProtocolError(ValueError):
+    def __init__(self, message: str, *, request_type: str = "unknown") -> None:
+        super().__init__(message)
+        self.request_type = request_type
+
+
+@dataclass(frozen=True, slots=True)
+class Hello:
+    protocol_version: int
+    boot_id: str
+    device: str
+
+
+@dataclass(frozen=True, slots=True)
+class Sample:
+    boot_id: str
+    seq: int
+    q: tuple[float, float, float, float]
+    p: tuple[float, float, float]
+    engaged: bool
+    gain: float
+    ready: bool
+
+
+@dataclass(frozen=True, slots=True)
+class Reset:
+    boot_id: str
+    op_id: int
+
+
+def _require_dict(msg: Any, request_type: str) -> dict[str, Any]:
+    if not isinstance(msg, dict):
+        raise ProtocolError("JSON root must be an object", request_type=request_type)
+    return msg
+
+
+def _require_str(msg: dict[str, Any], key: str, request_type: str) -> str:
+    val = msg.get(key)
+    if not isinstance(val, str) or not val:
+        raise ProtocolError(f"{key} must be a non-empty string", request_type=request_type)
+    return val
+
+
+def _require_int(msg: dict[str, Any], key: str, request_type: str) -> int:
+    val = msg.get(key)
+    if isinstance(val, bool) or not isinstance(val, int):
+        raise ProtocolError(f"{key} must be an integer", request_type=request_type)
+    return int(val)
+
+
+def _require_bool(msg: dict[str, Any], key: str, request_type: str) -> bool:
+    val = msg.get(key)
+    if not isinstance(val, bool):
+        raise ProtocolError(f"{key} must be a boolean", request_type=request_type)
+    return val
+
+
+def _require_finite_float(msg: dict[str, Any], key: str, request_type: str) -> float:
+    val = msg.get(key)
+    if isinstance(val, bool) or not isinstance(val, (int, float)):
+        raise ProtocolError(f"{key} must be a number", request_type=request_type)
+    out = float(val)
+    if not math.isfinite(out):
+        raise ProtocolError(f"{key} must be finite", request_type=request_type)
+    return out
+
+
+def _require_vec(msg: dict[str, Any], key: str, n: int, request_type: str) -> tuple[float, ...]:
+    val = msg.get(key)
+    if not isinstance(val, list) or len(val) != n:
+        raise ProtocolError(f"{key} must be an array of length {n}", request_type=request_type)
+    out: list[float] = []
+    for i, item in enumerate(val):
+        if isinstance(item, bool) or not isinstance(item, (int, float)):
+            raise ProtocolError(f"{key}[{i}] must be a number", request_type=request_type)
+        f = float(item)
+        if not math.isfinite(f):
+            raise ProtocolError(f"{key}[{i}] must be finite", request_type=request_type)
+        out.append(f)
+    return tuple(out)
+
+
+def parse_frame(raw: str | bytes) -> Hello | Sample | Reset:
+    if isinstance(raw, bytes):
+        if len(raw) > MAX_FRAME_BYTES:
+            raise ProtocolError("frame exceeds size limit", request_type="parse")
+        try:
+            text = raw.decode("utf-8")
+        except UnicodeDecodeError as exc:
+            raise ProtocolError(f"invalid UTF-8: {exc}", request_type="parse") from exc
+    else:
+        if len(raw.encode("utf-8")) > MAX_FRAME_BYTES:
+            raise ProtocolError("frame exceeds size limit", request_type="parse")
+        text = raw
+
+    try:
+        msg = json.loads(text)
+    except json.JSONDecodeError as exc:
+        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")
+
+    if msg_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)
+
+
+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,
+        boot_id=_require_str(msg, "boot_id", request_type),
+        device=_require_str(msg, "device", request_type) if "device" in msg else "esp32",
+    )
+
+
+def parse_sample(msg: dict[str, Any]) -> Sample:
+    request_type = "sample"
+    q = _require_vec(msg, "q", 4, request_type)
+    p = _require_vec(msg, "p", 3, request_type)
+    gain = _require_finite_float(msg, "gain", request_type)
+    if gain < GAIN_MIN or gain > GAIN_MAX:
+        raise ProtocolError(
+            f"gain out of range [{GAIN_MIN}, {GAIN_MAX}]",
+            request_type=request_type,
+        )
+    seq = _require_int(msg, "seq", request_type)
+    if seq < 0:
+        raise ProtocolError("seq must be non-negative", request_type=request_type)
+    return Sample(
+        boot_id=_require_str(msg, "boot_id", request_type),
+        seq=seq,
+        q=(q[0], q[1], q[2], q[3]),
+        p=(p[0], p[1], p[2]),
+        engaged=_require_bool(msg, "engaged", request_type),
+        gain=gain,
+        ready=_require_bool(msg, "ready", request_type),
+    )
+
+
+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(
+    *,
+    robot: bool,
+    mode: str,
+    error: str | None = None,
+) -> dict[str, Any]:
+    return {
+        "type": "host_state",
+        "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
+    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

@@ -0,0 +1,372 @@
+"""
+Single fixed-rate robot command owner for protocol v2.
+
+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.
+"""
+
+from __future__ import annotations
+
+import asyncio
+import logging
+import time
+from typing import Any
+
+import numpy as np
+from scipy.spatial.transform import Rotation
+
+from esp32_motion_controller.control import (
+    CONTROL_DT,
+    CONTROL_HZ,
+    Command,
+    ControlState,
+    begin_reset,
+    force_disengage,
+    initial_state,
+    mark_pose_unread,
+    mark_sdk_failure,
+    mark_sdk_success,
+    note_sample_receipt,
+    rebase_neutral,
+    seed_from_pose,
+    step,
+    zero_pose,
+)
+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
+
+
+class RobotGateway:
+    """Thin SDK adapter — all blocking calls run in an executor by RobotControl."""
+
+    def __init__(self, reachy_mini: Any | None, *, log_only: bool = False) -> None:
+        self.mini = reachy_mini
+        self.log_only = log_only
+
+    def read_pose(self) -> tuple[dict[str, float], float] | None:
+        if self.mini is None:
+            return zero_pose(), 0.0
+        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
+
+    def set_target(self, command: Command) -> None:
+        if self.mini is None or self.log_only:
+            return
+        from reachy_mini.utils import create_head_pose
+
+        head = create_head_pose(
+            x=command.pose["x"],
+            y=command.pose["y"],
+            z=command.pose["z"],
+            roll=command.pose["roll"],
+            pitch=command.pose["pitch"],
+            yaw=command.pose["yaw"],
+            degrees=False,
+        )
+        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__(
+        self,
+        session: SessionHub,
+        gateway: RobotGateway,
+        *,
+        robot_available: bool,
+        log_only: bool = False,
+        hz: float = CONTROL_HZ,
+    ) -> None:
+        self.session = session
+        self.gateway = gateway
+        self.robot_available = robot_available
+        self.log_only = log_only
+        self.dt = 1.0 / hz
+        self._task: asyncio.Task[None] | None = None
+        self._state = initial_state(robot_available=robot_available or log_only, now=time.monotonic())
+        self._last_seen_seq: int | None = None
+        self._last_reconcile = 0.0
+        self._sdk_lock = asyncio.Lock()
+        self._started = False
+
+    @property
+    def state(self) -> ControlState:
+        return self._state
+
+    def start(self) -> None:
+        if self._task is None or self._task.done():
+            self._task = asyncio.create_task(self._loop(), name="robot_control")
+            self._started = True
+
+    async def stop(self) -> None:
+        if self._task is not None and not self._task.done():
+            self._task.cancel()
+            try:
+                await self._task
+            except asyncio.CancelledError:
+                pass
+        self._task = None
+        self._started = False
+
+    async def _loop(self) -> None:
+        loop = asyncio.get_running_loop()
+        last = time.monotonic()
+        try:
+            # Initial seed attempt.
+            await self._seed(update_host=True)
+            while True:
+                t0 = time.monotonic()
+                dt = min(t0 - last, 0.05)
+                if dt <= 0:
+                    dt = self.dt
+                last = t0
+
+                await self._tick(loop, t0, dt)
+
+                elapsed = time.monotonic() - t0
+                await asyncio.sleep(max(0.0, self.dt - elapsed))
+        except asyncio.CancelledError:
+            pass
+
+    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
+
+        latest = await self.session.take_latest_sample()
+        sample = None
+        sample_is_fresh = False
+        if latest is not None:
+            sample = latest.sample
+            if self._last_seen_seq != sample.seq:
+                sample_is_fresh = True
+                self._last_seen_seq = sample.seq
+            self._state = note_sample_receipt(self._state, latest.receipt_time)
+
+        # Idle reconciliation when disengaged and not frozen.
+        if (
+            self._state.mode == "idle"
+            and not self._state.engaged
+            and not self._state.sends_frozen
+            and (now - self._last_reconcile) >= IDLE_RECONCILE_SEC
+        ):
+            await self._reconcile_idle(loop, now)
+
+        prev_mode = self._state.mode
+        result = step(
+            self._state,
+            now=now,
+            dt=dt,
+            sample=sample,
+            sample_is_fresh=sample_is_fresh,
+        )
+        self._state = result.state
+
+        if result.mode_changed or self._state.mode != prev_mode:
+            await self.session.push_host_state(
+                mode=self._state.mode,
+                robot=self.robot_available or self.log_only,
+                error=self._state.error,
+                clear_error=self._state.error is None,
+            )
+
+        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)
+
+    async def _seed(self, *, update_host: bool) -> bool:
+        loop = asyncio.get_running_loop()
+        read = await loop.run_in_executor(None, self.gateway.read_pose)
+        if read is None:
+            self._state = mark_pose_unread(self._state)
+            if update_host:
+                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._last_reconcile = time.monotonic()
+        if update_host:
+            await self.session.push_host_state(mode="idle", clear_error=True)
+        return True
+
+    async def _reconcile_idle(
+        self, loop: asyncio.AbstractEventLoop, now: float
+    ) -> None:
+        self._last_reconcile = now
+        if self.gateway.mini is None:
+            return
+        read = await loop.run_in_executor(None, self.gateway.read_pose)
+        if read is None:
+            self._state = mark_pose_unread(self._state)
+            await self.session.push_host_state(mode="fault", error=self._state.error)
+            return
+        pose, body = read
+        prev = self._state.baseline_pose
+        pos = (
+            (pose["x"] - prev["x"]) ** 2
+            + (pose["y"] - prev["y"]) ** 2
+            + (pose["z"] - prev["z"]) ** 2
+        ) ** 0.5
+        ang = max(
+            abs(pose["roll"] - prev["roll"]),
+            abs(pose["pitch"] - prev["pitch"]),
+            abs(pose["yaw"] - prev["yaw"]),
+            abs(body - self._state.baseline_body_yaw),
+        )
+        if pos <= RESYNC_EPS_POS_M and ang <= RESYNC_EPS_ANG_RAD:
+            self._state = mark_sdk_success(
+                self._state,
+                Command(pose=pose, body_yaw=body, antennas=tuple(self._state.baseline_antennas[:2])),
+            )
+            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 = begin_reset(self._state, now)
+        await self.session.push_host_state(mode="resetting", clear_error=True)
+
+        # 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 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",
+            )
+            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
+        )
+        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.
+
+        Do not push host_state here — the hello handler sends the first snapshot
+        so the device always sees hello before host_state.
+        """
+        self._state = force_disengage(self._state)
+        self._last_seen_seq = None
+        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)
+
+    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")

esp32_motion_controller/session.py

@@ -0,0 +1,333 @@
+"""
+WebSocket session ingress for protocol v2.
+
+Validates frames, keeps only the latest sample, manages session generation and
+reset operation mailbox. Never calls the robot SDK.
+"""
+
+from __future__ import annotations
+
+import asyncio
+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 (
+    Hello,
+    ProtocolError,
+    Reset,
+    Sample,
+    encode_error,
+    encode_hello_response,
+    encode_host_state,
+    encode_reset_result,
+    parse_frame,
+)
+
+logger = logging.getLogger(__name__)
+
+SendFn = Callable[[dict[str, Any]], Awaitable[None]]
+
+
+@dataclass
+class LatestSample:
+    sample: Sample
+    receipt_time: float
+    generation: int
+
+
+@dataclass
+class ResetRecord:
+    boot_id: str
+    op_id: int
+    status: str  # accepted | completed | failed
+    message: str | None = None
+
+
+@dataclass
+class SessionMailbox:
+    """Shared state between the WS 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
+    latest: LatestSample | None = None
+    last_seq: int | None = None
+    pending_reset: Reset | None = None
+    reset_cache: dict[tuple[str, int], ResetRecord] = field(default_factory=dict)
+    host_mode: str = "idle"
+    host_robot: bool = True
+    host_error: str | None = None
+    controller_present: bool = False
+
+
+class SessionHub:
+    def __init__(self, *, robot_available: bool) -> None:
+        self.mailbox = SessionMailbox(host_robot=robot_available)
+        self._send_lock = asyncio.Lock()
+
+    @property
+    def generation(self) -> int:
+        return self.mailbox.generation
+
+    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:
+        try:
+            msg = parse_frame(raw)
+        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
+            return
+
+        if isinstance(msg, Hello):
+            await self._handle_hello(websocket, generation, msg)
+            return
+        if isinstance(msg, Sample):
+            await self._handle_sample(websocket, generation, msg)
+            return
+        if isinstance(msg, Reset):
+            await self._handle_reset(websocket, generation, msg)
+            return
+
+    async def _handle_hello(self, websocket: WebSocket, generation: int, hello: Hello) -> 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
+            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),
+        )
+        logger.info("Hello ok boot_id=%s generation=%d", hello.boot_id, generation)
+
+    async def _handle_sample(self, websocket: WebSocket, generation: int, sample: Sample) -> 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
+            if mb.last_seq is not None:
+                # Duplicate
+                if sample.seq == mb.last_seq:
+                    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
+            mb.last_seq = sample.seq
+            mb.latest = LatestSample(sample=sample, receipt_time=now, generation=generation)
+
+    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")
+
+    async def take_latest_sample(self) -> LatestSample | None:
+        mb = self.mailbox
+        async with mb.lock:
+            return mb.latest
+
+    async def take_pending_reset(self) -> Reset | None:
+        mb = self.mailbox
+        async with mb.lock:
+            reset = mb.pending_reset
+            mb.pending_reset = None
+            return reset
+
+    async def complete_reset(
+        self,
+        *,
+        boot_id: str,
+        op_id: int,
+        status: str,
+        message: str | None = None,
+    ) -> None:
+        mb = self.mailbox
+        async with mb.lock:
+            mb.reset_cache[(boot_id, op_id)] = ResetRecord(
+                boot_id=boot_id, op_id=op_id, status=status, message=message
+            )
+            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,
+        *,
+        mode: str | None = None,
+        robot: bool | None = None,
+        error: str | None = None,
+        clear_error: bool = False,
+    ) -> None:
+        mb = self.mailbox
+        async with mb.lock:
+            if mode is not None:
+                mb.host_mode = mode
+            if robot is not None:
+                mb.host_robot = robot
+            if clear_error:
+                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:
+            return {
+                "robot": mb.host_robot,
+                "busy": mb.host_mode == "resetting",
+                "mode": mb.host_mode,
+                "connected": mb.controller_present,
+                "error": mb.host_error,
+            }
+
+    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)

esp32_motion_controller/static/index.html

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

pyproject.toml

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