v1: ESP32 Reachy Mini motion controller

Co-authored-by: Cursor <cursoragent@cursor.com>

.gitattributes

@@ -0,0 +1,35 @@
+*.7z filter=lfs diff=lfs merge=lfs -text
+*.arrow filter=lfs diff=lfs merge=lfs -text
+*.bin filter=lfs diff=lfs merge=lfs -text
+*.bz2 filter=lfs diff=lfs merge=lfs -text
+*.ckpt filter=lfs diff=lfs merge=lfs -text
+*.ftz filter=lfs diff=lfs merge=lfs -text
+*.gz filter=lfs diff=lfs merge=lfs -text
+*.h5 filter=lfs diff=lfs merge=lfs -text
+*.joblib filter=lfs diff=lfs merge=lfs -text
+*.lfs.* filter=lfs diff=lfs merge=lfs -text
+*.mlmodel filter=lfs diff=lfs merge=lfs -text
+*.model filter=lfs diff=lfs merge=lfs -text
+*.msgpack filter=lfs diff=lfs merge=lfs -text
+*.npy filter=lfs diff=lfs merge=lfs -text
+*.npz filter=lfs diff=lfs merge=lfs -text
+*.onnx filter=lfs diff=lfs merge=lfs -text
+*.ot filter=lfs diff=lfs merge=lfs -text
+*.parquet filter=lfs diff=lfs merge=lfs -text
+*.pb filter=lfs diff=lfs merge=lfs -text
+*.pickle filter=lfs diff=lfs merge=lfs -text
+*.pkl filter=lfs diff=lfs merge=lfs -text
+*.pt filter=lfs diff=lfs merge=lfs -text
+*.pth filter=lfs diff=lfs merge=lfs -text
+*.rar filter=lfs diff=lfs merge=lfs -text
+*.safetensors filter=lfs diff=lfs merge=lfs -text
+saved_model/**/* filter=lfs diff=lfs merge=lfs -text
+*.tar.* filter=lfs diff=lfs merge=lfs -text
+*.tar filter=lfs diff=lfs merge=lfs -text
+*.tflite filter=lfs diff=lfs merge=lfs -text
+*.tgz filter=lfs diff=lfs merge=lfs -text
+*.wasm filter=lfs diff=lfs merge=lfs -text
+*.xz filter=lfs diff=lfs merge=lfs -text
+*.zip filter=lfs diff=lfs merge=lfs -text
+*.zst filter=lfs diff=lfs merge=lfs -text
+*tfevents* filter=lfs diff=lfs merge=lfs -text

.gitignore

@@ -0,0 +1,5 @@
+.DS_Store
+__pycache__/
+*.pyc
+.venv/
+*.egg-info/

README.md

@@ -0,0 +1,57 @@
+---
+title: ESP32 Motion Controller — Reachy Mini
+emoji: 🕹️
+colorFrom: gray
+colorTo: yellow
+sdk: static
+pinned: false
+short_description: ESP32 handheld motion controller for Reachy Mini
+tags:
+  - reachy_mini
+  - reachy_mini_python_app
+  - esp32
+  - websocket
+  - imu
+---
+
+# 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.
+
+See the [repository README](https://github.com/V4C38/esp32-reachy-mini-controller) and [`PROTOCOL.md`](../PROTOCOL.md).
+
+## Local install
+
+```bash
+pip install -e .
+python -m esp32_motion_controller.main
+```
+
+WebSocket: `ws://<host>:8766/ws`
+
+
+Live Space: https://huggingface.co/spaces/V4C38/esp32_motion_controller
+
+## Publish to Hugging Face
+
+Tags required: `reachy_mini`, `reachy_mini_python_app`.
+
+```bash
+# from reachy-mini-app/
+hf auth login
+hf upload YOUR_USER/esp32_motion_controller . --repo-type space \
+  --exclude ".venv/*" \
+  --exclude "build/*" \
+  --exclude "dist/*" \
+  --exclude "*.egg-info/*" \
+  --exclude "**/__pycache__/*" \
+  --exclude "*.pyc" \
+  --exclude ".pytest_cache/*" \
+  --exclude ".DS_Store"
+```
+
+Ensure README frontmatter includes those tags before upload.
+
+The excludes matter: `packages.find` would otherwise pick up a stray `build/lib/`
+tree as a second copy of the package, and `.pyc` files built by a different
+Python end up installed into the daemon's 3.12 venv.

esp32_motion_controller/__init__.py

@@ -0,0 +1,3 @@
+"""ESP32 Motion Controller Reachy Mini app."""
+
+__version__ = "1.0.0"

esp32_motion_controller/behavior.py

@@ -0,0 +1,100 @@
+"""
+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/controller_state.py

@@ -0,0 +1,197 @@
+"""
+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

@@ -0,0 +1,298 @@
+"""
+Motion Controller Reachy Mini app entry point.
+
+FastAPI/uvicorn on port 8766, mDNS advertise _reachyctl._tcp, clutch + behavior bridge.
+"""
+
+from __future__ import annotations
+
+import argparse
+import logging
+import os
+import socket
+import sys
+import threading
+import time
+from pathlib import Path
+
+import uvicorn
+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
+
+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:
+        s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+        s.connect(("8.8.8.8", 80))
+        ips.append(s.getsockname()[0])
+        s.close()
+    except OSError:
+        pass
+    try:
+        for info in socket.getaddrinfo(socket.gethostname(), None, socket.AF_INET):
+            addr = info[4][0]
+            if not addr.startswith("127."):
+                ips.append(addr)
+    except (socket.gaierror, OSError):
+        pass
+    return list(dict.fromkeys(ips))
+
+
+class MdnsAdvertiser:
+    def __init__(self, port: int = WS_PORT) -> None:
+        self.port = port
+        self._zc = None
+        self._info = None
+
+    def start(self) -> None:
+        """Advertise over mDNS. Never raises — discovery is a convenience."""
+        try:
+            from zeroconf import ServiceInfo, Zeroconf
+        except ImportError:
+            logger.warning("zeroconf not installed; mDNS advertise skipped")
+            return
+
+        zc = None
+        try:
+            ips = get_local_ips()
+            if not ips:
+                logger.warning("No local IP for mDNS advertise")
+                return
+            info = ServiceInfo(
+                MDNS_SERVICE_TYPE,
+                f"esp32-motion-controller.{MDNS_SERVICE_TYPE}",
+                addresses=[socket.inet_aton(ip) for ip in ips],
+                port=self.port,
+                properties={"path": b"/ws"},
+                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(
+                "mDNS advertise failed; controller must use a configured host",
+                exc_info=True,
+            )
+            if zc is not None:
+                try:
+                    zc.close()
+                except Exception:
+                    pass
+            return
+
+        self._zc = zc
+        self._info = info
+        logger.info("mDNS advertised %s on %s:%d", info.name, ips[0], self.port)
+
+    def stop(self) -> None:
+        if self._zc is not None:
+            try:
+                if self._info is not None:
+                    self._zc.unregister_service(self._info)
+            except Exception:
+                pass
+            try:
+                self._zc.close()
+            except Exception:
+                pass
+        self._zc = None
+        self._info = None
+
+
+def create_app(
+    reachy_mini,
+    stop_event: threading.Event,
+    *,
+    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,
+        log_only=log_only,
+    )
+    app.state.ws_handler = ws_handler
+
+    @app.websocket("/ws")
+    async def websocket_endpoint(websocket: WebSocket) -> None:
+        admitted = await ws_handler.on_connect(websocket)
+        if not admitted:
+            return
+        try:
+            while not stop_event.is_set():
+                raw = await websocket.receive_text()
+                await ws_handler.handle_message(websocket, raw)
+        except WebSocketDisconnect:
+            logger.info("WebSocket disconnect")
+        except Exception as exc:
+            logger.error("WebSocket error: %s", exc)
+        finally:
+            ws_handler.cleanup(websocket)
+
+    @app.get("/api/info")
+    async def info() -> JSONResponse:
+        ips = get_local_ips()
+        return JSONResponse(
+            {
+                "ips": ips,
+                "port": WS_PORT,
+                "ws_url": f"ws://{ips[0]}:{WS_PORT}/ws" if ips else None,
+                "mdns": MDNS_SERVICE_TYPE,
+            }
+        )
+
+    @app.get("/api/status")
+    async def status() -> JSONResponse:
+        return JSONResponse(
+            {
+                "status": "ok",
+                "robot": robot_available or log_only,
+                "busy": ws_handler.busy,
+                "log_only": log_only,
+            }
+        )
+
+    @app.get("/")
+    async def root() -> FileResponse:
+        return FileResponse(STATIC_DIR / "index.html")
+
+    app.mount("/static", StaticFiles(directory=str(STATIC_DIR)), name="static")
+    return 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("=" * 50)
+    for ip in ips:
+        logger.info("  WebSocket: ws://%s:%d/ws", ip, WS_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)
+    # 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)
+    if not server.started:
+        raise RuntimeError(
+            f"Could not serve on port {WS_PORT} — another Motion Controller "
+            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()
+    server.should_exit = True
+    thread.join(timeout=5)
+    mdns.stop()
+    logger.info("Motion Controller stopped")
+
+
+try:
+    from reachy_mini import ReachyMini, ReachyMiniApp
+
+    class Esp32MotionController(ReachyMiniApp):
+        """Reachy Mini App that bridges an ESP32 motion controller."""
+
+        name = "ESP32 Motion Controller"
+        emoji = "🕹️"
+        custom_app_url: str | None = None
+
+        def run(self, reachy_mini: ReachyMini, stop_event: threading.Event) -> None:
+            _run_server(reachy_mini, stop_event, log_only=False)
+
+except ImportError:
+    class Esp32MotionController:  # type: ignore[no-redef]
+        name = "ESP32 Motion Controller"
+        emoji = "🕹️"
+        custom_app_url = None
+
+        def run(self, reachy_mini, stop_event: threading.Event) -> None:
+            _run_server(reachy_mini, stop_event, log_only=False)
+
+        def wrapped_run(self) -> None:
+            raise RuntimeError("reachy-mini SDK is required unless using --log-only")
+
+        def stop(self) -> None:
+            pass
+
+
+def main(argv: list[str] | None = None) -> int:
+    logging.basicConfig(level=logging.INFO, format="%(levelname)s %(name)s: %(message)s")
+    parser = argparse.ArgumentParser(description="ESP32 Motion Controller for Reachy Mini")
+    parser.add_argument(
+        "--log-only",
+        action="store_true",
+        help="Run without a ReachyMini SDK instance; log decoded controller state",
+    )
+    args = parser.parse_args(argv)
+    stop = threading.Event()
+
+    if args.log_only:
+        try:
+            _run_server(None, stop, log_only=True)
+        except KeyboardInterrupt:
+            stop.set()
+        return 0
+
+    app = Esp32MotionController()
+    try:
+        app.wrapped_run()
+    except KeyboardInterrupt:
+        app.stop()
+    return 0
+
+
+if __name__ == "__main__":
+    sys.exit(main())

esp32_motion_controller/movement_handler.py

@@ -0,0 +1,596 @@
+"""
+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/static/index.html

@@ -0,0 +1,24 @@
+<!DOCTYPE html>
+<html lang="en">
+<head>
+  <meta charset="utf-8" />
+  <meta name="viewport" content="width=device-width, initial-scale=1" />
+  <title>ESP32 Motion Controller — Reachy Mini</title>
+  <style>
+    body { font-family: system-ui, sans-serif; background: #111; color: #eee; margin: 2rem; }
+    a { color: #8bd3ff; }
+    code { background: #222; padding: 0.15rem 0.4rem; border-radius: 4px; }
+  </style>
+</head>
+<body>
+  <h1>🕹️ ESP32 Motion Controller</h1>
+  <p>ESP32 handheld IMU controller bridge for Reachy Mini.</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>
+    fetch('/api/info').then(r => r.json()).then(j => {
+      if (j.ws_url) document.getElementById('ws').textContent = j.ws_url;
+    }).catch(() => {});
+  </script>
+</body>
+</html>

esp32_motion_controller/ws_handler.py

@@ -0,0 +1,296 @@
+"""
+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

@@ -0,0 +1,19 @@
+<!doctype html>
+<html lang="en">
+  <head>
+    <meta charset="utf-8" />
+    <meta name="viewport" content="width=device-width, initial-scale=1" />
+    <title>ESP32 Motion Controller — Reachy Mini</title>
+    <link rel="stylesheet" href="style.css" />
+  </head>
+  <body>
+    <main class="card">
+      <h1>ESP32 Motion Controller</h1>
+      <p>
+        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>
+    </main>
+  </body>
+</html>

pyproject.toml

@@ -0,0 +1,43 @@
+[build-system]
+requires = ["setuptools>=61.0"]
+build-backend = "setuptools.build_meta"
+
+[project]
+name = "esp32_motion_controller"
+version = "1.0.0"
+description = "ESP32 Motion Controller — handheld IMU controller for Reachy Mini"
+readme = "README.md"
+requires-python = ">=3.10"
+dependencies = [
+    "reachy-mini",
+    "fastapi",
+    "uvicorn[standard]",
+    "numpy",
+    "scipy",
+    "zeroconf",
+]
+keywords = ["reachy-mini-app"]
+
+[project.optional-dependencies]
+dev = ["pytest", "pytest-asyncio", "httpx", "websockets"]
+
+[project.entry-points."reachy_mini_apps"]
+esp32_motion_controller = "esp32_motion_controller.main:Esp32MotionController"
+
+[tool.setuptools]
+package-dir = { "" = "." }
+include-package-data = true
+
+[tool.setuptools.packages.find]
+where = ["."]
+include = ["esp32_motion_controller*"]
+exclude = ["build*", "dist*", "tools*", "tests*"]
+
+[tool.setuptools.package-data]
+esp32_motion_controller = ["static/*"]
+
+[tool.pytest.ini_options]
+testpaths = ["../tools/tests"]
+pythonpath = ["."]
+asyncio_mode = "auto"
+asyncio_default_fixture_loop_scope = "function"

style.css

@@ -0,0 +1,28 @@
+body {
+  padding: 2rem;
+  font-family: "IBM Plex Sans", "Segoe UI", sans-serif;
+  background: linear-gradient(160deg, #edebe6 0%, #d8d4cc 100%);
+  color: #1c1b19;
+}
+
+h1 {
+  font-size: 1.5rem;
+  margin-top: 0;
+}
+
+p {
+  color: #3f3c38;
+  font-size: 1rem;
+  line-height: 1.45;
+}
+
+code {
+  font-family: "IBM Plex Mono", ui-monospace, monospace;
+  font-size: 0.9em;
+}
+
+.card {
+  max-width: 620px;
+  margin: 0 auto;
+  padding: 1.25rem 1.5rem;
+}