diff --git a/README.md b/README.md index 83c627e..43309de 100644 --- a/README.md +++ b/README.md @@ -150,3 +150,17 @@ See [Package authoring](docs/robots/package-authoring.md), [Microban details](do ## License The original application code is [Apache-2.0](LICENSE). Bundled third-party models, meshes, policies and samples retain their respective licenses; do **not** assume that every asset is Apache-2.0. Microban's copied XML retains its Apache-2.0 notice, and its upstream repository's mixed-license text, provenance and additional Apache notice are included in [`app/public/robots/microban/`](app/public/robots/microban/). Review those file-specific terms before redistribution. The bundled Anton font is SIL OFL-1.1; its notice is included at [`app/public/licenses/Anton-OFL.txt`](app/public/licenses/Anton-OFL.txt). + +## Hardware Deployment (Arduino UNO Q) + +Open **UNO Q 部署** in the Microduck toolbar. **XGO Duck** is the name shown by the original controller on the same Microduck robot. With the USB services running and ADB ports forwarded, **写入 ONNX 模型** opens the staging page (`127.0.0.1:18600` → board port `18600`) and **打开实机控制页** opens the existing controller (`127.0.0.1:19527` → board port `9527`). Staging a file does not activate it. + +Expand **高级:导出部署包或热更新策略** for the optional Arduino UNO Q deployment tools: + +- **HTTP Hot-Push (`POST /api/deploy`)**: Pushes the selected Microduck policies (`walk`, `stand`, `sitstand`, `kickL`, `kickR`, `groundpick`, `roll`, optional `drive`/`crouch`), custom `.onnx` / `move_script.json`, and `robot_manifest.json` directly over Wi-Fi while preserving on-robot `hardware_profile.json` calibration unless overwrite is requested. +- **Offline Turnkey `.zip` Export**: Generates `microduck-unoq-bundle.zip` containing [hardware/unoq/](hardware/unoq/) (`app.yaml`, Python 50 Hz ONNX runtime & offline mobile AP web UI, STM32U585 QMI8658 + Feetech HD-1910-C001 firmware, and Wi-Fi AP helper scripts) plus all selected ONNX files. +- **PC Pre-Hardware Dry-Run (No Board Required)**: + ```bash + python3 hardware/unoq/python/main.py --hal mock --policy-backend dummy --port 8000 + ``` + Then open the Mobile AP UI at `http://127.0.0.1:8000/` or point the website's `UNO Q 部署` dialog at `http://127.0.0.1:8000` to test zip push and WebSocket control end-to-end. See [hardware/unoq/README.md](hardware/unoq/README.md) for wiring and bring-up instructions. diff --git a/app/package.json b/app/package.json index dca7ac0..1355a66 100644 --- a/app/package.json +++ b/app/package.json @@ -6,13 +6,15 @@ "scripts": { "dev": "vite", "test": "node --test \"test/**/*.test.js\"", + "sync:unoq": "node scripts/sync-unoq-bundle.mjs", + "prebuild": "node scripts/sync-unoq-bundle.mjs", "build": "vite build", "preview": "vite preview", "test:e2e": "playwright test", "test:regression": "playwright test microduck-regression", "baseline:record": "RECORD_BASELINE=1 playwright test microduck-regression", "validate:packages": "node scripts/validate-packages.mjs", - "format:check": "prettier --check \"src/platform/**/*.js\" \"src/robots/**/*.js\" \"e2e/**/*.{js,html}\" \"scripts/*.mjs\" \"test/*.test.js\" \"src/ui/{ControlsHelp,SimulationToolbar,RuntimeStatus}.jsx\" \"src/{App,scene/GameCanvas}.jsx\"" + "format:check": "prettier --check \"src/platform/**/*.js\" \"src/robots/**/*.js\" \"e2e/**/*.{js,html}\" \"scripts/*.mjs\" \"test/*.test.js\" \"src/ui/{ControlsHelp,HardwareDeployDialog,SimulationToolbar,RuntimeStatus}.jsx\" \"src/{App,scene/GameCanvas}.jsx\"" }, "dependencies": { "@emotion/react": "^11.14.0", diff --git a/app/public/hardware/unoq/README.md b/app/public/hardware/unoq/README.md new file mode 100644 index 0000000..eb0a81a --- /dev/null +++ b/app/public/hardware/unoq/README.md @@ -0,0 +1,54 @@ +# Microduck — Arduino UNO Q 实机控制与无硬件联调框架 + +该目录为 **Microduck** 在 **Arduino UNO Q**(QRB2210 Debian Linux MPU + STM32U585 Zephyr/Arduino MCU)上的完整板端控制架构,兼容 **Arduino App Lab** 目录规范。 + +## 目录结构 + +```text +hardware/unoq/ +├── app.yaml # Arduino App Lab 清单 +├── config/ +│ ├── robot_manifest.json # 与网页仿真对齐的策略/关节/观测/状态机配置(可由网站导入覆盖) +│ └── hardware_profile.json # 14 路飞特 HD-1910-C001 与 QMI8658 标定参数(在线修改持久化) +├── policies/ # ONNX 策略存放目录(由网站导出包或 HTTP 推送填充) +├── python/ +│ ├── main.py # 服务入口(支持 --hal mock 与 --hal router) +│ ├── core/ # 配置、61D 观测、50Hz 状态机、ONNX 推理、部署解包 +│ ├── hal/ # RouterBridgeHal (实机) & MockHal (PC 无硬件联调) +│ └── web/ # FastAPI / Dev Fallback HTTP+WebSocket 服务与离线手机控制页 +├── sketch/ # STM32U585 Arduino 固件 (QMI8658 + 飞特 HD-1910-C001 + Bridge) +└── scripts/ + ├── install_deps.sh # 板端 Python 依赖安装 + └── setup_ap.sh # 一键开启 UNO Q Wi-Fi 热点 (Microduck-AP -> 192.168.4.1) +``` + +## 1. 无硬件 PC 端预联调(立即运行) + +即使暂时未连接 Arduino UNO Q 硬件,也可以在电脑本地启动完整服务测试: + +```bash +cd hardware/unoq +python3 python/main.py --hal mock --policy-backend dummy --port 8000 +``` + +> 若环境已安装 `onnxruntime`、`fastapi`、`uvicorn`,可直接省略 `--policy-backend dummy`(默认 `auto` 会优先使用真实 `onnxruntime` 与 `FastAPI`)。 + +随后: +1. 在浏览器启动 Microduck 仿真网站(`cd app && npm run dev`),点击顶部工具栏 **Hardware (UNO Q)**,输入 `http://127.0.0.1:8000` 并点击 **推送到 UNO Q**,即可测试在线导入控制文件与热重载。 +2. 用浏览器打开 `http://127.0.0.1:8000`(或模拟手机视图),可测试虚拟摇杆、技能按钮、E-STOP 急停、准备站立软启动与舵机/IMU 标定页。 + +## 2. 上电连接 Arduino UNO Q 调试流程 + +1. **导出并拷入项目**:在仿真网站点击 **Hardware (UNO Q) -> 下载完整部署包 (.zip)**,解压后得到完整 `microduck_unoq/` 目录(已包含你选中的 `.onnx` 策略与配置),放入 UNO Q `/home/arduino/ArduinoApps/microduck_unoq/`(或通过 Arduino App Lab 打开)。 +2. **确认 STM32 引脚与总线宏**([sketch/config.h](sketch/config.h)): + - `SERVO_SERIAL`(默认 `Serial1`)、`SERVO_BAUD`(默认 `1000000`)、`SERVO_DIR_PIN`(`-1` 为硬件自动流向/单线,使用 74HC126 等方向引脚时填对应 GPIO)。 + - `IMU_WIRE`(`Wire` 或 UNO Q Qwiic 接口对应的 `Wire1`)、`IMU_I2C_ADDR`(`0x6A` 或 `0x6B`)。 +3. **烧录 STM32 并启动 App Lab 应用**: + - 使用 Arduino App Lab 或 `arduino-app-cli` 编译上传 `sketch/` 并启动 `python/main.py`(默认 `--hal router`)。 + - 如需开启 Wi-Fi 热点:在 UNO Q Linux 终端执行 `sudo bash scripts/setup_ap.sh`,手机连接 `Microduck-AP` 后访问 `http://192.168.4.1:8000`。 +4. **首次上机标定(务必先悬空/架起机身!)**: + - 打开手机页面 **标定调试** 页签: + - 点击 **扫描舵机 ID** 确认 14 个飞特 HD-1910-C001 全部在线; + - 点击 **IMU 静止零偏校准**,观察站立放置时 `projected_gravity` 接近 `[0, 0, -1]`;若不一致调整 `imu.axis_map`; + - 逐关节小幅测试并校准 `sign`(`+1`/`-1`)与 `zero_offset_ticks`(一键设为当前物理中位); + - 确认无误后回到 **控制** 页签,拉低 **速度比例滑条(如 30%)**,点击 **准备站立** 完成 1.5s 软插值,再开始步态测试。 diff --git a/app/public/hardware/unoq/app.yaml b/app/public/hardware/unoq/app.yaml new file mode 100644 index 0000000..c31f2fc --- /dev/null +++ b/app/public/hardware/unoq/app.yaml @@ -0,0 +1,6 @@ +name: microduck-unoq +description: Microduck RL policy runtime, mobile Wi-Fi AP controller, and STM32 HAL for Arduino UNO Q (QMI8658 + Feetech HD-1910-C001) +version: 0.1.0 +ports: + - 8000 +bricks: [] diff --git a/app/public/hardware/unoq/config/hardware_profile.json b/app/public/hardware/unoq/config/hardware_profile.json new file mode 100644 index 0000000..e5e9d8d --- /dev/null +++ b/app/public/hardware/unoq/config/hardware_profile.json @@ -0,0 +1,147 @@ +{ + "schema": 1, + "board": "arduino_uno_q", + "servo": { + "model": "FEETECH_HD-1910-C001", + "protocol": "hls_sts_half_duplex", + "baudrate": 1000000, + "ticks_per_rev": 4096, + "neutral_tick": 2048, + "speed_limit": 0, + "acc": 50, + "torque_limit": 1000, + "vel_source": "fd", + "vel_lpf_alpha": 0.4, + "vel_scale_rad_s_per_unit": 0.0015339807878856412 + }, + "imu": { + "model": "QMI8658", + "i2c_addr": "0x6A", + "axis_map": [ + [ + 1, + 0, + 0 + ], + [ + 0, + 1, + 0 + ], + [ + 0, + 0, + 1 + ] + ], + "gyro_bias_rad_s": [0.0, 0.0, 0.0], + "accel_bias_g": [0.0, 0.0, 0.0] + }, + "joints": { + "left_hip_yaw": { + "id": 1, + "sign": 1, + "zero_offset_ticks": 0, + "min_rad": -0.4363323129985824, + "max_rad": 0.5235987755982988 + }, + "left_hip_roll": { + "id": 2, + "sign": 1, + "zero_offset_ticks": 0, + "min_rad": -0.3839724354387525, + "max_rad": 0.3839724354387525 + }, + "left_hip_pitch": { + "id": 3, + "sign": 1, + "zero_offset_ticks": 0, + "min_rad": -1.570796326795005, + "max_rad": 1.5707963267947882 + }, + "left_knee": { + "id": 4, + "sign": 1, + "zero_offset_ticks": 0, + "min_rad": -1.570796326795012, + "max_rad": 1.570796326794781 + }, + "left_ankle": { + "id": 5, + "sign": 1, + "zero_offset_ticks": 0, + "min_rad": -1.5707963267949019, + "max_rad": 1.5707963267948912 + }, + "neck_pitch": { + "id": 6, + "sign": 1, + "zero_offset_ticks": 0, + "min_rad": -1.5707963267948966, + "max_rad": 1.0471975511965976 + }, + "head_pitch": { + "id": 7, + "sign": 1, + "zero_offset_ticks": 0, + "min_rad": -1.5707963267948974, + "max_rad": 1.5707963267948957 + }, + "head_yaw": { + "id": 8, + "sign": 1, + "zero_offset_ticks": 0, + "min_rad": -2.967059728390373, + "max_rad": 2.967059728390348 + }, + "head_roll": { + "id": 9, + "sign": 1, + "zero_offset_ticks": 0, + "min_rad": -0.43633231299859127, + "max_rad": 0.4363323129985735 + }, + "right_hip_yaw": { + "id": 10, + "sign": 1, + "zero_offset_ticks": 0, + "min_rad": -0.5235987755982988, + "max_rad": 0.4363323129985824 + }, + "right_hip_roll": { + "id": 11, + "sign": 1, + "zero_offset_ticks": 0, + "min_rad": -0.3839724354387525, + "max_rad": 0.3839724354387525 + }, + "right_hip_pitch": { + "id": 12, + "sign": 1, + "zero_offset_ticks": 0, + "min_rad": -1.5707963267949268, + "max_rad": 1.5707963267948664 + }, + "right_knee": { + "id": 13, + "sign": 1, + "zero_offset_ticks": 0, + "min_rad": -1.570796326794932, + "max_rad": 1.570796326794861 + }, + "right_ankle": { + "id": 14, + "sign": 1, + "zero_offset_ticks": 0, + "min_rad": -1.5707963267949054, + "max_rad": 1.5707963267948877 + } + }, + "safety": { + "soft_start_s": 1.5, + "watchdog_ms": 150, + "ws_timeout_ms": 500, + "auto_stand_on_fall": false, + "cmd_vel_scale": 0.5 + } +} diff --git a/app/public/hardware/unoq/config/robot_manifest.json b/app/public/hardware/unoq/config/robot_manifest.json new file mode 100644 index 0000000..180cad3 --- /dev/null +++ b/app/public/hardware/unoq/config/robot_manifest.json @@ -0,0 +1,127 @@ +{ + "schema": 1, + "robot_id": "microduck-legs", + "family": "microduck", + "sim": { + "timestep": 0.005, + "decimation": 4, + "ctrl_dt": 0.02, + "hz": 50 + }, + "obs_size": 61, + "action_size": 14, + "command_size": 13, + "joint_names": [ + "left_hip_yaw", + "left_hip_roll", + "left_hip_pitch", + "left_knee", + "left_ankle", + "neck_pitch", + "head_pitch", + "head_yaw", + "head_roll", + "right_hip_yaw", + "right_hip_roll", + "right_hip_pitch", + "right_knee", + "right_ankle" + ], + "default_pose": [ + 0.0, + -0.0872664600610733, + -0.4579240083694458, + -0.004939999897032976, + 0.4529840052127838, + 0.3490658402442932, + 0.3490658402442932, + 0.0, + 0.0, + 0.0, + 0.0872664600610733, + 0.4579240083694458, + 0.004939999897032976, + -0.4529840052127838 + ], + "action_scale": 1.0, + "variants": { + "legs": { + "limits": [0.25, -0.2, 1.0], + "enabled": true + }, + "rollers": { + "limits": [0.6, -0.5, 0.3], + "enabled": false + } + }, + "legacy": { + "crouch_period": 5.0, + "crouch_end": 0.7, + "groundpick_period": 4.0, + "groundpick_end": 0.7 + }, + "head": { + "max_rad": 2.5, + "alpha": 0.2, + "signs": [1, -1, 1, -1] + }, + "safety": { + "tilt_z": -0.5, + "upright_z": -0.85, + "fall_debounce_steps": 10, + "fall_settle_steps": 15, + "recover_upright_steps": 50, + "recover_giveup_steps": 300, + "kick_steps": 25, + "post_kick_lock_steps": 20, + "roll_max_steps": 150, + "sit_delay_s": 0.8, + "stand_delay_s": 2.0 + }, + "skills": { + "walk": { + "path": "policies/walk.onnx", + "kind": "perpetual", + "slot": "walk", + "action_scale": 1.0 + }, + "stand": { + "path": "policies/stand.onnx", + "kind": "recovery", + "slot": "stand", + "action_scale": 1.0 + }, + "sitstand": { + "path": "policies/sitstand.onnx", + "kind": "perpetual", + "slot": "sitstand", + "action_scale": 1.0 + }, + "kickL": { + "path": "policies/kickL.onnx", + "kind": "episodic", + "slot": "kickL", + "action_scale": 1.0 + }, + "kickR": { + "path": "policies/kickR.onnx", + "kind": "episodic", + "slot": "kickR", + "action_scale": 1.0 + }, + "groundpick": { + "path": "policies/groundpick.onnx", + "kind": "episodic", + "slot": "groundpick", + "action_scale": 1.0 + }, + "roll": { + "path": "policies/roll.onnx", + "kind": "episodic", + "slot": "roll", + "action_scale": 1.0, + "duration_s": 3.0 + } + }, + "custom_move": null +} diff --git a/app/public/hardware/unoq/files.json b/app/public/hardware/unoq/files.json new file mode 100644 index 0000000..fb02e75 --- /dev/null +++ b/app/public/hardware/unoq/files.json @@ -0,0 +1,133 @@ +{ + "schema": 1, + "files": [ + { + "path": "app.yaml", + "executable": false + }, + { + "path": "config/hardware_profile.json", + "executable": false + }, + { + "path": "config/robot_manifest.json", + "executable": false + }, + { + "path": "python/__init__.py", + "executable": false + }, + { + "path": "python/core/__init__.py", + "executable": false + }, + { + "path": "python/core/config.py", + "executable": false + }, + { + "path": "python/core/controller.py", + "executable": false + }, + { + "path": "python/core/deploy_service.py", + "executable": false + }, + { + "path": "python/core/observations.py", + "executable": false + }, + { + "path": "python/core/policy_runner.py", + "executable": false + }, + { + "path": "python/core/state_machine.py", + "executable": false + }, + { + "path": "python/hal/__init__.py", + "executable": false + }, + { + "path": "python/hal/base.py", + "executable": false + }, + { + "path": "python/hal/mock_hal.py", + "executable": false + }, + { + "path": "python/hal/router_hal.py", + "executable": false + }, + { + "path": "python/main.py", + "executable": false + }, + { + "path": "python/requirements.txt", + "executable": false + }, + { + "path": "python/web/__init__.py", + "executable": false + }, + { + "path": "python/web/api.py", + "executable": false + }, + { + "path": "python/web/fallback_server.py", + "executable": false + }, + { + "path": "python/web/server.py", + "executable": false + }, + { + "path": "python/web/static/index.html", + "executable": false + }, + { + "path": "README.md", + "executable": false + }, + { + "path": "scripts/install_deps.sh", + "executable": true + }, + { + "path": "scripts/setup_ap.sh", + "executable": true + }, + { + "path": "sketch/config.h", + "executable": false + }, + { + "path": "sketch/feetech_hls.cpp", + "executable": false + }, + { + "path": "sketch/feetech_hls.h", + "executable": false + }, + { + "path": "sketch/qmi8658.cpp", + "executable": false + }, + { + "path": "sketch/qmi8658.h", + "executable": false + }, + { + "path": "sketch/sketch.ino", + "executable": false + }, + { + "path": "sketch/sketch.yaml", + "executable": false + } + ] +} diff --git a/app/public/hardware/unoq/python/__init__.py b/app/public/hardware/unoq/python/__init__.py new file mode 100644 index 0000000..67e72f9 --- /dev/null +++ b/app/public/hardware/unoq/python/__init__.py @@ -0,0 +1 @@ +"""Microduck Arduino UNO Q Python runtime package.""" diff --git a/app/public/hardware/unoq/python/core/__init__.py b/app/public/hardware/unoq/python/core/__init__.py new file mode 100644 index 0000000..d389b54 --- /dev/null +++ b/app/public/hardware/unoq/python/core/__init__.py @@ -0,0 +1 @@ +"""Core configuration, observation pipeline, ONNX runner, and control loop.""" diff --git a/app/public/hardware/unoq/python/core/config.py b/app/public/hardware/unoq/python/core/config.py new file mode 100644 index 0000000..9166d04 --- /dev/null +++ b/app/public/hardware/unoq/python/core/config.py @@ -0,0 +1,176 @@ +from __future__ import annotations + +import json +import math +from pathlib import Path +from typing import Any + +import numpy as np + + +TWO_PI = 2.0 * math.pi + + +def validate_robot_manifest(manifest: dict[str, Any]) -> dict[str, Any]: + if not isinstance(manifest, dict): + raise ValueError("robot_manifest must be a JSON object") + if manifest.get("obs_size") != 61: + raise ValueError(f"Unsupported obs_size {manifest.get('obs_size')}; expected 61") + if manifest.get("action_size") != 14: + raise ValueError(f"Unsupported action_size {manifest.get('action_size')}; expected 14") + if manifest.get("command_size") != 13: + raise ValueError(f"Unsupported command_size {manifest.get('command_size')}; expected 13") + + joint_names = manifest.get("joint_names") + default_pose = manifest.get("default_pose") + if not isinstance(joint_names, list) or len(joint_names) != 14: + raise ValueError("robot_manifest.joint_names must contain 14 joints") + if not isinstance(default_pose, list) or len(default_pose) != 14: + raise ValueError("robot_manifest.default_pose must contain 14 floats") + if not all(isinstance(v, (int, float)) and math.isfinite(float(v)) for v in default_pose): + raise ValueError("robot_manifest.default_pose contains non-finite value") + + skills = manifest.get("skills") + if not isinstance(skills, dict): + raise ValueError("robot_manifest.skills must be an object") + for required in ("walk", "stand"): + if required not in skills or not isinstance(skills[required], dict) or not skills[required].get("path"): + raise ValueError(f"robot_manifest.skills missing required '{required}' policy") + return manifest + + +def load_robot_manifest(path: str | Path) -> dict[str, Any]: + with Path(path).open("r", encoding="utf-8") as f: + data = json.load(f) + return validate_robot_manifest(data) + + +def validate_hardware_profile(profile: dict[str, Any], joint_names: list[str] | None = None) -> dict[str, Any]: + if not isinstance(profile, dict): + raise ValueError("hardware_profile must be a JSON object") + servo = profile.get("servo") + if not isinstance(servo, dict): + raise ValueError("hardware_profile.servo missing") + ticks_per_rev = int(servo.get("ticks_per_rev", 0)) + if ticks_per_rev <= 0: + raise ValueError("hardware_profile.servo.ticks_per_rev must be > 0") + + imu = profile.get("imu") + if not isinstance(imu, dict): + raise ValueError("hardware_profile.imu missing") + axis_map = np.asarray(imu.get("axis_map", []), dtype=np.float64) + if axis_map.shape != (3, 3) or not np.all(np.isfinite(axis_map)): + raise ValueError("hardware_profile.imu.axis_map must be a 3x3 finite matrix") + + joints = profile.get("joints") + if not isinstance(joints, dict): + raise ValueError("hardware_profile.joints missing") + expected = joint_names or list(joints.keys()) + if len(expected) != 14: + raise ValueError("hardware_profile.joints must configure all 14 joints") + seen_ids: set[int] = set() + for name in expected: + if name not in joints: + raise ValueError(f"hardware_profile.joints missing '{name}'") + cfg = joints[name] + jid = int(cfg["id"]) + if not (0 <= jid <= 253) or jid in seen_ids: + raise ValueError(f"Invalid or duplicate servo id {jid} for joint {name}") + seen_ids.add(jid) + if int(cfg["sign"]) not in (-1, 1): + raise ValueError(f"Joint {name} sign must be -1 or 1") + if float(cfg["min_rad"]) > float(cfg["max_rad"]): + raise ValueError(f"Joint {name} min_rad > max_rad") + return profile + + +def load_hardware_profile(path: str | Path, joint_names: list[str] | None = None) -> dict[str, Any]: + with Path(path).open("r", encoding="utf-8") as f: + data = json.load(f) + return validate_hardware_profile(data, joint_names=joint_names) + + +def save_hardware_profile(path: str | Path, profile: dict[str, Any], joint_names: list[str] | None = None) -> dict[str, Any]: + validated = validate_hardware_profile(profile, joint_names=joint_names) + target = Path(path) + target.parent.mkdir(parents=True, exist_ok=True) + tmp = target.with_suffix(".json.tmp") + with tmp.open("w", encoding="utf-8") as f: + json.dump(validated, f, indent=2, ensure_ascii=False) + f.write("\n") + tmp.replace(target) + return validated + + +def rad_to_ticks(q_rad: np.ndarray | list[float], joint_names: list[str], profile: dict[str, Any]) -> list[int]: + servo = profile["servo"] + ticks_per_rev = int(servo["ticks_per_rev"]) + neutral = int(servo["neutral_tick"]) + rad_per_tick = TWO_PI / ticks_per_rev + joints_cfg = profile["joints"] + max_tick = ticks_per_rev - 1 + + out: list[int] = [] + for idx, name in enumerate(joint_names): + cfg = joints_cfg[name] + angle = float(q_rad[idx]) + if not math.isfinite(angle): + raise ValueError(f"Non-finite angle for joint {name}") + angle = max(float(cfg["min_rad"]), min(float(cfg["max_rad"]), angle)) + tick = round(neutral + int(cfg["zero_offset_ticks"]) + int(cfg["sign"]) * (angle / rad_per_tick)) + tick = max(0, min(max_tick, tick)) + out.append(int(tick)) + return out + + +def ticks_to_rad(ticks: np.ndarray | list[int], joint_names: list[str], profile: dict[str, Any]) -> np.ndarray: + servo = profile["servo"] + ticks_per_rev = int(servo["ticks_per_rev"]) + neutral = int(servo["neutral_tick"]) + rad_per_tick = TWO_PI / ticks_per_rev + joints_cfg = profile["joints"] + + out = np.zeros(len(joint_names), dtype=np.float64) + for idx, name in enumerate(joint_names): + cfg = joints_cfg[name] + zero = neutral + int(cfg["zero_offset_ticks"]) + out[idx] = int(cfg["sign"]) * (float(ticks[idx]) - zero) * rad_per_tick + return out + + +def raw_vel_to_rad_s(raw_vel: list[int], joint_names: list[str], profile: dict[str, Any]) -> np.ndarray: + """Decode Feetech signed-magnitude speed ticks (bit 15 = negative) to rad/s.""" + servo = profile["servo"] + scale = float(servo.get("vel_scale_rad_s_per_unit", TWO_PI / int(servo["ticks_per_rev"]))) + joints_cfg = profile["joints"] + out = np.zeros(len(joint_names), dtype=np.float64) + for idx, name in enumerate(joint_names): + raw = int(raw_vel[idx]) + mag = raw & 0x7FFF + signed = -mag if (raw & 0x8000) else mag + out[idx] = int(joints_cfg[name]["sign"]) * signed * scale + return out + + +def apply_imu_calibration( + gyro_raw: np.ndarray | list[float], + grav_raw: np.ndarray | list[float], + profile: dict[str, Any], +) -> tuple[np.ndarray, np.ndarray]: + imu = profile["imu"] + axis_map = np.asarray(imu["axis_map"], dtype=np.float64) + gyro_bias = np.asarray(imu.get("gyro_bias_rad_s", [0.0, 0.0, 0.0]), dtype=np.float64) + + g_in = np.asarray(gyro_raw, dtype=np.float64) + p_in = np.asarray(grav_raw, dtype=np.float64) + if g_in.shape != (3,) or p_in.shape != (3,): + raise ValueError("IMU vectors must be 3D") + if not (np.all(np.isfinite(g_in)) and np.all(np.isfinite(p_in))): + raise ValueError("IMU readings contain non-finite values") + + gyro = axis_map @ (g_in - gyro_bias) + grav = axis_map @ p_in + norm = float(np.linalg.norm(grav)) + if norm > 1e-6: + grav = grav / norm + return gyro, grav diff --git a/app/public/hardware/unoq/python/core/controller.py b/app/public/hardware/unoq/python/core/controller.py new file mode 100644 index 0000000..da449e4 --- /dev/null +++ b/app/public/hardware/unoq/python/core/controller.py @@ -0,0 +1,380 @@ +from __future__ import annotations + +import threading +import time +from pathlib import Path +from typing import Any + +import numpy as np + +from hal.base import HardwareBridge, HardwareFrame + +from .config import ( + apply_imu_calibration, + rad_to_ticks, + raw_vel_to_rad_s, + save_hardware_profile, + ticks_to_rad, +) +from .observations import build_observation, decode_action +from .policy_runner import PolicySet +from .state_machine import LifecycleState, MicroduckStateMachine + + +class MicroduckController: + """50Hz real-time control loop coordinating HAL, 61D observations, ONNX policies, and safety.""" + + def __init__( + self, + base_dir: str | Path, + manifest: dict[str, Any], + profile: dict[str, Any], + hal: HardwareBridge, + policies: PolicySet, + custom_script: dict[str, Any] | None = None, + profile_path: str | Path | None = None, + ) -> None: + self.base_dir = Path(base_dir) + self.profile_path = Path(profile_path) if profile_path else self.base_dir / "config" / "hardware_profile.json" + self.manifest = manifest + self.profile = profile + self.hal = hal + self.policies = policies + + self.joint_names: list[str] = list(manifest["joint_names"]) + self.default_pose = np.asarray(manifest["default_pose"], dtype=np.float32) + self.ctrl_dt = float(manifest.get("sim", {}).get("ctrl_dt", 0.02)) + + self.state_machine = MicroduckStateMachine(manifest, custom_script=custom_script) + self._lock = threading.RLock() + self._running = False + self._thread: threading.Thread | None = None + + self.last_action = np.zeros(14, dtype=np.float32) + self._last_q_rad: np.ndarray | None = None + self._vel_lpf = np.zeros(14, dtype=np.float64) + self._last_remote_input_s: float = 0.0 + self._last_frame: HardwareFrame | None = None + self._last_gyro = np.zeros(3, dtype=np.float64) + self._last_grav = np.array([0.0, 0.0, -1.0], dtype=np.float64) + self._last_q = self.default_pose.astype(np.float64).copy() + self._last_targets = self.default_pose.astype(np.float64).copy() + self._ctrl_hz = 0.0 + self._last_error: str | None = None + self._calib_target_ticks: list[int] | None = None + self._calib_torque_on = False + + def connect(self) -> None: + with self._lock: + self.hal.connect() + self.hal.configure(self.joint_names, self.profile) + self._last_frame = self.hal.sync_step( + enable_torque=False, + target_ticks=rad_to_ticks(self.default_pose, self.joint_names, self.profile), + ) + self._last_gyro, self._last_grav = apply_imu_calibration( + self._last_frame.gyro_raw, self._last_frame.proj_gravity_raw, self.profile + ) + self._last_q = ticks_to_rad(self._last_frame.ticks, self.joint_names, self.profile) + + def start(self) -> None: + if self._running: + return + self.connect() + self._running = True + self._thread = threading.Thread(target=self._loop, name="microduck-50hz", daemon=True) + self._thread.start() + + def stop(self) -> None: + self._running = False + if self._thread is not None: + self._thread.join(timeout=2.0) + self._thread = None + with self._lock: + try: + self.state_machine.emergency_stop() + self.hal.set_torque(False) + finally: + self.hal.close() + + def _loop(self) -> None: + next_t = time.monotonic() + hz_t0 = next_t + ticks = 0 + while self._running: + now = time.monotonic() + try: + self.step_once(dt=self.ctrl_dt, now_s=now) + self._last_error = None + except Exception as exc: + self._last_error = str(exc) + with self._lock: + self.state_machine.emergency_stop() + try: + self.hal.set_torque(False) + except Exception: + pass + ticks += 1 + if now - hz_t0 >= 0.5: + self._ctrl_hz = ticks / max(now - hz_t0, 1e-6) + ticks = 0 + hz_t0 = now + next_t += self.ctrl_dt + sleep_s = next_t - time.monotonic() + if sleep_s > 0: + time.sleep(sleep_s) + else: + next_t = time.monotonic() + + def set_remote_input( + self, + norm_x: float, + norm_wz: float, + speed_scale: float | None = None, + now_s: float | None = None, + ) -> None: + with self._lock: + scale = ( + float(speed_scale) + if speed_scale is not None + else float(self.profile.get("safety", {}).get("cmd_vel_scale", 0.5)) + ) + self.state_machine.set_velocity_cmd(norm_x, norm_wz, speed_scale=scale) + self._last_remote_input_s = time.monotonic() if now_s is None else float(now_s) + + def set_head_target(self, neck_pitch: float, head_pitch: float, head_yaw: float, head_roll: float) -> None: + with self._lock: + self.state_machine.set_head_target(neck_pitch, head_pitch, head_yaw, head_roll) + + def set_auto_stand_on_fall(self, enabled: bool) -> None: + with self._lock: + self.profile.setdefault("safety", {})["auto_stand_on_fall"] = bool(enabled) + save_hardware_profile(self.profile_path, self.profile, self.joint_names) + + def set_speed_scale(self, scale: float) -> None: + with self._lock: + clamped = max(0.1, min(1.0, float(scale))) + self.profile.setdefault("safety", {})["cmd_vel_scale"] = clamped + save_hardware_profile(self.profile_path, self.profile, self.joint_names) + + def request_stand(self) -> None: + with self._lock: + if "walk" not in self.policies.policies: + raise RuntimeError("No control policies loaded yet — deploy a bundle from the website first") + self._calib_torque_on = False + self._calib_target_ticks = None + soft_start_s = float(self.profile.get("safety", {}).get("soft_start_s", 1.5)) + self.state_machine.request_stand(self._last_q, soft_start_s=soft_start_s) + + def emergency_stop(self) -> None: + with self._lock: + self._calib_torque_on = False + self._calib_target_ticks = None + self.state_machine.emergency_stop() + self.last_action.fill(0) + self.hal.set_torque(False) + + def trigger_action(self, action: str) -> bool: + with self._lock: + return self.state_machine.trigger_action(action) + + def reload_runtime( + self, + manifest: dict[str, Any], + policies: PolicySet, + profile: dict[str, Any] | None = None, + custom_script: dict[str, Any] | None = None, + ) -> None: + with self._lock: + self.state_machine.emergency_stop() + self.hal.set_torque(False) + self.manifest = manifest + self.policies = policies + self.joint_names = list(manifest["joint_names"]) + self.default_pose = np.asarray(manifest["default_pose"], dtype=np.float32) + self.ctrl_dt = float(manifest.get("sim", {}).get("ctrl_dt", 0.02)) + self.state_machine.update_manifest(manifest, custom_script=custom_script) + if profile is not None: + self.profile = profile + self.hal.configure(self.joint_names, self.profile) + self.last_action.fill(0) + self._last_error = None + + def update_profile(self, profile: dict[str, Any]) -> dict[str, Any]: + with self._lock: + self.profile = save_hardware_profile(self.profile_path, profile, self.joint_names) + self.hal.configure(self.joint_names, self.profile) + return self.profile + + def capture_joint_zero(self, joint_name: str) -> dict[str, Any]: + with self._lock: + if joint_name not in self.joint_names: + raise ValueError(f"Unknown joint '{joint_name}'") + idx = self.joint_names.index(joint_name) + if self._last_frame is None: + raise RuntimeError("No hardware frame captured yet") + neutral = int(self.profile["servo"]["neutral_tick"]) + current_tick = int(self._last_frame.ticks[idx]) + self.profile["joints"][joint_name]["zero_offset_ticks"] = current_tick - neutral + return self.update_profile(self.profile) + + def test_single_joint(self, joint_name: str, target_rad: float) -> list[int]: + with self._lock: + if joint_name not in self.joint_names: + raise ValueError(f"Unknown joint '{joint_name}'") + if self.state_machine.lifecycle not in (LifecycleState.IDLE, LifecycleState.CALIBRATION): + raise RuntimeError("Emergency stop first before testing individual joints") + self.state_machine.enter_calibration() + idx = self.joint_names.index(joint_name) + pose = self._last_q.copy() + pose[idx] = float(target_rad) + ticks = rad_to_ticks(pose, self.joint_names, self.profile) + self._calib_target_ticks = ticks + self._calib_torque_on = True + return ticks + + def exit_calibration(self) -> None: + with self._lock: + self._calib_torque_on = False + self._calib_target_ticks = None + self.state_machine.exit_calibration() + self.hal.set_torque(False) + + def calibrate_imu_zero(self, samples: int = 200) -> dict[str, Any]: + with self._lock: + bias = self.hal.calibrate_imu_raw(samples=samples) + self.profile["imu"]["gyro_bias_rad_s"] = [float(b) for b in bias] + self.update_profile(self.profile) + return self.profile["imu"] + + def ping_servos(self, max_id: int = 20) -> list[int]: + with self._lock: + return self.hal.ping_servos(max_id=max_id) + + def _compute_joint_vel(self, q_rad: np.ndarray, raw_speeds: list[int], dt: float) -> np.ndarray: + servo_cfg = self.profile["servo"] + if servo_cfg.get("vel_source") == "servo": + return raw_vel_to_rad_s(raw_speeds, self.joint_names, self.profile) + if self._last_q_rad is None: + self._last_q_rad = q_rad.copy() + self._vel_lpf.fill(0.0) + return self._vel_lpf.copy() + fd = (q_rad - self._last_q_rad) / max(dt, 1e-4) + self._last_q_rad = q_rad.copy() + alpha = float(servo_cfg.get("vel_lpf_alpha", 0.4)) + self._vel_lpf = (1.0 - alpha) * self._vel_lpf + alpha * fd + return self._vel_lpf.copy() + + def step_once(self, dt: float | None = None, now_s: float | None = None) -> dict[str, Any]: + with self._lock: + dt_s = self.ctrl_dt if dt is None else float(dt) + now = time.monotonic() if now_s is None else float(now_s) + + # Enforce mobile WS heartbeat timeout while walking + ws_timeout_s = float(self.profile.get("safety", {}).get("ws_timeout_ms", 500)) / 1000.0 + if self._last_remote_input_s > 0.0 and (now - self._last_remote_input_s) > ws_timeout_s: + self.state_machine.set_velocity_cmd(0.0, 0.0) + + servo_cfg = self.profile["servo"] + speed_limit = int(servo_cfg.get("speed_limit", 0)) + acc = int(servo_cfg.get("acc", 50)) + + # Determine command to send for THIS cycle from current state + previous sensor state + auto_stand = bool(self.profile.get("safety", {}).get("auto_stand_on_fall", False)) + if self.state_machine.lifecycle == LifecycleState.CALIBRATION: + enable_torque = self._calib_torque_on + cmd_ticks = ( + list(self._calib_target_ticks) + if self._calib_target_ticks is not None + else rad_to_ticks(self._last_q, self.joint_names, self.profile) + ) + decision = self.state_machine.step(dt_s, float(self._last_grav[2]), auto_stand) + else: + decision = self.state_machine.step(dt_s, float(self._last_grav[2]), auto_stand) + if decision.zero_last_action: + self.last_action.fill(0) + + if decision.lifecycle == LifecycleState.SOFT_START: + q_interp = ( + 1.0 - decision.soft_start_alpha + ) * self.state_machine.soft_start_q0 + decision.soft_start_alpha * self.default_pose + self._last_targets = q_interp + cmd_ticks = rad_to_ticks(q_interp, self.joint_names, self.profile) + enable_torque = True + elif decision.lifecycle == LifecycleState.ACTIVE and decision.policy_key is not None: + policy = self.policies.policies.get(decision.policy_key) or self.policies.policies["walk"] + q_vel = self._vel_lpf + obs = build_observation( + self._last_gyro, + self._last_grav, + self._last_q, + q_vel, + self.last_action, + decision.command_13, + self.default_pose, + ) + raw_act = policy.infer(obs) + self.last_action = raw_act + targets = decode_action(raw_act, self.default_pose, policy.action_scale) + self._last_targets = targets.astype(np.float64) + cmd_ticks = rad_to_ticks(targets, self.joint_names, self.profile) + enable_torque = True + elif decision.lifecycle == LifecycleState.ACTIVE and decision.recovery_state == "fallen": + cmd_ticks = rad_to_ticks(self._last_targets, self.joint_names, self.profile) + enable_torque = True + else: + cmd_ticks = rad_to_ticks(self._last_q, self.joint_names, self.profile) + enable_torque = False + + frame = self.hal.sync_step( + enable_torque=enable_torque, + target_ticks=cmd_ticks, + speed_limit=speed_limit, + acc=acc, + ) + self._last_frame = frame + self._last_gyro, self._last_grav = apply_imu_calibration( + frame.gyro_raw, frame.proj_gravity_raw, self.profile + ) + self._last_q = ticks_to_rad(frame.ticks, self.joint_names, self.profile) + self._compute_joint_vel(self._last_q, frame.raw_speeds, dt_s) + + return self._build_status_unlocked(decision, enable_torque) + + def _build_status_unlocked(self, decision: Any | None = None, torque_enabled: bool | None = None) -> dict[str, Any]: + frame = self._last_frame + life = self.state_machine.lifecycle.value + active_policy = decision.policy_key if decision else None + if torque_enabled is None: + torque_enabled = life in (LifecycleState.SOFT_START.value, LifecycleState.ACTIVE.value) or self._calib_torque_on + return { + "hal": self.hal.name, + "policy_backend": self.policies.backend, + "lifecycle": life, + "mode_label": decision.mode_label if decision else self.state_machine._mode_label(), + "skill_mode": self.state_machine.skill_mode, + "active_policy": active_policy, + "recovery_state": self.state_machine.recovery_state, + "loco": self.state_machine.loco, + "rollers_enabled": self.state_machine.rollers_enabled, + "head_mode": self.state_machine.head_mode, + "torque_enabled": bool(torque_enabled), + "auto_stand_on_fall": bool(self.profile.get("safety", {}).get("auto_stand_on_fall", False)), + "speed_scale": float(self.profile.get("safety", {}).get("cmd_vel_scale", 0.5)), + "ctrl_hz": round(self._ctrl_hz, 1), + "voltage_v": round(frame.voltage_v, 2) if frame else 0.0, + "max_temp_c": round(frame.max_temp_c, 1) if frame else 0.0, + "imu_ok": frame.imu_ok if frame else False, + "servo_err_mask": frame.servo_err_mask if frame else 0, + "gyro": [round(float(x), 4) for x in self._last_gyro], + "projected_gravity": [round(float(x), 4) for x in self._last_grav], + "ticks": list(frame.ticks) if frame else [2048] * 14, + "joint_pos_rad": [round(float(x), 4) for x in self._last_q], + "loaded_skills": sorted(self.policies.policies.keys()), + "custom_move": self.manifest.get("custom_move"), + "error": self._last_error, + } + + def get_status(self) -> dict[str, Any]: + with self._lock: + return self._build_status_unlocked() diff --git a/app/public/hardware/unoq/python/core/deploy_service.py b/app/public/hardware/unoq/python/core/deploy_service.py new file mode 100644 index 0000000..57cb932 --- /dev/null +++ b/app/public/hardware/unoq/python/core/deploy_service.py @@ -0,0 +1,252 @@ +from __future__ import annotations + +import io +import json +import shutil +import stat +import tempfile +import threading +import time +import zipfile +from pathlib import Path, PurePosixPath +from typing import Any + +from .config import save_hardware_profile, validate_hardware_profile, validate_robot_manifest +from .controller import MicroduckController +from .policy_runner import PolicySet + + +MAX_ZIP_FILES = 512 +MAX_ZIP_UNCOMPRESSED_BYTES = 128 * 1024 * 1024 + + +def _safe_rel_path(member_name: str) -> PurePosixPath: + if not member_name or "\\" in member_name: + raise ValueError(f"Unsafe zip member path: {member_name!r}") + if member_name.startswith("/") or ":" in member_name: + raise ValueError(f"Absolute or drive path forbidden in zip: {member_name!r}") + parts = PurePosixPath(member_name).parts + if not parts or any(part in ("..", ".", "") for part in parts): + raise ValueError(f"Path traversal forbidden in zip: {member_name!r}") + return PurePosixPath(*parts) + + +class DeployService: + """Atomic, validated hot-reload deployment of Microduck manifest + ONNX policies.""" + + def __init__( + self, + base_dir: str | Path, + controller: MicroduckController, + policy_backend: str = "auto", + ) -> None: + self.base_dir = Path(base_dir) + self.controller = controller + self.policy_backend = policy_backend + self._lock = threading.Lock() + self.last_deploy: dict[str, Any] | None = self._read_last_deploy() + + def _read_last_deploy(self) -> dict[str, Any] | None: + meta_path = self.base_dir / "config" / ".last_deploy.json" + if not meta_path.is_file(): + return None + try: + with meta_path.open("r", encoding="utf-8") as f: + return json.load(f) + except Exception: + return None + + def _extract_bundle_to_stage(self, raw_bytes: bytes, stage_dir: Path) -> None: + if not raw_bytes: + raise ValueError("Empty deployment bundle") + try: + zf = zipfile.ZipFile(io.BytesIO(raw_bytes), "r") + except zipfile.BadZipFile as exc: + raise ValueError("Deployment payload is not a valid ZIP archive") from exc + + with zf: + infos = zf.infolist() + if len(infos) > MAX_ZIP_FILES: + raise ValueError(f"Too many files in deployment zip ({len(infos)} > {MAX_ZIP_FILES})") + + total_size = 0 + entries: list[tuple[zipfile.ZipInfo, PurePosixPath]] = [] + for info in infos: + if info.is_dir(): + if info.filename.rstrip("/"): + _safe_rel_path(info.filename.rstrip("/")) + continue + mode = (info.external_attr >> 16) & 0o170000 + if mode == stat.S_IFLNK: + raise ValueError(f"Symlinks are forbidden in deployment zip: {info.filename!r}") + total_size += max(0, int(info.file_size)) + if total_size > MAX_ZIP_UNCOMPRESSED_BYTES: + raise ValueError("Deployment zip exceeds maximum uncompressed size (128 MB)") + entries.append((info, _safe_rel_path(info.filename))) + + if not entries: + raise ValueError("Deployment zip contains no files") + + top_dirs = {p.parts[0] for _, p in entries if len(p.parts) > 1} + has_root_manifest = any( + str(p) in ("config/robot_manifest.json", "robot_manifest.json") for _, p in entries + ) + strip_prefix: str | None = None + if not has_root_manifest and len(top_dirs) == 1: + only_top = next(iter(top_dirs)) + if any( + str(p) in (f"{only_top}/config/robot_manifest.json", f"{only_top}/robot_manifest.json") + for _, p in entries + ): + strip_prefix = only_top + + for info, rel in entries: + if strip_prefix and rel.parts[0] == strip_prefix: + if len(rel.parts) == 1: + continue + rel = PurePosixPath(*rel.parts[1:]) + + dest = (stage_dir / Path(*rel.parts)).resolve() + if not dest.is_relative_to(stage_dir.resolve()): + raise ValueError(f"Escaping path rejected: {info.filename!r}") + dest.parent.mkdir(parents=True, exist_ok=True) + with zf.open(info, "r") as src, dest.open("wb") as dst: + shutil.copyfileobj(src, dst) + + def apply_zip_bytes(self, raw_bytes: bytes, overwrite_profile: bool = False) -> dict[str, Any]: + with self._lock, tempfile.TemporaryDirectory(prefix="microduck_deploy_") as tmp: + stage = Path(tmp) / "stage" + backup = Path(tmp) / "backup" + stage.mkdir(parents=True, exist_ok=True) + backup.mkdir(parents=True, exist_ok=True) + + self._extract_bundle_to_stage(raw_bytes, stage) + + manifest_src = stage / "config" / "robot_manifest.json" + if not manifest_src.is_file(): + alt = stage / "robot_manifest.json" + if alt.is_file(): + manifest_src = alt + else: + raise ValueError("Deployment zip missing config/robot_manifest.json") + + try: + manifest = validate_robot_manifest(json.loads(manifest_src.read_text(encoding="utf-8"))) + except json.JSONDecodeError as exc: + raise ValueError(f"Invalid robot_manifest.json: {exc}") from exc + + # Normalize into stage/config & verify every referenced ONNX exists & is non-empty + (stage / "config").mkdir(parents=True, exist_ok=True) + (stage / "config" / "robot_manifest.json").write_text( + json.dumps(manifest, indent=2) + "\n", encoding="utf-8" + ) + + stage_policies: dict[str, Path] = {} + for key, spec in manifest["skills"].items(): + rel = spec.get("path") + if not rel: + continue + rel_posix = _safe_rel_path(str(rel)) + pol_file = (stage / Path(*rel_posix.parts)).resolve() + if not pol_file.is_relative_to(stage.resolve()) or not pol_file.is_file(): + raise ValueError(f"Deployment zip missing policy '{key}' at '{rel}'") + if pol_file.stat().st_size == 0: + raise ValueError(f"Policy '{key}' at '{rel}' is empty") + stage_policies[key] = pol_file + + # Optional custom move script + custom_script: dict[str, Any] | None = None + script_src = stage / "config" / "custom_move_script.json" + if not script_src.is_file(): + alt_script = stage / "custom_move_script.json" + if alt_script.is_file(): + script_src = alt_script + if script_src.is_file(): + try: + custom_script = json.loads(script_src.read_text(encoding="utf-8")) + except json.JSONDecodeError as exc: + raise ValueError(f"Invalid custom_move_script.json: {exc}") from exc + if not isinstance(custom_script, dict) or not isinstance(custom_script.get("keyframes"), list): + raise ValueError("custom_move_script.json must contain a 'keyframes' list") + elif isinstance(manifest.get("custom_move"), dict) and manifest["custom_move"].get("slot") == "script": + raise ValueError("Manifest references custom_move script, but config/custom_move_script.json is missing") + + # Optional hardware profile + target_profile_path = self.base_dir / "config" / "hardware_profile.json" + incoming_profile: dict[str, Any] | None = None + profile_src = stage / "config" / "hardware_profile.json" + if not profile_src.is_file(): + alt_prof = stage / "hardware_profile.json" + if alt_prof.is_file(): + profile_src = alt_prof + if (overwrite_profile or not target_profile_path.is_file()) and profile_src.is_file(): + try: + incoming_profile = validate_hardware_profile( + json.loads(profile_src.read_text(encoding="utf-8")), + joint_names=manifest["joint_names"], + ) + except json.JSONDecodeError as exc: + raise ValueError(f"Invalid hardware_profile.json: {exc}") from exc + + # Pre-validate ONNX graph & metadata inside stage before touching live files + PolicySet.load_from_manifest(stage, manifest, backend=self.policy_backend) + + # Backup existing config + policies for atomic rollback + live_config = self.base_dir / "config" + live_policies = self.base_dir / "policies" + live_config.mkdir(parents=True, exist_ok=True) + live_policies.mkdir(parents=True, exist_ok=True) + for sub in ("config", "policies"): + src_dir = self.base_dir / sub + if src_dir.is_dir(): + shutil.copytree(src_dir, backup / sub, dirs_exist_ok=True) + + try: + shutil.copy2(stage / "config" / "robot_manifest.json", live_config / "robot_manifest.json") + + target_script = live_config / "custom_move_script.json" + if custom_script is not None: + target_script.write_text(json.dumps(custom_script, indent=2) + "\n", encoding="utf-8") + elif target_script.is_file(): + target_script.unlink() + + for _, src_pol in stage_policies.items(): + rel_to_stage = src_pol.relative_to(stage) + dst_pol = self.base_dir / rel_to_stage + dst_pol.parent.mkdir(parents=True, exist_ok=True) + shutil.copy2(src_pol, dst_pol) + + if incoming_profile is not None: + save_hardware_profile(target_profile_path, incoming_profile, joint_names=manifest["joint_names"]) + + live_policies_set = PolicySet.load_from_manifest( + self.base_dir, manifest, backend=self.policy_backend + ) + self.controller.reload_runtime( + manifest=manifest, + policies=live_policies_set, + profile=incoming_profile, + custom_script=custom_script, + ) + except Exception: + # Roll back live config + policies on reload failure + for sub in ("config", "policies"): + bak_dir = backup / sub + live_dir = self.base_dir / sub + if bak_dir.is_dir(): + shutil.copytree(bak_dir, live_dir, dirs_exist_ok=True) + raise + + deploy_meta = { + "ok": True, + "deployed_at_s": round(time.time(), 3), + "robot_id": manifest.get("robot_id", "microduck"), + "loaded_skills": sorted(live_policies_set.policies.keys()), + "profile_overwritten": incoming_profile is not None, + "custom_move": manifest.get("custom_move"), + } + (live_config / ".last_deploy.json").write_text( + json.dumps(deploy_meta, indent=2) + "\n", encoding="utf-8" + ) + self.last_deploy = deploy_meta + return deploy_meta diff --git a/app/public/hardware/unoq/python/core/observations.py b/app/public/hardware/unoq/python/core/observations.py new file mode 100644 index 0000000..ff207c2 --- /dev/null +++ b/app/public/hardware/unoq/python/core/observations.py @@ -0,0 +1,85 @@ +from __future__ import annotations + +from typing import Any + +import numpy as np + + +def build_observation( + gyro: np.ndarray, + projected_gravity: np.ndarray, + joint_pos_rad: np.ndarray, + joint_vel_rad_s: np.ndarray, + last_action: np.ndarray, + command_13: np.ndarray, + default_pose: np.ndarray, +) -> np.ndarray: + """Assemble the exact 61D float32 observation expected by Microduck ONNX policies. + + Layout: + [0:3] base_ang_vel (gyro, rad/s) + [3:6] projected_gravity (trunk_base frame, unit vector pointing down: [0,0,-1] upright) + [6:20] joint_pos_rel (joint_pos_rad - default_pose) + [20:34] joint_vel (rad/s) + [34:48] last_action (previous raw actor output) + [48:61] command (13D: [vx, vy, wz, head(4), body(6)]) + """ + obs = np.empty(61, dtype=np.float32) + obs[0:3] = np.asarray(gyro, dtype=np.float32) + obs[3:6] = np.asarray(projected_gravity, dtype=np.float32) + obs[6:20] = np.asarray(joint_pos_rad, dtype=np.float32) - np.asarray(default_pose, dtype=np.float32) + obs[20:34] = np.asarray(joint_vel_rad_s, dtype=np.float32) + obs[34:48] = np.asarray(last_action, dtype=np.float32) + obs[48:61] = np.asarray(command_13, dtype=np.float32) + + if not np.all(np.isfinite(obs)): + raise ValueError("Observation vector contains non-finite values") + return obs + + +def decode_action( + raw_action: np.ndarray, + default_pose: np.ndarray, + action_scale: float = 1.0, +) -> np.ndarray: + act = np.asarray(raw_action, dtype=np.float32) + if act.shape != (14,) or not np.all(np.isfinite(act)): + raise ValueError("raw_action must be a finite 14D vector") + return np.asarray(default_pose, dtype=np.float32) + np.float32(action_scale) * act + + +def script_command_at(script: dict[str, Any], t_s: float, cmd_size: int = 13) -> np.ndarray: + """Linearly interpolate a custom move_script.json timeline at time t_s.""" + keyframes = script.get("keyframes", []) + out = np.zeros(cmd_size, dtype=np.float32) + if not keyframes: + return out + + duration_s = float(script.get("durationS") or script.get("duration_s") or 0.0) + loop = script.get("loop", True) is not False + tt = float(t_s) + if loop and duration_s > 0.0: + tt = ((tt % duration_s) + duration_s) % duration_s + + def _pad(raw_cmd: list[float]) -> np.ndarray: + arr = np.zeros(cmd_size, dtype=np.float32) + n = min(cmd_size, len(raw_cmd)) + if n > 0: + arr[:n] = np.asarray(raw_cmd[:n], dtype=np.float32) + return arr + + if tt <= float(keyframes[0]["t"]): + return _pad(keyframes[0]["cmd"]) + + for i in range(len(keyframes) - 1): + a = keyframes[i] + b = keyframes[i + 1] + ta = float(a["t"]) + tb = float(b["t"]) + if ta <= tt <= tb: + u = (tt - ta) / max(tb - ta, 1e-6) + ca = _pad(a["cmd"]) + cb = _pad(b["cmd"]) + return (1.0 - u) * ca + u * cb + + return _pad(keyframes[-1]["cmd"]) diff --git a/app/public/hardware/unoq/python/core/policy_runner.py b/app/public/hardware/unoq/python/core/policy_runner.py new file mode 100644 index 0000000..0bf6813 --- /dev/null +++ b/app/public/hardware/unoq/python/core/policy_runner.py @@ -0,0 +1,123 @@ +from __future__ import annotations + +from dataclasses import dataclass +from pathlib import Path +from typing import Any + +import numpy as np + + +@dataclass +class LoadedPolicy: + key: str + action_scale: float + backend: str + _session: Any | None = None + _input_name: str = "obs" + _output_name: str = "actions" + + def infer(self, obs_61: np.ndarray) -> np.ndarray: + if obs_61.shape != (61,): + raise ValueError(f"Expected 61D observation, got {obs_61.shape}") + if self.backend == "dummy": + # Deterministic zero-action dummy policy for PC dry-run without onnxruntime. + return np.zeros(14, dtype=np.float32) + + inp = np.asarray(obs_61, dtype=np.float32).reshape(1, 61) + out = self._session.run([self._output_name], {self._input_name: inp})[0] + act = np.asarray(out, dtype=np.float32).reshape(14) + if not np.all(np.isfinite(act)): + raise ValueError(f"Policy '{self.key}' returned non-finite action") + return act + + +class PolicySet: + def __init__(self, policies: dict[str, LoadedPolicy], backend: str) -> None: + self.policies = policies + self.backend = backend + + @classmethod + def resolve_backend(cls, requested: str) -> str: + if requested in ("onnx", "dummy"): + return requested + if requested != "auto": + raise ValueError(f"Unknown policy backend: {requested}") + try: + import onnxruntime # noqa: F401 + + return "onnx" + except ImportError as exc: + raise RuntimeError( + "onnxruntime is not installed. Run 'bash scripts/install_deps.sh' on UNO Q, " + "or pass '--policy-backend dummy' for pre-hardware UI/protocol dry-run." + ) from exc + + @classmethod + def load_from_manifest( + cls, + base_dir: str | Path, + manifest: dict[str, Any], + backend: str = "auto", + ) -> "PolicySet": + resolved_backend = cls.resolve_backend(backend) + ort = None + if resolved_backend == "onnx": + try: + import onnxruntime as ort_mod + + ort = ort_mod + except ImportError as exc: + raise RuntimeError("onnxruntime is required for backend='onnx'") from exc + + root = Path(base_dir) + default_scale = float(manifest.get("action_scale", 1.0)) + skills = manifest.get("skills", {}) + loaded: dict[str, LoadedPolicy] = {} + + for key, spec in skills.items(): + rel_path = spec.get("path") + if not rel_path: + continue + scale = float(spec.get("action_scale", default_scale)) + if resolved_backend == "dummy": + loaded[key] = LoadedPolicy(key=key, action_scale=scale, backend="dummy") + continue + + model_path = (root / rel_path).resolve() + if not model_path.is_file(): + raise FileNotFoundError(f"Missing ONNX policy for '{key}': {model_path}") + + sess_opts = ort.SessionOptions() + sess_opts.intra_op_num_threads = 2 + sess = ort.InferenceSession( + str(model_path), + sess_options=sess_opts, + providers=["CPUExecutionProvider"], + ) + inps = sess.get_inputs() + outs = sess.get_outputs() + if len(inps) != 1 or len(outs) != 1: + raise ValueError(f"Policy '{key}' must have 1 input / 1 output") + + # Smoke validation mirroring custom-policy.js + smoke_obs = np.zeros((1, 61), dtype=np.float32) + smoke_out = sess.run([outs[0].name], {inps[0].name: smoke_obs})[0] + if tuple(np.asarray(smoke_out).shape) != (1, 14): + raise ValueError( + f"Policy '{key}' output shape {np.asarray(smoke_out).shape} != (1, 14)" + ) + if not np.all(np.isfinite(smoke_out)): + raise ValueError(f"Policy '{key}' smoke output is non-finite") + + loaded[key] = LoadedPolicy( + key=key, + action_scale=scale, + backend="onnx", + _session=sess, + _input_name=inps[0].name, + _output_name=outs[0].name, + ) + + if "walk" not in loaded or "stand" not in loaded: + raise ValueError("PolicySet must load at least 'walk' and 'stand'") + return cls(policies=loaded, backend=resolved_backend) diff --git a/app/public/hardware/unoq/python/core/state_machine.py b/app/public/hardware/unoq/python/core/state_machine.py new file mode 100644 index 0000000..a096db3 --- /dev/null +++ b/app/public/hardware/unoq/python/core/state_machine.py @@ -0,0 +1,561 @@ +from __future__ import annotations + +import math +from dataclasses import dataclass +from enum import Enum +from typing import Any + +import numpy as np + +from .observations import script_command_at + + +class LifecycleState(str, Enum): + IDLE = "IDLE" + SOFT_START = "SOFT_START" + ACTIVE = "ACTIVE" + CALIBRATION = "CALIBRATION" + FALL_SAFE = "FALL_SAFE" + + +@dataclass +class StepDecision: + lifecycle: LifecycleState + skill_mode: str + policy_key: str | None + torque_enabled: bool + soft_start_alpha: float + command_13: np.ndarray + zero_last_action: bool + mode_label: str + recovery_state: str | None + loco: str + head_mode: bool + + +class MicroduckStateMachine: + """Hardware-safe 50Hz state machine mirroring app/src/robots/microduck/runtime.js.""" + + def __init__(self, manifest: dict[str, Any], custom_script: dict[str, Any] | None = None) -> None: + self.manifest = manifest + self.custom_script = custom_script + self._load_params(manifest) + + self.lifecycle: LifecycleState = LifecycleState.IDLE + self.loco: str = "legs" # "legs" | "rollers" + self.skill_mode: str = "walk" # "walk"|"sitstand"|"roll"|"kickL"|"kickR"|"crouch"|"groundpick" + self.sit_flag: int = 0 + self.head_mode: bool = False + + self.vel_cmd = np.zeros(3, dtype=np.float32) # [vx, 0, wz] + self.head_target = np.zeros(4, dtype=np.float32) + self.head_smooth = np.zeros(4, dtype=np.float32) + + self.soft_start_q0 = np.zeros(14, dtype=np.float64) + self.soft_start_elapsed = 0.0 + self.soft_start_duration = 1.5 + + self.sit_delay_rem: float | None = None + self.stand_delay_rem: float | None = None + self.kick_steps: int | None = None + self.post_kick_lock: int = 0 + self.crouch_phase: float | None = None + self.pick_phase: float | None = None + self.roll_steps: int | None = None + self.roll_tipped: bool = False + self.script_t: float | None = None + + self.fall_debounce: int = 0 + self.recovery_state: str | None = None # None | "fallen" | "recovering" + self.recovery_steps: int = 0 + self.recovery_upright_steps: int = 0 + + def _load_params(self, manifest: dict[str, Any]) -> None: + safety = manifest.get("safety", {}) + head = manifest.get("head", {}) + legacy = manifest.get("legacy", {}) + variants = manifest.get("variants", {}) + + self.tilt_z = float(safety.get("tilt_z", -0.5)) + self.upright_z = float(safety.get("upright_z", -0.85)) + self.fall_debounce_steps = int(safety.get("fall_debounce_steps", 10)) + self.fall_settle_steps = int(safety.get("fall_settle_steps", 15)) + self.recover_upright_steps_req = int(safety.get("recover_upright_steps", 50)) + self.recover_giveup_steps = int(safety.get("recover_giveup_steps", 300)) + self.kick_total_steps = int(safety.get("kick_steps", 25)) + self.post_kick_lock_total = int(safety.get("post_kick_lock_steps", 20)) + self.roll_max_steps = int(safety.get("roll_max_steps", 150)) + self.sit_delay_s = float(safety.get("sit_delay_s", 0.8)) + self.stand_delay_s = float(safety.get("stand_delay_s", 2.0)) + + self.head_max = float(head.get("max_rad", 2.5)) + self.head_alpha = float(head.get("alpha", 0.2)) + self.head_signs = np.asarray(head.get("signs", [1, -1, 1, -1]), dtype=np.float32) + + self.crouch_period_s = float(legacy.get("crouch_period", 5.0)) + self.crouch_end_phase = float(legacy.get("crouch_end", 0.7)) + self.groundpick_period_s = float(legacy.get("groundpick_period", 4.0)) + self.groundpick_end_phase = float(legacy.get("groundpick_end", 0.7)) + + self.legs_limits = list(variants.get("legs", {}).get("limits", [0.25, -0.2, 1.0])) + self.rollers_limits = list(variants.get("rollers", {}).get("limits", [0.6, -0.5, 0.3])) + self.rollers_enabled = bool(variants.get("rollers", {}).get("enabled", False)) + + custom_move = manifest.get("custom_move") + self.custom_slot = custom_move.get("slot") if isinstance(custom_move, dict) else None + roll_skill = manifest.get("skills", {}).get("roll", {}) + ctrl_dt = float(manifest.get("sim", {}).get("ctrl_dt", 0.02)) + if self.custom_slot == "trick" and roll_skill.get("duration_s"): + self.roll_max_steps = max(10, round(float(roll_skill["duration_s"]) / ctrl_dt)) + + def update_manifest(self, manifest: dict[str, Any], custom_script: dict[str, Any] | None = None) -> None: + self.manifest = manifest + self.custom_script = custom_script + self._load_params(manifest) + if not self.rollers_enabled and self.loco == "rollers": + self.loco = "legs" + + def _clear_skill_runs(self) -> None: + self.sit_delay_rem = None + self.stand_delay_rem = None + self.kick_steps = None + self.post_kick_lock = 0 + self.crouch_phase = None + self.pick_phase = None + self.roll_steps = None + self.roll_tipped = False + self.script_t = None + self.fall_debounce = 0 + self.recovery_state = None + self.recovery_steps = 0 + self.recovery_upright_steps = 0 + + def emergency_stop(self) -> None: + self._clear_skill_runs() + self.skill_mode = "walk" + self.sit_flag = 0 + self.head_mode = False + self.vel_cmd.fill(0) + self.head_target.fill(0) + self.head_smooth.fill(0) + self.lifecycle = LifecycleState.IDLE + + def enter_calibration(self) -> None: + self.emergency_stop() + self.lifecycle = LifecycleState.CALIBRATION + + def exit_calibration(self) -> None: + if self.lifecycle == LifecycleState.CALIBRATION: + self.lifecycle = LifecycleState.IDLE + + def request_stand(self, current_q_rad: np.ndarray, soft_start_s: float = 1.5) -> None: + self._clear_skill_runs() + self.skill_mode = "walk" + self.sit_flag = 0 + self.head_mode = False + self.vel_cmd.fill(0) + self.head_target.fill(0) + self.head_smooth.fill(0) + self.soft_start_q0 = np.asarray(current_q_rad, dtype=np.float64).copy() + self.soft_start_elapsed = 0.0 + self.soft_start_duration = max(0.02, float(soft_start_s)) + self.lifecycle = LifecycleState.SOFT_START + + def set_velocity_cmd(self, norm_x: float, norm_wz: float, speed_scale: float = 1.0) -> None: + lim_f, lim_b, lim_a = self.rollers_limits if self.loco == "rollers" else self.legs_limits + nx = max(-1.0, min(1.0, float(norm_x))) + nwz = max(-1.0, min(1.0, float(norm_wz))) + scale = max(0.05, min(1.0, float(speed_scale))) + vx = (nx * lim_f if nx >= 0.0 else nx * (-lim_b)) * scale + wz = nwz * lim_a * scale + self.vel_cmd[0] = vx + self.vel_cmd[1] = 0.0 + self.vel_cmd[2] = wz + + def set_head_target(self, neck_pitch: float, head_pitch: float, head_yaw: float, head_roll: float) -> None: + raw = np.array([neck_pitch, head_pitch, head_yaw, head_roll], dtype=np.float32) + raw = np.clip(raw, -1.0, 1.0) + self.head_target = self.head_signs * raw * self.head_max + + def _is_one_shot_busy(self) -> bool: + return ( + (self.skill_mode == "roll" and self.roll_steps is not None) + or (self.skill_mode in ("kickL", "kickR") and self.kick_steps is not None) + or (self.skill_mode == "crouch" and self.crouch_phase is not None) + or (self.skill_mode == "groundpick" and self.pick_phase is not None) + ) + + def trigger_action(self, action: str) -> bool: + if self.lifecycle != LifecycleState.ACTIVE or self.recovery_state is not None: + return False + + if action == "headToggle": + if self.head_mode: + self.head_mode = False + return True + if ( + self.skill_mode in ("walk", "sitstand") + and self.post_kick_lock == 0 + and self.stand_delay_rem is None + ): + self.head_mode = True + return True + return False + + if action == "locoToggle": + if not self.rollers_enabled: + return False + if self._is_one_shot_busy() or self.stand_delay_rem is not None: + return False + self._clear_skill_runs() + self.skill_mode = "walk" + self.sit_flag = 0 + self.head_mode = False + self.loco = "rollers" if self.loco == "legs" else "legs" + return True + + if action == "sitToggle": + if self.loco != "legs": + return self.trigger_action("crouch") + if self.custom_slot == "trick": + return self.trigger_action("roll") + if self.custom_slot == "script" and self.custom_script is not None: + return self.trigger_action("scriptToggle") + if self._is_one_shot_busy(): + return False + self.head_mode = False + if self.skill_mode == "sitstand" and self.sit_flag == 1: + self.sit_flag = 0 + self.sit_delay_rem = None + self.stand_delay_rem = self.stand_delay_s + return True + if self.skill_mode == "walk" and self.stand_delay_rem is None: + self.skill_mode = "sitstand" + self.sit_flag = 0 + self.sit_delay_rem = self.sit_delay_s + return True + return False + + if action == "scriptToggle": + if self.custom_script is None or self.loco != "legs": + return False + if self.script_t is not None: + self.script_t = None + return True + if self.skill_mode == "walk" and self.stand_delay_rem is None: + self.head_mode = False + self.script_t = 0.0 + return True + return False + + if action in ("kickL", "kickR"): + if self.loco != "legs" or self.skill_mode != "walk" or self.stand_delay_rem is not None: + return False + self.head_mode = False + self.sit_delay_rem = None + self.skill_mode = action + self.sit_flag = 0 + self.kick_steps = 0 + return True + + if action == "groundPick": + if self.loco != "legs" or self.skill_mode != "walk" or self.stand_delay_rem is not None: + return False + self.head_mode = False + self.sit_delay_rem = None + self.skill_mode = "groundpick" + self.sit_flag = 0 + self.pick_phase = 0.0 + return True + + if action == "roll": + if self.loco == "rollers": + return self.trigger_action("crouch") + if self.skill_mode != "walk" or self.stand_delay_rem is not None: + return False + self.head_mode = False + self.sit_delay_rem = None + self.skill_mode = "roll" + self.sit_flag = 0 + self.roll_steps = 0 + self.roll_tipped = False + return True + + if action == "crouch": + if self.loco != "rollers" or self.skill_mode != "walk": + return False + self.head_mode = False + self.skill_mode = "crouch" + self.crouch_phase = 0.0 + return True + + return False + + def _active_policy_key(self) -> str | None: + if self.recovery_state == "fallen": + return None + if self.recovery_state == "recovering": + return "stand" + if self.loco == "rollers" and self.skill_mode == "walk": + return "drive" + return self.skill_mode + + def _mode_label(self) -> str: + if self.lifecycle == LifecycleState.IDLE: + return "Idle (Torque Off)" + if self.lifecycle == LifecycleState.CALIBRATION: + return "Calibration" + if self.lifecycle == LifecycleState.FALL_SAFE: + return "Fall Safe" + if self.lifecycle == LifecycleState.SOFT_START: + return "Soft Start" + if self.recovery_state: + return "Recovery" + if self.skill_mode == "roll": + return "Trick" if self.custom_slot == "trick" else "Roll" + if self.script_t is not None: + return "Script" + if self.skill_mode == "crouch": + return "Crouch" + if self.skill_mode == "groundpick": + return "Pick" + if self.skill_mode in ("kickL", "kickR"): + return "Kick" + if self.head_mode: + return "Head" + if self.skill_mode == "sitstand" and self.sit_flag == 1: + return "Sit" + return "Drive" if self.loco == "rollers" else "Run" + + def _build_command_13(self) -> np.ndarray: + cmd = np.zeros(13, dtype=np.float32) + if self.skill_mode == "sitstand": + cmd[0] = float(self.sit_flag) + elif self.skill_mode == "crouch" and self.crouch_phase is not None: + a = 2.0 * math.pi * self.crouch_phase + cmd[0] = math.cos(a) + cmd[1] = math.sin(a) + elif self.skill_mode == "groundpick" and self.pick_phase is not None: + a = 2.0 * math.pi * self.pick_phase + cmd[0] = math.cos(a) + cmd[1] = math.sin(a) + elif not ( + self.head_mode + or self.skill_mode == "roll" + or self.skill_mode in ("kickL", "kickR") + or self.post_kick_lock > 0 + or self.recovery_state is not None + ): + cmd[0:3] = self.vel_cmd + + for h in range(4): + self.head_smooth[h] += self.head_alpha * (self.head_target[h] - self.head_smooth[h]) + + gp_zero = self.skill_mode == "groundpick" or self.recovery_state is not None + if not gp_zero: + cmd[3:7] = self.head_smooth + + if ( + self.script_t is not None + and self.skill_mode == "walk" + and self.recovery_state is None + and self.custom_script is not None + ): + cmd = script_command_at(self.custom_script, self.script_t, 13) + return cmd + + def step( + self, + dt: float, + proj_grav_z: float, + auto_stand_on_fall: bool = False, + ) -> StepDecision: + zero_last_action = False + + if self.lifecycle in (LifecycleState.IDLE, LifecycleState.CALIBRATION, LifecycleState.FALL_SAFE): + return StepDecision( + lifecycle=self.lifecycle, + skill_mode=self.skill_mode, + policy_key=None, + torque_enabled=False, + soft_start_alpha=0.0, + command_13=np.zeros(13, dtype=np.float32), + zero_last_action=False, + mode_label=self._mode_label(), + recovery_state=self.recovery_state, + loco=self.loco, + head_mode=self.head_mode, + ) + + if self.lifecycle == LifecycleState.SOFT_START: + self.soft_start_elapsed += dt + u = min(1.0, self.soft_start_elapsed / self.soft_start_duration) + alpha = 0.5 * (1.0 - math.cos(math.pi * u)) + if u < 1.0 - 1e-6: + return StepDecision( + lifecycle=self.lifecycle, + skill_mode="soft_start", + policy_key=None, + torque_enabled=True, + soft_start_alpha=alpha, + command_13=np.zeros(13, dtype=np.float32), + zero_last_action=True, + mode_label=self._mode_label(), + recovery_state=None, + loco=self.loco, + head_mode=False, + ) + self.lifecycle = LifecycleState.ACTIVE + self.skill_mode = "walk" + zero_last_action = True + + # Active lifecycle: advance timers before fall/recovery and command build + if self.sit_delay_rem is not None: + self.sit_delay_rem -= dt + if self.sit_delay_rem <= 0.0: + self.sit_delay_rem = None + if self.skill_mode == "sitstand": + self.sit_flag = 1 + + if self.stand_delay_rem is not None: + self.stand_delay_rem -= dt + if self.stand_delay_rem <= 0.0: + self.stand_delay_rem = None + self.skill_mode = "walk" + zero_last_action = True + + # Fall detection & recovery state machine + fallen_now = (not math.isfinite(proj_grav_z)) or (proj_grav_z > self.tilt_z) + if self.recovery_state == "fallen": + self.recovery_steps += 1 + if self.recovery_steps >= self.fall_settle_steps: + self.recovery_state = "recovering" + self.recovery_steps = 0 + self.recovery_upright_steps = 0 + zero_last_action = True + elif self.recovery_state == "recovering": + self.recovery_steps += 1 + if proj_grav_z < self.upright_z: + self.recovery_upright_steps += 1 + else: + self.recovery_upright_steps = 0 + if self.recovery_upright_steps >= self.recover_upright_steps_req: + self.recovery_state = None + self.recovery_steps = 0 + self.recovery_upright_steps = 0 + self.skill_mode = "walk" + zero_last_action = True + elif self.recovery_steps >= self.recover_giveup_steps: + self._clear_skill_runs() + self.lifecycle = LifecycleState.FALL_SAFE + return StepDecision( + lifecycle=self.lifecycle, + skill_mode="walk", + policy_key=None, + torque_enabled=False, + soft_start_alpha=0.0, + command_13=np.zeros(13, dtype=np.float32), + zero_last_action=True, + mode_label=self._mode_label(), + recovery_state=None, + loco=self.loco, + head_mode=False, + ) + elif fallen_now: + recoverable = ( + auto_stand_on_fall + and self.loco == "legs" + and self.skill_mode == "walk" + and self.post_kick_lock == 0 + and self.stand_delay_rem is None + ) + self.fall_debounce += 1 + if self.fall_debounce >= self.fall_debounce_steps: + self.fall_debounce = 0 + self.head_mode = False + if recoverable: + self.recovery_state = "fallen" + self.recovery_steps = 0 + else: + self._clear_skill_runs() + self.lifecycle = LifecycleState.FALL_SAFE + return StepDecision( + lifecycle=self.lifecycle, + skill_mode="walk", + policy_key=None, + torque_enabled=False, + soft_start_alpha=0.0, + command_13=np.zeros(13, dtype=np.float32), + zero_last_action=True, + mode_label=self._mode_label(), + recovery_state=None, + loco=self.loco, + head_mode=False, + ) + else: + self.fall_debounce = 0 + + policy_key = self._active_policy_key() + command_13 = self._build_command_13() + + # Advance step-counted skills AFTER choosing this tick's policy/command + if self.post_kick_lock > 0 and self.skill_mode == "walk": + self.post_kick_lock -= 1 + + if self.skill_mode in ("kickL", "kickR") and self.kick_steps is not None: + self.kick_steps += 1 + if self.kick_steps >= self.kick_total_steps: + self.kick_steps = None + self.skill_mode = "walk" + self.post_kick_lock = self.post_kick_lock_total + + if self.skill_mode == "crouch" and self.crouch_phase is not None: + self.crouch_phase += dt / self.crouch_period_s + if self.crouch_phase >= self.crouch_end_phase: + self.crouch_phase = None + self.skill_mode = "walk" + + if self.skill_mode == "groundpick" and self.pick_phase is not None: + self.pick_phase += dt / self.groundpick_period_s + if self.pick_phase >= self.groundpick_end_phase: + self.pick_phase = None + self.skill_mode = "walk" + + if self.skill_mode == "roll" and self.roll_steps is not None: + self.roll_steps += 1 + if proj_grav_z > -0.3: + self.roll_tipped = True + upright = proj_grav_z < self.upright_z + done = self.roll_tipped and upright and self.roll_steps >= 40 + expired = self.roll_steps >= self.roll_max_steps + if done or expired: + self.roll_steps = None + self.roll_tipped = False + self.skill_mode = "walk" + zero_last_action = True + if not upright: + self.lifecycle = LifecycleState.FALL_SAFE + + if self.script_t is not None: + self.script_t += dt + if self.custom_script is None: + self.script_t = None + else: + dur = float(self.custom_script.get("durationS") or self.custom_script.get("duration_s") or 0.0) + loop = self.custom_script.get("loop", True) is not False + if not loop and self.script_t >= dur: + self.script_t = None + + return StepDecision( + lifecycle=self.lifecycle, + skill_mode=self.skill_mode, + policy_key=policy_key, + torque_enabled=self.lifecycle == LifecycleState.ACTIVE, + soft_start_alpha=1.0, + command_13=command_13, + zero_last_action=zero_last_action, + mode_label=self._mode_label(), + recovery_state=self.recovery_state, + loco=self.loco, + head_mode=self.head_mode, + ) diff --git a/app/public/hardware/unoq/python/hal/__init__.py b/app/public/hardware/unoq/python/hal/__init__.py new file mode 100644 index 0000000..5904c35 --- /dev/null +++ b/app/public/hardware/unoq/python/hal/__init__.py @@ -0,0 +1 @@ +"""Hardware abstraction layers for Microduck on Arduino UNO Q.""" diff --git a/app/public/hardware/unoq/python/hal/base.py b/app/public/hardware/unoq/python/hal/base.py new file mode 100644 index 0000000..234b0ca --- /dev/null +++ b/app/public/hardware/unoq/python/hal/base.py @@ -0,0 +1,57 @@ +from __future__ import annotations + +from abc import ABC, abstractmethod +from dataclasses import dataclass, field +from typing import Any + + +@dataclass +class HardwareFrame: + gyro_raw: list[float] + proj_gravity_raw: list[float] + ticks: list[int] + raw_speeds: list[int] + voltage_v: float = 7.4 + max_temp_c: float = 35.0 + imu_ok: bool = True + servo_err_mask: int = 0 + timestamp_s: float = 0.0 + extra: dict[str, Any] = field(default_factory=dict) + + +class HardwareBridge(ABC): + name: str = "abstract" + + @abstractmethod + def connect(self) -> None: + """Initialize connection to the MCU / servo bus.""" + + @abstractmethod + def close(self) -> None: + """Release hardware resources safely.""" + + @abstractmethod + def configure(self, joint_names: list[str], profile: dict[str, Any]) -> None: + """Push servo IDs, I2C address, watchdog, and IMU bias config to HAL.""" + + @abstractmethod + def sync_step( + self, + enable_torque: bool, + target_ticks: list[int], + speed_limit: int = 0, + acc: int = 50, + ) -> HardwareFrame: + """Send target joint ticks and read fresh IMU + servo feedback.""" + + @abstractmethod + def set_torque(self, enable: bool) -> None: + """Immediately enable or disable torque on all configured servos.""" + + @abstractmethod + def ping_servos(self, max_id: int = 20) -> list[int]: + """Scan and return responding servo IDs.""" + + @abstractmethod + def calibrate_imu_raw(self, samples: int = 200) -> list[float]: + """Collect stationary gyro readings (rad/s in sensor frame) and return mean bias.""" diff --git a/app/public/hardware/unoq/python/hal/mock_hal.py b/app/public/hardware/unoq/python/hal/mock_hal.py new file mode 100644 index 0000000..3841e3f --- /dev/null +++ b/app/public/hardware/unoq/python/hal/mock_hal.py @@ -0,0 +1,82 @@ +from __future__ import annotations + +import math +import time +from typing import Any + +from .base import HardwareBridge, HardwareFrame + + +class MockHal(HardwareBridge): + """PC-runnable hardware simulator for pre-hardware verification.""" + + name = "mock" + + def __init__(self, initial_tick: int = 2048) -> None: + self.connected = False + self.torque_enabled = False + self.servo_ids = list(range(1, 15)) + self.ticks = [int(initial_tick)] * 14 + self.raw_speeds = [0] * 14 + self._gyro = [0.0, 0.0, 0.0] + self._gravity = [0.0, 0.0, -1.0] + self._alpha = 0.45 + + def connect(self) -> None: + self.connected = True + + def close(self) -> None: + self.torque_enabled = False + self.connected = False + + def configure(self, joint_names: list[str], profile: dict[str, Any]) -> None: + joints = profile.get("joints", {}) + self.servo_ids = [int(joints[n]["id"]) for n in joint_names] + + def set_mock_gravity(self, grav: list[float]) -> None: + gx, gy, gz = float(grav[0]), float(grav[1]), float(grav[2]) + norm = math.sqrt(gx * gx + gy * gy + gz * gz) or 1.0 + self._gravity = [gx / norm, gy / norm, gz / norm] + + def set_mock_gyro(self, gyro: list[float]) -> None: + self._gyro = [float(gyro[0]), float(gyro[1]), float(gyro[2])] + + def sync_step( + self, + enable_torque: bool, + target_ticks: list[int], + speed_limit: int = 0, + acc: int = 50, + ) -> HardwareFrame: + self.torque_enabled = bool(enable_torque) + if self.torque_enabled: + for i in range(14): + prev = self.ticks[i] + nxt = int(round(prev + self._alpha * (int(target_ticks[i]) - prev))) + delta = nxt - prev + self.ticks[i] = max(0, min(4095, nxt)) + mag = min(0x7FFF, abs(delta) * 50) + self.raw_speeds[i] = (mag | 0x8000) if delta < 0 else mag + else: + self.raw_speeds = [0] * 14 + + return HardwareFrame( + gyro_raw=list(self._gyro), + proj_gravity_raw=list(self._gravity), + ticks=list(self.ticks), + raw_speeds=list(self.raw_speeds), + voltage_v=7.8, + max_temp_c=34.0, + imu_ok=True, + servo_err_mask=0, + timestamp_s=time.monotonic(), + ) + + def set_torque(self, enable: bool) -> None: + self.torque_enabled = bool(enable) + + def ping_servos(self, max_id: int = 20) -> list[int]: + return [sid for sid in self.servo_ids if 0 <= sid <= max_id] + + def calibrate_imu_raw(self, samples: int = 200) -> list[float]: + return list(self._gyro) diff --git a/app/public/hardware/unoq/python/hal/router_hal.py b/app/public/hardware/unoq/python/hal/router_hal.py new file mode 100644 index 0000000..794dc9a --- /dev/null +++ b/app/public/hardware/unoq/python/hal/router_hal.py @@ -0,0 +1,109 @@ +from __future__ import annotations + +import time +from typing import Any + +from .base import HardwareBridge, HardwareFrame + + +class RouterBridgeHal(HardwareBridge): + """Arduino UNO Q HAL communicating with STM32U585 over Arduino_RouterBridge.""" + + name = "router" + + def __init__(self) -> None: + self._bridge = None + self._servo_ids = list(range(1, 15)) + + def connect(self) -> None: + try: + from arduino.app_utils import Bridge # type: ignore + except ImportError as exc: + raise RuntimeError( + "arduino.app_utils.Bridge is not available. Run on Arduino UNO Q via App Lab / " + "arduino-app-cli, or pass '--hal mock' for PC pre-hardware debugging." + ) from exc + self._bridge = Bridge + + def close(self) -> None: + if self._bridge is not None: + try: + self.set_torque(False) + except Exception: + pass + self._bridge = None + + def _require_bridge(self) -> Any: + if self._bridge is None: + raise RuntimeError("RouterBridgeHal is not connected") + return self._bridge + + def configure(self, joint_names: list[str], profile: dict[str, Any]) -> None: + bridge = self._require_bridge() + joints = profile["joints"] + servo = profile["servo"] + imu = profile["imu"] + safety = profile.get("safety", {}) + self._servo_ids = [int(joints[name]["id"]) for name in joint_names] + + addr_raw = imu.get("i2c_addr", "0x6A") + i2c_addr = int(addr_raw, 16) if isinstance(addr_raw, str) else int(addr_raw) + bias = [float(x) for x in imu.get("gyro_bias_rad_s", [0.0, 0.0, 0.0])] + payload: list[float] = [ + float(servo.get("baudrate", 1000000)), + float(i2c_addr), + float(safety.get("watchdog_ms", 150)), + bias[0], + bias[1], + bias[2], + *[float(sid) for sid in self._servo_ids], + ] + bridge.call("hw_configure", payload) + + def sync_step( + self, + enable_torque: bool, + target_ticks: list[int], + speed_limit: int = 0, + acc: int = 50, + ) -> HardwareFrame: + bridge = self._require_bridge() + if len(target_ticks) != 14: + raise ValueError("Expected 14 target_ticks") + cmd_frame = [ + 1 if enable_torque else 0, + int(speed_limit), + int(acc), + *[int(t) for t in target_ticks], + ] + res = bridge.call("hw_sync_step", cmd_frame) + if not isinstance(res, (list, tuple)) or len(res) < 38: + raise RuntimeError(f"Invalid hw_sync_step response from MCU: {res!r}") + + return HardwareFrame( + gyro_raw=[float(res[0]), float(res[1]), float(res[2])], + proj_gravity_raw=[float(res[3]), float(res[4]), float(res[5])], + ticks=[int(round(res[6 + i])) for i in range(14)], + raw_speeds=[int(round(res[20 + i])) for i in range(14)], + voltage_v=float(res[34]), + max_temp_c=float(res[35]), + imu_ok=bool(int(round(res[36]))), + servo_err_mask=int(round(res[37])), + timestamp_s=time.monotonic(), + ) + + def set_torque(self, enable: bool) -> None: + bridge = self._require_bridge() + bridge.call("hw_set_torque", 1 if enable else 0) + + def ping_servos(self, max_id: int = 20) -> list[int]: + bridge = self._require_bridge() + res = bridge.call("hw_ping_servos", int(max_id)) + return [int(x) for x in res] if isinstance(res, (list, tuple)) else [] + + def calibrate_imu_raw(self, samples: int = 200) -> list[float]: + bridge = self._require_bridge() + res = bridge.call("hw_calibrate_imu", int(samples)) + if not isinstance(res, (list, tuple)) or len(res) != 3: + raise RuntimeError(f"Invalid hw_calibrate_imu response: {res!r}") + return [float(res[0]), float(res[1]), float(res[2])] diff --git a/app/public/hardware/unoq/python/main.py b/app/public/hardware/unoq/python/main.py new file mode 100644 index 0000000..ae0eccd --- /dev/null +++ b/app/public/hardware/unoq/python/main.py @@ -0,0 +1,125 @@ +from __future__ import annotations + +import argparse +import os +import signal +import sys +from pathlib import Path + +PYTHON_ROOT = Path(__file__).resolve().parent +UNOQ_ROOT = PYTHON_ROOT.parent +if str(PYTHON_ROOT) not in sys.path: + sys.path.insert(0, str(PYTHON_ROOT)) + +from core.config import load_hardware_profile, load_robot_manifest # noqa: E402 +from core.controller import MicroduckController # noqa: E402 +from core.deploy_service import DeployService # noqa: E402 +from core.policy_runner import PolicySet # noqa: E402 +from hal.base import HardwareBridge # noqa: E402 +from hal.mock_hal import MockHal # noqa: E402 +from hal.router_hal import RouterBridgeHal # noqa: E402 +from web.fallback_server import FallbackWebServer # noqa: E402 + + +def _select_hal(hal_mode: str) -> HardwareBridge: + if hal_mode == "mock": + return MockHal() + if hal_mode == "router": + return RouterBridgeHal() + # auto: prefer RouterBridge when running on UNO Q App Lab, otherwise ask explicitly for --hal mock + try: + from arduino.app_utils import Bridge # type: ignore # noqa: F401 + + return RouterBridgeHal() + except ImportError as exc: + raise RuntimeError( + "arduino.app_utils is not available in this Python environment. " + "Launch inside UNO Q App Lab, or pass '--hal mock' for PC pre-hardware testing." + ) from exc + + +def main(argv: list[str] | None = None) -> int: + parser = argparse.ArgumentParser(description="Microduck UNO Q 50Hz Controller & Deploy Server") + parser.add_argument("--base-dir", type=Path, default=UNOQ_ROOT, help="UNO Q config/policies directory") + parser.add_argument( + "--hal", + choices=("auto", "router", "mock"), + default=os.environ.get("MICRODUCK_HAL", "auto"), + help="Hardware backend: 'router' (UNO Q STM32), 'mock' (PC simulator), or 'auto'", + ) + parser.add_argument( + "--policy-backend", + choices=("auto", "onnx", "dummy"), + default=os.environ.get("MICRODUCK_POLICY_BACKEND", "auto"), + help="Inference backend: 'onnx' / 'auto', or explicit 'dummy' dry-run", + ) + parser.add_argument("--host", default="0.0.0.0") + parser.add_argument("--port", type=int, default=8000) + args = parser.parse_args(argv) + + base_dir = Path(args.base_dir).resolve() + manifest = load_robot_manifest(base_dir / "config" / "robot_manifest.json") + profile = load_hardware_profile(base_dir / "config" / "hardware_profile.json", joint_names=manifest["joint_names"]) + + custom_script = None + script_path = base_dir / "config" / "custom_move_script.json" + if script_path.is_file(): + import json + + custom_script = json.loads(script_path.read_text(encoding="utf-8")) + + hal = _select_hal(args.hal) + + boot_error: str | None = None + try: + policies = PolicySet.load_from_manifest(base_dir, manifest, backend=args.policy_backend) + except FileNotFoundError as exc: + # Allow web server & /api/deploy to boot on a fresh board before first push from website + boot_error = f"Awaiting first policy deploy: {exc}" + policies = PolicySet({}, backend="awaiting_deploy") + + controller = MicroduckController( + base_dir=base_dir, + manifest=manifest, + profile=profile, + hal=hal, + policies=policies, + custom_script=custom_script, + ) + if boot_error: + controller._last_error = boot_error + + deploy_backend = args.policy_backend + deploy_service = DeployService(base_dir=base_dir, controller=controller, policy_backend=deploy_backend) + + controller.start() + + def _shutdown(*_args: object) -> None: + raise SystemExit(0) + + signal.signal(signal.SIGTERM, _shutdown) + + srv: FallbackWebServer | None = None + try: + try: + import uvicorn # type: ignore + + from web.server import create_fastapi_app + + app = create_fastapi_app(controller, deploy_service) + uvicorn.run(app, host=args.host, port=args.port, log_level="info") + except ImportError: + srv = FallbackWebServer(args.host, args.port, controller, deploy_service) + print(f"[microduck-unoq] FastAPI/uvicorn not installed; serving stdlib HTTP/WS on {args.host}:{srv.port}") + srv.serve_forever() + except (KeyboardInterrupt, SystemExit): + pass + finally: + if srv is not None: + srv.server_close() + controller.stop() + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/app/public/hardware/unoq/python/requirements.txt b/app/public/hardware/unoq/python/requirements.txt new file mode 100644 index 0000000..200314a --- /dev/null +++ b/app/public/hardware/unoq/python/requirements.txt @@ -0,0 +1,5 @@ +numpy>=1.26.0 +onnxruntime>=1.18.0 +fastapi>=0.115.0 +uvicorn[standard]>=0.30.0 +python-multipart>=0.0.9 diff --git a/app/public/hardware/unoq/python/web/__init__.py b/app/public/hardware/unoq/python/web/__init__.py new file mode 100644 index 0000000..18208bc --- /dev/null +++ b/app/public/hardware/unoq/python/web/__init__.py @@ -0,0 +1 @@ +"""Web HTTP/WebSocket control and deployment package for Microduck UNO Q.""" diff --git a/app/public/hardware/unoq/python/web/api.py b/app/public/hardware/unoq/python/web/api.py new file mode 100644 index 0000000..766b42c --- /dev/null +++ b/app/public/hardware/unoq/python/web/api.py @@ -0,0 +1,184 @@ +from __future__ import annotations + +import copy +import json +from email.parser import BytesParser +from email.policy import default as email_default_policy +from pathlib import Path +from typing import Any + +from core.controller import MicroduckController +from core.deploy_service import DeployService + + +STATIC_DIR = Path(__file__).resolve().parent / "static" + + +def load_index_html() -> bytes: + path = STATIC_DIR / "index.html" + if not path.is_file(): + return b"

Microduck controller UI missing

" + return path.read_bytes() + + +def parse_bool_flag(raw: str | None) -> bool: + if raw is None: + return False + return raw.strip().lower() in ("1", "true", "yes", "on") + + +def extract_deploy_payload( + content_type: str, + body: bytes, + query_overwrite: str | None = None, +) -> tuple[bytes, bool]: + overwrite = parse_bool_flag(query_overwrite) + ctype = (content_type or "").lower() + if "multipart/form-data" in ctype: + raw_msg = f"Content-Type: {content_type}\r\nMIME-Version: 1.0\r\n\r\n".encode("latin1") + body + msg = BytesParser(policy=email_default_policy).parsebytes(raw_msg) + if not msg.is_multipart(): + raise ValueError("Malformed multipart/form-data upload") + zip_bytes: bytes | None = None + for part in msg.iter_parts(): + field = part.get_param("name", header="content-disposition") + payload = part.get_payload(decode=True) or b"" + if field in ("bundle", "file", "zip") and payload: + zip_bytes = payload + elif field == "overwrite_profile": + overwrite = parse_bool_flag(payload.decode("utf-8", errors="ignore")) + if not zip_bytes: + raise ValueError("Multipart upload missing 'bundle' zip field") + return zip_bytes, overwrite + return body, overwrite + + +def build_status_payload(controller: MicroduckController, deploy_service: DeployService) -> dict[str, Any]: + st = controller.get_status() + st["last_deploy"] = copy.deepcopy(deploy_service.last_deploy) if deploy_service.last_deploy else None + return st + + +def handle_ws_message(controller: MicroduckController, msg: dict[str, Any]) -> dict[str, Any] | None: + kind = msg.get("type") + if kind == "input": + controller.set_remote_input( + norm_x=float(msg.get("vx", 0.0)), + norm_wz=float(msg.get("wz", 0.0)), + speed_scale=float(msg["speed_scale"]) if "speed_scale" in msg else None, + ) + return None + if kind == "head": + controller.set_head_target( + float(msg.get("neck_pitch", 0.0)), + float(msg.get("head_pitch", 0.0)), + float(msg.get("head_yaw", 0.0)), + float(msg.get("head_roll", 0.0)), + ) + return None + if kind == "stand": + try: + controller.request_stand() + return {"type": "ack", "event": "stand"} + except RuntimeError as exc: + return {"type": "error", "error": str(exc)} + if kind == "estop": + controller.emergency_stop() + return {"type": "ack", "event": "estop"} + if kind == "action": + action = str(msg.get("action", "")) + ok = controller.trigger_action(action) + return {"type": "ack", "event": "action", "action": action, "ok": ok} + if kind == "settings": + if "auto_stand_on_fall" in msg: + controller.set_auto_stand_on_fall(bool(msg["auto_stand_on_fall"])) + if "speed_scale" in msg: + controller.set_speed_scale(float(msg["speed_scale"])) + return {"type": "ack", "event": "settings"} + if kind == "ping": + return {"type": "pong", "t": msg.get("t")} + return None + + +def dispatch_api_get( + path: str, + controller: MicroduckController, + deploy_service: DeployService, +) -> tuple[int, dict[str, Any]]: + if path == "/api/status": + return 200, build_status_payload(controller, deploy_service) + if path == "/api/profile": + return 200, {"ok": True, "profile": copy.deepcopy(controller.profile)} + return 404, {"ok": False, "error": f"Unknown GET endpoint: {path}"} + + +def dispatch_api_post( + path: str, + body_bytes: bytes, + content_type: str, + query_params: dict[str, list[str]], + controller: MicroduckController, + deploy_service: DeployService, +) -> tuple[int, dict[str, Any]]: + if path == "/api/deploy": + try: + q_ow = query_params.get("overwrite_profile", [None])[0] + raw_zip, overwrite = extract_deploy_payload(content_type, body_bytes, q_ow) + res = dict(deploy_service.apply_zip_bytes(raw_zip, overwrite_profile=overwrite)) + res["status"] = build_status_payload(controller, deploy_service) + return 200, res + except ValueError as exc: + return 400, {"ok": False, "error": str(exc)} + except Exception as exc: + return 500, {"ok": False, "error": str(exc)} + + try: + payload = json.loads(body_bytes.decode("utf-8")) if body_bytes.strip() else {} + except Exception: + return 400, {"ok": False, "error": "Invalid JSON body"} + + try: + if path == "/api/stand": + controller.request_stand() + return 200, build_status_payload(controller, deploy_service) + if path == "/api/estop": + controller.emergency_stop() + return 200, build_status_payload(controller, deploy_service) + if path == "/api/action": + action = str(payload.get("action", "")) + ok = controller.trigger_action(action) + return 200, {"ok": ok, "status": build_status_payload(controller, deploy_service)} + if path == "/api/settings": + if "auto_stand_on_fall" in payload: + controller.set_auto_stand_on_fall(bool(payload["auto_stand_on_fall"])) + if "speed_scale" in payload: + controller.set_speed_scale(float(payload["speed_scale"])) + return 200, build_status_payload(controller, deploy_service) + if path == "/api/profile": + profile = controller.update_profile(payload) + return 200, {"ok": True, "profile": profile} + if path == "/api/calibrate/ping": + max_id = int(payload.get("max_id", 20)) + return 200, {"ok": True, "ids": controller.ping_servos(max_id=max_id)} + if path == "/api/calibrate/joint_zero": + joint = str(payload.get("joint", "")) + profile = controller.capture_joint_zero(joint) + return 200, {"ok": True, "profile": profile} + if path == "/api/calibrate/test_joint": + joint = str(payload.get("joint", "")) + rad = float(payload.get("rad", 0.0)) + ticks = controller.test_single_joint(joint, rad) + return 200, {"ok": True, "ticks": ticks} + if path == "/api/calibrate/exit": + controller.exit_calibration() + return 200, build_status_payload(controller, deploy_service) + if path == "/api/calibrate/imu_zero": + samples = int(payload.get("samples", 200)) + imu = controller.calibrate_imu_zero(samples=samples) + return 200, {"ok": True, "imu": imu} + except (ValueError, RuntimeError) as exc: + return 400, {"ok": False, "error": str(exc)} + except Exception as exc: + return 500, {"ok": False, "error": str(exc)} + + return 404, {"ok": False, "error": f"Unknown POST endpoint: {path}"} diff --git a/app/public/hardware/unoq/python/web/fallback_server.py b/app/public/hardware/unoq/python/web/fallback_server.py new file mode 100644 index 0000000..7452fef --- /dev/null +++ b/app/public/hardware/unoq/python/web/fallback_server.py @@ -0,0 +1,227 @@ +from __future__ import annotations + +import base64 +import hashlib +import json +import select +import socket +import struct +import threading +import time +from http.server import BaseHTTPRequestHandler, ThreadingHTTPServer +from typing import Any +from urllib.parse import parse_qs, urlparse + +from core.controller import MicroduckController +from core.deploy_service import DeployService + +from .api import ( + build_status_payload, + dispatch_api_get, + dispatch_api_post, + handle_ws_message, + load_index_html, +) + + +WS_GUID = "258EAFA5-E914-47DA-95CA-C5AB0DC85B11" + + +def _ws_encode_frame(opcode: int, payload: bytes) -> bytes: + header = bytearray([0x80 | (opcode & 0x0F)]) + n = len(payload) + if n < 126: + header.append(n) + elif n < 65536: + header.append(126) + header.extend(struct.pack("!H", n)) + else: + header.append(127) + header.extend(struct.pack("!Q", n)) + return bytes(header) + payload + + +def _recv_exact(sock: socket.socket, n: int) -> bytes | None: + chunks = bytearray() + while len(chunks) < n: + chunk = sock.recv(n - len(chunks)) + if not chunk: + return None + chunks.extend(chunk) + return bytes(chunks) + + +def _ws_read_frame(sock: socket.socket) -> tuple[int, bytes] | None: + head = _recv_exact(sock, 2) + if head is None: + return None + b1, b2 = head[0], head[1] + opcode = b1 & 0x0F + masked = bool(b2 & 0x80) + length = b2 & 0x7F + if length == 126: + ext = _recv_exact(sock, 2) + if ext is None: + return None + length = struct.unpack("!H", ext)[0] + elif length == 127: + ext = _recv_exact(sock, 8) + if ext is None: + return None + length = struct.unpack("!Q", ext)[0] + if length > 1024 * 1024: + return None + mask = _recv_exact(sock, 4) if masked else b"" + if masked and mask is None: + return None + payload = _recv_exact(sock, length) if length > 0 else b"" + if payload is None: + return None + if masked: + payload = bytes(b ^ mask[i % 4] for i, b in enumerate(payload)) + return opcode, payload + + +class FallbackWebServer(ThreadingHTTPServer): + """Zero-dependency HTTP + RFC6455 WebSocket server (runs on stock Python 3.12).""" + + allow_reuse_address = True + daemon_threads = True + + def __init__( + self, + host: str, + port: int, + controller: MicroduckController, + deploy_service: DeployService, + ) -> None: + self.controller = controller + self.deploy_service = deploy_service + super().__init__((host, port), _RequestHandler) + self.port = int(self.server_address[1]) + + +class _RequestHandler(BaseHTTPRequestHandler): + protocol_version = "HTTP/1.1" + + def log_message(self, format: str, *args: Any) -> None: # noqa: A002 + return + + def _send_cors_headers(self) -> None: + self.send_header("Access-Control-Allow-Origin", "*") + self.send_header("Access-Control-Allow-Methods", "GET, POST, OPTIONS") + self.send_header("Access-Control-Allow-Headers", "Content-Type") + self.send_header("Access-Control-Allow-Private-Network", "true") + + def _send_json(self, status_code: int, payload: dict[str, Any]) -> None: + raw = json.dumps(payload).encode("utf-8") + self.send_response(status_code) + self._send_cors_headers() + self.send_header("Content-Type", "application/json; charset=utf-8") + self.send_header("Content-Length", str(len(raw))) + self.end_headers() + self.wfile.write(raw) + + def _send_bytes(self, status_code: int, content_type: str, data: bytes) -> None: + self.send_response(status_code) + self._send_cors_headers() + self.send_header("Content-Type", content_type) + self.send_header("Content-Length", str(len(data))) + self.end_headers() + self.wfile.write(data) + + def do_OPTIONS(self) -> None: # noqa: N802 + self.send_response(204) + self._send_cors_headers() + self.send_header("Content-Length", "0") + self.end_headers() + + def do_GET(self) -> None: # noqa: N802 + parsed = urlparse(self.path) + if parsed.path == "/ws/control" and self.headers.get("Upgrade", "").lower() == "websocket": + self._handle_websocket() + return + if parsed.path in ("/", "/index.html"): + self._send_bytes(200, "text/html; charset=utf-8", load_index_html()) + return + + srv: FallbackWebServer = self.server # type: ignore[assignment] + code, payload = dispatch_api_get(parsed.path, srv.controller, srv.deploy_service) + self._send_json(code, payload) + + def do_POST(self) -> None: # noqa: N802 + parsed = urlparse(self.path) + length = int(self.headers.get("Content-Length", "0") or "0") + if length < 0 or length > 160 * 1024 * 1024: + self._send_json(413, {"ok": False, "error": "Payload too large"}) + return + body = self.rfile.read(length) if length > 0 else b"" + content_type = self.headers.get("Content-Type", "") + query = parse_qs(parsed.query) + srv: FallbackWebServer = self.server # type: ignore[assignment] + code, payload = dispatch_api_post( + parsed.path, + body, + content_type, + query, + srv.controller, + srv.deploy_service, + ) + self._send_json(code, payload) + + def _handle_websocket(self) -> None: + key = self.headers.get("Sec-WebSocket-Key") + if not key: + self.send_error(400, "Missing Sec-WebSocket-Key") + return + accept = base64.b64encode(hashlib.sha1((key.strip() + WS_GUID).encode("ascii")).digest()).decode("ascii") + self.send_response(101, "Switching Protocols") + self.send_header("Upgrade", "websocket") + self.send_header("Connection", "Upgrade") + self.send_header("Sec-WebSocket-Accept", accept) + self.end_headers() + + srv: FallbackWebServer = self.server # type: ignore[assignment] + sock: socket.socket = self.connection + sock.settimeout(5.0) + write_lock = threading.Lock() + last_telemetry = 0.0 + + try: + while True: + now = time.monotonic() + if now - last_telemetry >= 0.1: + last_telemetry = now + msg = {"type": "telemetry", **build_status_payload(srv.controller, srv.deploy_service)} + frame = _ws_encode_frame(0x1, json.dumps(msg).encode("utf-8")) + with write_lock: + sock.sendall(frame) + + readable, _, _ = select.select([sock], [], [], 0.05) + if not readable: + continue + parsed = _ws_read_frame(sock) + if parsed is None: + break + opcode, payload = parsed + if opcode == 0x8: # close + with write_lock: + sock.sendall(_ws_encode_frame(0x8, b"")) + break + if opcode == 0x9: # ping -> pong + with write_lock: + sock.sendall(_ws_encode_frame(0xA, payload)) + continue + if opcode == 0x1: # text + try: + data = json.loads(payload.decode("utf-8")) + except Exception: + continue + ack = handle_ws_message(srv.controller, data) + if ack is not None: + with write_lock: + sock.sendall(_ws_encode_frame(0x1, json.dumps(ack).encode("utf-8"))) + except (BrokenPipeError, ConnectionResetError, TimeoutError, OSError): + pass + finally: + self.close_connection = True diff --git a/app/public/hardware/unoq/python/web/server.py b/app/public/hardware/unoq/python/web/server.py new file mode 100644 index 0000000..6152390 --- /dev/null +++ b/app/public/hardware/unoq/python/web/server.py @@ -0,0 +1,92 @@ +from __future__ import annotations + +import asyncio +import json +from typing import Any +from urllib.parse import parse_qs + +from core.controller import MicroduckController +from core.deploy_service import DeployService + +from .api import ( + build_status_payload, + dispatch_api_get, + dispatch_api_post, + handle_ws_message, + load_index_html, +) + + +def create_fastapi_app(controller: MicroduckController, deploy_service: DeployService) -> Any: + """Create FastAPI app backed by the shared web.api dispatcher.""" + from fastapi import FastAPI, Request, WebSocket, WebSocketDisconnect + from fastapi.middleware.cors import CORSMiddleware + from fastapi.responses import HTMLResponse, JSONResponse + + app = FastAPI(title="Microduck UNO Q Controller", version="0.1.0") + app.add_middleware( + CORSMiddleware, + allow_origins=["*"], + allow_methods=["*"], + allow_headers=["*"], + ) + + @app.middleware("http") + async def add_pna_header(request: Request, call_next: Any) -> Any: + resp = await call_next(request) + resp.headers["Access-Control-Allow-Private-Network"] = "true" + return resp + + @app.get("/", response_class=HTMLResponse) + async def index() -> HTMLResponse: + return HTMLResponse(content=load_index_html().decode("utf-8")) + + @app.get("/api/{subpath:path}") + async def api_get(subpath: str) -> JSONResponse: + code, payload = dispatch_api_get(f"/api/{subpath}", controller, deploy_service) + return JSONResponse(status_code=code, content=payload) + + @app.post("/api/{subpath:path}") + async def api_post(subpath: str, request: Request) -> JSONResponse: + body = await request.body() + content_type = request.headers.get("content-type", "") + query = parse_qs(request.url.query) + code, payload = dispatch_api_post( + f"/api/{subpath}", + body, + content_type, + query, + controller, + deploy_service, + ) + return JSONResponse(status_code=code, content=payload) + + @app.websocket("/ws/control") + async def ws_control(websocket: WebSocket) -> None: + await websocket.accept() + stop_event = asyncio.Event() + + async def telemetry_loop() -> None: + while not stop_event.is_set(): + msg = {"type": "telemetry", **build_status_payload(controller, deploy_service)} + await websocket.send_text(json.dumps(msg)) + await asyncio.sleep(0.1) + + sender = asyncio.create_task(telemetry_loop()) + try: + while True: + raw = await websocket.receive_text() + try: + data = json.loads(raw) + except Exception: + continue + ack = handle_ws_message(controller, data) + if ack is not None: + await websocket.send_text(json.dumps(ack)) + except WebSocketDisconnect: + pass + finally: + stop_event.set() + sender.cancel() + + return app diff --git a/app/public/hardware/unoq/python/web/static/index.html b/app/public/hardware/unoq/python/web/static/index.html new file mode 100644 index 0000000..6d2fdad --- /dev/null +++ b/app/public/hardware/unoq/python/web/static/index.html @@ -0,0 +1,619 @@ + + + + + + Microduck UNO Q Controller + + + +
+
MICRODUCK // UNO Q
+
+ IDLE + Idle + WS ... + IMU -- + --.-V + --C + --Hz +
+
+ + +
+
+ +
+ + + + +
+ +
+
+
+

Virtual Joystick (Hold & Drag)

+
+
Touch or drag anywhere here to walk / turn
+ + +
+
+ vx norm: 0.00 + wz norm: 0.00 +
+ + +
+ +
+

Skills & Modes

+
+ + + + + + + + +
+
Ready. Tap "Prepare Stand" when the robot is upright on the floor.
+
+
+ + +
+ + + + diff --git a/app/public/hardware/unoq/scripts/install_deps.sh b/app/public/hardware/unoq/scripts/install_deps.sh new file mode 100644 index 0000000..15e15c2 --- /dev/null +++ b/app/public/hardware/unoq/scripts/install_deps.sh @@ -0,0 +1,10 @@ +#!/usr/bin/env bash +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +REQ_FILE="${SCRIPT_DIR}/../python/requirements.txt" + +echo "[microduck-unoq] Installing Python dependencies from ${REQ_FILE}..." +python3 -m pip install --upgrade pip +python3 -m pip install -r "${REQ_FILE}" +echo "[microduck-unoq] Done." diff --git a/app/public/hardware/unoq/scripts/setup_ap.sh b/app/public/hardware/unoq/scripts/setup_ap.sh new file mode 100644 index 0000000..05b3986 --- /dev/null +++ b/app/public/hardware/unoq/scripts/setup_ap.sh @@ -0,0 +1,44 @@ +#!/usr/bin/env bash +set -euo pipefail + +# Configure an offline Wi-Fi Access Point on Arduino UNO Q (Debian + NetworkManager) +# Phone connects to this hotspot and opens http://192.168.4.1:8000 + +CON_NAME="${AP_CON_NAME:-microduck-ap}" +SSID="${AP_SSID:-Microduck-AP}" +PASS="${AP_PASS:-microduck123}" +IFACE="${AP_IFACE:-wlan0}" +IP_CIDR="${AP_IP_CIDR:-192.168.4.1/24}" + +if ! command -v nmcli >/dev/null 2>&1; then + echo "Error: nmcli (NetworkManager) not found. Configure hostapd + dnsmasq manually for ${IFACE} -> ${IP_CIDR}." >&2 + exit 1 +fi + +echo "[microduck-ap] Configuring Wi-Fi AP '${SSID}' on ${IFACE} (${IP_CIDR})..." + +if nmcli connection show "${CON_NAME}" >/dev/null 2>&1; then + nmcli connection delete "${CON_NAME}" +fi + +nmcli connection add \ + type wifi \ + ifname "${IFACE}" \ + con-name "${CON_NAME}" \ + autoconnect yes \ + ssid "${SSID}" + +nmcli connection modify "${CON_NAME}" \ + 802-11-wireless.mode ap \ + 802-11-wireless.band bg \ + ipv4.method shared \ + ipv4.addresses "${IP_CIDR}" \ + wifi-sec.key-mgmt wpa-psk \ + wifi-sec.psk "${PASS}" + +nmcli connection up "${CON_NAME}" + +echo "[microduck-ap] Hotspot active!" +echo " SSID: ${SSID}" +echo " Pass: ${PASS}" +echo " Control URL: http://${IP_CIDR%/*}:8000" diff --git a/app/public/hardware/unoq/sketch/config.h b/app/public/hardware/unoq/sketch/config.h new file mode 100644 index 0000000..c24435a --- /dev/null +++ b/app/public/hardware/unoq/sketch/config.h @@ -0,0 +1,41 @@ +#pragma once + +#include +#include + +// ============================================================================= +// Microduck UNO Q Hardware Pin & Bus Configuration +// Centralizes physical wiring constants so schematic changes require one edit. +// ============================================================================= + +// Feetech HD-1910-C001 serial bus +// On Arduino UNO Q, Serial1 maps to header D0 (RX / PB7) and D1 (TX / PB6). +#define SERVO_SERIAL Serial1 +#define SERVO_BAUD 1000000UL + +// Set to a GPIO pin if using a 74HC126 / RS485 direction-controlled half-duplex +// transceiver (HIGH = TX, LOW = RX). Leave -1 for resistor/diode auto-direction +// single-wire boards (TX tied cleanly through hardware buffer) or Waveshare board. +#define SERVO_DIR_PIN -1 +#define SERVO_DIR_TX_LEVEL HIGH +#define SERVO_DIR_RX_LEVEL LOW + +// QMI8658 6-axis IMU on I2C +// 0x6A when SA0/AD0 is LOW (default on many breakouts), 0x6B when HIGH. +#define IMU_WIRE Wire +#define IMU_I2C_ADDR 0x6A +#define IMU_I2C_CLOCK_HZ 400000UL + +// IMU Mahony filter loop rate (runs non-blocking inside MCU loop()) +#define IMU_LOOP_INTERVAL_US 4000UL // 250 Hz + +// Safety watchdog: disable servo torque if Linux stops sending hw_sync_step +#define WATCHDOG_DEFAULT_MS 150UL + +// Servo feedback strategy: +// 1 = Feetech SyncRead (0x82) followed by single-read fallback if bus drops packet +// 0 = Sequential single Read (0x02) per joint (slower, simpler fallback for noisy lines) +#define USE_FEETECH_SYNC_READ 1 + +// Total joints on Microduck +#define NUM_JOINTS 14 diff --git a/app/public/hardware/unoq/sketch/feetech_hls.cpp b/app/public/hardware/unoq/sketch/feetech_hls.cpp new file mode 100644 index 0000000..dbded83 --- /dev/null +++ b/app/public/hardware/unoq/sketch/feetech_hls.cpp @@ -0,0 +1,349 @@ +#include "feetech_hls.h" + +#include + +namespace { +constexpr uint8_t kHeader = 0xFF; +constexpr uint8_t kBroadcastId = 0xFE; + +constexpr uint8_t kInstPing = 0x01; +constexpr uint8_t kInstRead = 0x02; +constexpr uint8_t kInstWrite = 0x03; +constexpr uint8_t kInstSyncRead = 0x82; +constexpr uint8_t kInstSyncWrite = 0x83; + +constexpr uint8_t kRegTorqueEnable = 0x28; +constexpr uint8_t kRegAcc = 0x29; +constexpr uint8_t kRegPresentPos = 0x38; +constexpr uint8_t kFeedbackReadLen = 8; // pos(2), speed(2), load(2), volt(1), temp(1) +} // namespace + +void FeetechHlsBus::begin(HardwareSerial& serial, uint32_t baud, int dir_pin) { + serial_ = &serial; + dir_pin_ = dir_pin; + if (dir_pin_ >= 0) { + pinMode(dir_pin_, OUTPUT); + setTxMode(false); + } + serial_->begin(baud); + delay(5); + flushInput(); +} + +void FeetechHlsBus::setTxMode(bool tx) { + if (dir_pin_ >= 0) { + digitalWrite(dir_pin_, tx ? SERVO_DIR_TX_LEVEL : SERVO_DIR_RX_LEVEL); + } +} + +void FeetechHlsBus::flushInput() { + if (!serial_) return; + while (serial_->available() > 0) { + (void)serial_->read(); + } +} + +uint8_t FeetechHlsBus::computeChecksum(const uint8_t* body_from_id, size_t body_len) { + uint32_t sum = 0; + for (size_t i = 0; i < body_len; ++i) { + sum += body_from_id[i]; + } + return static_cast((~sum) & 0xFFU); +} + +void FeetechHlsBus::sendRawPacket(const uint8_t* pkt, size_t len) { + if (!serial_ || len == 0 || len > sizeof(last_tx_)) return; + flushInput(); + memcpy(last_tx_, pkt, len); + last_tx_len_ = len; + setTxMode(true); + serial_->write(pkt, len); + serial_->flush(); + delayMicroseconds(8); + setTxMode(false); + + // Strip local half-duplex TX echo if TX/RX share a single physical trace without gating + if (dir_pin_ < 0) { + uint32_t t0 = micros(); + size_t matched = 0; + while (matched < len && (micros() - t0) < 400) { + if (serial_->available() > 0) { + int b = serial_->peek(); + if (b == last_tx_[matched]) { + (void)serial_->read(); + matched++; + } else { + break; + } + } + } + } +} + +void FeetechHlsBus::sendPacket(uint8_t id, uint8_t inst, const uint8_t* params, uint8_t param_len) { + uint8_t pkt[128]; + uint8_t length_field = static_cast(param_len + 2); + pkt[0] = kHeader; + pkt[1] = kHeader; + pkt[2] = id; + pkt[3] = length_field; + pkt[4] = inst; + for (uint8_t i = 0; i < param_len; ++i) { + pkt[5 + i] = params[i]; + } + pkt[5 + param_len] = computeChecksum(&pkt[2], static_cast(3 + param_len)); + sendRawPacket(pkt, static_cast(6 + param_len)); +} + +bool FeetechHlsBus::readPacketForId( + uint8_t expected_id, + uint8_t* out_err, + uint8_t* params, + uint8_t expected_len, + uint32_t timeout_us) { + if (!serial_) return false; + uint32_t t0 = micros(); + uint8_t state = 0; + uint8_t id = 0, len_field = 0, err = 0; + uint8_t buf[64] = {}; + uint8_t idx = 0; + uint8_t payload_len = 0; + + while ((micros() - t0) < timeout_us) { + if (serial_->available() <= 0) { + continue; + } + uint8_t b = static_cast(serial_->read()); + switch (state) { + case 0: + if (b == kHeader) state = 1; + break; + case 1: + if (b == kHeader) { + state = 2; + } else { + state = 0; + } + break; + case 2: + if (b == kHeader) break; // extra 0xFF header alignment + id = b; + state = 3; + break; + case 3: + len_field = b; + if (len_field < 2 || len_field > 60) { + state = 0; + } else { + payload_len = static_cast(len_field - 2); + state = 4; + } + break; + case 4: + err = b; + idx = 0; + state = (payload_len == 0) ? 6 : 5; + break; + case 5: + buf[idx++] = b; + if (idx >= payload_len) state = 6; + break; + case 6: { + uint8_t check_buf[64]; + check_buf[0] = id; + check_buf[1] = len_field; + check_buf[2] = err; + for (uint8_t i = 0; i < payload_len; ++i) check_buf[3 + i] = buf[i]; + uint8_t chk = computeChecksum(check_buf, static_cast(3 + payload_len)); + if (chk == b && id == expected_id && payload_len >= expected_len) { + if (out_err) *out_err = err; + if (params && expected_len > 0) memcpy(params, buf, expected_len); + return true; + } + state = 0; + break; + } + default: + state = 0; + break; + } + } + return false; +} + +bool FeetechHlsBus::ping(uint8_t id, uint32_t timeout_us) { + sendPacket(id, kInstPing, nullptr, 0); + uint8_t err = 0; + return readPacketForId(id, &err, nullptr, 0, timeout_us); +} + +int FeetechHlsBus::pingRange(uint8_t max_id, int32_t* out_ids, int max_out) { + int found = 0; + for (uint8_t id = 1; id <= max_id && found < max_out; ++id) { + if (ping(id, 2000)) { + out_ids[found++] = static_cast(id); + } + } + return found; +} + +bool FeetechHlsBus::setTorqueBroadcast(bool enable) { + uint8_t params[2] = {kRegTorqueEnable, static_cast(enable ? 1 : 0)}; + sendPacket(kBroadcastId, kInstWrite, params, 2); + delayMicroseconds(120); + return true; +} + +bool FeetechHlsBus::setTorqueAll(const uint8_t* ids, int count, bool enable) { + setTorqueBroadcast(enable); + // Also sync-write individual entries cleanly in case broadcast is masked in servo EEPROM + uint8_t pkt[64]; + if (count <= 0 || count > NUM_JOINTS) return false; + uint8_t param_len = static_cast(2 + count * 2); + pkt[0] = kHeader; + pkt[1] = kHeader; + pkt[2] = kBroadcastId; + pkt[3] = static_cast(param_len + 2); + pkt[4] = kInstSyncWrite; + pkt[5] = kRegTorqueEnable; + pkt[6] = 1; + uint8_t val = enable ? 1 : 0; + for (int i = 0; i < count; ++i) { + pkt[7 + i * 2] = ids[i]; + pkt[8 + i * 2] = val; + } + pkt[5 + param_len] = computeChecksum(&pkt[2], static_cast(3 + param_len)); + sendRawPacket(pkt, static_cast(6 + param_len)); + return true; +} + +bool FeetechHlsBus::syncWriteTargets( + const uint8_t* ids, + int count, + const int32_t* target_ticks, + uint8_t acc, + uint16_t speed_limit) { + if (!serial_ || count <= 0 || count > NUM_JOINTS) return false; + // 7 bytes starting at 0x29: [acc, pos_L, pos_H, time_L=0, time_H=0, spd_L, spd_H] + constexpr uint8_t kPerServo = 7; + uint8_t pkt[128]; + uint8_t param_len = static_cast(2 + count * (1 + kPerServo)); + pkt[0] = kHeader; + pkt[1] = kHeader; + pkt[2] = kBroadcastId; + pkt[3] = static_cast(param_len + 2); + pkt[4] = kInstSyncWrite; + pkt[5] = kRegAcc; + pkt[6] = kPerServo; + + for (int i = 0; i < count; ++i) { + int32_t ticks = target_ticks[i]; + if (ticks < 0) ticks = 0; + if (ticks > 4095) ticks = 4095; + uint16_t pos = static_cast(ticks); + size_t off = 7 + static_cast(i) * (1 + kPerServo); + pkt[off + 0] = ids[i]; + pkt[off + 1] = acc; + pkt[off + 2] = static_cast(pos & 0xFFU); + pkt[off + 3] = static_cast((pos >> 8) & 0xFFU); + pkt[off + 4] = 0x00; // moving time low + pkt[off + 5] = 0x00; // moving time high + pkt[off + 6] = static_cast(speed_limit & 0xFFU); + pkt[off + 7] = static_cast((speed_limit >> 8) & 0xFFU); + } + pkt[5 + param_len] = computeChecksum(&pkt[2], static_cast(3 + param_len)); + sendRawPacket(pkt, static_cast(6 + param_len)); + return true; +} + +bool FeetechHlsBus::readSingleJoint( + uint8_t id, + int32_t* out_tick, + int32_t* out_speed, + float* out_voltage_v, + float* out_temp_c) { + uint8_t params[2] = {kRegPresentPos, kFeedbackReadLen}; + sendPacket(id, kInstRead, params, 2); + uint8_t err = 0; + uint8_t buf[kFeedbackReadLen] = {}; + if (!readPacketForId(id, &err, buf, kFeedbackReadLen, 1600)) { + return false; + } + uint16_t pos_u16 = static_cast(buf[0]) | (static_cast(buf[1]) << 8); + uint16_t spd_u16 = static_cast(buf[2]) | (static_cast(buf[3]) << 8); + *out_tick = static_cast(pos_u16 & 0x7FFFU); + *out_speed = static_cast(spd_u16); + *out_voltage_v = static_cast(buf[6]) * 0.1f; + *out_temp_c = static_cast(buf[7]); + return err == 0; +} + +uint32_t FeetechHlsBus::readFeedback( + const uint8_t* ids, + int count, + int32_t* out_ticks, + int32_t* out_speeds, + float* out_min_voltage_v, + float* out_max_temp_c) { + uint32_t err_mask = 0; + float min_v = 99.0f; + float max_t = -40.0f; + bool any_ok = false; + +#if USE_FEETECH_SYNC_READ + uint8_t sr_params[2 + NUM_JOINTS]; + sr_params[0] = kRegPresentPos; + sr_params[1] = kFeedbackReadLen; + for (int i = 0; i < count; ++i) { + sr_params[2 + i] = ids[i]; + } + sendPacket(kBroadcastId, kInstSyncRead, sr_params, static_cast(2 + count)); + + bool got[NUM_JOINTS] = {}; + for (int i = 0; i < count; ++i) { + uint8_t err = 0; + uint8_t buf[kFeedbackReadLen] = {}; + if (readPacketForId(ids[i], &err, buf, kFeedbackReadLen, 1200)) { + got[i] = true; + uint16_t pos_u16 = static_cast(buf[0]) | (static_cast(buf[1]) << 8); + uint16_t spd_u16 = static_cast(buf[2]) | (static_cast(buf[3]) << 8); + out_ticks[i] = static_cast(pos_u16 & 0x7FFFU); + out_speeds[i] = static_cast(spd_u16); + float v = static_cast(buf[6]) * 0.1f; + float t = static_cast(buf[7]); + if (v > 0.1f && v < min_v) min_v = v; + if (t > max_t) max_t = t; + any_ok = true; + if (err != 0) err_mask |= (1UL << i); + } + } + // Fallback single-read for any joints dropped during SyncRead + for (int i = 0; i < count; ++i) { + if (!got[i]) { + float v = 0.0f, t = 0.0f; + if (readSingleJoint(ids[i], &out_ticks[i], &out_speeds[i], &v, &t)) { + if (v > 0.1f && v < min_v) min_v = v; + if (t > max_t) max_t = t; + any_ok = true; + } else { + err_mask |= (1UL << i); + } + } + } +#else + for (int i = 0; i < count; ++i) { + float v = 0.0f, t = 0.0f; + if (readSingleJoint(ids[i], &out_ticks[i], &out_speeds[i], &v, &t)) { + if (v > 0.1f && v < min_v) min_v = v; + if (t > max_t) max_t = t; + any_ok = true; + } else { + err_mask |= (1UL << i); + } + } +#endif + + *out_min_voltage_v = any_ok ? min_v : 0.0f; + *out_max_temp_c = any_ok ? max_t : 0.0f; + return err_mask; +} diff --git a/app/public/hardware/unoq/sketch/feetech_hls.h b/app/public/hardware/unoq/sketch/feetech_hls.h new file mode 100644 index 0000000..cea4365 --- /dev/null +++ b/app/public/hardware/unoq/sketch/feetech_hls.h @@ -0,0 +1,50 @@ +#pragma once + +#include +#include "config.h" + +class FeetechHlsBus { + public: + void begin(HardwareSerial& serial, uint32_t baud, int dir_pin = SERVO_DIR_PIN); + + bool ping(uint8_t id, uint32_t timeout_us = 1800); + int pingRange(uint8_t max_id, int32_t* out_ids, int max_out); + + bool setTorqueBroadcast(bool enable); + bool setTorqueAll(const uint8_t* ids, int count, bool enable); + + bool syncWriteTargets( + const uint8_t* ids, + int count, + const int32_t* target_ticks, + uint8_t acc, + uint16_t speed_limit); + + // Returns bitmask of joints with comm/servo error (bit i set -> joint i fault/timeout) + uint32_t readFeedback( + const uint8_t* ids, + int count, + int32_t* out_ticks, + int32_t* out_speeds, + float* out_min_voltage_v, + float* out_max_temp_c); + + private: + HardwareSerial* serial_ = nullptr; + int dir_pin_ = -1; + uint8_t last_tx_[128] = {}; + size_t last_tx_len_ = 0; + + void setTxMode(bool tx); + void flushInput(); + void sendRawPacket(const uint8_t* pkt, size_t len); + void sendPacket(uint8_t id, uint8_t inst, const uint8_t* params, uint8_t param_len); + bool readPacketForId(uint8_t expected_id, uint8_t* out_err, uint8_t* params, uint8_t expected_len, uint32_t timeout_us); + bool readSingleJoint( + uint8_t id, + int32_t* out_tick, + int32_t* out_speed, + float* out_voltage_v, + float* out_temp_c); + static uint8_t computeChecksum(const uint8_t* body_from_id, size_t body_len); +}; diff --git a/app/public/hardware/unoq/sketch/qmi8658.cpp b/app/public/hardware/unoq/sketch/qmi8658.cpp new file mode 100644 index 0000000..62dc08d --- /dev/null +++ b/app/public/hardware/unoq/sketch/qmi8658.cpp @@ -0,0 +1,216 @@ +#include "qmi8658.h" + +#include + +namespace { +constexpr uint8_t kRegWhoAmI = 0x00; +constexpr uint8_t kRegCtrl1 = 0x02; +constexpr uint8_t kRegCtrl2 = 0x03; +constexpr uint8_t kRegCtrl3 = 0x04; +constexpr uint8_t kRegCtrl5 = 0x06; +constexpr uint8_t kRegCtrl7 = 0x08; +constexpr uint8_t kRegAxL = 0x35; +constexpr uint8_t kExpectedWhoAmI = 0x05; + +// CTRL2: aFS=±4g (1 << 4), aODR=250Hz (5) -> 8192 LSB/g +constexpr uint8_t kCtrl2Val = 0x15; +constexpr float kAccelScaleG = 4.0f / 32768.0f; + +// CTRL3: gFS=±1024 dps (6 << 4), gODR=448.4Hz (4) -> 32 LSB/dps +constexpr uint8_t kCtrl3Val = 0x64; +constexpr float kGyroScaleRadS = (1024.0f / 32768.0f) * (PI / 180.0f); + +// CTRL5: enable LPF mode 2 on both accel and gyro +constexpr uint8_t kCtrl5Val = 0x55; + +constexpr float kTwoKp = 2.0f * 1.2f; // Proportional gain +constexpr float kTwoKi = 2.0f * 0.02f; // Integral gain +} // namespace + +bool Qmi8658Imu::writeReg(uint8_t reg, uint8_t val) { + wire_->beginTransmission(addr_); + wire_->write(reg); + wire_->write(val); + return wire_->endTransmission(true) == 0; +} + +bool Qmi8658Imu::readRegs(uint8_t reg, uint8_t* buf, size_t len) { + wire_->beginTransmission(addr_); + wire_->write(reg); + if (wire_->endTransmission(false) != 0) { + return false; + } + size_t got = wire_->requestFrom(static_cast(addr_), static_cast(len)); + if (got != len) { + while (wire_->available()) { + (void)wire_->read(); + } + return false; + } + for (size_t i = 0; i < len; ++i) { + buf[i] = static_cast(wire_->read()); + } + return true; +} + +bool Qmi8658Imu::begin(TwoWire& wire, uint8_t addr) { + wire_ = &wire; + addr_ = addr; + healthy_ = false; + err_streak_ = 0; + + uint8_t who = 0; + if (!readRegs(kRegWhoAmI, &who, 1) || who != kExpectedWhoAmI) { + return false; + } + + // CTRL1 = 0x40 sets ADDR_AI (auto-increment for burst reads) and little-endian data + if (!writeReg(kRegCtrl7, 0x00)) return false; + delay(2); + if (!writeReg(kRegCtrl1, 0x40)) return false; + if (!writeReg(kRegCtrl2, kCtrl2Val)) return false; + if (!writeReg(kRegCtrl3, kCtrl3Val)) return false; + if (!writeReg(kRegCtrl5, kCtrl5Val)) return false; + if (!writeReg(kRegCtrl7, 0x03)) return false; // Enable accel + gyro + delay(10); + + // Align initial Mahony orientation from static gravity vector if available + q0_ = 1.0f; + q1_ = 0.0f; + q2_ = 0.0f; + q3_ = 0.0f; + ix_ = 0.0f; + iy_ = 0.0f; + iz_ = 0.0f; + + if (readSampleRaw()) { + for (int i = 0; i < 40; ++i) { + mahonyUpdate(0.0f, 0.0f, 0.0f, raw_accel_[0], raw_accel_[1], raw_accel_[2], 0.01f); + } + } + healthy_ = true; + return true; +} + +void Qmi8658Imu::setGyroBias(float bx, float by, float bz) { + gyro_bias_[0] = bx; + gyro_bias_[1] = by; + gyro_bias_[2] = bz; +} + +bool Qmi8658Imu::readSampleRaw() { + if (!wire_) return false; + uint8_t buf[12]; + if (!readRegs(kRegAxL, buf, sizeof(buf))) { + return false; + } + auto to_i16 = [](uint8_t lo, uint8_t hi) -> int16_t { + return static_cast((static_cast(hi) << 8) | lo); + }; + raw_accel_[0] = static_cast(to_i16(buf[0], buf[1])) * kAccelScaleG; + raw_accel_[1] = static_cast(to_i16(buf[2], buf[3])) * kAccelScaleG; + raw_accel_[2] = static_cast(to_i16(buf[4], buf[5])) * kAccelScaleG; + raw_gyro_[0] = static_cast(to_i16(buf[6], buf[7])) * kGyroScaleRadS; + raw_gyro_[1] = static_cast(to_i16(buf[8], buf[9])) * kGyroScaleRadS; + raw_gyro_[2] = static_cast(to_i16(buf[10], buf[11])) * kGyroScaleRadS; + return true; +} + +bool Qmi8658Imu::update(float dt_s) { + if (!readSampleRaw()) { + if (err_streak_ < 255) err_streak_++; + if (err_streak_ >= 10) healthy_ = false; + return false; + } + err_streak_ = 0; + healthy_ = true; + + float gx = raw_gyro_[0] - gyro_bias_[0]; + float gy = raw_gyro_[1] - gyro_bias_[1]; + float gz = raw_gyro_[2] - gyro_bias_[2]; + mahonyUpdate(gx, gy, gz, raw_accel_[0], raw_accel_[1], raw_accel_[2], dt_s); + return true; +} + +void Qmi8658Imu::mahonyUpdate(float gx, float gy, float gz, float ax, float ay, float az, float dt_s) { + float norm = sqrtf(ax * ax + ay * ay + az * az); + if (norm > 0.25f) { + float inv = 1.0f / norm; + ax *= inv; + ay *= inv; + az *= inv; + + // Estimated world-UP in body frame + float vx = 2.0f * (q1_ * q3_ - q0_ * q2_); + float vy = 2.0f * (q0_ * q1_ + q2_ * q3_); + float vz = 1.0f - 2.0f * (q1_ * q1_ + q2_ * q2_); + + float ex = ay * vz - az * vy; + float ey = az * vx - ax * vz; + float ez = ax * vy - ay * vx; + + if (kTwoKi > 0.0f) { + ix_ += kTwoKi * ex * dt_s; + iy_ += kTwoKi * ey * dt_s; + iz_ += kTwoKi * ez * dt_s; + gx += ix_; + gy += iy_; + gz += iz_; + } + gx += kTwoKp * ex; + gy += kTwoKp * ey; + gz += kTwoKp * ez; + } + + float half_dt = 0.5f * dt_s; + float dq0 = (-q1_ * gx - q2_ * gy - q3_ * gz) * half_dt; + float dq1 = (q0_ * gx + q2_ * gz - q3_ * gy) * half_dt; + float dq2 = (q0_ * gy - q1_ * gz + q3_ * gx) * half_dt; + float dq3 = (q0_ * gz + q1_ * gy - q2_ * gx) * half_dt; + q0_ += dq0; + q1_ += dq1; + q2_ += dq2; + q3_ += dq3; + + float qnorm = sqrtf(q0_ * q0_ + q1_ * q1_ + q2_ * q2_ + q3_ * q3_); + if (qnorm > 1e-6f) { + float inv_q = 1.0f / qnorm; + q0_ *= inv_q; + q1_ *= inv_q; + q2_ *= inv_q; + q3_ *= inv_q; + } +} + +bool Qmi8658Imu::sampleStationaryGyroBias(int samples, float out_bias[3]) { + if (samples <= 0) samples = 1; + double sx = 0.0, sy = 0.0, sz = 0.0; + int ok_cnt = 0; + for (int i = 0; i < samples; ++i) { + if (readSampleRaw()) { + sx += raw_gyro_[0]; + sy += raw_gyro_[1]; + sz += raw_gyro_[2]; + ok_cnt++; + } + delay(4); + } + if (ok_cnt == 0) return false; + out_bias[0] = static_cast(sx / ok_cnt); + out_bias[1] = static_cast(sy / ok_cnt); + out_bias[2] = static_cast(sz / ok_cnt); + return true; +} + +void Qmi8658Imu::getRawGyro(float out[3]) const { + out[0] = raw_gyro_[0]; + out[1] = raw_gyro_[1]; + out[2] = raw_gyro_[2]; +} + +void Qmi8658Imu::getProjectedGravity(float out[3]) const { + // Gravity DOWN in world [0, 0, -1] rotated into body frame = -v_up + out[0] = -2.0f * (q1_ * q3_ - q0_ * q2_); + out[1] = -2.0f * (q0_ * q1_ + q2_ * q3_); + out[2] = -(1.0f - 2.0f * (q1_ * q1_ + q2_ * q2_)); +} diff --git a/app/public/hardware/unoq/sketch/qmi8658.h b/app/public/hardware/unoq/sketch/qmi8658.h new file mode 100644 index 0000000..38f5327 --- /dev/null +++ b/app/public/hardware/unoq/sketch/qmi8658.h @@ -0,0 +1,36 @@ +#pragma once + +#include +#include + +class Qmi8658Imu { + public: + bool begin(TwoWire& wire, uint8_t addr); + void setGyroBias(float bx, float by, float bz); + bool update(float dt_s); + bool sampleStationaryGyroBias(int samples, float out_bias[3]); + + bool isHealthy() const { return healthy_; } + void getRawGyro(float out[3]) const; + void getProjectedGravity(float out[3]) const; + + private: + TwoWire* wire_ = nullptr; + uint8_t addr_ = 0x6A; + bool healthy_ = false; + uint8_t err_streak_ = 0; + + // Bias subtracted strictly inside Mahony filter (raw gyro stays unbiased for Python HAL) + float gyro_bias_[3] = {0.0f, 0.0f, 0.0f}; + float raw_gyro_[3] = {0.0f, 0.0f, 0.0f}; + float raw_accel_[3] = {0.0f, 0.0f, 1.0f}; + + // Mahony quaternion (body -> world) and integral feedback + float q0_ = 1.0f, q1_ = 0.0f, q2_ = 0.0f, q3_ = 0.0f; + float ix_ = 0.0f, iy_ = 0.0f, iz_ = 0.0f; + + bool writeReg(uint8_t reg, uint8_t val); + bool readRegs(uint8_t reg, uint8_t* buf, size_t len); + bool readSampleRaw(); + void mahonyUpdate(float gx, float gy, float gz, float ax, float ay, float az, float dt_s); +}; diff --git a/app/public/hardware/unoq/sketch/sketch.ino b/app/public/hardware/unoq/sketch/sketch.ino new file mode 100644 index 0000000..2040dbc --- /dev/null +++ b/app/public/hardware/unoq/sketch/sketch.ino @@ -0,0 +1,195 @@ +#include +#include +#include + +#include "config.h" +#include "feetech_hls.h" +#include "qmi8658.h" + +namespace { +Qmi8658Imu g_imu; +FeetechHlsBus g_servo; + +uint8_t g_servo_ids[NUM_JOINTS] = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14}; +uint32_t g_watchdog_ms = WATCHDOG_DEFAULT_MS; +uint32_t g_last_sync_ms = 0; +uint32_t g_last_imu_us = 0; + +bool g_torque_enabled = false; +bool g_imu_ready = false; + +int32_t g_last_ticks[NUM_JOINTS] = {2048, 2048, 2048, 2048, 2048, 2048, 2048, 2048, 2048, 2048, 2048, 2048, 2048, 2048}; +int32_t g_last_speeds[NUM_JOINTS] = {}; +} // namespace + +int32_t hw_configure(std::vector payload) { + g_torque_enabled = false; + g_servo.setTorqueAll(g_servo_ids, NUM_JOINTS, false); + + uint32_t baud = SERVO_BAUD; + uint8_t i2c_addr = IMU_I2C_ADDR; + if (payload.size() >= 3) { + baud = static_cast(payload[0] > 9600.0f ? payload[0] : SERVO_BAUD); + i2c_addr = static_cast(static_cast(payload[1] + 0.5f)); + g_watchdog_ms = static_cast(payload[2] >= 40.0f ? payload[2] : WATCHDOG_DEFAULT_MS); + } + if (payload.size() >= 6) { + g_imu.setGyroBias(payload[3], payload[4], payload[5]); + } + if (payload.size() >= 6 + NUM_JOINTS) { + for (int i = 0; i < NUM_JOINTS; ++i) { + int sid = static_cast(payload[6 + i] + 0.5f); + if (sid >= 0 && sid <= 253) { + g_servo_ids[i] = static_cast(sid); + } + } + } + + g_servo.begin(SERVO_SERIAL, baud, SERVO_DIR_PIN); + g_servo.setTorqueAll(g_servo_ids, NUM_JOINTS, false); + + IMU_WIRE.begin(); + IMU_WIRE.setClock(IMU_I2C_CLOCK_HZ); + g_imu_ready = g_imu.begin(IMU_WIRE, i2c_addr); + if (payload.size() >= 6) { + g_imu.setGyroBias(payload[3], payload[4], payload[5]); + } + + g_last_sync_ms = millis(); + g_last_imu_us = micros(); + return g_imu_ready ? 1 : 0; +} + +std::vector hw_sync_step(std::vector cmd_frame) { + std::vector res(38, 0.0f); + if (cmd_frame.size() < 3 + NUM_JOINTS) { + res[36] = g_imu.isHealthy() ? 1.0f : 0.0f; + res[37] = static_cast(0xFFFF); + return res; + } + + bool want_torque = cmd_frame[0] != 0; + uint16_t speed_limit = static_cast(cmd_frame[1] > 0 ? cmd_frame[1] : 0); + uint8_t acc = static_cast(cmd_frame[2] >= 0 ? (cmd_frame[2] <= 254 ? cmd_frame[2] : 254) : 0); + + int32_t target_ticks[NUM_JOINTS]; + for (int i = 0; i < NUM_JOINTS; ++i) { + target_ticks[i] = cmd_frame[3 + i]; + } + + // Write targets BEFORE enabling torque on 0->1 transition to prevent initial snap + if (want_torque) { + g_servo.syncWriteTargets(g_servo_ids, NUM_JOINTS, target_ticks, acc, speed_limit); + if (!g_torque_enabled) { + g_servo.setTorqueAll(g_servo_ids, NUM_JOINTS, true); + g_torque_enabled = true; + } + } else { + if (g_torque_enabled) { + g_servo.setTorqueAll(g_servo_ids, NUM_JOINTS, false); + g_torque_enabled = false; + } + } + + g_last_sync_ms = millis(); + + // Sample fresh IMU reading if at least 2ms elapsed since last 250Hz integration tick + uint32_t now_us = micros(); + uint32_t dt_us = now_us - g_last_imu_us; + if (dt_us >= 2000UL && dt_us < 100000UL) { + g_imu.update(static_cast(dt_us) * 1e-6f); + g_last_imu_us = now_us; + } + + float min_v = 0.0f; + float max_t = 0.0f; + uint32_t err_mask = g_servo.readFeedback( + g_servo_ids, NUM_JOINTS, g_last_ticks, g_last_speeds, &min_v, &max_t); + + float gyro[3] = {0.0f, 0.0f, 0.0f}; + float grav[3] = {0.0f, 0.0f, -1.0f}; + g_imu.getRawGyro(gyro); + g_imu.getProjectedGravity(grav); + + res[0] = gyro[0]; + res[1] = gyro[1]; + res[2] = gyro[2]; + res[3] = grav[0]; + res[4] = grav[1]; + res[5] = grav[2]; + for (int i = 0; i < NUM_JOINTS; ++i) { + res[6 + i] = static_cast(g_last_ticks[i]); + res[20 + i] = static_cast(g_last_speeds[i]); + } + res[34] = min_v; + res[35] = max_t; + res[36] = g_imu.isHealthy() ? 1.0f : 0.0f; + res[37] = static_cast(err_mask); + return res; +} + +int32_t hw_set_torque(int32_t enable) { + bool on = enable != 0; + g_torque_enabled = on; + g_servo.setTorqueAll(g_servo_ids, NUM_JOINTS, on); + g_last_sync_ms = millis(); + return 1; +} + +std::vector hw_ping_servos(int32_t max_id) { + if (max_id < 1) max_id = 1; + if (max_id > 253) max_id = 253; + int32_t found[64]; + int n = g_servo.pingRange(static_cast(max_id), found, 64); + return std::vector(found, found + n); +} + +std::vector hw_calibrate_imu(int32_t samples) { + if (samples < 10) samples = 10; + if (samples > 1000) samples = 1000; + float bias[3] = {0.0f, 0.0f, 0.0f}; + g_imu.sampleStationaryGyroBias(static_cast(samples), bias); + g_imu.setGyroBias(bias[0], bias[1], bias[2]); + return {bias[0], bias[1], bias[2]}; +} + +void setup() { + Bridge.begin(); + Monitor.begin(115200); + + g_servo.begin(SERVO_SERIAL, SERVO_BAUD, SERVO_DIR_PIN); + g_servo.setTorqueBroadcast(false); + + IMU_WIRE.begin(); + IMU_WIRE.setClock(IMU_I2C_CLOCK_HZ); + g_imu_ready = g_imu.begin(IMU_WIRE, IMU_I2C_ADDR); + + Bridge.provide_safe("hw_configure", hw_configure); + Bridge.provide_safe("hw_sync_step", hw_sync_step); + Bridge.provide_safe("hw_set_torque", hw_set_torque); + Bridge.provide_safe("hw_ping_servos", hw_ping_servos); + Bridge.provide_safe("hw_calibrate_imu", hw_calibrate_imu); + + g_last_sync_ms = millis(); + g_last_imu_us = micros(); + Monitor.println("Microduck UNO Q STM32 firmware initialized (torque off)."); +} + +void loop() { + uint32_t now_us = micros(); + uint32_t dt_us = now_us - g_last_imu_us; + if (dt_us >= IMU_LOOP_INTERVAL_US) { + float dt_s = static_cast(dt_us) * 1e-6f; + if (dt_s > 0.05f) dt_s = 0.004f; + g_imu.update(dt_s); + g_last_imu_us = now_us; + } + + // Hardware safety watchdog: disable servo torque if Linux 50Hz loop stalls + if (g_torque_enabled && (millis() - g_last_sync_ms) > g_watchdog_ms) { + g_torque_enabled = false; + g_servo.setTorqueBroadcast(false); + g_servo.setTorqueAll(g_servo_ids, NUM_JOINTS, false); + Monitor.println("WATCHDOG: Linux sync_step timeout -> servo torque disabled."); + } +} diff --git a/app/public/hardware/unoq/sketch/sketch.yaml b/app/public/hardware/unoq/sketch/sketch.yaml new file mode 100644 index 0000000..e442ad0 --- /dev/null +++ b/app/public/hardware/unoq/sketch/sketch.yaml @@ -0,0 +1,12 @@ +profiles: + default: + fqbn: arduino:zephyr:unoq + platforms: + - platform: arduino:zephyr + libraries: + - Arduino_RouterBridge + - Arduino_RPClite + - ArxContainer + - ArxTypeTraits + - DebugLog + - MsgPack diff --git a/app/public/microduck-shadow-status.html b/app/public/microduck-shadow-status.html new file mode 100644 index 0000000..91289ef --- /dev/null +++ b/app/public/microduck-shadow-status.html @@ -0,0 +1,8 @@ +Microduck 影子策略状态 + +

正在读取板端状态…

+ diff --git a/app/scripts/sync-unoq-bundle.mjs b/app/scripts/sync-unoq-bundle.mjs new file mode 100644 index 0000000..1e3639a --- /dev/null +++ b/app/scripts/sync-unoq-bundle.mjs @@ -0,0 +1,73 @@ +import { + mkdir, + readdir, + readFile, + rm, + stat, + writeFile, +} from "node:fs/promises"; +import { dirname, posix, resolve } from "node:path"; +import { fileURLToPath } from "node:url"; + +const SCRIPT_DIR = dirname(fileURLToPath(import.meta.url)); +const APP_DIR = resolve(SCRIPT_DIR, ".."); +const SRC_ROOT = resolve(APP_DIR, "../hardware/unoq"); +const DST_ROOT = resolve(APP_DIR, "public/hardware/unoq"); + +const SKIP_DIRS = new Set([ + "__pycache__", + ".pytest_cache", + "tests", + "policies", +]); +const SKIP_FILES = new Set([".last_deploy.json", ".gitkeep", "files.json"]); + +async function collectFiles(dir, relPrefix = "", out = []) { + const entries = await readdir(dir, { withFileTypes: true }); + entries.sort((a, b) => a.name.localeCompare(b.name)); + for (const entry of entries) { + const rel = relPrefix ? posix.join(relPrefix, entry.name) : entry.name; + const full = resolve(dir, entry.name); + if (entry.isDirectory()) { + if (SKIP_DIRS.has(entry.name)) continue; + await collectFiles(full, rel, out); + } else if (entry.isFile()) { + if (SKIP_FILES.has(entry.name) || entry.name.endsWith(".pyc")) continue; + const st = await stat(full); + const executable = (st.mode & 0o111) !== 0 || rel.endsWith(".sh"); + out.push({ path: rel, full, executable }); + } + } + return out; +} + +export async function syncUnoqBundle() { + const files = await collectFiles(SRC_ROOT); + await rm(DST_ROOT, { recursive: true, force: true }); + await mkdir(DST_ROOT, { recursive: true }); + + const manifest = []; + for (const item of files) { + const dest = resolve(DST_ROOT, item.path); + await mkdir(dirname(dest), { recursive: true }); + await writeFile(dest, await readFile(item.full)); + manifest.push({ path: item.path, executable: item.executable }); + } + + await writeFile( + resolve(DST_ROOT, "files.json"), + JSON.stringify({ schema: 1, files: manifest }, null, 2) + "\n", + "utf8", + ); + return manifest; +} + +const isDirect = + process.argv[1] && + resolve(process.argv[1]) === fileURLToPath(import.meta.url); +if (isDirect) { + const synced = await syncUnoqBundle(); + console.log( + `Synced ${synced.length} UNO Q runtime files to app/public/hardware/unoq/`, + ); +} diff --git a/app/src/App.jsx b/app/src/App.jsx index 07abf4d..ec7498d 100644 --- a/app/src/App.jsx +++ b/app/src/App.jsx @@ -1,16 +1,20 @@ // A single simulation surface: loading, errors and pause are inline states, // never separate title/pause/BIOS screens. -import { useEffect } from "react"; +import { useEffect, useRef, useState } from "react"; import GameCanvas from "./scene/GameCanvas.jsx"; import { Halftone, CrtOverlay } from "./ui/Overlays.jsx"; import Hud from "./ui/Hud.jsx"; import SimulationToolbar from "./ui/SimulationToolbar.jsx"; import ControlsHelp from "./ui/ControlsHelp.jsx"; import RuntimeStatus from "./ui/RuntimeStatus.jsx"; +import HardwareDeployDialog from "./ui/HardwareDeployDialog.jsx"; import { useGame, gameApi } from "./store.js"; export default function App() { const comic = useGame((s) => s.presentationTheme === "comic"); + const [deployOpen, setDeployOpen] = useState(false); + const deployOpenRef = useRef(false); + deployOpenRef.current = deployOpen; useEffect(() => { const params = new URLSearchParams(location.search); let ref = params.get("move") || params.get("policy"); @@ -28,6 +32,10 @@ export default function App() { ) return; event.preventDefault(); + if (deployOpenRef.current) { + setDeployOpen(false); + return; + } if (useGame.getState().runtimePhase === "ready") { if (useGame.getState().paused) gameApi.resume?.(); else gameApi.pause?.(); @@ -46,9 +54,13 @@ export default function App() { )} - + setDeployOpen(true)} /> + setDeployOpen(false)} + /> ); } diff --git a/app/src/platform/export/zip.js b/app/src/platform/export/zip.js new file mode 100644 index 0000000..582e90f --- /dev/null +++ b/app/src/platform/export/zip.js @@ -0,0 +1,136 @@ +const CRC_TABLE = (() => { + const table = new Uint32Array(256); + for (let i = 0; i < 256; i++) { + let c = i; + for (let k = 0; k < 8; k++) { + c = c & 1 ? 0xedb88320 ^ (c >>> 1) : c >>> 1; + } + table[i] = c >>> 0; + } + return table; +})(); + +const encoder = new TextEncoder(); + +export function crc32(bytes) { + let crc = 0xffffffff; + for (let i = 0; i < bytes.length; i++) { + crc = CRC_TABLE[(crc ^ bytes[i]) & 0xff] ^ (crc >>> 8); + } + return (crc ^ 0xffffffff) >>> 0; +} + +function toBytes(data) { + if (data instanceof Uint8Array) return data; + if (data instanceof ArrayBuffer) return new Uint8Array(data); + if (typeof data === "string") return encoder.encode(data); + throw new TypeError( + "ZIP entry data must be Uint8Array, ArrayBuffer, or string", + ); +} + +function normalizeZipPath(rawPath) { + const cleaned = String(rawPath || "") + .replace(/\\/g, "/") + .replace(/^\/+/, ""); + const parts = cleaned.split("/").filter(Boolean); + if (parts.length === 0 || parts.some((p) => p === "." || p === "..")) { + throw new Error(`Unsafe ZIP entry path: '${rawPath}'`); + } + return parts.join("/"); +} + +/** + * Assemble an uncompressed (STORE) standard ZIP archive as a single Uint8Array. + * Preserves Unix permissions in the central directory (0755 for executable scripts). + */ +export function createZip(entries) { + if (!Array.isArray(entries) || entries.length === 0) { + throw new Error("createZip requires at least one entry"); + } + + const prepared = []; + let localTotal = 0; + let centralTotal = 0; + + for (const entry of entries) { + const name = normalizeZipPath(entry.path); + const nameBytes = encoder.encode(name); + const dataBytes = toBytes(entry.data); + const crc = crc32(dataBytes); + const executable = Boolean(entry.executable); + const localOffset = localTotal; + + localTotal += 30 + nameBytes.length + dataBytes.length; + centralTotal += 46 + nameBytes.length; + + prepared.push({ + nameBytes, + dataBytes, + crc, + executable, + localOffset, + }); + } + + const out = new Uint8Array(localTotal + centralTotal + 22); + const view = new DataView(out.buffer); + let pos = 0; + + // Local file headers + file bytes + for (const item of prepared) { + view.setUint32(pos, 0x04034b50, true); // Local file header signature + view.setUint16(pos + 4, 20, true); // Version needed (2.0) + view.setUint16(pos + 6, 0x0800, true); // UTF-8 filename flag + view.setUint16(pos + 8, 0, true); // Compression: STORE + view.setUint16(pos + 10, 0, true); // Mod time + view.setUint16(pos + 12, 0x5421, true); // Mod date (arbitrary valid DOS date) + view.setUint32(pos + 14, item.crc, true); + view.setUint32(pos + 18, item.dataBytes.length, true); + view.setUint32(pos + 22, item.dataBytes.length, true); + view.setUint16(pos + 26, item.nameBytes.length, true); + view.setUint16(pos + 28, 0, true); // Extra length + pos += 30; + out.set(item.nameBytes, pos); + pos += item.nameBytes.length; + out.set(item.dataBytes, pos); + pos += item.dataBytes.length; + } + + const centralStart = pos; + for (const item of prepared) { + const mode = item.executable ? 0o100755 : 0o100644; + view.setUint32(pos, 0x02014b50, true); // Central directory signature + view.setUint16(pos + 4, (3 << 8) | 20, true); // Unix creator + v2.0 + view.setUint16(pos + 6, 20, true); + view.setUint16(pos + 8, 0x0800, true); // UTF-8 + view.setUint16(pos + 10, 0, true); // STORE + view.setUint16(pos + 12, 0, true); + view.setUint16(pos + 14, 0x5421, true); + view.setUint32(pos + 16, item.crc, true); + view.setUint32(pos + 20, item.dataBytes.length, true); + view.setUint32(pos + 24, item.dataBytes.length, true); + view.setUint16(pos + 28, item.nameBytes.length, true); + view.setUint16(pos + 30, 0, true); + view.setUint16(pos + 32, 0, true); + view.setUint16(pos + 34, 0, true); + view.setUint16(pos + 36, 0, true); + view.setUint32(pos + 38, (mode << 16) >>> 0, true); + view.setUint32(pos + 42, item.localOffset, true); + pos += 46; + out.set(item.nameBytes, pos); + pos += item.nameBytes.length; + } + + // End of central directory record + view.setUint32(pos, 0x06054b50, true); + view.setUint16(pos + 4, 0, true); + view.setUint16(pos + 6, 0, true); + view.setUint16(pos + 8, prepared.length, true); + view.setUint16(pos + 10, prepared.length, true); + view.setUint32(pos + 12, centralTotal, true); + view.setUint32(pos + 16, centralStart, true); + view.setUint16(pos + 20, 0, true); + + return out; +} diff --git a/app/src/robots/microduck/hardware-export.js b/app/src/robots/microduck/hardware-export.js new file mode 100644 index 0000000..244d6c8 --- /dev/null +++ b/app/src/robots/microduck/hardware-export.js @@ -0,0 +1,523 @@ +import { readOnnxMetadata } from "../../platform/loader/metadata.js"; +import { createZip } from "../../platform/export/zip.js"; + +export const MICRODUCK_OBS_SIZE = 61; +export const MICRODUCK_ACTION_SIZE = 14; +export const MICRODUCK_COMMAND_SIZE = 13; + +export const MICRODUCK_JOINT_NAMES = [ + "left_hip_yaw", + "left_hip_roll", + "left_hip_pitch", + "left_knee", + "left_ankle", + "neck_pitch", + "head_pitch", + "head_yaw", + "head_roll", + "right_hip_yaw", + "right_hip_roll", + "right_hip_pitch", + "right_knee", + "right_ankle", +]; + +export const MICRODUCK_DEFAULT_POSE = [ + 0.0, -0.0872664600610733, -0.4579240083694458, -0.004939999897032976, + 0.4529840052127838, 0.3490658402442932, 0.3490658402442932, 0.0, 0.0, 0.0, + 0.0872664600610733, 0.4579240083694458, 0.004939999897032976, + -0.4529840052127838, +]; + +const MICRODUCK_JOINT_LIMITS = { + left_hip_yaw: [-0.4363323129985824, 0.5235987755982988], + left_hip_roll: [-0.3839724354387525, 0.3839724354387525], + left_hip_pitch: [-1.570796326795005, 1.5707963267947882], + left_knee: [-1.570796326795012, 1.570796326794781], + left_ankle: [-1.5707963267949019, 1.5707963267948912], + neck_pitch: [-1.5707963267948966, 1.0471975511965976], + head_pitch: [-1.5707963267948974, 1.5707963267948957], + head_yaw: [-2.967059728390373, 2.967059728390348], + head_roll: [-0.43633231299859127, 0.4363323129985735], + right_hip_yaw: [-0.5235987755982988, 0.4363323129985824], + right_hip_roll: [-0.3839724354387525, 0.3839724354387525], + right_hip_pitch: [-1.5707963267949268, 1.5707948664], + right_knee: [-1.570796326794932, 1.570796326794861], + right_ankle: [-1.5707963267949054, 1.5707963267948877], +}; +// Ensure right_hip_pitch exact upper bound matches MJCF/hardware_profile.json +MICRODUCK_JOINT_LIMITS.right_hip_pitch = [ + -1.5707963267949268, 1.5707963267948664, +]; + +export const MICRODUCK_SKILL_CATALOG = [ + { + key: "walk", + label: "Walk (Legs)", + variant: "legs", + kind: "perpetual", + slot: "walk", + required: true, + defaultPolicyPath: "policies/BEST_alpha_walking.onnx", + targetPath: "policies/walk.onnx", + }, + { + key: "stand", + label: "Stand / Auto-Recover", + variant: "legs", + kind: "recovery", + slot: "stand", + required: true, + defaultPolicyPath: "policies/BEST_alpha_stand.onnx", + targetPath: "policies/stand.onnx", + }, + { + key: "sitstand", + label: "Sit / Stand", + variant: "legs", + kind: "perpetual", + slot: "sitstand", + required: false, + defaultPolicyPath: "policies/BEST_alpha_sitstand.onnx", + targetPath: "policies/sitstand.onnx", + }, + { + key: "kickL", + label: "Kick Left", + variant: "legs", + kind: "episodic", + slot: "kickL", + required: false, + defaultPolicyPath: "policies/ball_kick_left.onnx", + targetPath: "policies/kickL.onnx", + }, + { + key: "kickR", + label: "Kick Right", + variant: "legs", + kind: "episodic", + slot: "kickR", + required: false, + defaultPolicyPath: "policies/ball_kick_right.onnx", + targetPath: "policies/kickR.onnx", + }, + { + key: "groundpick", + label: "Ground Pick", + variant: "legs", + kind: "episodic", + slot: "groundpick", + required: false, + defaultPolicyPath: "policies/alpha_ground_pick.onnx", + targetPath: "policies/groundpick.onnx", + }, + { + key: "roll", + label: "Roll / Trick", + variant: "legs", + kind: "episodic", + slot: "roll", + durationS: 3.0, + required: false, + defaultPolicyPath: "policies/roulade.onnx", + targetPath: "policies/roll.onnx", + }, + { + key: "drive", + label: "Rollers Drive", + variant: "rollers", + kind: "perpetual", + slot: "drive", + required: false, + defaultPolicyPath: "policies/BEST_roller.onnx", + targetPath: "policies/drive.onnx", + }, + { + key: "crouch", + label: "Rollers Crouch", + variant: "rollers", + kind: "episodic", + slot: "crouch", + required: false, + defaultPolicyPath: "policies/BEST_roller_crouch.onnx", + targetPath: "policies/crouch.onnx", + }, +]; + +export function buildDefaultHardwareProfile() { + const joints = {}; + MICRODUCK_JOINT_NAMES.forEach((name, idx) => { + const [min_rad, max_rad] = MICRODUCK_JOINT_LIMITS[name]; + joints[name] = { + id: idx + 1, + sign: 1, + zero_offset_ticks: 0, + min_rad, + max_rad, + }; + }); + return { + schema: 1, + board: "arduino_uno_q", + servo: { + model: "FEETECH_HD-1910-C001", + protocol: "hls_sts_half_duplex", + baudrate: 1000000, + ticks_per_rev: 4096, + neutral_tick: 2048, + speed_limit: 0, + acc: 50, + torque_limit: 1000, + vel_source: "fd", + vel_lpf_alpha: 0.4, + vel_scale_rad_s_per_unit: (2 * Math.PI) / 4096, + }, + imu: { + model: "QMI8658", + i2c_addr: "0x6A", + axis_map: [ + [1, 0, 0], + [0, 1, 0], + [0, 0, 1], + ], + gyro_bias_rad_s: [0.0, 0.0, 0.0], + accel_bias_g: [0.0, 0.0, 0.0], + }, + joints, + safety: { + soft_start_s: 1.5, + watchdog_ms: 150, + ws_timeout_ms: 500, + auto_stand_on_fall: false, + cmd_vel_scale: 0.5, + }, + }; +} + +export function validateCustomOnnxMetadata(bytes, declaredDims = null) { + if (!(bytes instanceof Uint8Array) || bytes.byteLength === 0) { + throw new Error("Uploaded ONNX policy buffer is empty"); + } + if (declaredDims) { + const { obsLen, actionLen } = declaredDims; + if ( + (obsLen != null && obsLen !== MICRODUCK_OBS_SIZE) || + (actionLen != null && actionLen !== MICRODUCK_ACTION_SIZE) + ) { + throw new Error( + `Incompatible Microduck dimensions (got obs=${obsLen}, action=${actionLen}; expected obs=${MICRODUCK_OBS_SIZE}, action=${MICRODUCK_ACTION_SIZE})`, + ); + } + } + let meta = {}; + try { + meta = readOnnxMetadata(bytes); + } catch (err) { + if (declaredDims == null) { + throw err; + } + } + if (meta.joint_names) { + const names = meta.joint_names + .split(",") + .map((s) => s.trim()) + .filter(Boolean); + if (names.length !== MICRODUCK_ACTION_SIZE) { + throw new Error( + `Incompatible ONNX joint_names count (${names.length}; expected ${MICRODUCK_ACTION_SIZE})`, + ); + } + } + return meta; +} + +export function buildMicroduckManifest({ + selectedSkills, + includeRollers = false, + customMove = null, + actionScale = 1.0, +} = {}) { + const chosen = new Set( + selectedSkills && selectedSkills.length + ? selectedSkills + : MICRODUCK_SKILL_CATALOG.filter( + (s) => + s.variant === "legs" || (includeRollers && s.variant === "rollers"), + ).map((s) => s.key), + ); + if (!chosen.has("walk") || !chosen.has("stand")) { + throw new Error( + "Both 'walk' and 'stand' policies are required for hardware safety", + ); + } + + const rollersIncluded = + Boolean(includeRollers) || chosen.has("drive") || chosen.has("crouch"); + if (rollersIncluded && !chosen.has("drive")) { + throw new Error("'drive' policy is required when Rollers mode is enabled"); + } + + const skills = {}; + for (const item of MICRODUCK_SKILL_CATALOG) { + if (!chosen.has(item.key)) continue; + if (item.variant === "rollers" && !rollersIncluded) continue; + skills[item.key] = { + path: item.targetPath, + kind: item.kind, + slot: item.slot, + action_scale: actionScale, + ...(item.durationS ? { duration_s: item.durationS } : {}), + }; + } + + let manifestCustomMove = null; + if (customMove) { + if (customMove.type === "script") { + if ( + !Array.isArray(customMove.script?.keyframes) || + !customMove.script.keyframes.length + ) { + throw new Error("Custom command script has no keyframes"); + } + manifestCustomMove = { + name: customMove.name || "custom_script", + slot: "script", + type: "script", + }; + } else if (customMove.type === "onnx") { + validateCustomOnnxMetadata(customMove.bytes, { + obsLen: customMove.obsLen, + actionLen: customMove.actionLen, + }); + const slot = customMove.slot === "trick" ? "trick" : "walk"; + if (slot === "trick") { + skills.roll = { + path: "policies/custom_roll.onnx", + kind: "episodic", + slot: "roll", + action_scale: actionScale, + duration_s: + Number(customMove.durationS) > 0 + ? Number(customMove.durationS) + : 3.0, + }; + } else { + skills.walk = { + path: "policies/custom_walk.onnx", + kind: "perpetual", + slot: "walk", + action_scale: actionScale, + }; + } + manifestCustomMove = { + name: customMove.name || `custom_${slot}`, + slot, + type: "onnx", + }; + } + } + + return { + schema: 1, + robot_id: rollersIncluded ? "microduck-legs+rollers" : "microduck-legs", + family: "microduck", + sim: { timestep: 0.005, decimation: 4, ctrl_dt: 0.02, hz: 50 }, + obs_size: MICRODUCK_OBS_SIZE, + action_size: MICRODUCK_ACTION_SIZE, + command_size: MICRODUCK_COMMAND_SIZE, + joint_names: [...MICRODUCK_JOINT_NAMES], + default_pose: [...MICRODUCK_DEFAULT_POSE], + action_scale: actionScale, + variants: { + legs: { limits: [0.25, -0.2, 1.0], enabled: true }, + rollers: { limits: [0.6, -0.5, 0.3], enabled: rollersIncluded }, + }, + legacy: { + crouch_period: 5.0, + crouch_end: 0.7, + groundpick_period: 4.0, + groundpick_end: 0.7, + }, + head: { max_rad: 2.5, alpha: 0.2, signs: [1, -1, 1, -1] }, + safety: { + tilt_z: -0.5, + upright_z: -0.85, + fall_debounce_steps: 10, + fall_settle_steps: 15, + recover_upright_steps: 50, + recover_giveup_steps: 300, + kick_steps: 25, + post_kick_lock_steps: 20, + roll_max_steps: 150, + sit_delay_s: 0.8, + stand_delay_s: 2.0, + }, + skills, + custom_move: manifestCustomMove, + }; +} + +export function buildUnoqBundleFiles({ + manifest, + policyBlobs, + profile = null, + customScript = null, + templateEntries = [], +}) { + const entries = []; + const rootPrefix = "microduck_unoq"; + + for (const tpl of templateEntries) { + if (!tpl.path || tpl.path.startsWith("config/robot_manifest.json")) + continue; + if (profile && tpl.path === "config/hardware_profile.json") continue; + entries.push({ + path: `${rootPrefix}/${tpl.path}`, + data: tpl.data, + executable: Boolean(tpl.executable), + }); + } + + entries.push({ + path: `${rootPrefix}/config/robot_manifest.json`, + data: JSON.stringify(manifest, null, 2) + "\n", + }); + + if (profile) { + entries.push({ + path: `${rootPrefix}/config/hardware_profile.json`, + data: JSON.stringify(profile, null, 2) + "\n", + }); + } + + if (customScript) { + entries.push({ + path: `${rootPrefix}/config/custom_move_script.json`, + data: JSON.stringify(customScript, null, 2) + "\n", + }); + } + + for (const [skillKey, spec] of Object.entries(manifest.skills || {})) { + const customBlobKey = + spec.path === "policies/custom_walk.onnx" + ? "custom_walk" + : spec.path === "policies/custom_roll.onnx" + ? "custom_roll" + : skillKey; + const blob = policyBlobs[customBlobKey] ?? policyBlobs[skillKey]; + if (!(blob instanceof Uint8Array) || blob.byteLength === 0) { + throw new Error( + `Missing ONNX binary bytes for skill '${skillKey}' (${spec.path})`, + ); + } + entries.push({ + path: `${rootPrefix}/${spec.path}`, + data: blob, + }); + } + + return entries; +} + +export function resolveSkillSourceUrls(baseUrl = "/") { + const base = baseUrl.endsWith("/") ? baseUrl : `${baseUrl}/`; + const map = {}; + for (const item of MICRODUCK_SKILL_CATALOG) { + map[item.key] = `${base}${item.defaultPolicyPath}`; + } + return map; +} + +export async function assembleUnoqDeployZip({ + baseUrl = "/", + selectedSkills, + includeRollers = false, + customMove = null, + includeProfile = true, + includeRuntimeTemplates = false, + fetchFn = globalThis.fetch, +} = {}) { + const manifest = buildMicroduckManifest({ + selectedSkills, + includeRollers, + customMove, + }); + + const urls = resolveSkillSourceUrls(baseUrl); + const policyBlobs = {}; + + const skillKeys = Object.keys(manifest.skills); + await Promise.all( + skillKeys.map(async (key) => { + const spec = manifest.skills[key]; + if (customMove?.type === "onnx") { + if (spec.path === "policies/custom_walk.onnx") { + policyBlobs.custom_walk = customMove.bytes; + return; + } + if (spec.path === "policies/custom_roll.onnx") { + policyBlobs.custom_roll = customMove.bytes; + return; + } + } + const res = await fetchFn(urls[key]); + if (!res.ok) { + throw new Error( + `Failed to fetch policy '${key}' from ${urls[key]} (HTTP ${res.status})`, + ); + } + policyBlobs[key] = new Uint8Array(await res.arrayBuffer()); + }), + ); + + let templateEntries = []; + if (includeRuntimeTemplates) { + const base = baseUrl.endsWith("/") ? baseUrl : `${baseUrl}/`; + const indexRes = await fetchFn(`${base}hardware/unoq/files.json`); + if (!indexRes.ok) { + throw new Error( + `UNO Q runtime template index missing (${base}hardware/unoq/files.json)`, + ); + } + const index = await indexRes.json(); + templateEntries = await Promise.all( + (index.files || []).map(async (item) => { + const fileRes = await fetchFn(`${base}hardware/unoq/${item.path}`); + if (!fileRes.ok) { + throw new Error( + `Failed to fetch template file hardware/unoq/${item.path}`, + ); + } + return { + path: item.path, + executable: Boolean(item.executable), + data: new Uint8Array(await fileRes.arrayBuffer()), + }; + }), + ); + } + + const customScript = + customMove?.type === "script" + ? { + name: customMove.name || "custom_script", + durationS: customMove.script.durationS, + loop: customMove.script.loop !== false, + keyframes: customMove.script.keyframes, + } + : null; + + const entries = buildUnoqBundleFiles({ + manifest, + policyBlobs, + profile: includeProfile ? buildDefaultHardwareProfile() : null, + customScript, + templateEntries, + }); + + return { + manifest, + zipBytes: createZip(entries), + fileCount: entries.length, + }; +} diff --git a/app/src/ui/HardwareDeployDialog.jsx b/app/src/ui/HardwareDeployDialog.jsx new file mode 100644 index 0000000..edfdfd9 --- /dev/null +++ b/app/src/ui/HardwareDeployDialog.jsx @@ -0,0 +1,801 @@ +import { useEffect, useMemo, useState } from "react"; +import Box from "@mui/material/Box"; +import { useGame } from "../store.js"; +import { normalizeMoveScript } from "../game/custom-policy.js"; +import { + MICRODUCK_COMMAND_SIZE, + MICRODUCK_SKILL_CATALOG, + assembleUnoqDeployZip, + validateCustomOnnxMetadata, +} from "../robots/microduck/hardware-export.js"; + +const DEFAULT_UNOQ_URL = "http://192.168.4.1:8000"; +const USB_UNOQ_URL = "http://127.0.0.1:18000"; +const STORAGE_KEY = "microduck.unoq.url"; + +const skillNames = { + walk: "双足行走", + stand: "站立/自动起身", + sitstand: "坐下/站起", + kickL: "左脚踢球", + kickR: "右脚踢球", + groundpick: "从地面拾取", + roll: "翻滚特技", + drive: "轮式行驶", + crouch: "轮式蹲伏", +}; +const slotNames = { walk: "行走", trick: "特技", script: "动作脚本" }; +const states = { + IDLE: "空闲", + SOFT_START: "软启动", + ACTIVE: "运行中", + CALIBRATION: "标定中", + FALL_SAFE: "跌倒保护", +}; +const usbInterfaces = ["router", "xgoduck-shadow"]; +const halNames = { + "xgoduck-shadow": "原机控制器(仅影子推理)", + router: "实机(router)", + mock: "模拟(mock)", +}; +function errorDetail(err) { + if (["AbortError", "TimeoutError"].includes(err.name)) return "连接超时"; + if (err.message === "Failed to fetch") + return "请求失败,请确认目标地址和板端服务是否可用"; + return err.message; +} + +function readInitialUrl() { + try { + return localStorage.getItem(STORAGE_KEY) || DEFAULT_UNOQ_URL; + } catch { + return DEFAULT_UNOQ_URL; + } +} + +function normalizeUrl(raw) { + return String(raw || DEFAULT_UNOQ_URL) + .trim() + .replace(/\/+$/, ""); +} + +function downloadBlob(bytes, filename) { + const blob = new Blob([bytes], { type: "application/zip" }); + const href = URL.createObjectURL(blob); + const a = document.createElement("a"); + a.href = href; + a.download = filename; + document.body.appendChild(a); + a.click(); + a.remove(); + setTimeout(() => URL.revokeObjectURL(href), 2000); +} + +export default function HardwareDeployDialog({ open, onClose }) { + const neutral = useGame((s) => s.presentationTheme === "neutral"); + const activeCustomPolicy = useGame((s) => s.customPolicy); + + const [targetUrl, setTargetUrl] = useState(readInitialUrl); + const [selectedSkills, setSelectedSkills] = useState(() => + MICRODUCK_SKILL_CATALOG.filter((s) => s.variant === "legs").map( + (s) => s.key, + ), + ); + const [includeRollers, setIncludeRollers] = useState(false); + const [overwriteProfile, setOverwriteProfile] = useState(false); + + // Uploaded local override: null | { type: 'onnx'|'script', slot, name, durationS, bytes?, script? } + const [localCustom, setLocalCustom] = useState(null); + const [onnxSlot, setOnnxSlot] = useState("walk"); + const [trickDurationS, setTrickDurationS] = useState("3.0"); + const [useActiveSimulatorRef, setUseActiveSimulatorRef] = useState(false); + + const [boardStatus, setBoardStatus] = useState(null); + const [busy, setBusy] = useState(false); + const [feedback, setFeedback] = useState(null); + + useEffect(() => { + if (!open) return; + setFeedback(null); + }, [open]); + + const effectiveSkillSet = useMemo(() => { + const next = new Set(selectedSkills); + next.add("walk"); + next.add("stand"); + if (includeRollers) { + next.add("drive"); + } else { + next.delete("drive"); + next.delete("crouch"); + } + return next; + }, [selectedSkills, includeRollers]); + + if (!open) return null; + + const baseUrl = import.meta.env.BASE_URL || "/"; + const cleanUrl = normalizeUrl(targetUrl); + + const rememberUrl = (value) => { + setTargetUrl(value); + setBoardStatus(null); + setFeedback(null); + try { + localStorage.setItem(STORAGE_KEY, value); + } catch { + /* ignore storage errors */ + } + }; + + const toggleSkill = (key) => { + const item = MICRODUCK_SKILL_CATALOG.find((s) => s.key === key); + if (!item || item.required) return; + if (item.variant === "rollers" && !includeRollers) return; + if (key === "drive" && includeRollers) return; + setSelectedSkills((prev) => + prev.includes(key) ? prev.filter((k) => k !== key) : [...prev, key], + ); + }; + + const handleToggleRollers = (checked) => { + setIncludeRollers(checked); + setSelectedSkills((prev) => { + const next = new Set(prev); + if (checked) { + next.add("drive"); + next.add("crouch"); + } else { + next.delete("drive"); + next.delete("crouch"); + } + return Array.from(next); + }); + }; + + const handleUploadCustomFile = async (event) => { + const file = event.target.files?.[0]; + event.target.value = ""; + if (!file) return; + try { + if (file.name.toLowerCase().endsWith(".json")) { + const text = await file.text(); + const raw = JSON.parse(text); + const script = normalizeMoveScript(raw, MICRODUCK_COMMAND_SIZE); + setLocalCustom({ + type: "script", + slot: "script", + name: raw.name || file.name.replace(/\.json$/i, ""), + script, + }); + setUseActiveSimulatorRef(false); + setFeedback({ + ok: true, + text: `已载入脚本「${file.name}」:${script.keyframes.length} 个关键帧,时长 ${script.durationS.toFixed(1)} 秒。`, + }); + } else { + const bytes = new Uint8Array(await file.arrayBuffer()); + const meta = validateCustomOnnxMetadata(bytes); + setLocalCustom({ + type: "onnx", + slot: onnxSlot, + name: file.name.replace(/\.onnx$/i, ""), + durationS: Number(trickDurationS) || 3.0, + bytes, + meta, + }); + setUseActiveSimulatorRef(false); + setFeedback({ + ok: true, + text: `已载入 ONNX 文件「${file.name}」(${(bytes.byteLength / 1024).toFixed(0)} KB),目标策略:${slotNames[onnxSlot]}。`, + }); + } + } catch (err) { + setLocalCustom(null); + setFeedback({ ok: false, text: `自定义文件无效:${errorDetail(err)}` }); + } + }; + + const resolveCustomMoveForDeploy = async () => { + if (localCustom) { + if (localCustom.type === "onnx") { + return { + ...localCustom, + slot: onnxSlot, + durationS: Number(trickDurationS) || 3.0, + }; + } + return localCustom; + } + if ( + useActiveSimulatorRef && + activeCustomPolicy?.status === "active" && + activeCustomPolicy?.ref + ) { + const ref = String(activeCustomPolicy.ref); + if ( + activeCustomPolicy.kind === "script" || + ref.toLowerCase().endsWith(".json") + ) { + const res = await fetch(ref); + if (!res.ok) throw new Error(`无法读取当前动作脚本:${ref}`); + const raw = await res.json(); + const script = normalizeMoveScript(raw, MICRODUCK_COMMAND_SIZE); + return { + type: "script", + slot: "script", + name: activeCustomPolicy.name || "custom_script", + script, + }; + } + const res = await fetch(ref); + if (!res.ok) throw new Error(`无法读取当前策略:${ref}`); + const bytes = new Uint8Array(await res.arrayBuffer()); + validateCustomOnnxMetadata(bytes); + return { + type: "onnx", + slot: activeCustomPolicy.slot === "trick" ? "trick" : "walk", + name: activeCustomPolicy.name || "custom_policy", + durationS: Number(activeCustomPolicy.durationS) || 3.0, + bytes, + }; + } + return null; + }; + + const handleTestConnection = async () => { + setBusy(true); + setFeedback(null); + const ctrl = new AbortController(); + const timer = setTimeout(() => ctrl.abort(), 3000); + try { + const res = await fetch(`${cleanUrl}/api/status`, { + signal: ctrl.signal, + }); + const data = await res.json(); + if (!res.ok) { + throw new Error(data.error || `HTTP ${res.status}`); + } + setBoardStatus(data); + if (data.hal === "xgoduck-shadow") setOverwriteProfile(false); + setFeedback({ + ok: true, + text: `已连接${halNames[data.hal] ?? data.hal},状态:${states[data.lifecycle] ?? data.lifecycle},已加载 ${data.loaded_skills?.length || 0} 个技能。`, + }); + } catch (err) { + setBoardStatus(null); + setFeedback({ + ok: false, + text: `无法连接 ${cleanUrl}:${errorDetail(err)}`, + }); + } finally { + clearTimeout(timer); + setBusy(false); + } + }; + + const handlePushDeploy = async () => { + setBusy(true); + setFeedback(null); + try { + const customMove = await resolveCustomMoveForDeploy(); + const { zipBytes, manifest } = await assembleUnoqDeployZip({ + baseUrl, + selectedSkills: Array.from(effectiveSkillSet), + includeRollers, + customMove, + includeProfile: overwriteProfile, + includeRuntimeTemplates: false, + }); + + if (cleanUrl === USB_UNOQ_URL) { + const statusRes = await fetch(`${cleanUrl}/api/status`, { + cache: "no-store", + signal: AbortSignal.timeout(3000), + }); + if (!statusRes.ok) + throw new Error(`无法读取 USB 设备状态:HTTP ${statusRes.status}`); + const status = await statusRes.json(); + if ( + !usbInterfaces.includes(status.hal) || + status.lifecycle !== "IDLE" || + status.torque_enabled !== false + ) { + throw new Error( + "仅当已连接板端热更新接口、设备处于空闲状态且舵机扭矩已关闭时,才能通过 USB 推送。", + ); + } + } + const res = await fetch( + `${cleanUrl}/api/deploy?overwrite_profile=${overwriteProfile ? "1" : "0"}`, + { + method: "POST", + headers: { "Content-Type": "application/zip" }, + body: zipBytes, + }, + ); + const data = await res.json(); + if (!res.ok || data.ok === false) { + throw new Error(data.error || `HTTP ${res.status}`); + } + if (data.status) setBoardStatus(data.status); + setFeedback({ + ok: true, + text: + data.status?.hal === "xgoduck-shadow" + ? `板端已加载 ${data.status.loaded_skills.length} 个影子策略,版本 ${data.status.version.slice(0, 12)}。仅用于不带扭矩的推理;原机运动策略未替换。` + : `已向 ${cleanUrl} 推送 ${Object.keys(manifest.skills).length} 个策略(${(zipBytes.byteLength / 1024 / 1024).toFixed(2)} MB)。点击「打开控制页」即可查看设备。`, + }); + } catch (err) { + setFeedback({ ok: false, text: `部署失败:${errorDetail(err)}` }); + } finally { + setBusy(false); + } + }; + + const handleDownloadZip = async () => { + setBusy(true); + setFeedback(null); + try { + const customMove = await resolveCustomMoveForDeploy(); + const { zipBytes, fileCount } = await assembleUnoqDeployZip({ + baseUrl, + selectedSkills: Array.from(effectiveSkillSet), + includeRollers, + customMove, + includeProfile: true, + includeRuntimeTemplates: true, + }); + downloadBlob(zipBytes, "microduck-unoq-bundle.zip"); + setFeedback({ + ok: true, + text: `已导出 UNO Q 完整部署包(${fileCount} 个文件,${(zipBytes.byteLength / 1024 / 1024).toFixed(2)} MB)。`, + }); + } catch (err) { + setFeedback({ ok: false, text: `部署包导出失败:${errorDetail(err)}` }); + } finally { + setBusy(false); + } + }; + + const panelBg = neutral ? "rgba(24,24,28,0.96)" : "#faf8f2"; + const panelInk = neutral ? "#f5f5f5" : "#08080c"; + const borderColor = neutral ? "rgba(255,255,255,0.22)" : "#08080c"; + + return ( + { + if (e.target === e.currentTarget && !busy) onClose?.(); + }} + > + + + + Microduck · UNO Q 部署 + + + + + + Microduck · USB 实机 +

+ 板端控制页显示的「XGO Duck」是这台 Microduck 的另一个名称。写入 ONNX + 只会保存文件,不会切换当前运行的策略。 +

+ + 写入 ONNX 模型 + + {" · "} + + 打开实机控制页 + +
+ +
+ 高级:导出部署包或热更新策略 + + {/* Connection row */} + + + + rememberUrl(e.target.value)} + style={{ + flex: "1 1 240px", + fontFamily: "monospace", + fontSize: 12, + padding: "5px 8px", + }} + /> + + + + + 预设地址: + + + + + + {cleanUrl === USB_UNOQ_URL && ( + + 点击「测试连接」。原机接口仅更新影子推理策略,不覆盖标定、不使能舵机。 + + )} + {boardStatus && ( + + 硬件接口: + {halNames[boardStatus.hal] ?? boardStatus.hal} + |状态: + + {states[boardStatus.lifecycle] ?? boardStatus.lifecycle} + + |IMU: + {boardStatus.imu_ok ? "正常" : "异常"}|已加载技能: + {(boardStatus.loaded_skills || []) + .map((key) => skillNames[key] ?? key) + .join("、") || "无"} + {boardStatus.version && ( +
+ 影子版本:{boardStatus.version.slice(0, 12)}|当前策略: + {skillNames[boardStatus.shadow_skill] ?? + boardStatus.shadow_skill ?? + "无"} + |推理次数:{boardStatus.shadow_steps}{" "} + {boardStatus.shadow_error} + {boardStatus.native_mode === "shadow" && + boardStatus.loaded_skills.includes("kickL") && ( + + )} +
+ )} +
+ )} +
+ + {/* Skills checklist */} + +
+ 要部署的技能与策略(61 维观测,14 维动作) +
+ + + {MICRODUCK_SKILL_CATALOG.map((item) => { + const disabled = + item.required || + (item.variant === "rollers" && !includeRollers) || + (item.key === "drive" && includeRollers); + return ( + + ); + })} + +
+ + {/* Custom move / local file import */} + +
+ 自定义策略(.onnx)或动作脚本(.json),可选 +
+ {activeCustomPolicy?.status === "active" && + activeCustomPolicy?.ref && ( + + )} + + + + + {onnxSlot === "trick" && ( + + )} + {localCustom && ( + + )} + +
+ + {/* Calibration safety option */} + + + {feedback && ( + + {feedback.text} + + )} + + {/* Primary actions */} + + + + +
+
+
+
+ ); +} diff --git a/app/src/ui/SimulationToolbar.jsx b/app/src/ui/SimulationToolbar.jsx index 42ede94..f2b30bf 100644 --- a/app/src/ui/SimulationToolbar.jsx +++ b/app/src/ui/SimulationToolbar.jsx @@ -28,7 +28,7 @@ export const toolbarStyle = { outlineOffset: 2, }, }; -export default function SimulationToolbar() { +export default function SimulationToolbar({ onOpenDeploy }) { const phase = useGame((s) => s.runtimePhase), paused = useGame((s) => s.paused), robots = useGame((s) => s.robots), @@ -84,6 +84,15 @@ export default function SimulationToolbar() { + {family === "microduck" && ( + + )} ); } diff --git a/app/test/hardware-export.test.js b/app/test/hardware-export.test.js new file mode 100644 index 0000000..3d86569 --- /dev/null +++ b/app/test/hardware-export.test.js @@ -0,0 +1,193 @@ +import test from "node:test"; +import assert from "node:assert/strict"; +import { execFileSync } from "node:child_process"; +import { mkdtempSync, rmSync, writeFileSync } from "node:fs"; +import { tmpdir } from "node:os"; +import { join } from "node:path"; + +import { createZip } from "../src/platform/export/zip.js"; +import { + MICRODUCK_JOINT_NAMES, + buildDefaultHardwareProfile, + buildMicroduckManifest, + buildUnoqBundleFiles, +} from "../src/robots/microduck/hardware-export.js"; + +test("createZip produces a CRC-valid archive readable by Python zipfile", () => { + const bytes = createZip([ + { path: "microduck_unoq/hello.txt", data: "hello microduck\n" }, + { + path: "microduck_unoq/bin/run.sh", + data: "#!/bin/sh\necho ok\n", + executable: true, + }, + { + path: "microduck_unoq/raw.bin", + data: new Uint8Array([0, 1, 2, 255, 128, 42]), + }, + ]); + + const dir = mkdtempSync(join(tmpdir(), "microduck-zip-test-")); + const zipPath = join(dir, "bundle.zip"); + try { + writeFileSync(zipPath, bytes); + const pyOut = execFileSync( + "python3", + [ + "-c", + [ + "import sys, zipfile, json", + "zf = zipfile.ZipFile(sys.argv[1], 'r')", + "assert zf.testzip() is None", + "names = sorted(zf.namelist())", + "text = zf.read('microduck_unoq/hello.txt').decode()", + "raw = list(zf.read('microduck_unoq/raw.bin'))", + "mode = (zf.getinfo('microduck_unoq/bin/run.sh').external_attr >> 16) & 0o777", + "print(json.dumps({'names': names, 'text': text, 'raw': raw, 'mode': mode}))", + ].join("; "), + zipPath, + ], + { encoding: "utf8" }, + ); + const parsed = JSON.parse(pyOut); + assert.deepEqual(parsed.names, [ + "microduck_unoq/bin/run.sh", + "microduck_unoq/hello.txt", + "microduck_unoq/raw.bin", + ]); + assert.equal(parsed.text, "hello microduck\n"); + assert.deepEqual(parsed.raw, [0, 1, 2, 255, 128, 42]); + assert.equal(parsed.mode, 0o755); + } finally { + rmSync(dir, { recursive: true, force: true }); + } +}); + +test("buildMicroduckManifest supports legs, optional rollers, and custom script/policy slots", () => { + const legsManifest = buildMicroduckManifest({ + selectedSkills: [ + "walk", + "stand", + "sitstand", + "kickL", + "kickR", + "groundpick", + "roll", + ], + includeRollers: false, + }); + assert.equal(legsManifest.obs_size, 61); + assert.equal(legsManifest.action_size, 14); + assert.equal(legsManifest.command_size, 13); + assert.equal(legsManifest.joint_names.length, 14); + assert.equal(legsManifest.variants.rollers.enabled, false); + assert.equal(Object.keys(legsManifest.skills).length, 7); + + const fullManifest = buildMicroduckManifest({ + selectedSkills: [ + "walk", + "stand", + "sitstand", + "kickL", + "kickR", + "groundpick", + "roll", + "drive", + "crouch", + ], + includeRollers: true, + customMove: { + type: "onnx", + slot: "trick", + name: "backflip", + obsLen: 61, + actionLen: 14, + durationS: 2.5, + bytes: new Uint8Array([1, 2, 3]), + }, + }); + assert.equal(fullManifest.variants.rollers.enabled, true); + assert.equal(fullManifest.skills.drive.path, "policies/drive.onnx"); + assert.equal(fullManifest.skills.roll.path, "policies/custom_roll.onnx"); + assert.equal(fullManifest.skills.roll.duration_s, 2.5); + assert.deepEqual(fullManifest.custom_move, { + name: "backflip", + slot: "trick", + type: "onnx", + }); + + // Dimension mismatch must be rejected immediately + assert.throws( + () => + buildMicroduckManifest({ + selectedSkills: ["walk", "stand"], + customMove: { + type: "onnx", + slot: "walk", + name: "bad", + obsLen: 63, + actionLen: 18, + bytes: new Uint8Array([1]), + }, + }), + /61.*14|dimension|incompatible/i, + ); +}); + +test("buildDefaultHardwareProfile matches 14-joint Feetech + QMI8658 schema", () => { + const profile = buildDefaultHardwareProfile(); + assert.equal(profile.servo.ticks_per_rev, 4096); + assert.equal(profile.servo.neutral_tick, 2048); + assert.equal(profile.imu.i2c_addr, "0x6A"); + assert.deepEqual(Object.keys(profile.joints), MICRODUCK_JOINT_NAMES); + const ids = Object.values(profile.joints).map((j) => j.id); + assert.equal(new Set(ids).size, 14); +}); + +test("buildUnoqBundleFiles verifies referenced policies exist and packages cleanly", () => { + const manifest = buildMicroduckManifest({ + selectedSkills: ["walk", "stand"], + customMove: { + type: "script", + slot: "script", + name: "wave", + script: { + durationS: 1.0, + loop: true, + keyframes: [{ t: 0, cmd: [0.1, 0, 0] }], + }, + }, + }); + const profile = buildDefaultHardwareProfile(); + const dummyOnnx = new Uint8Array([0x08, 0x09, 0x12, 0x04]); + const entries = buildUnoqBundleFiles({ + manifest, + policyBlobs: { + walk: dummyOnnx, + stand: dummyOnnx, + }, + profile, + customScript: { + name: "wave", + durationS: 1.0, + loop: true, + keyframes: [{ t: 0, cmd: [0.1, 0, 0] }], + }, + }); + const paths = entries.map((e) => e.path); + assert.ok(paths.includes("microduck_unoq/config/robot_manifest.json")); + assert.ok(paths.includes("microduck_unoq/config/hardware_profile.json")); + assert.ok(paths.includes("microduck_unoq/config/custom_move_script.json")); + assert.ok(paths.includes("microduck_unoq/policies/walk.onnx")); + assert.ok(paths.includes("microduck_unoq/policies/stand.onnx")); + + assert.throws( + () => + buildUnoqBundleFiles({ + manifest, + policyBlobs: { walk: dummyOnnx }, // stand missing + profile, + }), + /stand/i, + ); +}); diff --git a/docs/superpowers/plans/2026-11-13-microduck-unoq-hardware-implementation.md b/docs/superpowers/plans/2026-11-13-microduck-unoq-hardware-implementation.md new file mode 100644 index 0000000..a914563 --- /dev/null +++ b/docs/superpowers/plans/2026-11-13-microduck-unoq-hardware-implementation.md @@ -0,0 +1,183 @@ +# Microduck Arduino UNO Q 硬件导出与实机控制实施计划 + +> **For agentic workers:** REQUIRED SUB-SKILL: Use superpowers:subagent-driven-development (recommended) or superpowers:executing-plans to implement this plan task-by-task. Steps use checkbox (`- [ ]`) syntax for tracking. + +**Goal:** 为 Microduck 构建完整的“网站导出/热推送 → Arduino UNO Q 双脑控制 → 手机热点网页操控”架构与可执行框架(含 PC 无硬件 Mock 联调)。 + +**Architecture:** 网页端 (`app/`) 通过纯 JS 打包器与部署面板将 Microduck 的策略(`legs` 7 技能、可选 `rollers`、自定义策略/脚本)、标准化 `robot_manifest.json`、默认 `hardware_profile.json` 及板端运行时导出为离线 `.zip` 或直接 HTTP 推送至 UNO Q。板端 (`hardware/unoq/`) 采用 Arduino App Lab 目录规范:QRB2210 Linux 运行 50 Hz ONNX 推理、61D 观测组装、状态机、HTTP 部署 API 与离线手机 WebSocket 控制页;STM32U585 运行 250 Hz QMI8658 I2C 姿态融合、飞特 HD-1910-C001 半双工 TTL 串口读写及 150ms 安全看门狗,二者通过 `Arduino_RouterBridge` 单帧同步通信,并通过 `MockHal` 支持无硬件本地端到端联调。 + +**Tech Stack:** React 19, Vite 8, Node.js test runner (`node:test`), Python 3.12 (`numpy`, `onnxruntime`, `fastapi`, `uvicorn`, stdlib fallback server), `pytest`, Arduino Zephyr Core + `Arduino_RouterBridge` (C++)。 + +**Spec:** [2026-11-13-microduck-unoq-hardware-design.md](../specs/2026-11-13-microduck-unoq-hardware-design.md) + +## Global Constraints + +- 观测顺序必须严格为 `[gyro(3), projected_gravity(3), joint_pos_rel(14), joint_vel(14), last_action(14), command(13)]`(共 61 维)。 +- 14 关节顺序与默认姿态以 `app/public/robots/microduck-legs/robot.json` 为准,关节机械限位以 `app/public/robot/mjlab/robot_allcollisions.xml` 为准。 +- 网页端 `npm test`、`npm run validate:packages`、`npm run format:check` 与 `npm run build` 必须保持全绿;不得引入有高危审计问题或非必要外部 npm 依赖。 +- 物理硬件未接入前,`hardware/unoq/` 必须可通过 `--hal mock` 在 PC 上独立启动,并与网站部署及手机网页控制端到端联通。 + +--- + +### Task 1: 板端基础配置与双核目录骨架 (`hardware/unoq/`) + +**Files:** +- Create: `hardware/unoq/app.yaml` +- Create: `hardware/unoq/config/robot_manifest.json` +- Create: `hardware/unoq/config/hardware_profile.json` +- Create: `hardware/unoq/python/requirements.txt` +- Create: `hardware/unoq/scripts/install_deps.sh` +- Create: `hardware/unoq/scripts/setup_ap.sh` +- Create: `hardware/unoq/README.md` + +**Interfaces:** +- Produces: Arduino App Lab 清单 `app.yaml`,与网站导出完全同构的默认 `robot_manifest.json` 和 `hardware_profile.json`,依赖安装与热点脚本。 + +- [ ] **Step 1:** 创建 `hardware/unoq/app.yaml`、`python/requirements.txt`、标准 `config/robot_manifest.json`(匹配 `microduck-legs/robot.json`)、默认 `config/hardware_profile.json`(含 `FEETECH_HD-1910-C001` 参数、`QMI8658` 轴映射与零偏、14 路限位及安全参数)。 +- [ ] **Step 2:** 创建 `scripts/install_deps.sh`、`scripts/setup_ap.sh`(配置 NetworkManager AP `Microduck-AP` 固定 IP `192.168.4.1/24`)及完整接线/部署/调试说明 `hardware/unoq/README.md`。 + +--- + +### Task 2: UNO Q Python 核心数学、标定映射与 61D 观测/状态机模块 + +**Files:** +- Create: `hardware/unoq/python/__init__.py` +- Create: `hardware/unoq/python/core/__init__.py` +- Create: `hardware/unoq/python/core/config.py` +- Create: `hardware/unoq/python/core/observations.py` +- Create: `hardware/unoq/python/core/state_machine.py` +- Create: `hardware/unoq/python/tests/test_core.py` + +**Interfaces:** +- Produces: + - `load_robot_manifest(path)`, `load_hardware_profile(path)`, `save_hardware_profile(path, profile)` + - `rad_to_ticks(q_rad, profile) -> list[int]`, `ticks_to_rad(ticks, profile) -> np.ndarray`, `apply_imu_calibration(gyro, accel_or_grav, profile)` + - `build_observation(sensor_frame, last_action, cmd_13, default_pose) -> np.ndarray` (shape `(61,)`, float32) + - `MicroduckStateMachine`: 精确实现 `IDLE` / `SOFT_START` / `WALK` / `DRIVE` / `SITSTAND` / `KICK_L` / `KICK_R` / `GROUNDPICK` / `ROLL` / `CROUCH` / `FALL_SAFE` / `FALL_SETTLE` / `RECOVERING` 状态转移、`HEAD` 模式平滑和脚本回放 + +- [ ] **Step 1:** 在 `hardware/unoq/python/tests/test_core.py` 编写核心测试:关节弧度↔刻度正逆映射(含反转符号与零点偏置)、限幅保护、61 维观测各分段切片位置校验、状态机全部转移时序(软启动、坐站、踢腿 25+20 步、捡地 phase 0.7、摔倒保护与自动起身开关)。 +- [ ] **Step 2:** 运行 `pytest hardware/unoq/python/tests/test_core.py` 确认初始失败(模块尚未实现)。 +- [ ] **Step 3:** 实现 `core/config.py`、`core/observations.py` 与 `core/state_machine.py`。 +- [ ] **Step 4:** 运行 `pytest hardware/unoq/python/tests/test_core.py` 确认全部通过。 + +--- + +### Task 3: HAL 抽象 (`RouterBridgeHal` + `MockHal`)、ONNX 策略运行器与 50Hz 控制调度器 + +**Files:** +- Create: `hardware/unoq/python/hal/__init__.py` +- Create: `hardware/unoq/python/hal/base.py` +- Create: `hardware/unoq/python/hal/mock_hal.py` +- Create: `hardware/unoq/python/hal/router_hal.py` +- Create: `hardware/unoq/python/core/policy_runner.py` +- Create: `hardware/unoq/python/core/controller.py` +- Create: `hardware/unoq/python/tests/test_controller.py` + +**Interfaces:** +- Consumes: Task 2 的配置、观测拼接、刻度转换和状态机。 +- Produces: + - `HardwareBridge` 抽象基类与 `HardwareFrame` 数据结构 + - `MockHal`(一阶滞后关节响应 + 可注入倾斜角用于跌倒测试) + - `RouterBridgeHal`(基于 `arduino.app_utils.Bridge` 的 `hw_configure` / `hw_sync_step` / `hw_set_torque` / `hw_ping_servos` / `hw_calibrate_imu`) + - `PolicySet`(加载 `onnxruntime.InferenceSession` 或显式 `--policy-backend dummy` 供无 `onnxruntime` 环境测试;校验 `[1,61] -> [1,14]` 且无 NaN/Inf) + - `MicroduckController`(50Hz 主循环、线程安全指令输入、热重载 `reload_policies()`、标定工具方法与状态遥测快照) + +- [ ] **Step 1:** 在 `test_controller.py` 编写测试:验证 `MockHal` 跟踪目标位置、`MicroduckController` 在 `--policy-backend dummy` 下执行 `prepare_stand -> walk -> skill -> estop` 全闭环、跌倒触发 `FALL_SAFE` 以及开启 `auto_stand_on_fall` 时触发 `RECOVERING`、断连超时归零。 +- [ ] **Step 2:** 运行 `pytest hardware/unoq/python/tests/test_controller.py` 验证失败。 +- [ ] **Step 3:** 实现 `hal/base.py`、`hal/mock_hal.py`、`hal/router_hal.py`、`core/policy_runner.py`、`core/controller.py`。 +- [ ] **Step 4:** 运行 `pytest` 验证全部通过。 + +--- + +### Task 4: 部署解包服务、HTTP/WebSocket 服务端与离线手机控制页 + +**Files:** +- Create: `hardware/unoq/python/core/deploy_service.py` +- Create: `hardware/unoq/python/web/__init__.py` +- Create: `hardware/unoq/python/web/server.py` +- Create: `hardware/unoq/python/web/fallback_server.py` +- Create: `hardware/unoq/python/web/static/index.html` +- Create: `hardware/unoq/python/main.py` +- Create: `hardware/unoq/python/tests/test_deploy.py` + +**Interfaces:** +- Consumes: `MicroduckController`、`PolicySet`、`hardware_profile.json` 读写。 +- Produces: + - `DeployService.apply_zip_bytes(zip_bytes, overwrite_profile=False)`:安全解压 ZIP(拒绝路径穿越)、校验 `robot_manifest.json` 维度与必需策略文件、原子更新 `policies/` 与 `robot_manifest.json` 并触发控制器热重载。 + - HTTP REST API(允许跨域 CORS):`GET /api/status`、`POST /api/deploy`、`GET/PUT /api/profile`、`POST /api/calibrate/zero/{joint}`、`POST /api/calibrate/imu`、`POST /api/servos/ping`、`POST /api/servos/test`。 + - WebSocket `/ws/control`:接收手机实时摇杆/按键/急停/站立/开关,回推 20Hz 状态遥测;断连/超时 `>500ms` 自动归零速度。 + - `web/static/index.html`:单文件离线自适应手机界面(控制页签:浮动虚拟摇杆、速度上限比例滑条、技能按钮、Head 模式、E-STOP/准备站立/自动起身;标定页签:舵机扫描、14 关节一键设为零点、反转方向、单关节测试滑条、IMU 静止校准)。 + - `main.py`:支持 `--hal mock|router`、`--policy-backend auto|onnx|dummy`、`--host`、`--port`,并在缺失 `fastapi/uvicorn` 的开发环境自动启用 `fallback_server.py`(纯标准库 HTTP + RFC6455 WebSocket)。 + +- [ ] **Step 1:** 在 `test_deploy.py` 编写测试:验证合法控制包解压热重载、保留已有 `hardware_profile.json`、路径穿越/缺文件/错误维度拒收、以及 `fallback_server` / API 端到端 `/api/status` 与 `/api/deploy` 实测。 +- [ ] **Step 2:** 运行 `pytest hardware/unoq/python/tests/test_deploy.py` 确认初始失败。 +- [ ] **Step 3:** 实现 `deploy_service.py`、`web/server.py`、`web/fallback_server.py`、`web/static/index.html` 与 `main.py`。 +- [ ] **Step 4:** 运行 `pytest hardware/unoq/python/tests/` 全部通过。 + +--- + +### Task 5: STM32U585 MCU 固件实现 (`hardware/unoq/sketch/`) + +**Files:** +- Create: `hardware/unoq/sketch/sketch.yaml` +- Create: `hardware/unoq/sketch/config.h` +- Create: `hardware/unoq/sketch/qmi8658.h` +- Create: `hardware/unoq/sketch/qmi8658.cpp` +- Create: `hardware/unoq/sketch/feetech_hls.h` +- Create: `hardware/unoq/sketch/feetech_hls.cpp` +- Create: `hardware/unoq/sketch/sketch.ino` + +**Interfaces:** +- Produces: 与 `RouterBridgeHal` 严格匹配的 5 个 `Bridge.provide_safe` 端点: + - `hw_configure(std::vector payload)` + - `hw_sync_step(std::vector cmd_frame) -> std::vector` + - `hw_set_torque(int32_t enable) -> int32_t` + - `hw_ping_servos(int32_t max_id) -> std::vector` + - `hw_calibrate_imu(int32_t samples) -> std::vector` +- 详细实现飞特 HD-1910-C001 协议帧校验和、`SyncWrite (0x83)`、`SyncRead (0x82)`(含 `USE_FEETECH_SYNC_READ` 逐个读取回退开关)、可配置 `SERVO_DIR_PIN` 半双工方向切换、QMI8658 寄存器初始化与 Mahony 6 轴滤波、150ms 看门狗自动卸力。 + +- [ ] **Step 1:** 编写 `sketch.yaml` 与集中宏配置 `config.h`。 +- [ ] **Step 2:** 编写 `qmi8658.h/.cpp`(QMI8658 初始化、轴映射、静止零偏与 Mahony 滤波输出 `gyro[3]` 和 `projected_gravity[3]`)。 +- [ ] **Step 3:** 编写 `feetech_hls.h/.cpp`(飞特 HD-1910-C001 数据包构建/解析、扭矩使能、`SyncWrite`、`SyncRead`/单读降级、`Ping`)。 +- [ ] **Step 4:** 编写 `sketch.ino` 连接 `Arduino_RouterBridge`、`Bridge.provide_safe`、`Bridge.update_safe()`、250Hz IMU 融合与 150ms 看门狗。 + +--- + +### Task 6: 网站端导出打包器、ZIP 生成器、同步脚本与部署 UI (`app/`) + +**Files:** +- Create: `app/src/platform/export/zip.js` +- Create: `app/src/robots/microduck/hardware-export.js` +- Create: `app/src/ui/HardwareDeployDialog.jsx` +- Create: `app/scripts/sync-unoq-bundle.mjs` +- Modify: `app/package.json` +- Modify: `app/src/ui/SimulationToolbar.jsx` +- Modify: `app/src/App.jsx` +- Modify: `app/src/robots/microduck/runtime.js` (暴露 `status().customPolicy` 或通过 store 读,无需侵入核心循环) +- Create: `app/test/hardware-export.test.js` + +**Interfaces:** +- Produces: + - `createZip(entries) -> Uint8Array`(纯 JS STORE ZIP,含标准 CRC32 与 UTF-8 标志) + - `buildMicroduckManifest({ legsConfig, rollersConfig, includeRollers, customMove })` + - `buildDefaultHardwareProfile(mjcfJointLimits?)` + - `buildHardwareDeployZip({ ... }) -> Promise` + - `sync-unoq-bundle.mjs` 将 `hardware/unoq/` 源码与模板同步到 `app/public/hardware/unoq/` 并生成 `files.json` 索引,支持纯静态部署后离线下载完整部署包。 + +- [ ] **Step 1:** 在 `app/test/hardware-export.test.js` 编写测试:ZIP 格式校验(使用 Node + `python3 -m zipfile -t` 或字节级校验)、`buildMicroduckManifest` 与 `microduck-legs/robot.json` 关节/默认值/技能路径一致、`customMove` 与脚本导出正确、错误维度拒绝导出。 +- [ ] **Step 2:** 运行 `cd app && node --test test/hardware-export.test.js` 确认初始失败。 +- [ ] **Step 3:** 实现 `zip.js`、`hardware-export.js`、`sync-unoq-bundle.mjs`(并运行生成 `app/public/hardware/unoq/`)。 +- [ ] **Step 4:** 实现 `HardwareDeployDialog.jsx`,并在 `SimulationToolbar.jsx`(仅 `family === "microduck"` 显示按钮)与 `App.jsx` 挂载;确保 `npm run format:check` 覆盖范围全部格式合规。 +- [ ] **Step 5:** 运行 `npm test`、`npm run validate:packages`、`npm run format:check`、`npm run build`。 + +--- + +### Task 7: 端到端无硬件预联调验证与文档检查 + +**Files:** +- Modify: `README.md`(补充 UNO Q 硬件部署入口说明) + +- [ ] **Step 1:** 运行 Python 全部单元测试 `pytest hardware/unoq/python/tests -v`。 +- [ ] **Step 2:** 运行 Node 全部单元测试 `npm test`、包校验 `npm run validate:packages`、格式检查 `npm run format:check`、生产构建 `npm run build`。 +- [ ] **Step 3:** 启动本地 Mock 服务 `python3 hardware/unoq/python/main.py --hal mock --policy-backend dummy --port 18000` 并通过真实打包脚本生成 `.zip` 测试 `POST /api/deploy` + `GET /api/status` + 手机控制页 `GET /` 完整链路连通。 +- [ ] **Step 4:** 如有 Playwright 可用,检查主界面在加了工具栏按钮后既有 `simulation-shell` / `deployment` 测试仍正常通过。 diff --git a/docs/superpowers/specs/2026-11-13-microduck-unoq-hardware-design.md b/docs/superpowers/specs/2026-11-13-microduck-unoq-hardware-design.md new file mode 100644 index 0000000..e971481 --- /dev/null +++ b/docs/superpowers/specs/2026-11-13-microduck-unoq-hardware-design.md @@ -0,0 +1,173 @@ +# Microduck Arduino UNO Q 硬件导出与实机控制架构设计文档 + +- 日期:2026-11-13 +- 状态:用户已完成 `/grill-me` 需求对齐并确认设计方案 +- 适用范围: + 1. `app/` 网页端(Microduck 硬件部署导出/HTTP 热更新推送界面、控制包生成与离线 ZIP 打包) + 2. `hardware/unoq/` UNO Q 板端工程(符合 Arduino App Lab 规范的 Linux MPU 运行时 + 手机热点离线控制网页 + STM32U585 MCU 固件) + +--- + +## 1. 已确认的系统决策 + +| 决策项 | 确认结果 | +|---|---| +| 网站导出/导入方式 | 双模式:支持浏览器直接通过 HTTP API 热推送至 UNO Q Linux 端(无需重刷 STM32),同时支持下载完整离线部署包(`.zip`) | +| 策略与状态机范围 | 默认导出双足 `legs` 全部 7 个 ONNX 策略(`walk`、`stand`、`sitstand`、`kickL`、`kickR`、`groundpick`、`roll`)及当前自定义策略/脚本;可选勾选 `rollers`(`drive`、`crouch`);板端复刻网页状态机 | +| UNO Q 双脑分工 | **Linux MPU (QRB2210)**:50 Hz ONNX 推理、技能状态机、HTTP 部署服务、Wi-Fi 热点手机控制页;**STM32 MCU (STM32U585)**:高频 QMI8658 I2C 采样与姿态融合、飞特串口总线舵机读写、看门狗安全保护;通过 `Arduino_RouterBridge` 单次打包同步帧通信 | +| 舵机型号与协议 | 飞特 **HD-1910-C001**(12 位磁编码 4096 步/圈、中位 2048、默认 1 Mbps 半双工 TTL 串口、支持 `SyncRead 0x82` / `SyncWrite 0x83`) | +| 硬件接线策略 | 当前物理接线尚未最终确定;STM32 `config.h` 集中预留 `Serial1` / 波特率 / 可选半双工 `DIR` 引脚以及 `Wire` / `Wire1` / QMI8658 I2C 地址 (`0x6A`/`0x6B`) 宏定义,连接硬件后可直接实测调整 | +| 标定与 Sim-to-Real 对齐 | 引入可持久化 `hardware_profile.json`(14 路舵机 ID、方向 `sign`、中位偏置 `zero_offset_ticks`、关节限位、IMU 轴映射矩阵与陀螺零偏),并在手机/网页调试页提供在线扫描、中位标定、单关节微调、IMU 静止零偏采集 | +| 实机安全与摔倒保护 | 上电保持卸力/空闲;点击“准备站立”后 1.5s 软插值过渡到 `DEFAULT_POSE` 再激活策略;摔倒时默认先保护性卸力(手机端可切换开启自动调用 `stand.onnx` 爬起);STM32 150ms 心跳看门狗 + 手机端一键 E-STOP | +| 手机端热点控制网页 | UNO Q 开启离线 Wi-Fi AP(`192.168.4.1`),提供单页控制网页(WebSocket 实时控制):虚拟摇杆、速度上限比例滑条、技能按钮、Head 模式、E-STOP、准备站立/卸力、自动起身开关、遥测显示及“标定调试”页签;断连 >0.5s 自动归零速度指令 | +| 工程规范与无硬件预联调 | `hardware/unoq/` 采用标准 Arduino App Lab 结构(`app.yaml` + `python/` + `sketch/`),内置 `MockHal`(`--hal mock`)支持在无硬件时于 PC 端完整联调网页导出推送、50Hz ONNX 循环与手机控制 | + +--- + +## 2. 端到端系统拓扑与数据流 + +```mermaid +flowchart LR + Web["Web 仿真平台 (app/)\nMicroduck 部署弹窗"] -->|"1a. HTTP POST /api/deploy (热更新)"| Server["UNO Q Linux Web & Deploy Server\n(FastAPI / Dev Fallback)"] + Web -->|"1b. 下载 microduck_unoq_bundle.zip"| Zip["离线部署包"] + Phone["手机浏览器 (Wi-Fi AP 192.168.4.1)"] <-->|"2. WebSocket 30-50Hz\n指令 / 状态 / 标定"| Server + Server <-->|"3. 共享控制状态 & 策略热重载"| Ctrl["50Hz 控制器\nObservation(61D) -> ONNX -> Action(14D)"] + Ctrl <-->|"4a. RouterBridgeHal (UNO Q 实机)"| Bridge["Arduino_RouterBridge\nsync_step 单帧 RPC"] + Ctrl <-->|"4b. MockHal (PC 无硬件联调)"| Mock["一阶关节模型 + 直立 IMU 模拟"] + Bridge <-->|"5. STM32 200-400Hz 实时环"| MCU["STM32U585 固件 (sketch/)"] + MCU <-->|"I2C Wire/Wire1"| IMU["QMI8658 IMU"] + MCU <-->|"Half-Duplex UART Serial1"| Servo["14x 飞特 HD-1910-C001"] +``` + +--- + +## 3. 网站端导出协议与控制包结构 + +### 3.1 前端入口与交互流程 + +- 入口位于 [SimulationToolbar.jsx](file:///home/cen/Embodied_Workspace/Mujoco_Projects/microduck-simulator/app/src/ui/SimulationToolbar.jsx),仅在当前选中机器人家族为 `microduck` 时渲染 **Hardware (UNO Q)** 按钮(不改变既有其它机器人界面及既有 E2E 回归行为)。 +- 点击后打开部署抽屉/弹窗 `HardwareDeployDialog.jsx`,包含: + 1. **目标硬件连接区**:UNO Q 地址输入(默认 `http://192.168.4.1:8000`,提供一键切换 `http://127.0.0.1:8000` 本地 Mock 调试)、连接探测(`GET /api/status` 显示在线状态、当前 HAL 模式、已加载策略数)。 + 2. **导出内容选择区**: + - 必选双足基础策略组(`walk`, `stand`, `sitstand`, `kickL`, `kickR`, `groundpick`, `roll`); + - 可选勾选 `rollers` 策略组(`drive`, `crouch`); + - 自定义策略支持:自动识别当前仿真的 `customPolicy`,并允许选择本地 `.onnx` 或 `move_script.json` 覆盖目标槽位(`walk` / `sitstand` / `trick` / `script`),同时校验输出维度为 `14`、输入维度为 `61`; + - 选项:“覆盖板端 `hardware_profile.json`”(默认**不勾选**,防止 HTTP 热更新策略时冲掉实机已标定的舵机零偏和正反号;离线下载完整包时总是附带一份默认模板)。 + 3. **动作按钮**: + - **推送到 UNO Q (HTTP)**:在浏览器生成 `robot_manifest.json` + 所选策略二进制(打包为 `.zip` 上传至 `POST /api/deploy`),触发 UNO Q 热重载; + - **下载完整部署包 (.zip)**:将所选策略、生成的 `config/robot_manifest.json`、`config/hardware_profile.json` 以及同步在 `app/public/hardware/unoq/` 的完整 `hardware/unoq/` 运行代码/脚本/STM32 固件一并打包下载。 + +### 3.2 `robot_manifest.json`(与仿真严格对齐的单一真源) + +由 `app/src/robots/microduck/hardware-export.js` 直接依据 [microduck-legs/robot.json](file:///home/cen/Embodied_Workspace/Mujoco_Projects/microduck-simulator/app/public/robots/microduck-legs/robot.json) 与 [microduck-rollers/robot.json](file:///home/cen/Embodied_Workspace/Mujoco_Projects/microduck-simulator/app/public/robots/microduck-rollers/robot.json) 生成: + +- `sim`: `timestep = 0.005`, `decimation = 4`, `ctrl_dt = 0.02`, `hz = 50` +- `obs_size`: `61`,`action_size`: `14`,`command_size`: `13` +- `joint_names`: 14 个关节精确顺序(`left_hip_yaw` .. `right_ankle`) +- `default_pose`: 与 `robot.json` 中 `joints.defaults` 一一对应 +- `action_scale`: 默认 `1.0`(各技能及自定义策略允许各自覆盖 `action_scale`) +- `variants.legs.limits`: `[0.25, -0.2, 1.0]`,`variants.rollers.limits`: `[0.6, -0.5, 0.3]` +- `legacy`: `crouch_period = 5`, `crouch_end = 0.7`, `groundpick_period = 4`, `groundpick_end = 0.7` +- `head`: `max_rad = 2.5`, `alpha = 0.2`, `signs = [1, -1, 1, -1]` +- `safety`: `tilt_z = -0.5`, `upright_z = -0.85`, `fall_debounce_steps = 10`, `fall_settle_steps = 15`, `recover_upright_steps = 50`, `recover_giveup_steps = 300`, `kick_steps = 25`, `post_kick_lock_steps = 20`, `roll_max_steps = 150` +- `skills`: 映射每个技能 ID 到本地 `.onnx` 路径、类型(`perpetual` / `episodic` / `recovery`)、`action_scale` 与可选 `duration_s` +- `custom_move`: 当存在自定义策略或 `script` 时描述其槽位与文件路径 + +### 3.3 `hardware_profile.json`(实体舵机与 IMU 标定参数) + +- `servo`: `model = "FEETECH_HD-1910-C001"`, `baudrate = 1000000`, `ticks_per_rev = 4096`, `neutral_tick = 2048`, `vel_source = "fd"`(默认用高精度位置差分低通滤波估算关节角速度,可选切换 `"servo"` 寄存器直读) +- `imu`: `model = "QMI8658"`, `i2c_addr = "0x6A"`, `axis_map`(3×3 正交轴映射矩阵,默认单位阵), `gyro_bias_rad_s = [0, 0, 0]`, `accel_bias_g = [0, 0, 0]` +- `joints`: 14 路关节每路配置 `id`(默认 1..14)、`sign`(`+1` / `-1`)、`zero_offset_ticks`(整数偏移)、`min_rad` / `max_rad`(取自 [robot_allcollisions.xml](file:///home/cen/Embodied_Workspace/Mujoco_Projects/microduck-simulator/app/public/robot/mjlab/robot_allcollisions.xml) 关节机械限位) +- `safety`: `soft_start_s = 1.5`, `watchdog_ms = 150`, `ws_timeout_ms = 500`, `auto_stand_on_fall = false`, `cmd_vel_scale = 0.5` + +--- + +## 4. UNO Q Linux 端控制器设计(`hardware/unoq/python/`) + +### 4.1 61 维观测与 14 维动作计算 + +严格复刻 [ObservationPipeline.js](file:///home/cen/Embodied_Workspace/Mujoco_Projects/microduck-simulator/app/src/platform/policy/ObservationPipeline.js) 与 [runtime.js](file:///home/cen/Embodied_Workspace/Mujoco_Projects/microduck-simulator/app/src/robots/microduck/runtime.js) 的顺序与符号: + +1. `obs[0:3]`:`gyro`(`trunk_base` 坐标系下角速度,rad/s) +2. `obs[3:6]`:`projected_gravity`(世界重力 `[0, 0, -1]` 经 `trunk_base` 姿态共轭旋转,直立时约为 `[0, 0, -1]`) +3. `obs[6:20]`:`joint_pos_rel = q_rad - default_pose` +4. `obs[20:34]`:`joint_vel`(rad/s) +5. `obs[34:48]`:`last_action`(上一次有效策略输出 raw action;恢复前清零行为按 `runtime.js` 对齐) +6. `obs[48:61]`:`command(13)`: + - `cmd[0:3]`:在 `walk`/`drive` 下为有效速度指令 `[vx, 0, wz]`(当 `HEAD`、单次动作、软启动、未就绪或输入锁定期间归零);`sitstand` 下 `[sitFlag, 0, 0]`;`groundpick`/`crouch` 下 `[cos(2π·phase), sin(2π·phase), 0]`; + - `cmd[3:7]`:Head 槽位(每步 50 Hz 做 `headSmooth += 0.2 * (headTarget - headSmooth)`;在 `groundpick` 与 `recovery` 中强制清零); + - `cmd[7:13]`:Body 槽位保留 `0`;若当前在 `walk` 且激活了 `script` 动作脚本,按仿真脚本时间线插值覆盖 `cmd[0:13]`。 +7. **动作解码与限幅**: + - `q_target = default_pose + action_scale * raw_action` + - 经 `hardware_profile.json` 各关节 `min_rad` / `max_rad` 限幅后,再转为舵机目标刻度下发 HAL。 + +### 4.2 实机状态机与安全保护 + +```mermaid +stateDiagram-v2 + [*] --> IDLE: 上电/急停(舵机卸力) + IDLE --> SOFT_START: 点击"准备站立" + SOFT_START --> WALK: 1.5s 余弦插值到达 DEFAULT_POSE + WALK --> SITSTAND: Sit/Stand 指令 (800ms 过渡置位) + SITSTAND --> WALK: 站起 (2000ms 过渡后回 WALK) + WALK --> KICK_L: 左踢 (25 步 + 20 步锁止) + WALK --> KICK_R: 右踢 (25 步 + 20 步锁止) + WALK --> GROUNDPICK: 捡地 (4s 周期到 phase 0.7) + WALK --> ROLL: 翻滚 (最多 150 步, 翻转后直立返回) + WALK --> CROUCH: 轮足下蹲 (rollers 模式) + WALK --> FALL_SAFE: gz > -0.5 持续 10 步 & 自动起身关闭 + WALK --> FALL_SETTLE: gz > -0.5 持续 10 步 & 自动起身开启 + FALL_SETTLE --> RECOVERING: 保持 15 步 (0.3s) 后切换 stand.onnx + RECOVERING --> WALK: gz < -0.85 连续 50 步 (1.0s) + RECOVERING --> FALL_SAFE: 超时 300 步 (6.0s) 保护卸力 + SOFT_START --> IDLE: E-STOP / 卸力 + WALK --> IDLE: E-STOP / 卸力 / HAL 错误 + SITSTAND --> IDLE: E-STOP / 卸力 + RECOVERING --> IDLE: E-STOP / 卸力 +``` + +### 4.3 `HardwareBridge` HAL 与 `RouterBridgeHal` / `MockHal` + +- `HardwareBridge` 定义统一接口: + - `connect()` / `close()` + - `configure(profile_dict)`:下发 I2C 地址、IMU 轴映射/零偏、14 路舵机 ID 列表、看门狗超时 + - `sync_step(enable_torque: bool, target_ticks: list[int], speed_limit: int, acc: int) -> HardwareFrame` + - `set_torque(enable: bool)` / `ping_servos() -> list[int]` / `calibrate_imu_bias(samples: int)` +- **`RouterBridgeHal`**:通过 `arduino.app_utils.Bridge.call(...)` 与 STM32 `Bridge.provide_safe(...)` 单帧通信,将控制参数打包为轻量数组,单周期仅 1 次往返 RPC。 +- **`MockHal`**:在本地以一阶滞后动力学跟踪 `target_ticks`,模拟直立状态重力 `[0, 0, -1]` 与微弱噪声,使 PC 无硬件情况下即可跑通 `50 Hz` 控制闭环、网页推送导入与手机控制测试。 + +--- + +## 5. STM32U585 MCU 固件设计(`hardware/unoq/sketch/`) + +1. **`config.h` 集中可配置硬件宏**: + - `SERVO_SERIAL Serial1`、`SERVO_BAUD 1000000`、`SERVO_DIR_PIN -1`(`-1` 表示硬件自动方向转换或单线模式;如使用 74HC126 等方向控制芯片则填具体引脚号)。 + - `IMU_WIRE Wire`(可切换 `Wire1` 用于 UNO Q Qwiic 3.3V I2C)、`IMU_I2C_ADDR 0x6A`(可切换 `0x6B`)。 + - `WATCHDOG_TIMEOUT_MS 150`:超时未收到 Linux `sync_step` 自动调用 `feetech.enableTorqueAll(false)` 卸力。 +2. **`qmi8658.h/.cpp`**: + - 初始化 QMI8658(验证 `WHO_AM_I == 0x05`,配置 6-DOF 加速度计 `±4g` / 陀螺仪 `±512°/s` 或 `±1024°/s`、ODR `>= 250Hz`)。 + - 支持轴映射矩阵和静止零偏校正。 + - 内置 Mahony 6 轴互补/四元数滤波器在 `loop()` 中以 ~250 Hz 实时更新姿态四元数与 `projected_gravity`。 +3. **`feetech_hls.h/.cpp`**: + - 实现飞特标准串行总线帧格式(`0xFF 0xFF ID LEN INST PARAMS CHECKSUM`)。 + - `setTorqueEnable(id, enable)`(写 `0x28`)。 + - `syncWritePosition(ids, ticks, speed, acc)`(`0x83` 广播写起始地址 `0x2A`)。 + - `syncReadPositionSpeed(ids, out_ticks, out_speeds)`(`0x82` 同步读,若部分总线/分线不支持 `SyncRead` 响应则预留逐个 `Read` 降级开关 `USE_FEETECH_SYNC_READ`)。 + - `ping(id)` 用于标定页扫描在线舵机 ID。 +4. **`sketch.ino`**: + - 使用 `Bridge.begin()` 与 `Bridge.provide_safe(...)` 注册 `hw_configure`、`hw_sync_step`、`hw_set_torque`、`hw_ping_servos`、`hw_calibrate_imu`,在 `loop()` 调用 `Bridge.update_safe()` 与高频 IMU 更新及看门狗检查。 + +--- + +## 6. 验证与测试范围 + +1. **JavaScript 单元测试**(`app/test/hardware-export.test.js`): + - 验证生成的 `robot_manifest.json` 与 `microduck-legs` / `microduck-rollers` 的 14 个关节名、默认姿态、61D obs / 14D action / 13D cmd 完全匹配; + - 验证自定义策略与 `script` 导出清单、校验拒绝非法维度模型; + - 验证纯 JS STORE ZIP 生成器的魔数、CRC32 与文件列表完整性。 +2. **既有套件回归**:`npm test`、`npm run validate:packages`、`npm run format:check`、`npm run build`。 +3. **Python 单元测试与端到端联调**(`hardware/unoq/python/tests/`): + - 验证 61D 观测拼接向量顺序、数值与关节映射/标定转换正逆一致; + - 验证状态机完整生命周期(`IDLE` -> `SOFT_START` -> `WALK` -> `SITSTAND`/`KICK`/`GROUNDPICK`/`ROLL` -> `FALL_SAFE`/`RECOVERING` -> `IDLE`); + - 验证部署解包安全校验(路径穿越防护、缺失策略拒收、保留实机 `hardware_profile.json` 行为); + - 验证 `--hal mock` 下 `DeployService` + 控制器 + `MockHal` 50Hz 闭环真实运行;如环境中安装了 `onnxruntime` 也跑真实 ONNX 推理冒烟。 diff --git a/docs/xgoduck-campaign-20261118-onnx.md b/docs/xgoduck-campaign-20261118-onnx.md new file mode 100644 index 0000000..a3cbce8 --- /dev/null +++ b/docs/xgoduck-campaign-20261118-onnx.md @@ -0,0 +1,33 @@ +# XGO Duck ONNX 对比:campaign_20261118_175213 + +`runs` 含 12 组、24 个 ONNX;22 个不同 SHA-256。仅左脚踢球和平地坐立两组的 best.onnx 与同名导出文件一致,其余 10 组不同。末行是 runs 之外的平地行走,单列参考。 + +参考:原机 4 个加载模型;USB 部署包 SHA-256 `8e0ae6404747ea4d972299e8cd4a0420067b452bbd3dc24221d90b28f8120bcc`。使用同一段 50 帧静态实机反馈、原机 HOME、相同的零命令与初始动作历史,各模型随后使用自身上一帧输出,平滑系数 0.45。下表为 14 路平滑目标相对于同技能参考的 RMS / 最大绝对差(度)。不评估闭环,未部署;轮式没有匹配参考,仅检查推理。 + +|任务|技能|对原机 RMS / 最大 °|对现部署 RMS / 最大 °|SHA-256 前缀|相对路径| +|---|---|---:|---:|---|---| +|BallKick-Flat|kickR|—|76.8 / 175.2|68c4b718415a|runs/BallKick-Flat__1/BallKick-Flat__1.onnx| +|BallKick-Flat (best)|kickR|—|77.4 / 169.2|5ab0f49ca53e|runs/BallKick-Flat__1/best.onnx| +|BallKick-Left-Flat|kickL|—|184.1 / 607.3|44eeb1913085|runs/BallKick-Left-Flat__1/BallKick-Left-Flat__1.onnx| +|BallKick-Left-Flat (best)|kickL|—|184.1 / 607.3|44eeb1913085|runs/BallKick-Left-Flat__1/best.onnx| +|GroundPick-Flat|groundpick|103.5 / 192.5|41.7 / 82.1|f93066f66acb|runs/GroundPick-Flat__1/GroundPick-Flat__1.onnx| +|GroundPick-Flat (best)|groundpick|100.0 / 197.3|42.2 / 91.1|3143665a2909|runs/GroundPick-Flat__1/best.onnx| +|GroundPick-Rough|groundpick|87.1 / 190.6|53.9 / 122.9|7f11e32d6fae|runs/GroundPick-Rough__1/GroundPick-Rough__1.onnx| +|GroundPick-Rough (best)|groundpick|83.5 / 187.4|51.0 / 121.5|363e9896d6f6|runs/GroundPick-Rough__1/best.onnx| +|RollerStandUp-Flat|轮式专用|—|—|06947a260085|runs/RollerStandUp-Flat__1/RollerStandUp-Flat__1.onnx| +|RollerStandUp-Flat (best)|轮式专用|—|—|58e69c653793|runs/RollerStandUp-Flat__1/best.onnx| +|Roulade-Flat|roll|42.9 / 93.9|37.2 / 82.8|a1f7592120a4|runs/Roulade-Flat__1/Roulade-Flat__1.onnx| +|Roulade-Flat (best)|roll|46.2 / 99.5|37.4 / 77.3|1083c754b220|runs/Roulade-Flat__1/best.onnx| +|SitStand-Flat|sitstand|—|117.1 / 359.4|b0fc7852cfc1|runs/SitStand-Flat__1/SitStand-Flat__1.onnx| +|SitStand-Flat (best)|sitstand|—|117.1 / 359.4|b0fc7852cfc1|runs/SitStand-Flat__1/best.onnx| +|SitStand-Rough|sitstand|—|79.1 / 234.2|72a8c4128c83|runs/SitStand-Rough__1/SitStand-Rough__1.onnx| +|SitStand-Rough (best)|sitstand|—|85.7 / 222.6|2831ee268b13|runs/SitStand-Rough__1/best.onnx| +|StandUp-Flat|stand|90.9 / 281.1|83.3 / 278.9|9f26eff1455f|runs/StandUp-Flat__1/StandUp-Flat__1.onnx| +|StandUp-Flat (best)|stand|67.4 / 138.5|59.4 / 136.3|bb1e4c9d61d8|runs/StandUp-Flat__1/best.onnx| +|StandUp-Rough|stand|120.3 / 307.9|117.0 / 318.3|ad2f9460fc72|runs/StandUp-Rough__1/StandUp-Rough__1.onnx| +|StandUp-Rough (best)|stand|146.2 / 378.8|140.5 / 399.6|88a71b4201df|runs/StandUp-Rough__1/best.onnx| +|Velocity-Rough|walk|20.6 / 47.7|31.3 / 63.7|75f766c95088|runs/Velocity-Rough__1/Velocity-Rough__1.onnx| +|Velocity-Rough (best)|walk|22.6 / 55.5|29.1 / 63.1|117f8a3ac265|runs/Velocity-Rough__1/best.onnx| +|Velocity-Swizzle|轮式专用|—|—|b2e06f5b6b19|runs/Velocity-Swizzle__1/Velocity-Swizzle__1.onnx| +|Velocity-Swizzle (best)|轮式专用|—|—|6d8d9c9cfd4f|runs/Velocity-Swizzle__1/best.onnx| +|Velocity-Flat|walk|15.7 / 39.1|33.8 / 79.2|056b6b2f0561|flat/velocity-flat.onnx| diff --git a/hardware/unoq/README.md b/hardware/unoq/README.md new file mode 100644 index 0000000..eb0a81a --- /dev/null +++ b/hardware/unoq/README.md @@ -0,0 +1,54 @@ +# Microduck — Arduino UNO Q 实机控制与无硬件联调框架 + +该目录为 **Microduck** 在 **Arduino UNO Q**(QRB2210 Debian Linux MPU + STM32U585 Zephyr/Arduino MCU)上的完整板端控制架构,兼容 **Arduino App Lab** 目录规范。 + +## 目录结构 + +```text +hardware/unoq/ +├── app.yaml # Arduino App Lab 清单 +├── config/ +│ ├── robot_manifest.json # 与网页仿真对齐的策略/关节/观测/状态机配置(可由网站导入覆盖) +│ └── hardware_profile.json # 14 路飞特 HD-1910-C001 与 QMI8658 标定参数(在线修改持久化) +├── policies/ # ONNX 策略存放目录(由网站导出包或 HTTP 推送填充) +├── python/ +│ ├── main.py # 服务入口(支持 --hal mock 与 --hal router) +│ ├── core/ # 配置、61D 观测、50Hz 状态机、ONNX 推理、部署解包 +│ ├── hal/ # RouterBridgeHal (实机) & MockHal (PC 无硬件联调) +│ └── web/ # FastAPI / Dev Fallback HTTP+WebSocket 服务与离线手机控制页 +├── sketch/ # STM32U585 Arduino 固件 (QMI8658 + 飞特 HD-1910-C001 + Bridge) +└── scripts/ + ├── install_deps.sh # 板端 Python 依赖安装 + └── setup_ap.sh # 一键开启 UNO Q Wi-Fi 热点 (Microduck-AP -> 192.168.4.1) +``` + +## 1. 无硬件 PC 端预联调(立即运行) + +即使暂时未连接 Arduino UNO Q 硬件,也可以在电脑本地启动完整服务测试: + +```bash +cd hardware/unoq +python3 python/main.py --hal mock --policy-backend dummy --port 8000 +``` + +> 若环境已安装 `onnxruntime`、`fastapi`、`uvicorn`,可直接省略 `--policy-backend dummy`(默认 `auto` 会优先使用真实 `onnxruntime` 与 `FastAPI`)。 + +随后: +1. 在浏览器启动 Microduck 仿真网站(`cd app && npm run dev`),点击顶部工具栏 **Hardware (UNO Q)**,输入 `http://127.0.0.1:8000` 并点击 **推送到 UNO Q**,即可测试在线导入控制文件与热重载。 +2. 用浏览器打开 `http://127.0.0.1:8000`(或模拟手机视图),可测试虚拟摇杆、技能按钮、E-STOP 急停、准备站立软启动与舵机/IMU 标定页。 + +## 2. 上电连接 Arduino UNO Q 调试流程 + +1. **导出并拷入项目**:在仿真网站点击 **Hardware (UNO Q) -> 下载完整部署包 (.zip)**,解压后得到完整 `microduck_unoq/` 目录(已包含你选中的 `.onnx` 策略与配置),放入 UNO Q `/home/arduino/ArduinoApps/microduck_unoq/`(或通过 Arduino App Lab 打开)。 +2. **确认 STM32 引脚与总线宏**([sketch/config.h](sketch/config.h)): + - `SERVO_SERIAL`(默认 `Serial1`)、`SERVO_BAUD`(默认 `1000000`)、`SERVO_DIR_PIN`(`-1` 为硬件自动流向/单线,使用 74HC126 等方向引脚时填对应 GPIO)。 + - `IMU_WIRE`(`Wire` 或 UNO Q Qwiic 接口对应的 `Wire1`)、`IMU_I2C_ADDR`(`0x6A` 或 `0x6B`)。 +3. **烧录 STM32 并启动 App Lab 应用**: + - 使用 Arduino App Lab 或 `arduino-app-cli` 编译上传 `sketch/` 并启动 `python/main.py`(默认 `--hal router`)。 + - 如需开启 Wi-Fi 热点:在 UNO Q Linux 终端执行 `sudo bash scripts/setup_ap.sh`,手机连接 `Microduck-AP` 后访问 `http://192.168.4.1:8000`。 +4. **首次上机标定(务必先悬空/架起机身!)**: + - 打开手机页面 **标定调试** 页签: + - 点击 **扫描舵机 ID** 确认 14 个飞特 HD-1910-C001 全部在线; + - 点击 **IMU 静止零偏校准**,观察站立放置时 `projected_gravity` 接近 `[0, 0, -1]`;若不一致调整 `imu.axis_map`; + - 逐关节小幅测试并校准 `sign`(`+1`/`-1`)与 `zero_offset_ticks`(一键设为当前物理中位); + - 确认无误后回到 **控制** 页签,拉低 **速度比例滑条(如 30%)**,点击 **准备站立** 完成 1.5s 软插值,再开始步态测试。 diff --git a/hardware/unoq/app.yaml b/hardware/unoq/app.yaml new file mode 100644 index 0000000..c31f2fc --- /dev/null +++ b/hardware/unoq/app.yaml @@ -0,0 +1,6 @@ +name: microduck-unoq +description: Microduck RL policy runtime, mobile Wi-Fi AP controller, and STM32 HAL for Arduino UNO Q (QMI8658 + Feetech HD-1910-C001) +version: 0.1.0 +ports: + - 8000 +bricks: [] diff --git a/hardware/unoq/config/hardware_profile.json b/hardware/unoq/config/hardware_profile.json new file mode 100644 index 0000000..e5e9d8d --- /dev/null +++ b/hardware/unoq/config/hardware_profile.json @@ -0,0 +1,147 @@ +{ + "schema": 1, + "board": "arduino_uno_q", + "servo": { + "model": "FEETECH_HD-1910-C001", + "protocol": "hls_sts_half_duplex", + "baudrate": 1000000, + "ticks_per_rev": 4096, + "neutral_tick": 2048, + "speed_limit": 0, + "acc": 50, + "torque_limit": 1000, + "vel_source": "fd", + "vel_lpf_alpha": 0.4, + "vel_scale_rad_s_per_unit": 0.0015339807878856412 + }, + "imu": { + "model": "QMI8658", + "i2c_addr": "0x6A", + "axis_map": [ + [ + 1, + 0, + 0 + ], + [ + 0, + 1, + 0 + ], + [ + 0, + 0, + 1 + ] + ], + "gyro_bias_rad_s": [0.0, 0.0, 0.0], + "accel_bias_g": [0.0, 0.0, 0.0] + }, + "joints": { + "left_hip_yaw": { + "id": 1, + "sign": 1, + "zero_offset_ticks": 0, + "min_rad": -0.4363323129985824, + "max_rad": 0.5235987755982988 + }, + "left_hip_roll": { + "id": 2, + "sign": 1, + "zero_offset_ticks": 0, + "min_rad": -0.3839724354387525, + "max_rad": 0.3839724354387525 + }, + "left_hip_pitch": { + "id": 3, + "sign": 1, + "zero_offset_ticks": 0, + "min_rad": -1.570796326795005, + "max_rad": 1.5707963267947882 + }, + "left_knee": { + "id": 4, + "sign": 1, + "zero_offset_ticks": 0, + "min_rad": -1.570796326795012, + "max_rad": 1.570796326794781 + }, + "left_ankle": { + "id": 5, + "sign": 1, + "zero_offset_ticks": 0, + "min_rad": -1.5707963267949019, + "max_rad": 1.5707963267948912 + }, + "neck_pitch": { + "id": 6, + "sign": 1, + "zero_offset_ticks": 0, + "min_rad": -1.5707963267948966, + "max_rad": 1.0471975511965976 + }, + "head_pitch": { + "id": 7, + "sign": 1, + "zero_offset_ticks": 0, + "min_rad": -1.5707963267948974, + "max_rad": 1.5707963267948957 + }, + "head_yaw": { + "id": 8, + "sign": 1, + "zero_offset_ticks": 0, + "min_rad": -2.967059728390373, + "max_rad": 2.967059728390348 + }, + "head_roll": { + "id": 9, + "sign": 1, + "zero_offset_ticks": 0, + "min_rad": -0.43633231299859127, + "max_rad": 0.4363323129985735 + }, + "right_hip_yaw": { + "id": 10, + "sign": 1, + "zero_offset_ticks": 0, + "min_rad": -0.5235987755982988, + "max_rad": 0.4363323129985824 + }, + "right_hip_roll": { + "id": 11, + "sign": 1, + "zero_offset_ticks": 0, + "min_rad": -0.3839724354387525, + "max_rad": 0.3839724354387525 + }, + "right_hip_pitch": { + "id": 12, + "sign": 1, + "zero_offset_ticks": 0, + "min_rad": -1.5707963267949268, + "max_rad": 1.5707963267948664 + }, + "right_knee": { + "id": 13, + "sign": 1, + "zero_offset_ticks": 0, + "min_rad": -1.570796326794932, + "max_rad": 1.570796326794861 + }, + "right_ankle": { + "id": 14, + "sign": 1, + "zero_offset_ticks": 0, + "min_rad": -1.5707963267949054, + "max_rad": 1.5707963267948877 + } + }, + "safety": { + "soft_start_s": 1.5, + "watchdog_ms": 150, + "ws_timeout_ms": 500, + "auto_stand_on_fall": false, + "cmd_vel_scale": 0.5 + } +} diff --git a/hardware/unoq/config/robot_manifest.json b/hardware/unoq/config/robot_manifest.json new file mode 100644 index 0000000..180cad3 --- /dev/null +++ b/hardware/unoq/config/robot_manifest.json @@ -0,0 +1,127 @@ +{ + "schema": 1, + "robot_id": "microduck-legs", + "family": "microduck", + "sim": { + "timestep": 0.005, + "decimation": 4, + "ctrl_dt": 0.02, + "hz": 50 + }, + "obs_size": 61, + "action_size": 14, + "command_size": 13, + "joint_names": [ + "left_hip_yaw", + "left_hip_roll", + "left_hip_pitch", + "left_knee", + "left_ankle", + "neck_pitch", + "head_pitch", + "head_yaw", + "head_roll", + "right_hip_yaw", + "right_hip_roll", + "right_hip_pitch", + "right_knee", + "right_ankle" + ], + "default_pose": [ + 0.0, + -0.0872664600610733, + -0.4579240083694458, + -0.004939999897032976, + 0.4529840052127838, + 0.3490658402442932, + 0.3490658402442932, + 0.0, + 0.0, + 0.0, + 0.0872664600610733, + 0.4579240083694458, + 0.004939999897032976, + -0.4529840052127838 + ], + "action_scale": 1.0, + "variants": { + "legs": { + "limits": [0.25, -0.2, 1.0], + "enabled": true + }, + "rollers": { + "limits": [0.6, -0.5, 0.3], + "enabled": false + } + }, + "legacy": { + "crouch_period": 5.0, + "crouch_end": 0.7, + "groundpick_period": 4.0, + "groundpick_end": 0.7 + }, + "head": { + "max_rad": 2.5, + "alpha": 0.2, + "signs": [1, -1, 1, -1] + }, + "safety": { + "tilt_z": -0.5, + "upright_z": -0.85, + "fall_debounce_steps": 10, + "fall_settle_steps": 15, + "recover_upright_steps": 50, + "recover_giveup_steps": 300, + "kick_steps": 25, + "post_kick_lock_steps": 20, + "roll_max_steps": 150, + "sit_delay_s": 0.8, + "stand_delay_s": 2.0 + }, + "skills": { + "walk": { + "path": "policies/walk.onnx", + "kind": "perpetual", + "slot": "walk", + "action_scale": 1.0 + }, + "stand": { + "path": "policies/stand.onnx", + "kind": "recovery", + "slot": "stand", + "action_scale": 1.0 + }, + "sitstand": { + "path": "policies/sitstand.onnx", + "kind": "perpetual", + "slot": "sitstand", + "action_scale": 1.0 + }, + "kickL": { + "path": "policies/kickL.onnx", + "kind": "episodic", + "slot": "kickL", + "action_scale": 1.0 + }, + "kickR": { + "path": "policies/kickR.onnx", + "kind": "episodic", + "slot": "kickR", + "action_scale": 1.0 + }, + "groundpick": { + "path": "policies/groundpick.onnx", + "kind": "episodic", + "slot": "groundpick", + "action_scale": 1.0 + }, + "roll": { + "path": "policies/roll.onnx", + "kind": "episodic", + "slot": "roll", + "action_scale": 1.0, + "duration_s": 3.0 + } + }, + "custom_move": null +} diff --git a/hardware/unoq/python/__init__.py b/hardware/unoq/python/__init__.py new file mode 100644 index 0000000..67e72f9 --- /dev/null +++ b/hardware/unoq/python/__init__.py @@ -0,0 +1 @@ +"""Microduck Arduino UNO Q Python runtime package.""" diff --git a/hardware/unoq/python/core/__init__.py b/hardware/unoq/python/core/__init__.py new file mode 100644 index 0000000..d389b54 --- /dev/null +++ b/hardware/unoq/python/core/__init__.py @@ -0,0 +1 @@ +"""Core configuration, observation pipeline, ONNX runner, and control loop.""" diff --git a/hardware/unoq/python/core/config.py b/hardware/unoq/python/core/config.py new file mode 100644 index 0000000..9166d04 --- /dev/null +++ b/hardware/unoq/python/core/config.py @@ -0,0 +1,176 @@ +from __future__ import annotations + +import json +import math +from pathlib import Path +from typing import Any + +import numpy as np + + +TWO_PI = 2.0 * math.pi + + +def validate_robot_manifest(manifest: dict[str, Any]) -> dict[str, Any]: + if not isinstance(manifest, dict): + raise ValueError("robot_manifest must be a JSON object") + if manifest.get("obs_size") != 61: + raise ValueError(f"Unsupported obs_size {manifest.get('obs_size')}; expected 61") + if manifest.get("action_size") != 14: + raise ValueError(f"Unsupported action_size {manifest.get('action_size')}; expected 14") + if manifest.get("command_size") != 13: + raise ValueError(f"Unsupported command_size {manifest.get('command_size')}; expected 13") + + joint_names = manifest.get("joint_names") + default_pose = manifest.get("default_pose") + if not isinstance(joint_names, list) or len(joint_names) != 14: + raise ValueError("robot_manifest.joint_names must contain 14 joints") + if not isinstance(default_pose, list) or len(default_pose) != 14: + raise ValueError("robot_manifest.default_pose must contain 14 floats") + if not all(isinstance(v, (int, float)) and math.isfinite(float(v)) for v in default_pose): + raise ValueError("robot_manifest.default_pose contains non-finite value") + + skills = manifest.get("skills") + if not isinstance(skills, dict): + raise ValueError("robot_manifest.skills must be an object") + for required in ("walk", "stand"): + if required not in skills or not isinstance(skills[required], dict) or not skills[required].get("path"): + raise ValueError(f"robot_manifest.skills missing required '{required}' policy") + return manifest + + +def load_robot_manifest(path: str | Path) -> dict[str, Any]: + with Path(path).open("r", encoding="utf-8") as f: + data = json.load(f) + return validate_robot_manifest(data) + + +def validate_hardware_profile(profile: dict[str, Any], joint_names: list[str] | None = None) -> dict[str, Any]: + if not isinstance(profile, dict): + raise ValueError("hardware_profile must be a JSON object") + servo = profile.get("servo") + if not isinstance(servo, dict): + raise ValueError("hardware_profile.servo missing") + ticks_per_rev = int(servo.get("ticks_per_rev", 0)) + if ticks_per_rev <= 0: + raise ValueError("hardware_profile.servo.ticks_per_rev must be > 0") + + imu = profile.get("imu") + if not isinstance(imu, dict): + raise ValueError("hardware_profile.imu missing") + axis_map = np.asarray(imu.get("axis_map", []), dtype=np.float64) + if axis_map.shape != (3, 3) or not np.all(np.isfinite(axis_map)): + raise ValueError("hardware_profile.imu.axis_map must be a 3x3 finite matrix") + + joints = profile.get("joints") + if not isinstance(joints, dict): + raise ValueError("hardware_profile.joints missing") + expected = joint_names or list(joints.keys()) + if len(expected) != 14: + raise ValueError("hardware_profile.joints must configure all 14 joints") + seen_ids: set[int] = set() + for name in expected: + if name not in joints: + raise ValueError(f"hardware_profile.joints missing '{name}'") + cfg = joints[name] + jid = int(cfg["id"]) + if not (0 <= jid <= 253) or jid in seen_ids: + raise ValueError(f"Invalid or duplicate servo id {jid} for joint {name}") + seen_ids.add(jid) + if int(cfg["sign"]) not in (-1, 1): + raise ValueError(f"Joint {name} sign must be -1 or 1") + if float(cfg["min_rad"]) > float(cfg["max_rad"]): + raise ValueError(f"Joint {name} min_rad > max_rad") + return profile + + +def load_hardware_profile(path: str | Path, joint_names: list[str] | None = None) -> dict[str, Any]: + with Path(path).open("r", encoding="utf-8") as f: + data = json.load(f) + return validate_hardware_profile(data, joint_names=joint_names) + + +def save_hardware_profile(path: str | Path, profile: dict[str, Any], joint_names: list[str] | None = None) -> dict[str, Any]: + validated = validate_hardware_profile(profile, joint_names=joint_names) + target = Path(path) + target.parent.mkdir(parents=True, exist_ok=True) + tmp = target.with_suffix(".json.tmp") + with tmp.open("w", encoding="utf-8") as f: + json.dump(validated, f, indent=2, ensure_ascii=False) + f.write("\n") + tmp.replace(target) + return validated + + +def rad_to_ticks(q_rad: np.ndarray | list[float], joint_names: list[str], profile: dict[str, Any]) -> list[int]: + servo = profile["servo"] + ticks_per_rev = int(servo["ticks_per_rev"]) + neutral = int(servo["neutral_tick"]) + rad_per_tick = TWO_PI / ticks_per_rev + joints_cfg = profile["joints"] + max_tick = ticks_per_rev - 1 + + out: list[int] = [] + for idx, name in enumerate(joint_names): + cfg = joints_cfg[name] + angle = float(q_rad[idx]) + if not math.isfinite(angle): + raise ValueError(f"Non-finite angle for joint {name}") + angle = max(float(cfg["min_rad"]), min(float(cfg["max_rad"]), angle)) + tick = round(neutral + int(cfg["zero_offset_ticks"]) + int(cfg["sign"]) * (angle / rad_per_tick)) + tick = max(0, min(max_tick, tick)) + out.append(int(tick)) + return out + + +def ticks_to_rad(ticks: np.ndarray | list[int], joint_names: list[str], profile: dict[str, Any]) -> np.ndarray: + servo = profile["servo"] + ticks_per_rev = int(servo["ticks_per_rev"]) + neutral = int(servo["neutral_tick"]) + rad_per_tick = TWO_PI / ticks_per_rev + joints_cfg = profile["joints"] + + out = np.zeros(len(joint_names), dtype=np.float64) + for idx, name in enumerate(joint_names): + cfg = joints_cfg[name] + zero = neutral + int(cfg["zero_offset_ticks"]) + out[idx] = int(cfg["sign"]) * (float(ticks[idx]) - zero) * rad_per_tick + return out + + +def raw_vel_to_rad_s(raw_vel: list[int], joint_names: list[str], profile: dict[str, Any]) -> np.ndarray: + """Decode Feetech signed-magnitude speed ticks (bit 15 = negative) to rad/s.""" + servo = profile["servo"] + scale = float(servo.get("vel_scale_rad_s_per_unit", TWO_PI / int(servo["ticks_per_rev"]))) + joints_cfg = profile["joints"] + out = np.zeros(len(joint_names), dtype=np.float64) + for idx, name in enumerate(joint_names): + raw = int(raw_vel[idx]) + mag = raw & 0x7FFF + signed = -mag if (raw & 0x8000) else mag + out[idx] = int(joints_cfg[name]["sign"]) * signed * scale + return out + + +def apply_imu_calibration( + gyro_raw: np.ndarray | list[float], + grav_raw: np.ndarray | list[float], + profile: dict[str, Any], +) -> tuple[np.ndarray, np.ndarray]: + imu = profile["imu"] + axis_map = np.asarray(imu["axis_map"], dtype=np.float64) + gyro_bias = np.asarray(imu.get("gyro_bias_rad_s", [0.0, 0.0, 0.0]), dtype=np.float64) + + g_in = np.asarray(gyro_raw, dtype=np.float64) + p_in = np.asarray(grav_raw, dtype=np.float64) + if g_in.shape != (3,) or p_in.shape != (3,): + raise ValueError("IMU vectors must be 3D") + if not (np.all(np.isfinite(g_in)) and np.all(np.isfinite(p_in))): + raise ValueError("IMU readings contain non-finite values") + + gyro = axis_map @ (g_in - gyro_bias) + grav = axis_map @ p_in + norm = float(np.linalg.norm(grav)) + if norm > 1e-6: + grav = grav / norm + return gyro, grav diff --git a/hardware/unoq/python/core/controller.py b/hardware/unoq/python/core/controller.py new file mode 100644 index 0000000..da449e4 --- /dev/null +++ b/hardware/unoq/python/core/controller.py @@ -0,0 +1,380 @@ +from __future__ import annotations + +import threading +import time +from pathlib import Path +from typing import Any + +import numpy as np + +from hal.base import HardwareBridge, HardwareFrame + +from .config import ( + apply_imu_calibration, + rad_to_ticks, + raw_vel_to_rad_s, + save_hardware_profile, + ticks_to_rad, +) +from .observations import build_observation, decode_action +from .policy_runner import PolicySet +from .state_machine import LifecycleState, MicroduckStateMachine + + +class MicroduckController: + """50Hz real-time control loop coordinating HAL, 61D observations, ONNX policies, and safety.""" + + def __init__( + self, + base_dir: str | Path, + manifest: dict[str, Any], + profile: dict[str, Any], + hal: HardwareBridge, + policies: PolicySet, + custom_script: dict[str, Any] | None = None, + profile_path: str | Path | None = None, + ) -> None: + self.base_dir = Path(base_dir) + self.profile_path = Path(profile_path) if profile_path else self.base_dir / "config" / "hardware_profile.json" + self.manifest = manifest + self.profile = profile + self.hal = hal + self.policies = policies + + self.joint_names: list[str] = list(manifest["joint_names"]) + self.default_pose = np.asarray(manifest["default_pose"], dtype=np.float32) + self.ctrl_dt = float(manifest.get("sim", {}).get("ctrl_dt", 0.02)) + + self.state_machine = MicroduckStateMachine(manifest, custom_script=custom_script) + self._lock = threading.RLock() + self._running = False + self._thread: threading.Thread | None = None + + self.last_action = np.zeros(14, dtype=np.float32) + self._last_q_rad: np.ndarray | None = None + self._vel_lpf = np.zeros(14, dtype=np.float64) + self._last_remote_input_s: float = 0.0 + self._last_frame: HardwareFrame | None = None + self._last_gyro = np.zeros(3, dtype=np.float64) + self._last_grav = np.array([0.0, 0.0, -1.0], dtype=np.float64) + self._last_q = self.default_pose.astype(np.float64).copy() + self._last_targets = self.default_pose.astype(np.float64).copy() + self._ctrl_hz = 0.0 + self._last_error: str | None = None + self._calib_target_ticks: list[int] | None = None + self._calib_torque_on = False + + def connect(self) -> None: + with self._lock: + self.hal.connect() + self.hal.configure(self.joint_names, self.profile) + self._last_frame = self.hal.sync_step( + enable_torque=False, + target_ticks=rad_to_ticks(self.default_pose, self.joint_names, self.profile), + ) + self._last_gyro, self._last_grav = apply_imu_calibration( + self._last_frame.gyro_raw, self._last_frame.proj_gravity_raw, self.profile + ) + self._last_q = ticks_to_rad(self._last_frame.ticks, self.joint_names, self.profile) + + def start(self) -> None: + if self._running: + return + self.connect() + self._running = True + self._thread = threading.Thread(target=self._loop, name="microduck-50hz", daemon=True) + self._thread.start() + + def stop(self) -> None: + self._running = False + if self._thread is not None: + self._thread.join(timeout=2.0) + self._thread = None + with self._lock: + try: + self.state_machine.emergency_stop() + self.hal.set_torque(False) + finally: + self.hal.close() + + def _loop(self) -> None: + next_t = time.monotonic() + hz_t0 = next_t + ticks = 0 + while self._running: + now = time.monotonic() + try: + self.step_once(dt=self.ctrl_dt, now_s=now) + self._last_error = None + except Exception as exc: + self._last_error = str(exc) + with self._lock: + self.state_machine.emergency_stop() + try: + self.hal.set_torque(False) + except Exception: + pass + ticks += 1 + if now - hz_t0 >= 0.5: + self._ctrl_hz = ticks / max(now - hz_t0, 1e-6) + ticks = 0 + hz_t0 = now + next_t += self.ctrl_dt + sleep_s = next_t - time.monotonic() + if sleep_s > 0: + time.sleep(sleep_s) + else: + next_t = time.monotonic() + + def set_remote_input( + self, + norm_x: float, + norm_wz: float, + speed_scale: float | None = None, + now_s: float | None = None, + ) -> None: + with self._lock: + scale = ( + float(speed_scale) + if speed_scale is not None + else float(self.profile.get("safety", {}).get("cmd_vel_scale", 0.5)) + ) + self.state_machine.set_velocity_cmd(norm_x, norm_wz, speed_scale=scale) + self._last_remote_input_s = time.monotonic() if now_s is None else float(now_s) + + def set_head_target(self, neck_pitch: float, head_pitch: float, head_yaw: float, head_roll: float) -> None: + with self._lock: + self.state_machine.set_head_target(neck_pitch, head_pitch, head_yaw, head_roll) + + def set_auto_stand_on_fall(self, enabled: bool) -> None: + with self._lock: + self.profile.setdefault("safety", {})["auto_stand_on_fall"] = bool(enabled) + save_hardware_profile(self.profile_path, self.profile, self.joint_names) + + def set_speed_scale(self, scale: float) -> None: + with self._lock: + clamped = max(0.1, min(1.0, float(scale))) + self.profile.setdefault("safety", {})["cmd_vel_scale"] = clamped + save_hardware_profile(self.profile_path, self.profile, self.joint_names) + + def request_stand(self) -> None: + with self._lock: + if "walk" not in self.policies.policies: + raise RuntimeError("No control policies loaded yet — deploy a bundle from the website first") + self._calib_torque_on = False + self._calib_target_ticks = None + soft_start_s = float(self.profile.get("safety", {}).get("soft_start_s", 1.5)) + self.state_machine.request_stand(self._last_q, soft_start_s=soft_start_s) + + def emergency_stop(self) -> None: + with self._lock: + self._calib_torque_on = False + self._calib_target_ticks = None + self.state_machine.emergency_stop() + self.last_action.fill(0) + self.hal.set_torque(False) + + def trigger_action(self, action: str) -> bool: + with self._lock: + return self.state_machine.trigger_action(action) + + def reload_runtime( + self, + manifest: dict[str, Any], + policies: PolicySet, + profile: dict[str, Any] | None = None, + custom_script: dict[str, Any] | None = None, + ) -> None: + with self._lock: + self.state_machine.emergency_stop() + self.hal.set_torque(False) + self.manifest = manifest + self.policies = policies + self.joint_names = list(manifest["joint_names"]) + self.default_pose = np.asarray(manifest["default_pose"], dtype=np.float32) + self.ctrl_dt = float(manifest.get("sim", {}).get("ctrl_dt", 0.02)) + self.state_machine.update_manifest(manifest, custom_script=custom_script) + if profile is not None: + self.profile = profile + self.hal.configure(self.joint_names, self.profile) + self.last_action.fill(0) + self._last_error = None + + def update_profile(self, profile: dict[str, Any]) -> dict[str, Any]: + with self._lock: + self.profile = save_hardware_profile(self.profile_path, profile, self.joint_names) + self.hal.configure(self.joint_names, self.profile) + return self.profile + + def capture_joint_zero(self, joint_name: str) -> dict[str, Any]: + with self._lock: + if joint_name not in self.joint_names: + raise ValueError(f"Unknown joint '{joint_name}'") + idx = self.joint_names.index(joint_name) + if self._last_frame is None: + raise RuntimeError("No hardware frame captured yet") + neutral = int(self.profile["servo"]["neutral_tick"]) + current_tick = int(self._last_frame.ticks[idx]) + self.profile["joints"][joint_name]["zero_offset_ticks"] = current_tick - neutral + return self.update_profile(self.profile) + + def test_single_joint(self, joint_name: str, target_rad: float) -> list[int]: + with self._lock: + if joint_name not in self.joint_names: + raise ValueError(f"Unknown joint '{joint_name}'") + if self.state_machine.lifecycle not in (LifecycleState.IDLE, LifecycleState.CALIBRATION): + raise RuntimeError("Emergency stop first before testing individual joints") + self.state_machine.enter_calibration() + idx = self.joint_names.index(joint_name) + pose = self._last_q.copy() + pose[idx] = float(target_rad) + ticks = rad_to_ticks(pose, self.joint_names, self.profile) + self._calib_target_ticks = ticks + self._calib_torque_on = True + return ticks + + def exit_calibration(self) -> None: + with self._lock: + self._calib_torque_on = False + self._calib_target_ticks = None + self.state_machine.exit_calibration() + self.hal.set_torque(False) + + def calibrate_imu_zero(self, samples: int = 200) -> dict[str, Any]: + with self._lock: + bias = self.hal.calibrate_imu_raw(samples=samples) + self.profile["imu"]["gyro_bias_rad_s"] = [float(b) for b in bias] + self.update_profile(self.profile) + return self.profile["imu"] + + def ping_servos(self, max_id: int = 20) -> list[int]: + with self._lock: + return self.hal.ping_servos(max_id=max_id) + + def _compute_joint_vel(self, q_rad: np.ndarray, raw_speeds: list[int], dt: float) -> np.ndarray: + servo_cfg = self.profile["servo"] + if servo_cfg.get("vel_source") == "servo": + return raw_vel_to_rad_s(raw_speeds, self.joint_names, self.profile) + if self._last_q_rad is None: + self._last_q_rad = q_rad.copy() + self._vel_lpf.fill(0.0) + return self._vel_lpf.copy() + fd = (q_rad - self._last_q_rad) / max(dt, 1e-4) + self._last_q_rad = q_rad.copy() + alpha = float(servo_cfg.get("vel_lpf_alpha", 0.4)) + self._vel_lpf = (1.0 - alpha) * self._vel_lpf + alpha * fd + return self._vel_lpf.copy() + + def step_once(self, dt: float | None = None, now_s: float | None = None) -> dict[str, Any]: + with self._lock: + dt_s = self.ctrl_dt if dt is None else float(dt) + now = time.monotonic() if now_s is None else float(now_s) + + # Enforce mobile WS heartbeat timeout while walking + ws_timeout_s = float(self.profile.get("safety", {}).get("ws_timeout_ms", 500)) / 1000.0 + if self._last_remote_input_s > 0.0 and (now - self._last_remote_input_s) > ws_timeout_s: + self.state_machine.set_velocity_cmd(0.0, 0.0) + + servo_cfg = self.profile["servo"] + speed_limit = int(servo_cfg.get("speed_limit", 0)) + acc = int(servo_cfg.get("acc", 50)) + + # Determine command to send for THIS cycle from current state + previous sensor state + auto_stand = bool(self.profile.get("safety", {}).get("auto_stand_on_fall", False)) + if self.state_machine.lifecycle == LifecycleState.CALIBRATION: + enable_torque = self._calib_torque_on + cmd_ticks = ( + list(self._calib_target_ticks) + if self._calib_target_ticks is not None + else rad_to_ticks(self._last_q, self.joint_names, self.profile) + ) + decision = self.state_machine.step(dt_s, float(self._last_grav[2]), auto_stand) + else: + decision = self.state_machine.step(dt_s, float(self._last_grav[2]), auto_stand) + if decision.zero_last_action: + self.last_action.fill(0) + + if decision.lifecycle == LifecycleState.SOFT_START: + q_interp = ( + 1.0 - decision.soft_start_alpha + ) * self.state_machine.soft_start_q0 + decision.soft_start_alpha * self.default_pose + self._last_targets = q_interp + cmd_ticks = rad_to_ticks(q_interp, self.joint_names, self.profile) + enable_torque = True + elif decision.lifecycle == LifecycleState.ACTIVE and decision.policy_key is not None: + policy = self.policies.policies.get(decision.policy_key) or self.policies.policies["walk"] + q_vel = self._vel_lpf + obs = build_observation( + self._last_gyro, + self._last_grav, + self._last_q, + q_vel, + self.last_action, + decision.command_13, + self.default_pose, + ) + raw_act = policy.infer(obs) + self.last_action = raw_act + targets = decode_action(raw_act, self.default_pose, policy.action_scale) + self._last_targets = targets.astype(np.float64) + cmd_ticks = rad_to_ticks(targets, self.joint_names, self.profile) + enable_torque = True + elif decision.lifecycle == LifecycleState.ACTIVE and decision.recovery_state == "fallen": + cmd_ticks = rad_to_ticks(self._last_targets, self.joint_names, self.profile) + enable_torque = True + else: + cmd_ticks = rad_to_ticks(self._last_q, self.joint_names, self.profile) + enable_torque = False + + frame = self.hal.sync_step( + enable_torque=enable_torque, + target_ticks=cmd_ticks, + speed_limit=speed_limit, + acc=acc, + ) + self._last_frame = frame + self._last_gyro, self._last_grav = apply_imu_calibration( + frame.gyro_raw, frame.proj_gravity_raw, self.profile + ) + self._last_q = ticks_to_rad(frame.ticks, self.joint_names, self.profile) + self._compute_joint_vel(self._last_q, frame.raw_speeds, dt_s) + + return self._build_status_unlocked(decision, enable_torque) + + def _build_status_unlocked(self, decision: Any | None = None, torque_enabled: bool | None = None) -> dict[str, Any]: + frame = self._last_frame + life = self.state_machine.lifecycle.value + active_policy = decision.policy_key if decision else None + if torque_enabled is None: + torque_enabled = life in (LifecycleState.SOFT_START.value, LifecycleState.ACTIVE.value) or self._calib_torque_on + return { + "hal": self.hal.name, + "policy_backend": self.policies.backend, + "lifecycle": life, + "mode_label": decision.mode_label if decision else self.state_machine._mode_label(), + "skill_mode": self.state_machine.skill_mode, + "active_policy": active_policy, + "recovery_state": self.state_machine.recovery_state, + "loco": self.state_machine.loco, + "rollers_enabled": self.state_machine.rollers_enabled, + "head_mode": self.state_machine.head_mode, + "torque_enabled": bool(torque_enabled), + "auto_stand_on_fall": bool(self.profile.get("safety", {}).get("auto_stand_on_fall", False)), + "speed_scale": float(self.profile.get("safety", {}).get("cmd_vel_scale", 0.5)), + "ctrl_hz": round(self._ctrl_hz, 1), + "voltage_v": round(frame.voltage_v, 2) if frame else 0.0, + "max_temp_c": round(frame.max_temp_c, 1) if frame else 0.0, + "imu_ok": frame.imu_ok if frame else False, + "servo_err_mask": frame.servo_err_mask if frame else 0, + "gyro": [round(float(x), 4) for x in self._last_gyro], + "projected_gravity": [round(float(x), 4) for x in self._last_grav], + "ticks": list(frame.ticks) if frame else [2048] * 14, + "joint_pos_rad": [round(float(x), 4) for x in self._last_q], + "loaded_skills": sorted(self.policies.policies.keys()), + "custom_move": self.manifest.get("custom_move"), + "error": self._last_error, + } + + def get_status(self) -> dict[str, Any]: + with self._lock: + return self._build_status_unlocked() diff --git a/hardware/unoq/python/core/deploy_service.py b/hardware/unoq/python/core/deploy_service.py new file mode 100644 index 0000000..57cb932 --- /dev/null +++ b/hardware/unoq/python/core/deploy_service.py @@ -0,0 +1,252 @@ +from __future__ import annotations + +import io +import json +import shutil +import stat +import tempfile +import threading +import time +import zipfile +from pathlib import Path, PurePosixPath +from typing import Any + +from .config import save_hardware_profile, validate_hardware_profile, validate_robot_manifest +from .controller import MicroduckController +from .policy_runner import PolicySet + + +MAX_ZIP_FILES = 512 +MAX_ZIP_UNCOMPRESSED_BYTES = 128 * 1024 * 1024 + + +def _safe_rel_path(member_name: str) -> PurePosixPath: + if not member_name or "\\" in member_name: + raise ValueError(f"Unsafe zip member path: {member_name!r}") + if member_name.startswith("/") or ":" in member_name: + raise ValueError(f"Absolute or drive path forbidden in zip: {member_name!r}") + parts = PurePosixPath(member_name).parts + if not parts or any(part in ("..", ".", "") for part in parts): + raise ValueError(f"Path traversal forbidden in zip: {member_name!r}") + return PurePosixPath(*parts) + + +class DeployService: + """Atomic, validated hot-reload deployment of Microduck manifest + ONNX policies.""" + + def __init__( + self, + base_dir: str | Path, + controller: MicroduckController, + policy_backend: str = "auto", + ) -> None: + self.base_dir = Path(base_dir) + self.controller = controller + self.policy_backend = policy_backend + self._lock = threading.Lock() + self.last_deploy: dict[str, Any] | None = self._read_last_deploy() + + def _read_last_deploy(self) -> dict[str, Any] | None: + meta_path = self.base_dir / "config" / ".last_deploy.json" + if not meta_path.is_file(): + return None + try: + with meta_path.open("r", encoding="utf-8") as f: + return json.load(f) + except Exception: + return None + + def _extract_bundle_to_stage(self, raw_bytes: bytes, stage_dir: Path) -> None: + if not raw_bytes: + raise ValueError("Empty deployment bundle") + try: + zf = zipfile.ZipFile(io.BytesIO(raw_bytes), "r") + except zipfile.BadZipFile as exc: + raise ValueError("Deployment payload is not a valid ZIP archive") from exc + + with zf: + infos = zf.infolist() + if len(infos) > MAX_ZIP_FILES: + raise ValueError(f"Too many files in deployment zip ({len(infos)} > {MAX_ZIP_FILES})") + + total_size = 0 + entries: list[tuple[zipfile.ZipInfo, PurePosixPath]] = [] + for info in infos: + if info.is_dir(): + if info.filename.rstrip("/"): + _safe_rel_path(info.filename.rstrip("/")) + continue + mode = (info.external_attr >> 16) & 0o170000 + if mode == stat.S_IFLNK: + raise ValueError(f"Symlinks are forbidden in deployment zip: {info.filename!r}") + total_size += max(0, int(info.file_size)) + if total_size > MAX_ZIP_UNCOMPRESSED_BYTES: + raise ValueError("Deployment zip exceeds maximum uncompressed size (128 MB)") + entries.append((info, _safe_rel_path(info.filename))) + + if not entries: + raise ValueError("Deployment zip contains no files") + + top_dirs = {p.parts[0] for _, p in entries if len(p.parts) > 1} + has_root_manifest = any( + str(p) in ("config/robot_manifest.json", "robot_manifest.json") for _, p in entries + ) + strip_prefix: str | None = None + if not has_root_manifest and len(top_dirs) == 1: + only_top = next(iter(top_dirs)) + if any( + str(p) in (f"{only_top}/config/robot_manifest.json", f"{only_top}/robot_manifest.json") + for _, p in entries + ): + strip_prefix = only_top + + for info, rel in entries: + if strip_prefix and rel.parts[0] == strip_prefix: + if len(rel.parts) == 1: + continue + rel = PurePosixPath(*rel.parts[1:]) + + dest = (stage_dir / Path(*rel.parts)).resolve() + if not dest.is_relative_to(stage_dir.resolve()): + raise ValueError(f"Escaping path rejected: {info.filename!r}") + dest.parent.mkdir(parents=True, exist_ok=True) + with zf.open(info, "r") as src, dest.open("wb") as dst: + shutil.copyfileobj(src, dst) + + def apply_zip_bytes(self, raw_bytes: bytes, overwrite_profile: bool = False) -> dict[str, Any]: + with self._lock, tempfile.TemporaryDirectory(prefix="microduck_deploy_") as tmp: + stage = Path(tmp) / "stage" + backup = Path(tmp) / "backup" + stage.mkdir(parents=True, exist_ok=True) + backup.mkdir(parents=True, exist_ok=True) + + self._extract_bundle_to_stage(raw_bytes, stage) + + manifest_src = stage / "config" / "robot_manifest.json" + if not manifest_src.is_file(): + alt = stage / "robot_manifest.json" + if alt.is_file(): + manifest_src = alt + else: + raise ValueError("Deployment zip missing config/robot_manifest.json") + + try: + manifest = validate_robot_manifest(json.loads(manifest_src.read_text(encoding="utf-8"))) + except json.JSONDecodeError as exc: + raise ValueError(f"Invalid robot_manifest.json: {exc}") from exc + + # Normalize into stage/config & verify every referenced ONNX exists & is non-empty + (stage / "config").mkdir(parents=True, exist_ok=True) + (stage / "config" / "robot_manifest.json").write_text( + json.dumps(manifest, indent=2) + "\n", encoding="utf-8" + ) + + stage_policies: dict[str, Path] = {} + for key, spec in manifest["skills"].items(): + rel = spec.get("path") + if not rel: + continue + rel_posix = _safe_rel_path(str(rel)) + pol_file = (stage / Path(*rel_posix.parts)).resolve() + if not pol_file.is_relative_to(stage.resolve()) or not pol_file.is_file(): + raise ValueError(f"Deployment zip missing policy '{key}' at '{rel}'") + if pol_file.stat().st_size == 0: + raise ValueError(f"Policy '{key}' at '{rel}' is empty") + stage_policies[key] = pol_file + + # Optional custom move script + custom_script: dict[str, Any] | None = None + script_src = stage / "config" / "custom_move_script.json" + if not script_src.is_file(): + alt_script = stage / "custom_move_script.json" + if alt_script.is_file(): + script_src = alt_script + if script_src.is_file(): + try: + custom_script = json.loads(script_src.read_text(encoding="utf-8")) + except json.JSONDecodeError as exc: + raise ValueError(f"Invalid custom_move_script.json: {exc}") from exc + if not isinstance(custom_script, dict) or not isinstance(custom_script.get("keyframes"), list): + raise ValueError("custom_move_script.json must contain a 'keyframes' list") + elif isinstance(manifest.get("custom_move"), dict) and manifest["custom_move"].get("slot") == "script": + raise ValueError("Manifest references custom_move script, but config/custom_move_script.json is missing") + + # Optional hardware profile + target_profile_path = self.base_dir / "config" / "hardware_profile.json" + incoming_profile: dict[str, Any] | None = None + profile_src = stage / "config" / "hardware_profile.json" + if not profile_src.is_file(): + alt_prof = stage / "hardware_profile.json" + if alt_prof.is_file(): + profile_src = alt_prof + if (overwrite_profile or not target_profile_path.is_file()) and profile_src.is_file(): + try: + incoming_profile = validate_hardware_profile( + json.loads(profile_src.read_text(encoding="utf-8")), + joint_names=manifest["joint_names"], + ) + except json.JSONDecodeError as exc: + raise ValueError(f"Invalid hardware_profile.json: {exc}") from exc + + # Pre-validate ONNX graph & metadata inside stage before touching live files + PolicySet.load_from_manifest(stage, manifest, backend=self.policy_backend) + + # Backup existing config + policies for atomic rollback + live_config = self.base_dir / "config" + live_policies = self.base_dir / "policies" + live_config.mkdir(parents=True, exist_ok=True) + live_policies.mkdir(parents=True, exist_ok=True) + for sub in ("config", "policies"): + src_dir = self.base_dir / sub + if src_dir.is_dir(): + shutil.copytree(src_dir, backup / sub, dirs_exist_ok=True) + + try: + shutil.copy2(stage / "config" / "robot_manifest.json", live_config / "robot_manifest.json") + + target_script = live_config / "custom_move_script.json" + if custom_script is not None: + target_script.write_text(json.dumps(custom_script, indent=2) + "\n", encoding="utf-8") + elif target_script.is_file(): + target_script.unlink() + + for _, src_pol in stage_policies.items(): + rel_to_stage = src_pol.relative_to(stage) + dst_pol = self.base_dir / rel_to_stage + dst_pol.parent.mkdir(parents=True, exist_ok=True) + shutil.copy2(src_pol, dst_pol) + + if incoming_profile is not None: + save_hardware_profile(target_profile_path, incoming_profile, joint_names=manifest["joint_names"]) + + live_policies_set = PolicySet.load_from_manifest( + self.base_dir, manifest, backend=self.policy_backend + ) + self.controller.reload_runtime( + manifest=manifest, + policies=live_policies_set, + profile=incoming_profile, + custom_script=custom_script, + ) + except Exception: + # Roll back live config + policies on reload failure + for sub in ("config", "policies"): + bak_dir = backup / sub + live_dir = self.base_dir / sub + if bak_dir.is_dir(): + shutil.copytree(bak_dir, live_dir, dirs_exist_ok=True) + raise + + deploy_meta = { + "ok": True, + "deployed_at_s": round(time.time(), 3), + "robot_id": manifest.get("robot_id", "microduck"), + "loaded_skills": sorted(live_policies_set.policies.keys()), + "profile_overwritten": incoming_profile is not None, + "custom_move": manifest.get("custom_move"), + } + (live_config / ".last_deploy.json").write_text( + json.dumps(deploy_meta, indent=2) + "\n", encoding="utf-8" + ) + self.last_deploy = deploy_meta + return deploy_meta diff --git a/hardware/unoq/python/core/observations.py b/hardware/unoq/python/core/observations.py new file mode 100644 index 0000000..ff207c2 --- /dev/null +++ b/hardware/unoq/python/core/observations.py @@ -0,0 +1,85 @@ +from __future__ import annotations + +from typing import Any + +import numpy as np + + +def build_observation( + gyro: np.ndarray, + projected_gravity: np.ndarray, + joint_pos_rad: np.ndarray, + joint_vel_rad_s: np.ndarray, + last_action: np.ndarray, + command_13: np.ndarray, + default_pose: np.ndarray, +) -> np.ndarray: + """Assemble the exact 61D float32 observation expected by Microduck ONNX policies. + + Layout: + [0:3] base_ang_vel (gyro, rad/s) + [3:6] projected_gravity (trunk_base frame, unit vector pointing down: [0,0,-1] upright) + [6:20] joint_pos_rel (joint_pos_rad - default_pose) + [20:34] joint_vel (rad/s) + [34:48] last_action (previous raw actor output) + [48:61] command (13D: [vx, vy, wz, head(4), body(6)]) + """ + obs = np.empty(61, dtype=np.float32) + obs[0:3] = np.asarray(gyro, dtype=np.float32) + obs[3:6] = np.asarray(projected_gravity, dtype=np.float32) + obs[6:20] = np.asarray(joint_pos_rad, dtype=np.float32) - np.asarray(default_pose, dtype=np.float32) + obs[20:34] = np.asarray(joint_vel_rad_s, dtype=np.float32) + obs[34:48] = np.asarray(last_action, dtype=np.float32) + obs[48:61] = np.asarray(command_13, dtype=np.float32) + + if not np.all(np.isfinite(obs)): + raise ValueError("Observation vector contains non-finite values") + return obs + + +def decode_action( + raw_action: np.ndarray, + default_pose: np.ndarray, + action_scale: float = 1.0, +) -> np.ndarray: + act = np.asarray(raw_action, dtype=np.float32) + if act.shape != (14,) or not np.all(np.isfinite(act)): + raise ValueError("raw_action must be a finite 14D vector") + return np.asarray(default_pose, dtype=np.float32) + np.float32(action_scale) * act + + +def script_command_at(script: dict[str, Any], t_s: float, cmd_size: int = 13) -> np.ndarray: + """Linearly interpolate a custom move_script.json timeline at time t_s.""" + keyframes = script.get("keyframes", []) + out = np.zeros(cmd_size, dtype=np.float32) + if not keyframes: + return out + + duration_s = float(script.get("durationS") or script.get("duration_s") or 0.0) + loop = script.get("loop", True) is not False + tt = float(t_s) + if loop and duration_s > 0.0: + tt = ((tt % duration_s) + duration_s) % duration_s + + def _pad(raw_cmd: list[float]) -> np.ndarray: + arr = np.zeros(cmd_size, dtype=np.float32) + n = min(cmd_size, len(raw_cmd)) + if n > 0: + arr[:n] = np.asarray(raw_cmd[:n], dtype=np.float32) + return arr + + if tt <= float(keyframes[0]["t"]): + return _pad(keyframes[0]["cmd"]) + + for i in range(len(keyframes) - 1): + a = keyframes[i] + b = keyframes[i + 1] + ta = float(a["t"]) + tb = float(b["t"]) + if ta <= tt <= tb: + u = (tt - ta) / max(tb - ta, 1e-6) + ca = _pad(a["cmd"]) + cb = _pad(b["cmd"]) + return (1.0 - u) * ca + u * cb + + return _pad(keyframes[-1]["cmd"]) diff --git a/hardware/unoq/python/core/policy_runner.py b/hardware/unoq/python/core/policy_runner.py new file mode 100644 index 0000000..0bf6813 --- /dev/null +++ b/hardware/unoq/python/core/policy_runner.py @@ -0,0 +1,123 @@ +from __future__ import annotations + +from dataclasses import dataclass +from pathlib import Path +from typing import Any + +import numpy as np + + +@dataclass +class LoadedPolicy: + key: str + action_scale: float + backend: str + _session: Any | None = None + _input_name: str = "obs" + _output_name: str = "actions" + + def infer(self, obs_61: np.ndarray) -> np.ndarray: + if obs_61.shape != (61,): + raise ValueError(f"Expected 61D observation, got {obs_61.shape}") + if self.backend == "dummy": + # Deterministic zero-action dummy policy for PC dry-run without onnxruntime. + return np.zeros(14, dtype=np.float32) + + inp = np.asarray(obs_61, dtype=np.float32).reshape(1, 61) + out = self._session.run([self._output_name], {self._input_name: inp})[0] + act = np.asarray(out, dtype=np.float32).reshape(14) + if not np.all(np.isfinite(act)): + raise ValueError(f"Policy '{self.key}' returned non-finite action") + return act + + +class PolicySet: + def __init__(self, policies: dict[str, LoadedPolicy], backend: str) -> None: + self.policies = policies + self.backend = backend + + @classmethod + def resolve_backend(cls, requested: str) -> str: + if requested in ("onnx", "dummy"): + return requested + if requested != "auto": + raise ValueError(f"Unknown policy backend: {requested}") + try: + import onnxruntime # noqa: F401 + + return "onnx" + except ImportError as exc: + raise RuntimeError( + "onnxruntime is not installed. Run 'bash scripts/install_deps.sh' on UNO Q, " + "or pass '--policy-backend dummy' for pre-hardware UI/protocol dry-run." + ) from exc + + @classmethod + def load_from_manifest( + cls, + base_dir: str | Path, + manifest: dict[str, Any], + backend: str = "auto", + ) -> "PolicySet": + resolved_backend = cls.resolve_backend(backend) + ort = None + if resolved_backend == "onnx": + try: + import onnxruntime as ort_mod + + ort = ort_mod + except ImportError as exc: + raise RuntimeError("onnxruntime is required for backend='onnx'") from exc + + root = Path(base_dir) + default_scale = float(manifest.get("action_scale", 1.0)) + skills = manifest.get("skills", {}) + loaded: dict[str, LoadedPolicy] = {} + + for key, spec in skills.items(): + rel_path = spec.get("path") + if not rel_path: + continue + scale = float(spec.get("action_scale", default_scale)) + if resolved_backend == "dummy": + loaded[key] = LoadedPolicy(key=key, action_scale=scale, backend="dummy") + continue + + model_path = (root / rel_path).resolve() + if not model_path.is_file(): + raise FileNotFoundError(f"Missing ONNX policy for '{key}': {model_path}") + + sess_opts = ort.SessionOptions() + sess_opts.intra_op_num_threads = 2 + sess = ort.InferenceSession( + str(model_path), + sess_options=sess_opts, + providers=["CPUExecutionProvider"], + ) + inps = sess.get_inputs() + outs = sess.get_outputs() + if len(inps) != 1 or len(outs) != 1: + raise ValueError(f"Policy '{key}' must have 1 input / 1 output") + + # Smoke validation mirroring custom-policy.js + smoke_obs = np.zeros((1, 61), dtype=np.float32) + smoke_out = sess.run([outs[0].name], {inps[0].name: smoke_obs})[0] + if tuple(np.asarray(smoke_out).shape) != (1, 14): + raise ValueError( + f"Policy '{key}' output shape {np.asarray(smoke_out).shape} != (1, 14)" + ) + if not np.all(np.isfinite(smoke_out)): + raise ValueError(f"Policy '{key}' smoke output is non-finite") + + loaded[key] = LoadedPolicy( + key=key, + action_scale=scale, + backend="onnx", + _session=sess, + _input_name=inps[0].name, + _output_name=outs[0].name, + ) + + if "walk" not in loaded or "stand" not in loaded: + raise ValueError("PolicySet must load at least 'walk' and 'stand'") + return cls(policies=loaded, backend=resolved_backend) diff --git a/hardware/unoq/python/core/state_machine.py b/hardware/unoq/python/core/state_machine.py new file mode 100644 index 0000000..a096db3 --- /dev/null +++ b/hardware/unoq/python/core/state_machine.py @@ -0,0 +1,561 @@ +from __future__ import annotations + +import math +from dataclasses import dataclass +from enum import Enum +from typing import Any + +import numpy as np + +from .observations import script_command_at + + +class LifecycleState(str, Enum): + IDLE = "IDLE" + SOFT_START = "SOFT_START" + ACTIVE = "ACTIVE" + CALIBRATION = "CALIBRATION" + FALL_SAFE = "FALL_SAFE" + + +@dataclass +class StepDecision: + lifecycle: LifecycleState + skill_mode: str + policy_key: str | None + torque_enabled: bool + soft_start_alpha: float + command_13: np.ndarray + zero_last_action: bool + mode_label: str + recovery_state: str | None + loco: str + head_mode: bool + + +class MicroduckStateMachine: + """Hardware-safe 50Hz state machine mirroring app/src/robots/microduck/runtime.js.""" + + def __init__(self, manifest: dict[str, Any], custom_script: dict[str, Any] | None = None) -> None: + self.manifest = manifest + self.custom_script = custom_script + self._load_params(manifest) + + self.lifecycle: LifecycleState = LifecycleState.IDLE + self.loco: str = "legs" # "legs" | "rollers" + self.skill_mode: str = "walk" # "walk"|"sitstand"|"roll"|"kickL"|"kickR"|"crouch"|"groundpick" + self.sit_flag: int = 0 + self.head_mode: bool = False + + self.vel_cmd = np.zeros(3, dtype=np.float32) # [vx, 0, wz] + self.head_target = np.zeros(4, dtype=np.float32) + self.head_smooth = np.zeros(4, dtype=np.float32) + + self.soft_start_q0 = np.zeros(14, dtype=np.float64) + self.soft_start_elapsed = 0.0 + self.soft_start_duration = 1.5 + + self.sit_delay_rem: float | None = None + self.stand_delay_rem: float | None = None + self.kick_steps: int | None = None + self.post_kick_lock: int = 0 + self.crouch_phase: float | None = None + self.pick_phase: float | None = None + self.roll_steps: int | None = None + self.roll_tipped: bool = False + self.script_t: float | None = None + + self.fall_debounce: int = 0 + self.recovery_state: str | None = None # None | "fallen" | "recovering" + self.recovery_steps: int = 0 + self.recovery_upright_steps: int = 0 + + def _load_params(self, manifest: dict[str, Any]) -> None: + safety = manifest.get("safety", {}) + head = manifest.get("head", {}) + legacy = manifest.get("legacy", {}) + variants = manifest.get("variants", {}) + + self.tilt_z = float(safety.get("tilt_z", -0.5)) + self.upright_z = float(safety.get("upright_z", -0.85)) + self.fall_debounce_steps = int(safety.get("fall_debounce_steps", 10)) + self.fall_settle_steps = int(safety.get("fall_settle_steps", 15)) + self.recover_upright_steps_req = int(safety.get("recover_upright_steps", 50)) + self.recover_giveup_steps = int(safety.get("recover_giveup_steps", 300)) + self.kick_total_steps = int(safety.get("kick_steps", 25)) + self.post_kick_lock_total = int(safety.get("post_kick_lock_steps", 20)) + self.roll_max_steps = int(safety.get("roll_max_steps", 150)) + self.sit_delay_s = float(safety.get("sit_delay_s", 0.8)) + self.stand_delay_s = float(safety.get("stand_delay_s", 2.0)) + + self.head_max = float(head.get("max_rad", 2.5)) + self.head_alpha = float(head.get("alpha", 0.2)) + self.head_signs = np.asarray(head.get("signs", [1, -1, 1, -1]), dtype=np.float32) + + self.crouch_period_s = float(legacy.get("crouch_period", 5.0)) + self.crouch_end_phase = float(legacy.get("crouch_end", 0.7)) + self.groundpick_period_s = float(legacy.get("groundpick_period", 4.0)) + self.groundpick_end_phase = float(legacy.get("groundpick_end", 0.7)) + + self.legs_limits = list(variants.get("legs", {}).get("limits", [0.25, -0.2, 1.0])) + self.rollers_limits = list(variants.get("rollers", {}).get("limits", [0.6, -0.5, 0.3])) + self.rollers_enabled = bool(variants.get("rollers", {}).get("enabled", False)) + + custom_move = manifest.get("custom_move") + self.custom_slot = custom_move.get("slot") if isinstance(custom_move, dict) else None + roll_skill = manifest.get("skills", {}).get("roll", {}) + ctrl_dt = float(manifest.get("sim", {}).get("ctrl_dt", 0.02)) + if self.custom_slot == "trick" and roll_skill.get("duration_s"): + self.roll_max_steps = max(10, round(float(roll_skill["duration_s"]) / ctrl_dt)) + + def update_manifest(self, manifest: dict[str, Any], custom_script: dict[str, Any] | None = None) -> None: + self.manifest = manifest + self.custom_script = custom_script + self._load_params(manifest) + if not self.rollers_enabled and self.loco == "rollers": + self.loco = "legs" + + def _clear_skill_runs(self) -> None: + self.sit_delay_rem = None + self.stand_delay_rem = None + self.kick_steps = None + self.post_kick_lock = 0 + self.crouch_phase = None + self.pick_phase = None + self.roll_steps = None + self.roll_tipped = False + self.script_t = None + self.fall_debounce = 0 + self.recovery_state = None + self.recovery_steps = 0 + self.recovery_upright_steps = 0 + + def emergency_stop(self) -> None: + self._clear_skill_runs() + self.skill_mode = "walk" + self.sit_flag = 0 + self.head_mode = False + self.vel_cmd.fill(0) + self.head_target.fill(0) + self.head_smooth.fill(0) + self.lifecycle = LifecycleState.IDLE + + def enter_calibration(self) -> None: + self.emergency_stop() + self.lifecycle = LifecycleState.CALIBRATION + + def exit_calibration(self) -> None: + if self.lifecycle == LifecycleState.CALIBRATION: + self.lifecycle = LifecycleState.IDLE + + def request_stand(self, current_q_rad: np.ndarray, soft_start_s: float = 1.5) -> None: + self._clear_skill_runs() + self.skill_mode = "walk" + self.sit_flag = 0 + self.head_mode = False + self.vel_cmd.fill(0) + self.head_target.fill(0) + self.head_smooth.fill(0) + self.soft_start_q0 = np.asarray(current_q_rad, dtype=np.float64).copy() + self.soft_start_elapsed = 0.0 + self.soft_start_duration = max(0.02, float(soft_start_s)) + self.lifecycle = LifecycleState.SOFT_START + + def set_velocity_cmd(self, norm_x: float, norm_wz: float, speed_scale: float = 1.0) -> None: + lim_f, lim_b, lim_a = self.rollers_limits if self.loco == "rollers" else self.legs_limits + nx = max(-1.0, min(1.0, float(norm_x))) + nwz = max(-1.0, min(1.0, float(norm_wz))) + scale = max(0.05, min(1.0, float(speed_scale))) + vx = (nx * lim_f if nx >= 0.0 else nx * (-lim_b)) * scale + wz = nwz * lim_a * scale + self.vel_cmd[0] = vx + self.vel_cmd[1] = 0.0 + self.vel_cmd[2] = wz + + def set_head_target(self, neck_pitch: float, head_pitch: float, head_yaw: float, head_roll: float) -> None: + raw = np.array([neck_pitch, head_pitch, head_yaw, head_roll], dtype=np.float32) + raw = np.clip(raw, -1.0, 1.0) + self.head_target = self.head_signs * raw * self.head_max + + def _is_one_shot_busy(self) -> bool: + return ( + (self.skill_mode == "roll" and self.roll_steps is not None) + or (self.skill_mode in ("kickL", "kickR") and self.kick_steps is not None) + or (self.skill_mode == "crouch" and self.crouch_phase is not None) + or (self.skill_mode == "groundpick" and self.pick_phase is not None) + ) + + def trigger_action(self, action: str) -> bool: + if self.lifecycle != LifecycleState.ACTIVE or self.recovery_state is not None: + return False + + if action == "headToggle": + if self.head_mode: + self.head_mode = False + return True + if ( + self.skill_mode in ("walk", "sitstand") + and self.post_kick_lock == 0 + and self.stand_delay_rem is None + ): + self.head_mode = True + return True + return False + + if action == "locoToggle": + if not self.rollers_enabled: + return False + if self._is_one_shot_busy() or self.stand_delay_rem is not None: + return False + self._clear_skill_runs() + self.skill_mode = "walk" + self.sit_flag = 0 + self.head_mode = False + self.loco = "rollers" if self.loco == "legs" else "legs" + return True + + if action == "sitToggle": + if self.loco != "legs": + return self.trigger_action("crouch") + if self.custom_slot == "trick": + return self.trigger_action("roll") + if self.custom_slot == "script" and self.custom_script is not None: + return self.trigger_action("scriptToggle") + if self._is_one_shot_busy(): + return False + self.head_mode = False + if self.skill_mode == "sitstand" and self.sit_flag == 1: + self.sit_flag = 0 + self.sit_delay_rem = None + self.stand_delay_rem = self.stand_delay_s + return True + if self.skill_mode == "walk" and self.stand_delay_rem is None: + self.skill_mode = "sitstand" + self.sit_flag = 0 + self.sit_delay_rem = self.sit_delay_s + return True + return False + + if action == "scriptToggle": + if self.custom_script is None or self.loco != "legs": + return False + if self.script_t is not None: + self.script_t = None + return True + if self.skill_mode == "walk" and self.stand_delay_rem is None: + self.head_mode = False + self.script_t = 0.0 + return True + return False + + if action in ("kickL", "kickR"): + if self.loco != "legs" or self.skill_mode != "walk" or self.stand_delay_rem is not None: + return False + self.head_mode = False + self.sit_delay_rem = None + self.skill_mode = action + self.sit_flag = 0 + self.kick_steps = 0 + return True + + if action == "groundPick": + if self.loco != "legs" or self.skill_mode != "walk" or self.stand_delay_rem is not None: + return False + self.head_mode = False + self.sit_delay_rem = None + self.skill_mode = "groundpick" + self.sit_flag = 0 + self.pick_phase = 0.0 + return True + + if action == "roll": + if self.loco == "rollers": + return self.trigger_action("crouch") + if self.skill_mode != "walk" or self.stand_delay_rem is not None: + return False + self.head_mode = False + self.sit_delay_rem = None + self.skill_mode = "roll" + self.sit_flag = 0 + self.roll_steps = 0 + self.roll_tipped = False + return True + + if action == "crouch": + if self.loco != "rollers" or self.skill_mode != "walk": + return False + self.head_mode = False + self.skill_mode = "crouch" + self.crouch_phase = 0.0 + return True + + return False + + def _active_policy_key(self) -> str | None: + if self.recovery_state == "fallen": + return None + if self.recovery_state == "recovering": + return "stand" + if self.loco == "rollers" and self.skill_mode == "walk": + return "drive" + return self.skill_mode + + def _mode_label(self) -> str: + if self.lifecycle == LifecycleState.IDLE: + return "Idle (Torque Off)" + if self.lifecycle == LifecycleState.CALIBRATION: + return "Calibration" + if self.lifecycle == LifecycleState.FALL_SAFE: + return "Fall Safe" + if self.lifecycle == LifecycleState.SOFT_START: + return "Soft Start" + if self.recovery_state: + return "Recovery" + if self.skill_mode == "roll": + return "Trick" if self.custom_slot == "trick" else "Roll" + if self.script_t is not None: + return "Script" + if self.skill_mode == "crouch": + return "Crouch" + if self.skill_mode == "groundpick": + return "Pick" + if self.skill_mode in ("kickL", "kickR"): + return "Kick" + if self.head_mode: + return "Head" + if self.skill_mode == "sitstand" and self.sit_flag == 1: + return "Sit" + return "Drive" if self.loco == "rollers" else "Run" + + def _build_command_13(self) -> np.ndarray: + cmd = np.zeros(13, dtype=np.float32) + if self.skill_mode == "sitstand": + cmd[0] = float(self.sit_flag) + elif self.skill_mode == "crouch" and self.crouch_phase is not None: + a = 2.0 * math.pi * self.crouch_phase + cmd[0] = math.cos(a) + cmd[1] = math.sin(a) + elif self.skill_mode == "groundpick" and self.pick_phase is not None: + a = 2.0 * math.pi * self.pick_phase + cmd[0] = math.cos(a) + cmd[1] = math.sin(a) + elif not ( + self.head_mode + or self.skill_mode == "roll" + or self.skill_mode in ("kickL", "kickR") + or self.post_kick_lock > 0 + or self.recovery_state is not None + ): + cmd[0:3] = self.vel_cmd + + for h in range(4): + self.head_smooth[h] += self.head_alpha * (self.head_target[h] - self.head_smooth[h]) + + gp_zero = self.skill_mode == "groundpick" or self.recovery_state is not None + if not gp_zero: + cmd[3:7] = self.head_smooth + + if ( + self.script_t is not None + and self.skill_mode == "walk" + and self.recovery_state is None + and self.custom_script is not None + ): + cmd = script_command_at(self.custom_script, self.script_t, 13) + return cmd + + def step( + self, + dt: float, + proj_grav_z: float, + auto_stand_on_fall: bool = False, + ) -> StepDecision: + zero_last_action = False + + if self.lifecycle in (LifecycleState.IDLE, LifecycleState.CALIBRATION, LifecycleState.FALL_SAFE): + return StepDecision( + lifecycle=self.lifecycle, + skill_mode=self.skill_mode, + policy_key=None, + torque_enabled=False, + soft_start_alpha=0.0, + command_13=np.zeros(13, dtype=np.float32), + zero_last_action=False, + mode_label=self._mode_label(), + recovery_state=self.recovery_state, + loco=self.loco, + head_mode=self.head_mode, + ) + + if self.lifecycle == LifecycleState.SOFT_START: + self.soft_start_elapsed += dt + u = min(1.0, self.soft_start_elapsed / self.soft_start_duration) + alpha = 0.5 * (1.0 - math.cos(math.pi * u)) + if u < 1.0 - 1e-6: + return StepDecision( + lifecycle=self.lifecycle, + skill_mode="soft_start", + policy_key=None, + torque_enabled=True, + soft_start_alpha=alpha, + command_13=np.zeros(13, dtype=np.float32), + zero_last_action=True, + mode_label=self._mode_label(), + recovery_state=None, + loco=self.loco, + head_mode=False, + ) + self.lifecycle = LifecycleState.ACTIVE + self.skill_mode = "walk" + zero_last_action = True + + # Active lifecycle: advance timers before fall/recovery and command build + if self.sit_delay_rem is not None: + self.sit_delay_rem -= dt + if self.sit_delay_rem <= 0.0: + self.sit_delay_rem = None + if self.skill_mode == "sitstand": + self.sit_flag = 1 + + if self.stand_delay_rem is not None: + self.stand_delay_rem -= dt + if self.stand_delay_rem <= 0.0: + self.stand_delay_rem = None + self.skill_mode = "walk" + zero_last_action = True + + # Fall detection & recovery state machine + fallen_now = (not math.isfinite(proj_grav_z)) or (proj_grav_z > self.tilt_z) + if self.recovery_state == "fallen": + self.recovery_steps += 1 + if self.recovery_steps >= self.fall_settle_steps: + self.recovery_state = "recovering" + self.recovery_steps = 0 + self.recovery_upright_steps = 0 + zero_last_action = True + elif self.recovery_state == "recovering": + self.recovery_steps += 1 + if proj_grav_z < self.upright_z: + self.recovery_upright_steps += 1 + else: + self.recovery_upright_steps = 0 + if self.recovery_upright_steps >= self.recover_upright_steps_req: + self.recovery_state = None + self.recovery_steps = 0 + self.recovery_upright_steps = 0 + self.skill_mode = "walk" + zero_last_action = True + elif self.recovery_steps >= self.recover_giveup_steps: + self._clear_skill_runs() + self.lifecycle = LifecycleState.FALL_SAFE + return StepDecision( + lifecycle=self.lifecycle, + skill_mode="walk", + policy_key=None, + torque_enabled=False, + soft_start_alpha=0.0, + command_13=np.zeros(13, dtype=np.float32), + zero_last_action=True, + mode_label=self._mode_label(), + recovery_state=None, + loco=self.loco, + head_mode=False, + ) + elif fallen_now: + recoverable = ( + auto_stand_on_fall + and self.loco == "legs" + and self.skill_mode == "walk" + and self.post_kick_lock == 0 + and self.stand_delay_rem is None + ) + self.fall_debounce += 1 + if self.fall_debounce >= self.fall_debounce_steps: + self.fall_debounce = 0 + self.head_mode = False + if recoverable: + self.recovery_state = "fallen" + self.recovery_steps = 0 + else: + self._clear_skill_runs() + self.lifecycle = LifecycleState.FALL_SAFE + return StepDecision( + lifecycle=self.lifecycle, + skill_mode="walk", + policy_key=None, + torque_enabled=False, + soft_start_alpha=0.0, + command_13=np.zeros(13, dtype=np.float32), + zero_last_action=True, + mode_label=self._mode_label(), + recovery_state=None, + loco=self.loco, + head_mode=False, + ) + else: + self.fall_debounce = 0 + + policy_key = self._active_policy_key() + command_13 = self._build_command_13() + + # Advance step-counted skills AFTER choosing this tick's policy/command + if self.post_kick_lock > 0 and self.skill_mode == "walk": + self.post_kick_lock -= 1 + + if self.skill_mode in ("kickL", "kickR") and self.kick_steps is not None: + self.kick_steps += 1 + if self.kick_steps >= self.kick_total_steps: + self.kick_steps = None + self.skill_mode = "walk" + self.post_kick_lock = self.post_kick_lock_total + + if self.skill_mode == "crouch" and self.crouch_phase is not None: + self.crouch_phase += dt / self.crouch_period_s + if self.crouch_phase >= self.crouch_end_phase: + self.crouch_phase = None + self.skill_mode = "walk" + + if self.skill_mode == "groundpick" and self.pick_phase is not None: + self.pick_phase += dt / self.groundpick_period_s + if self.pick_phase >= self.groundpick_end_phase: + self.pick_phase = None + self.skill_mode = "walk" + + if self.skill_mode == "roll" and self.roll_steps is not None: + self.roll_steps += 1 + if proj_grav_z > -0.3: + self.roll_tipped = True + upright = proj_grav_z < self.upright_z + done = self.roll_tipped and upright and self.roll_steps >= 40 + expired = self.roll_steps >= self.roll_max_steps + if done or expired: + self.roll_steps = None + self.roll_tipped = False + self.skill_mode = "walk" + zero_last_action = True + if not upright: + self.lifecycle = LifecycleState.FALL_SAFE + + if self.script_t is not None: + self.script_t += dt + if self.custom_script is None: + self.script_t = None + else: + dur = float(self.custom_script.get("durationS") or self.custom_script.get("duration_s") or 0.0) + loop = self.custom_script.get("loop", True) is not False + if not loop and self.script_t >= dur: + self.script_t = None + + return StepDecision( + lifecycle=self.lifecycle, + skill_mode=self.skill_mode, + policy_key=policy_key, + torque_enabled=self.lifecycle == LifecycleState.ACTIVE, + soft_start_alpha=1.0, + command_13=command_13, + zero_last_action=zero_last_action, + mode_label=self._mode_label(), + recovery_state=self.recovery_state, + loco=self.loco, + head_mode=self.head_mode, + ) diff --git a/hardware/unoq/python/hal/__init__.py b/hardware/unoq/python/hal/__init__.py new file mode 100644 index 0000000..5904c35 --- /dev/null +++ b/hardware/unoq/python/hal/__init__.py @@ -0,0 +1 @@ +"""Hardware abstraction layers for Microduck on Arduino UNO Q.""" diff --git a/hardware/unoq/python/hal/base.py b/hardware/unoq/python/hal/base.py new file mode 100644 index 0000000..234b0ca --- /dev/null +++ b/hardware/unoq/python/hal/base.py @@ -0,0 +1,57 @@ +from __future__ import annotations + +from abc import ABC, abstractmethod +from dataclasses import dataclass, field +from typing import Any + + +@dataclass +class HardwareFrame: + gyro_raw: list[float] + proj_gravity_raw: list[float] + ticks: list[int] + raw_speeds: list[int] + voltage_v: float = 7.4 + max_temp_c: float = 35.0 + imu_ok: bool = True + servo_err_mask: int = 0 + timestamp_s: float = 0.0 + extra: dict[str, Any] = field(default_factory=dict) + + +class HardwareBridge(ABC): + name: str = "abstract" + + @abstractmethod + def connect(self) -> None: + """Initialize connection to the MCU / servo bus.""" + + @abstractmethod + def close(self) -> None: + """Release hardware resources safely.""" + + @abstractmethod + def configure(self, joint_names: list[str], profile: dict[str, Any]) -> None: + """Push servo IDs, I2C address, watchdog, and IMU bias config to HAL.""" + + @abstractmethod + def sync_step( + self, + enable_torque: bool, + target_ticks: list[int], + speed_limit: int = 0, + acc: int = 50, + ) -> HardwareFrame: + """Send target joint ticks and read fresh IMU + servo feedback.""" + + @abstractmethod + def set_torque(self, enable: bool) -> None: + """Immediately enable or disable torque on all configured servos.""" + + @abstractmethod + def ping_servos(self, max_id: int = 20) -> list[int]: + """Scan and return responding servo IDs.""" + + @abstractmethod + def calibrate_imu_raw(self, samples: int = 200) -> list[float]: + """Collect stationary gyro readings (rad/s in sensor frame) and return mean bias.""" diff --git a/hardware/unoq/python/hal/mock_hal.py b/hardware/unoq/python/hal/mock_hal.py new file mode 100644 index 0000000..3841e3f --- /dev/null +++ b/hardware/unoq/python/hal/mock_hal.py @@ -0,0 +1,82 @@ +from __future__ import annotations + +import math +import time +from typing import Any + +from .base import HardwareBridge, HardwareFrame + + +class MockHal(HardwareBridge): + """PC-runnable hardware simulator for pre-hardware verification.""" + + name = "mock" + + def __init__(self, initial_tick: int = 2048) -> None: + self.connected = False + self.torque_enabled = False + self.servo_ids = list(range(1, 15)) + self.ticks = [int(initial_tick)] * 14 + self.raw_speeds = [0] * 14 + self._gyro = [0.0, 0.0, 0.0] + self._gravity = [0.0, 0.0, -1.0] + self._alpha = 0.45 + + def connect(self) -> None: + self.connected = True + + def close(self) -> None: + self.torque_enabled = False + self.connected = False + + def configure(self, joint_names: list[str], profile: dict[str, Any]) -> None: + joints = profile.get("joints", {}) + self.servo_ids = [int(joints[n]["id"]) for n in joint_names] + + def set_mock_gravity(self, grav: list[float]) -> None: + gx, gy, gz = float(grav[0]), float(grav[1]), float(grav[2]) + norm = math.sqrt(gx * gx + gy * gy + gz * gz) or 1.0 + self._gravity = [gx / norm, gy / norm, gz / norm] + + def set_mock_gyro(self, gyro: list[float]) -> None: + self._gyro = [float(gyro[0]), float(gyro[1]), float(gyro[2])] + + def sync_step( + self, + enable_torque: bool, + target_ticks: list[int], + speed_limit: int = 0, + acc: int = 50, + ) -> HardwareFrame: + self.torque_enabled = bool(enable_torque) + if self.torque_enabled: + for i in range(14): + prev = self.ticks[i] + nxt = int(round(prev + self._alpha * (int(target_ticks[i]) - prev))) + delta = nxt - prev + self.ticks[i] = max(0, min(4095, nxt)) + mag = min(0x7FFF, abs(delta) * 50) + self.raw_speeds[i] = (mag | 0x8000) if delta < 0 else mag + else: + self.raw_speeds = [0] * 14 + + return HardwareFrame( + gyro_raw=list(self._gyro), + proj_gravity_raw=list(self._gravity), + ticks=list(self.ticks), + raw_speeds=list(self.raw_speeds), + voltage_v=7.8, + max_temp_c=34.0, + imu_ok=True, + servo_err_mask=0, + timestamp_s=time.monotonic(), + ) + + def set_torque(self, enable: bool) -> None: + self.torque_enabled = bool(enable) + + def ping_servos(self, max_id: int = 20) -> list[int]: + return [sid for sid in self.servo_ids if 0 <= sid <= max_id] + + def calibrate_imu_raw(self, samples: int = 200) -> list[float]: + return list(self._gyro) diff --git a/hardware/unoq/python/hal/router_hal.py b/hardware/unoq/python/hal/router_hal.py new file mode 100644 index 0000000..794dc9a --- /dev/null +++ b/hardware/unoq/python/hal/router_hal.py @@ -0,0 +1,109 @@ +from __future__ import annotations + +import time +from typing import Any + +from .base import HardwareBridge, HardwareFrame + + +class RouterBridgeHal(HardwareBridge): + """Arduino UNO Q HAL communicating with STM32U585 over Arduino_RouterBridge.""" + + name = "router" + + def __init__(self) -> None: + self._bridge = None + self._servo_ids = list(range(1, 15)) + + def connect(self) -> None: + try: + from arduino.app_utils import Bridge # type: ignore + except ImportError as exc: + raise RuntimeError( + "arduino.app_utils.Bridge is not available. Run on Arduino UNO Q via App Lab / " + "arduino-app-cli, or pass '--hal mock' for PC pre-hardware debugging." + ) from exc + self._bridge = Bridge + + def close(self) -> None: + if self._bridge is not None: + try: + self.set_torque(False) + except Exception: + pass + self._bridge = None + + def _require_bridge(self) -> Any: + if self._bridge is None: + raise RuntimeError("RouterBridgeHal is not connected") + return self._bridge + + def configure(self, joint_names: list[str], profile: dict[str, Any]) -> None: + bridge = self._require_bridge() + joints = profile["joints"] + servo = profile["servo"] + imu = profile["imu"] + safety = profile.get("safety", {}) + self._servo_ids = [int(joints[name]["id"]) for name in joint_names] + + addr_raw = imu.get("i2c_addr", "0x6A") + i2c_addr = int(addr_raw, 16) if isinstance(addr_raw, str) else int(addr_raw) + bias = [float(x) for x in imu.get("gyro_bias_rad_s", [0.0, 0.0, 0.0])] + payload: list[float] = [ + float(servo.get("baudrate", 1000000)), + float(i2c_addr), + float(safety.get("watchdog_ms", 150)), + bias[0], + bias[1], + bias[2], + *[float(sid) for sid in self._servo_ids], + ] + bridge.call("hw_configure", payload) + + def sync_step( + self, + enable_torque: bool, + target_ticks: list[int], + speed_limit: int = 0, + acc: int = 50, + ) -> HardwareFrame: + bridge = self._require_bridge() + if len(target_ticks) != 14: + raise ValueError("Expected 14 target_ticks") + cmd_frame = [ + 1 if enable_torque else 0, + int(speed_limit), + int(acc), + *[int(t) for t in target_ticks], + ] + res = bridge.call("hw_sync_step", cmd_frame) + if not isinstance(res, (list, tuple)) or len(res) < 38: + raise RuntimeError(f"Invalid hw_sync_step response from MCU: {res!r}") + + return HardwareFrame( + gyro_raw=[float(res[0]), float(res[1]), float(res[2])], + proj_gravity_raw=[float(res[3]), float(res[4]), float(res[5])], + ticks=[int(round(res[6 + i])) for i in range(14)], + raw_speeds=[int(round(res[20 + i])) for i in range(14)], + voltage_v=float(res[34]), + max_temp_c=float(res[35]), + imu_ok=bool(int(round(res[36]))), + servo_err_mask=int(round(res[37])), + timestamp_s=time.monotonic(), + ) + + def set_torque(self, enable: bool) -> None: + bridge = self._require_bridge() + bridge.call("hw_set_torque", 1 if enable else 0) + + def ping_servos(self, max_id: int = 20) -> list[int]: + bridge = self._require_bridge() + res = bridge.call("hw_ping_servos", int(max_id)) + return [int(x) for x in res] if isinstance(res, (list, tuple)) else [] + + def calibrate_imu_raw(self, samples: int = 200) -> list[float]: + bridge = self._require_bridge() + res = bridge.call("hw_calibrate_imu", int(samples)) + if not isinstance(res, (list, tuple)) or len(res) != 3: + raise RuntimeError(f"Invalid hw_calibrate_imu response: {res!r}") + return [float(res[0]), float(res[1]), float(res[2])] diff --git a/hardware/unoq/python/main.py b/hardware/unoq/python/main.py new file mode 100644 index 0000000..ae0eccd --- /dev/null +++ b/hardware/unoq/python/main.py @@ -0,0 +1,125 @@ +from __future__ import annotations + +import argparse +import os +import signal +import sys +from pathlib import Path + +PYTHON_ROOT = Path(__file__).resolve().parent +UNOQ_ROOT = PYTHON_ROOT.parent +if str(PYTHON_ROOT) not in sys.path: + sys.path.insert(0, str(PYTHON_ROOT)) + +from core.config import load_hardware_profile, load_robot_manifest # noqa: E402 +from core.controller import MicroduckController # noqa: E402 +from core.deploy_service import DeployService # noqa: E402 +from core.policy_runner import PolicySet # noqa: E402 +from hal.base import HardwareBridge # noqa: E402 +from hal.mock_hal import MockHal # noqa: E402 +from hal.router_hal import RouterBridgeHal # noqa: E402 +from web.fallback_server import FallbackWebServer # noqa: E402 + + +def _select_hal(hal_mode: str) -> HardwareBridge: + if hal_mode == "mock": + return MockHal() + if hal_mode == "router": + return RouterBridgeHal() + # auto: prefer RouterBridge when running on UNO Q App Lab, otherwise ask explicitly for --hal mock + try: + from arduino.app_utils import Bridge # type: ignore # noqa: F401 + + return RouterBridgeHal() + except ImportError as exc: + raise RuntimeError( + "arduino.app_utils is not available in this Python environment. " + "Launch inside UNO Q App Lab, or pass '--hal mock' for PC pre-hardware testing." + ) from exc + + +def main(argv: list[str] | None = None) -> int: + parser = argparse.ArgumentParser(description="Microduck UNO Q 50Hz Controller & Deploy Server") + parser.add_argument("--base-dir", type=Path, default=UNOQ_ROOT, help="UNO Q config/policies directory") + parser.add_argument( + "--hal", + choices=("auto", "router", "mock"), + default=os.environ.get("MICRODUCK_HAL", "auto"), + help="Hardware backend: 'router' (UNO Q STM32), 'mock' (PC simulator), or 'auto'", + ) + parser.add_argument( + "--policy-backend", + choices=("auto", "onnx", "dummy"), + default=os.environ.get("MICRODUCK_POLICY_BACKEND", "auto"), + help="Inference backend: 'onnx' / 'auto', or explicit 'dummy' dry-run", + ) + parser.add_argument("--host", default="0.0.0.0") + parser.add_argument("--port", type=int, default=8000) + args = parser.parse_args(argv) + + base_dir = Path(args.base_dir).resolve() + manifest = load_robot_manifest(base_dir / "config" / "robot_manifest.json") + profile = load_hardware_profile(base_dir / "config" / "hardware_profile.json", joint_names=manifest["joint_names"]) + + custom_script = None + script_path = base_dir / "config" / "custom_move_script.json" + if script_path.is_file(): + import json + + custom_script = json.loads(script_path.read_text(encoding="utf-8")) + + hal = _select_hal(args.hal) + + boot_error: str | None = None + try: + policies = PolicySet.load_from_manifest(base_dir, manifest, backend=args.policy_backend) + except FileNotFoundError as exc: + # Allow web server & /api/deploy to boot on a fresh board before first push from website + boot_error = f"Awaiting first policy deploy: {exc}" + policies = PolicySet({}, backend="awaiting_deploy") + + controller = MicroduckController( + base_dir=base_dir, + manifest=manifest, + profile=profile, + hal=hal, + policies=policies, + custom_script=custom_script, + ) + if boot_error: + controller._last_error = boot_error + + deploy_backend = args.policy_backend + deploy_service = DeployService(base_dir=base_dir, controller=controller, policy_backend=deploy_backend) + + controller.start() + + def _shutdown(*_args: object) -> None: + raise SystemExit(0) + + signal.signal(signal.SIGTERM, _shutdown) + + srv: FallbackWebServer | None = None + try: + try: + import uvicorn # type: ignore + + from web.server import create_fastapi_app + + app = create_fastapi_app(controller, deploy_service) + uvicorn.run(app, host=args.host, port=args.port, log_level="info") + except ImportError: + srv = FallbackWebServer(args.host, args.port, controller, deploy_service) + print(f"[microduck-unoq] FastAPI/uvicorn not installed; serving stdlib HTTP/WS on {args.host}:{srv.port}") + srv.serve_forever() + except (KeyboardInterrupt, SystemExit): + pass + finally: + if srv is not None: + srv.server_close() + controller.stop() + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/hardware/unoq/python/requirements.txt b/hardware/unoq/python/requirements.txt new file mode 100644 index 0000000..200314a --- /dev/null +++ b/hardware/unoq/python/requirements.txt @@ -0,0 +1,5 @@ +numpy>=1.26.0 +onnxruntime>=1.18.0 +fastapi>=0.115.0 +uvicorn[standard]>=0.30.0 +python-multipart>=0.0.9 diff --git a/hardware/unoq/python/tests/test_controller.py b/hardware/unoq/python/tests/test_controller.py new file mode 100644 index 0000000..e247c0f --- /dev/null +++ b/hardware/unoq/python/tests/test_controller.py @@ -0,0 +1,134 @@ +import sys +from pathlib import Path + +import numpy as np +import pytest + +PYTHON_ROOT = Path(__file__).resolve().parents[1] +UNOQ_ROOT = PYTHON_ROOT.parent +if str(PYTHON_ROOT) not in sys.path: + sys.path.insert(0, str(PYTHON_ROOT)) + +from core.config import load_hardware_profile, load_robot_manifest # noqa: E402 +from core.controller import MicroduckController # noqa: E402 +from core.policy_runner import PolicySet # noqa: E402 +from core.state_machine import LifecycleState # noqa: E402 +from hal.mock_hal import MockHal # noqa: E402 + + +def _make_controller(tmp_path: Path, auto_stand_on_fall: bool = False): + manifest = load_robot_manifest(UNOQ_ROOT / "config" / "robot_manifest.json") + profile = load_hardware_profile(UNOQ_ROOT / "config" / "hardware_profile.json") + profile["safety"]["auto_stand_on_fall"] = auto_stand_on_fall + profile["safety"]["soft_start_s"] = 0.04 # 2 steps at 50Hz for fast test + + hal = MockHal() + policies = PolicySet.load_from_manifest(UNOQ_ROOT, manifest, backend="dummy") + ctrl = MicroduckController( + base_dir=UNOQ_ROOT, + manifest=manifest, + profile=profile, + hal=hal, + policies=policies, + profile_path=tmp_path / "hardware_profile.json", + ) + return ctrl, hal + + +def test_controller_stand_walk_estop_and_calibration_with_mock_hal(tmp_path: Path): + ctrl, hal = _make_controller(tmp_path, auto_stand_on_fall=False) + ctrl.connect() + + # Initially idle, torque off + st = ctrl.step_once(dt=0.02) + assert st["lifecycle"] == LifecycleState.IDLE.value + assert st["torque_enabled"] is False + + # Prepare stand -> soft start -> active walk + ctrl.request_stand() + ctrl.step_once(dt=0.02) + assert ctrl.state_machine.lifecycle == LifecycleState.SOFT_START + st = ctrl.step_once(dt=0.02) + assert st["lifecycle"] == LifecycleState.ACTIVE + assert st["torque_enabled"] is True + + # Remote control timeout > 0.5s zeroes velocity command + ctrl.set_remote_input(0.8, 0.4, now_s=10.0) + ctrl.step_once(dt=0.02, now_s=10.1) + assert ctrl.state_machine.vel_cmd[0] > 0.0 + ctrl.step_once(dt=0.02, now_s=10.7) + assert np.allclose(ctrl.state_machine.vel_cmd, 0.0) + + # E-STOP disables torque immediately + ctrl.emergency_stop() + st = ctrl.step_once(dt=0.02) + assert st["lifecycle"] == LifecycleState.IDLE.value + assert st["torque_enabled"] is False + assert hal.torque_enabled is False + + +def test_controller_fall_protection_and_auto_stand_recovery(tmp_path: Path): + # Case 1: default auto_stand_on_fall=False -> FALL_SAFE torque off + ctrl, hal = _make_controller(tmp_path / "c1", auto_stand_on_fall=False) + ctrl.connect() + ctrl.request_stand() + ctrl.step_once(dt=0.02) + ctrl.step_once(dt=0.02) + assert ctrl.state_machine.lifecycle == LifecycleState.ACTIVE + + # sync_step samples the updated mock gravity on the 1st tick, then 10 debounce ticks follow + hal.set_mock_gravity([0.9, 0.0, -0.1]) + ctrl.step_once(dt=0.02) + for _ in range(10): + st = ctrl.step_once(dt=0.02) + assert st["lifecycle"] == LifecycleState.FALL_SAFE.value + assert st["torque_enabled"] is False + + # Case 2: auto_stand_on_fall=True -> enters recovery using stand policy + ctrl2, hal2 = _make_controller(tmp_path / "c2", auto_stand_on_fall=True) + ctrl2.connect() + ctrl2.request_stand() + ctrl2.step_once(dt=0.02) + ctrl2.step_once(dt=0.02) + hal2.set_mock_gravity([0.9, 0.0, -0.1]) + ctrl2.step_once(dt=0.02) + for _ in range(10): + ctrl2.step_once(dt=0.02) + assert ctrl2.state_machine.recovery_state == "fallen" + for _ in range(15): + st = ctrl2.step_once(dt=0.02) + assert st["recovery_state"] == "recovering" + assert st["active_policy"] == "stand" + + # Upright restored -> 1 step to sample upright + 50 steps to return to walk + hal2.set_mock_gravity([0.0, 0.0, -1.0]) + ctrl2.step_once(dt=0.02) + for _ in range(50): + st = ctrl2.step_once(dt=0.02) + assert st["recovery_state"] is None + assert st["active_policy"] == "walk" + + +def test_calibration_zero_capture_and_imu_bias(tmp_path: Path): + ctrl, hal = _make_controller(tmp_path) + ctrl.connect() + # Simulate joint 0 sitting at tick 2100 instead of 2048 + hal.ticks[0] = 2100 + ctrl.step_once(dt=0.02) + + updated = ctrl.capture_joint_zero("left_hip_yaw") + assert updated["joints"]["left_hip_yaw"]["zero_offset_ticks"] == 2100 - 2048 + + hal.set_mock_gyro([0.03, -0.02, 0.01]) + imu_res = ctrl.calibrate_imu_zero(samples=10) + assert np.allclose(imu_res["gyro_bias_rad_s"], [0.03, -0.02, 0.01], atol=1e-5) + + # Ping servos + ids = ctrl.ping_servos() + assert len(ids) == 14 + + +def test_policy_set_onnx_requires_explicit_dummy_when_missing(): + manifest = load_robot_manifest(UNOQ_ROOT / "config" / "robot_manifest.json") + with pytest.raises((FileNotFoundError, RuntimeError)): + PolicySet.load_from_manifest(UNOQ_ROOT, manifest, backend="onnx") diff --git a/hardware/unoq/python/tests/test_core.py b/hardware/unoq/python/tests/test_core.py new file mode 100644 index 0000000..e036571 --- /dev/null +++ b/hardware/unoq/python/tests/test_core.py @@ -0,0 +1,216 @@ +import json +import math +import sys +from pathlib import Path + +import numpy as np +import pytest + +PYTHON_ROOT = Path(__file__).resolve().parents[1] +UNOQ_ROOT = PYTHON_ROOT.parent +if str(PYTHON_ROOT) not in sys.path: + sys.path.insert(0, str(PYTHON_ROOT)) + +from core.config import ( # noqa: E402 + apply_imu_calibration, + load_hardware_profile, + load_robot_manifest, + rad_to_ticks, + ticks_to_rad, +) +from core.observations import ( # noqa: E402 + build_observation, + decode_action, + script_command_at, +) +from core.state_machine import LifecycleState, MicroduckStateMachine # noqa: E402 + + +def test_default_manifest_and_profile_match_microduck_contract(): + manifest = load_robot_manifest(UNOQ_ROOT / "config" / "robot_manifest.json") + profile = load_hardware_profile(UNOQ_ROOT / "config" / "hardware_profile.json") + + assert manifest["obs_size"] == 61 + assert manifest["action_size"] == 14 + assert manifest["command_size"] == 13 + assert len(manifest["joint_names"]) == 14 + assert len(manifest["default_pose"]) == 14 + assert set(manifest["joint_names"]) == set(profile["joints"].keys()) + assert manifest["skills"]["walk"]["path"] == "policies/walk.onnx" + assert manifest["skills"]["stand"]["path"] == "policies/stand.onnx" + + +def test_rad_to_ticks_and_back_with_sign_and_offset(): + manifest = load_robot_manifest(UNOQ_ROOT / "config" / "robot_manifest.json") + profile = load_hardware_profile(UNOQ_ROOT / "config" / "hardware_profile.json") + names = manifest["joint_names"] + + # Customize two joints with inverted direction and mechanical offset + profile["joints"]["left_hip_pitch"]["sign"] = -1 + profile["joints"]["left_hip_pitch"]["zero_offset_ticks"] = 35 + profile["joints"]["right_knee"]["sign"] = 1 + profile["joints"]["right_knee"]["zero_offset_ticks"] = -20 + + q_in = np.array(manifest["default_pose"], dtype=np.float64) + ticks = rad_to_ticks(q_in, names, profile) + assert len(ticks) == 14 + assert all(0 <= t <= 4095 for t in ticks) + + q_out = ticks_to_rad(ticks, names, profile) + # 12-bit quantization resolution ~ 2*pi/4096 = 0.001534 rad -> max error <= 0.001 rad + assert np.allclose(q_out, q_in, atol=1e-3) + + +def test_rad_to_ticks_clamps_to_joint_mechanical_limits(): + manifest = load_robot_manifest(UNOQ_ROOT / "config" / "robot_manifest.json") + profile = load_hardware_profile(UNOQ_ROOT / "config" / "hardware_profile.json") + names = manifest["joint_names"] + + q_extreme = np.full(14, 10.0, dtype=np.float64) + ticks = rad_to_ticks(q_extreme, names, profile) + q_clamped = ticks_to_rad(ticks, names, profile) + for i, name in enumerate(names): + assert q_clamped[i] <= profile["joints"][name]["max_rad"] + 2e-3 + + +def test_imu_calibration_axis_remap_and_gyro_bias(): + profile = load_hardware_profile(UNOQ_ROOT / "config" / "hardware_profile.json") + profile["imu"]["axis_map"] = [ + [0, 1, 0], + [-1, 0, 0], + [0, 0, 1], + ] + profile["imu"]["gyro_bias_rad_s"] = [0.1, -0.05, 0.02] + + gyro, grav = apply_imu_calibration( + np.array([0.2, 0.35, -0.08]), + np.array([0.0, 0.0, -1.0]), + profile, + ) + # First subtract bias [0.1, 0.4, -0.1], then rotate via axis_map -> [0.4, -0.1, -0.1] + assert np.allclose(gyro, [0.4, -0.1, -0.1], atol=1e-6) + assert np.allclose(grav, [0.0, 0.0, -1.0], atol=1e-6) + + +def test_observation_layout_and_action_decoding(): + default_pose = np.arange(14, dtype=np.float32) * 0.01 + gyro = np.array([0.1, 0.2, 0.3], dtype=np.float32) + grav = np.array([0.0, 0.0, -1.0], dtype=np.float32) + q = default_pose + 0.05 + qd = np.full(14, 0.25, dtype=np.float32) + last_act = np.full(14, -0.5, dtype=np.float32) + cmd = np.arange(13, dtype=np.float32) * 0.1 + + obs = build_observation(gyro, grav, q, qd, last_act, cmd, default_pose) + assert obs.shape == (61,) + assert obs.dtype == np.float32 + assert np.allclose(obs[0:3], gyro) + assert np.allclose(obs[3:6], grav) + assert np.allclose(obs[6:20], 0.05, atol=1e-6) + assert np.allclose(obs[20:34], 0.25) + assert np.allclose(obs[34:48], -0.5) + assert np.allclose(obs[48:61], cmd) + + targets = decode_action(last_act, default_pose, action_scale=1.0) + assert np.allclose(targets, default_pose - 0.5) + + with pytest.raises(ValueError): + build_observation(np.array([np.nan, 0.0, 0.0]), grav, q, qd, last_act, cmd, default_pose) + + +def test_script_command_interpolation_matches_js(): + script = { + "duration_s": 2.0, + "loop": True, + "keyframes": [ + {"t": 0.0, "cmd": [0.0, 0.0, 0.0]}, + {"t": 1.0, "cmd": [0.2, 0.0, -0.4]}, + ], + } + mid = script_command_at(script, 0.5, 13) + assert mid.shape == (13,) + assert np.allclose(mid[:3], [0.1, 0.0, -0.2]) + assert np.allclose(mid[3:], 0.0) + + +def test_state_machine_lifecycle_soft_start_skills_and_fall(): + manifest = load_robot_manifest(UNOQ_ROOT / "config" / "robot_manifest.json") + sm = MicroduckStateMachine(manifest) + assert sm.lifecycle == LifecycleState.IDLE + + # Soft start transitions to ACTIVE walk after 1.5s (75 steps at 50Hz) + sm.request_stand(np.zeros(14, dtype=np.float64), soft_start_s=1.5) + assert sm.lifecycle == LifecycleState.SOFT_START + + for _ in range(74): + dec = sm.step(0.02, proj_grav_z=-1.0, auto_stand_on_fall=False) + assert dec.torque_enabled is True + assert dec.policy_key is None + assert 0.0 < dec.soft_start_alpha <= 1.0 + + dec = sm.step(0.02, proj_grav_z=-1.0, auto_stand_on_fall=False) + assert sm.lifecycle == LifecycleState.ACTIVE + assert dec.policy_key == "walk" + + # Kick runs 25 steps then locks velocity for 20 steps + sm.set_velocity_cmd(0.2, 0.5, speed_scale=1.0) + assert sm.trigger_action("kickL") is True + for i in range(25): + dec = sm.step(0.02, proj_grav_z=-1.0, auto_stand_on_fall=False) + assert dec.policy_key == "kickL" + assert np.allclose(dec.command_13[:3], [0.0, 0.0, 0.0]) + + # Now back in walk, but velocity locked for 20 steps + for i in range(20): + dec = sm.step(0.02, proj_grav_z=-1.0, auto_stand_on_fall=False) + assert dec.policy_key == "walk" + assert np.allclose(dec.command_13[:3], [0.0, 0.0, 0.0]) + + # Lock expired: normalized [0.2, 0.5] scales by legs limits [0.25, -0.2, 1.0] + dec = sm.step(0.02, proj_grav_z=-1.0, auto_stand_on_fall=False) + assert dec.command_13[0] == pytest.approx(0.05) + assert dec.command_13[2] == pytest.approx(0.5) + + sm.set_velocity_cmd(-1.0, 0.0, speed_scale=0.5) + dec = sm.step(0.02, proj_grav_z=-1.0, auto_stand_on_fall=False) + assert dec.command_13[0] == pytest.approx(-0.1) + + # Ground pick encodes cos/sin phase and zeroes head offsets + sm.trigger_action("headToggle") + sm.set_head_target(1.0, 0.0, 0.0, 0.0) + sm.step(0.02, proj_grav_z=-1.0, auto_stand_on_fall=False) + assert sm.trigger_action("groundPick") is True + dec = sm.step(0.02, proj_grav_z=-1.0, auto_stand_on_fall=False) + assert dec.policy_key == "groundpick" + assert np.allclose(dec.command_13[3:7], 0.0) + assert math.isclose(dec.command_13[0] ** 2 + dec.command_13[1] ** 2, 1.0, rel_tol=1e-5) + + # Clear groundpick to finish + for _ in range(145): + sm.step(0.02, proj_grav_z=-1.0, auto_stand_on_fall=False) + assert sm.skill_mode == "walk" + + # Fall detection: 10 steps of gz > -0.5 triggers FALL_SAFE when auto_stand_on_fall=False + for _ in range(9): + dec = sm.step(0.02, proj_grav_z=-0.2, auto_stand_on_fall=False) + assert sm.lifecycle == LifecycleState.ACTIVE + dec = sm.step(0.02, proj_grav_z=-0.2, auto_stand_on_fall=False) + assert sm.lifecycle == LifecycleState.FALL_SAFE + assert dec.torque_enabled is False + + # Re-stand and test auto_stand_on_fall=True + sm.request_stand(np.zeros(14, dtype=np.float64), soft_start_s=0.02) + sm.step(0.02, proj_grav_z=-1.0, auto_stand_on_fall=True) + assert sm.lifecycle == LifecycleState.ACTIVE + + for _ in range(10): + dec = sm.step(0.02, proj_grav_z=-0.1, auto_stand_on_fall=True) + assert sm.recovery_state == "fallen" + for _ in range(15): + dec = sm.step(0.02, proj_grav_z=-0.1, auto_stand_on_fall=True) + assert sm.recovery_state == "recovering" + assert dec.policy_key == "stand" + for _ in range(50): + dec = sm.step(0.02, proj_grav_z=-0.95, auto_stand_on_fall=True) + assert sm.recovery_state is None + assert sm.skill_mode == "walk" diff --git a/hardware/unoq/python/tests/test_deploy.py b/hardware/unoq/python/tests/test_deploy.py new file mode 100644 index 0000000..33dd434 --- /dev/null +++ b/hardware/unoq/python/tests/test_deploy.py @@ -0,0 +1,197 @@ +import io +import json +import shutil +import sys +import threading +import urllib.error +import urllib.request +import zipfile +from pathlib import Path + +import pytest + +PYTHON_ROOT = Path(__file__).resolve().parents[1] +UNOQ_ROOT = PYTHON_ROOT.parent +if str(PYTHON_ROOT) not in sys.path: + sys.path.insert(0, str(PYTHON_ROOT)) + +from core.config import load_hardware_profile, load_robot_manifest # noqa: E402 +from core.controller import MicroduckController # noqa: E402 +from core.deploy_service import DeployService # noqa: E402 +from core.policy_runner import PolicySet # noqa: E402 +from hal.mock_hal import MockHal # noqa: E402 +from web.fallback_server import FallbackWebServer # noqa: E402 + + +def _make_runtime_dir(tmp_path: Path) -> tuple[Path, MicroduckController]: + base = tmp_path / "unoq" + (base / "config").mkdir(parents=True) + (base / "policies").mkdir(parents=True) + shutil.copy2(UNOQ_ROOT / "config" / "robot_manifest.json", base / "config" / "robot_manifest.json") + shutil.copy2(UNOQ_ROOT / "config" / "hardware_profile.json", base / "config" / "hardware_profile.json") + + manifest = load_robot_manifest(base / "config" / "robot_manifest.json") + profile = load_hardware_profile(base / "config" / "hardware_profile.json") + profile["joints"]["left_hip_yaw"]["zero_offset_ticks"] = 77 + from core.config import save_hardware_profile + + save_hardware_profile(base / "config" / "hardware_profile.json", profile, manifest["joint_names"]) + + for skill in manifest["skills"].values(): + (base / skill["path"]).write_bytes(b"MOCK_ONNX_V1") + + hal = MockHal() + policies = PolicySet.load_from_manifest(base, manifest, backend="dummy") + ctrl = MicroduckController(base_dir=base, manifest=manifest, profile=profile, hal=hal, policies=policies) + ctrl.connect() + return base, ctrl + + +def _build_zip_bytes( + manifest: dict, + files: dict[str, bytes] | None = None, + profile: dict | None = None, + custom_script: dict | None = None, + prefix: str = "", +) -> bytes: + buf = io.BytesIO() + with zipfile.ZipFile(buf, "w", compression=zipfile.ZIP_DEFLATED) as zf: + zf.writestr(f"{prefix}config/robot_manifest.json", json.dumps(manifest)) + if profile is not None: + zf.writestr(f"{prefix}config/hardware_profile.json", json.dumps(profile)) + if custom_script is not None: + zf.writestr(f"{prefix}config/custom_move_script.json", json.dumps(custom_script)) + if files is not None: + for rel, data in files.items(): + zf.writestr(f"{prefix}{rel}", data) + else: + for skill in manifest.get("skills", {}).values(): + zf.writestr(f"{prefix}{skill['path']}", b"MOCK_ONNX_V2") + return buf.getvalue() + + +def test_deploy_service_applies_valid_zip_and_preserves_profile_by_default(tmp_path: Path): + base, ctrl = _make_runtime_dir(tmp_path) + deploy = DeployService(base_dir=base, controller=ctrl, policy_backend="dummy") + + manifest = load_robot_manifest(base / "config" / "robot_manifest.json") + manifest["variants"]["rollers"]["enabled"] = True + manifest["skills"]["drive"] = { + "path": "policies/drive.onnx", + "kind": "perpetual", + "slot": "drive", + "action_scale": 1.0, + } + incoming_profile = load_hardware_profile(base / "config" / "hardware_profile.json") + incoming_profile["joints"]["left_hip_yaw"]["zero_offset_ticks"] = 0 + + custom_script = { + "name": "wave", + "durationS": 1.0, + "loop": True, + "keyframes": [{"t": 0.0, "cmd": [0.1] + [0.0] * 12}], + } + manifest["custom_move"] = {"name": "wave", "slot": "script", "type": "script"} + + zip_bytes = _build_zip_bytes(manifest, profile=incoming_profile, custom_script=custom_script, prefix="microduck_unoq/") + result = deploy.apply_zip_bytes(zip_bytes, overwrite_profile=False) + + assert result["ok"] is True + assert "drive" in result["loaded_skills"] + # On-robot calibration must be preserved unless overwrite_profile=True + assert ctrl.profile["joints"]["left_hip_yaw"]["zero_offset_ticks"] == 77 + assert ctrl.state_machine.rollers_enabled is True + assert ctrl.state_machine.custom_script["name"] == "wave" + assert (base / "policies" / "drive.onnx").read_bytes() == b"MOCK_ONNX_V2" + + # Now deploy with overwrite_profile=True + result2 = deploy.apply_zip_bytes(zip_bytes, overwrite_profile=True) + assert result2["ok"] is True + assert ctrl.profile["joints"]["left_hip_yaw"]["zero_offset_ticks"] == 0 + + +def test_deploy_service_rejects_traversal_and_invalid_manifest(tmp_path: Path): + base, ctrl = _make_runtime_dir(tmp_path) + deploy = DeployService(base_dir=base, controller=ctrl, policy_backend="dummy") + manifest = load_robot_manifest(base / "config" / "robot_manifest.json") + + # 1) Path traversal inside zip must be rejected without touching files + bad_buf = io.BytesIO() + with zipfile.ZipFile(bad_buf, "w") as zf: + zf.writestr("../evil.txt", b"pwn") + zf.writestr("config/robot_manifest.json", json.dumps(manifest)) + with pytest.raises(ValueError): + deploy.apply_zip_bytes(bad_buf.getvalue()) + assert not (base.parent / "evil.txt").exists() + + # 2) Wrong obs_size must be rejected + bad_manifest = dict(manifest) + bad_manifest["obs_size"] = 99 + with pytest.raises(ValueError): + deploy.apply_zip_bytes(_build_zip_bytes(bad_manifest)) + + # 3) Missing skill onnx file referenced in manifest must be rejected + partial_files = {"policies/stand.onnx": b"MOCK_ONNX"} # walk.onnx missing + with pytest.raises(ValueError): + deploy.apply_zip_bytes(_build_zip_bytes(manifest, files=partial_files)) + + +def test_fallback_http_server_status_deploy_control_and_mobile_ui(tmp_path: Path): + base, ctrl = _make_runtime_dir(tmp_path) + deploy = DeployService(base_dir=base, controller=ctrl, policy_backend="dummy") + + server = FallbackWebServer(host="127.0.0.1", port=0, controller=ctrl, deploy_service=deploy) + port = server.port + thread = threading.Thread(target=server.serve_forever, daemon=True) + thread.start() + try: + # 1) Mobile index page + with urllib.request.urlopen(f"http://127.0.0.1:{port}/", timeout=3) as resp: + html = resp.read().decode("utf-8") + assert resp.status == 200 + assert "Microduck" in html + assert "E-STOP" in html + + # 2) Initial status + with urllib.request.urlopen(f"http://127.0.0.1:{port}/api/status", timeout=3) as resp: + status = json.loads(resp.read().decode("utf-8")) + assert status["lifecycle"] == "IDLE" + assert "walk" in status["loaded_skills"] + + # 3) HTTP push hot deploy (raw application/zip) + manifest = load_robot_manifest(base / "config" / "robot_manifest.json") + manifest["variants"]["rollers"]["enabled"] = True + manifest["skills"]["drive"] = { + "path": "policies/drive.onnx", + "kind": "perpetual", + "slot": "drive", + "action_scale": 1.0, + } + zip_bytes = _build_zip_bytes(manifest) + req = urllib.request.Request( + f"http://127.0.0.1:{port}/api/deploy?overwrite_profile=0", + data=zip_bytes, + method="POST", + headers={"Content-Type": "application/zip"}, + ) + with urllib.request.urlopen(req, timeout=5) as resp: + deployed = json.loads(resp.read().decode("utf-8")) + assert resp.status == 200 + assert deployed["ok"] is True + assert "drive" in deployed["loaded_skills"] + + # 4) Stand + step + E-STOP over HTTP + req_stand = urllib.request.Request(f"http://127.0.0.1:{port}/api/stand", data=b"{}", method="POST") + with urllib.request.urlopen(req_stand, timeout=3) as resp: + st = json.loads(resp.read().decode("utf-8")) + assert st["lifecycle"] == "SOFT_START" + + ctrl.step_once(dt=0.02) + req_estop = urllib.request.Request(f"http://127.0.0.1:{port}/api/estop", data=b"{}", method="POST") + with urllib.request.urlopen(req_estop, timeout=3) as resp: + st2 = json.loads(resp.read().decode("utf-8")) + assert st2["lifecycle"] == "IDLE" + assert ctrl.profile["joints"]["left_hip_yaw"]["zero_offset_ticks"] == 77 + finally: + server.shutdown() + server.server_close() diff --git a/hardware/unoq/python/web/__init__.py b/hardware/unoq/python/web/__init__.py new file mode 100644 index 0000000..18208bc --- /dev/null +++ b/hardware/unoq/python/web/__init__.py @@ -0,0 +1 @@ +"""Web HTTP/WebSocket control and deployment package for Microduck UNO Q.""" diff --git a/hardware/unoq/python/web/api.py b/hardware/unoq/python/web/api.py new file mode 100644 index 0000000..766b42c --- /dev/null +++ b/hardware/unoq/python/web/api.py @@ -0,0 +1,184 @@ +from __future__ import annotations + +import copy +import json +from email.parser import BytesParser +from email.policy import default as email_default_policy +from pathlib import Path +from typing import Any + +from core.controller import MicroduckController +from core.deploy_service import DeployService + + +STATIC_DIR = Path(__file__).resolve().parent / "static" + + +def load_index_html() -> bytes: + path = STATIC_DIR / "index.html" + if not path.is_file(): + return b"

Microduck controller UI missing

" + return path.read_bytes() + + +def parse_bool_flag(raw: str | None) -> bool: + if raw is None: + return False + return raw.strip().lower() in ("1", "true", "yes", "on") + + +def extract_deploy_payload( + content_type: str, + body: bytes, + query_overwrite: str | None = None, +) -> tuple[bytes, bool]: + overwrite = parse_bool_flag(query_overwrite) + ctype = (content_type or "").lower() + if "multipart/form-data" in ctype: + raw_msg = f"Content-Type: {content_type}\r\nMIME-Version: 1.0\r\n\r\n".encode("latin1") + body + msg = BytesParser(policy=email_default_policy).parsebytes(raw_msg) + if not msg.is_multipart(): + raise ValueError("Malformed multipart/form-data upload") + zip_bytes: bytes | None = None + for part in msg.iter_parts(): + field = part.get_param("name", header="content-disposition") + payload = part.get_payload(decode=True) or b"" + if field in ("bundle", "file", "zip") and payload: + zip_bytes = payload + elif field == "overwrite_profile": + overwrite = parse_bool_flag(payload.decode("utf-8", errors="ignore")) + if not zip_bytes: + raise ValueError("Multipart upload missing 'bundle' zip field") + return zip_bytes, overwrite + return body, overwrite + + +def build_status_payload(controller: MicroduckController, deploy_service: DeployService) -> dict[str, Any]: + st = controller.get_status() + st["last_deploy"] = copy.deepcopy(deploy_service.last_deploy) if deploy_service.last_deploy else None + return st + + +def handle_ws_message(controller: MicroduckController, msg: dict[str, Any]) -> dict[str, Any] | None: + kind = msg.get("type") + if kind == "input": + controller.set_remote_input( + norm_x=float(msg.get("vx", 0.0)), + norm_wz=float(msg.get("wz", 0.0)), + speed_scale=float(msg["speed_scale"]) if "speed_scale" in msg else None, + ) + return None + if kind == "head": + controller.set_head_target( + float(msg.get("neck_pitch", 0.0)), + float(msg.get("head_pitch", 0.0)), + float(msg.get("head_yaw", 0.0)), + float(msg.get("head_roll", 0.0)), + ) + return None + if kind == "stand": + try: + controller.request_stand() + return {"type": "ack", "event": "stand"} + except RuntimeError as exc: + return {"type": "error", "error": str(exc)} + if kind == "estop": + controller.emergency_stop() + return {"type": "ack", "event": "estop"} + if kind == "action": + action = str(msg.get("action", "")) + ok = controller.trigger_action(action) + return {"type": "ack", "event": "action", "action": action, "ok": ok} + if kind == "settings": + if "auto_stand_on_fall" in msg: + controller.set_auto_stand_on_fall(bool(msg["auto_stand_on_fall"])) + if "speed_scale" in msg: + controller.set_speed_scale(float(msg["speed_scale"])) + return {"type": "ack", "event": "settings"} + if kind == "ping": + return {"type": "pong", "t": msg.get("t")} + return None + + +def dispatch_api_get( + path: str, + controller: MicroduckController, + deploy_service: DeployService, +) -> tuple[int, dict[str, Any]]: + if path == "/api/status": + return 200, build_status_payload(controller, deploy_service) + if path == "/api/profile": + return 200, {"ok": True, "profile": copy.deepcopy(controller.profile)} + return 404, {"ok": False, "error": f"Unknown GET endpoint: {path}"} + + +def dispatch_api_post( + path: str, + body_bytes: bytes, + content_type: str, + query_params: dict[str, list[str]], + controller: MicroduckController, + deploy_service: DeployService, +) -> tuple[int, dict[str, Any]]: + if path == "/api/deploy": + try: + q_ow = query_params.get("overwrite_profile", [None])[0] + raw_zip, overwrite = extract_deploy_payload(content_type, body_bytes, q_ow) + res = dict(deploy_service.apply_zip_bytes(raw_zip, overwrite_profile=overwrite)) + res["status"] = build_status_payload(controller, deploy_service) + return 200, res + except ValueError as exc: + return 400, {"ok": False, "error": str(exc)} + except Exception as exc: + return 500, {"ok": False, "error": str(exc)} + + try: + payload = json.loads(body_bytes.decode("utf-8")) if body_bytes.strip() else {} + except Exception: + return 400, {"ok": False, "error": "Invalid JSON body"} + + try: + if path == "/api/stand": + controller.request_stand() + return 200, build_status_payload(controller, deploy_service) + if path == "/api/estop": + controller.emergency_stop() + return 200, build_status_payload(controller, deploy_service) + if path == "/api/action": + action = str(payload.get("action", "")) + ok = controller.trigger_action(action) + return 200, {"ok": ok, "status": build_status_payload(controller, deploy_service)} + if path == "/api/settings": + if "auto_stand_on_fall" in payload: + controller.set_auto_stand_on_fall(bool(payload["auto_stand_on_fall"])) + if "speed_scale" in payload: + controller.set_speed_scale(float(payload["speed_scale"])) + return 200, build_status_payload(controller, deploy_service) + if path == "/api/profile": + profile = controller.update_profile(payload) + return 200, {"ok": True, "profile": profile} + if path == "/api/calibrate/ping": + max_id = int(payload.get("max_id", 20)) + return 200, {"ok": True, "ids": controller.ping_servos(max_id=max_id)} + if path == "/api/calibrate/joint_zero": + joint = str(payload.get("joint", "")) + profile = controller.capture_joint_zero(joint) + return 200, {"ok": True, "profile": profile} + if path == "/api/calibrate/test_joint": + joint = str(payload.get("joint", "")) + rad = float(payload.get("rad", 0.0)) + ticks = controller.test_single_joint(joint, rad) + return 200, {"ok": True, "ticks": ticks} + if path == "/api/calibrate/exit": + controller.exit_calibration() + return 200, build_status_payload(controller, deploy_service) + if path == "/api/calibrate/imu_zero": + samples = int(payload.get("samples", 200)) + imu = controller.calibrate_imu_zero(samples=samples) + return 200, {"ok": True, "imu": imu} + except (ValueError, RuntimeError) as exc: + return 400, {"ok": False, "error": str(exc)} + except Exception as exc: + return 500, {"ok": False, "error": str(exc)} + + return 404, {"ok": False, "error": f"Unknown POST endpoint: {path}"} diff --git a/hardware/unoq/python/web/fallback_server.py b/hardware/unoq/python/web/fallback_server.py new file mode 100644 index 0000000..7452fef --- /dev/null +++ b/hardware/unoq/python/web/fallback_server.py @@ -0,0 +1,227 @@ +from __future__ import annotations + +import base64 +import hashlib +import json +import select +import socket +import struct +import threading +import time +from http.server import BaseHTTPRequestHandler, ThreadingHTTPServer +from typing import Any +from urllib.parse import parse_qs, urlparse + +from core.controller import MicroduckController +from core.deploy_service import DeployService + +from .api import ( + build_status_payload, + dispatch_api_get, + dispatch_api_post, + handle_ws_message, + load_index_html, +) + + +WS_GUID = "258EAFA5-E914-47DA-95CA-C5AB0DC85B11" + + +def _ws_encode_frame(opcode: int, payload: bytes) -> bytes: + header = bytearray([0x80 | (opcode & 0x0F)]) + n = len(payload) + if n < 126: + header.append(n) + elif n < 65536: + header.append(126) + header.extend(struct.pack("!H", n)) + else: + header.append(127) + header.extend(struct.pack("!Q", n)) + return bytes(header) + payload + + +def _recv_exact(sock: socket.socket, n: int) -> bytes | None: + chunks = bytearray() + while len(chunks) < n: + chunk = sock.recv(n - len(chunks)) + if not chunk: + return None + chunks.extend(chunk) + return bytes(chunks) + + +def _ws_read_frame(sock: socket.socket) -> tuple[int, bytes] | None: + head = _recv_exact(sock, 2) + if head is None: + return None + b1, b2 = head[0], head[1] + opcode = b1 & 0x0F + masked = bool(b2 & 0x80) + length = b2 & 0x7F + if length == 126: + ext = _recv_exact(sock, 2) + if ext is None: + return None + length = struct.unpack("!H", ext)[0] + elif length == 127: + ext = _recv_exact(sock, 8) + if ext is None: + return None + length = struct.unpack("!Q", ext)[0] + if length > 1024 * 1024: + return None + mask = _recv_exact(sock, 4) if masked else b"" + if masked and mask is None: + return None + payload = _recv_exact(sock, length) if length > 0 else b"" + if payload is None: + return None + if masked: + payload = bytes(b ^ mask[i % 4] for i, b in enumerate(payload)) + return opcode, payload + + +class FallbackWebServer(ThreadingHTTPServer): + """Zero-dependency HTTP + RFC6455 WebSocket server (runs on stock Python 3.12).""" + + allow_reuse_address = True + daemon_threads = True + + def __init__( + self, + host: str, + port: int, + controller: MicroduckController, + deploy_service: DeployService, + ) -> None: + self.controller = controller + self.deploy_service = deploy_service + super().__init__((host, port), _RequestHandler) + self.port = int(self.server_address[1]) + + +class _RequestHandler(BaseHTTPRequestHandler): + protocol_version = "HTTP/1.1" + + def log_message(self, format: str, *args: Any) -> None: # noqa: A002 + return + + def _send_cors_headers(self) -> None: + self.send_header("Access-Control-Allow-Origin", "*") + self.send_header("Access-Control-Allow-Methods", "GET, POST, OPTIONS") + self.send_header("Access-Control-Allow-Headers", "Content-Type") + self.send_header("Access-Control-Allow-Private-Network", "true") + + def _send_json(self, status_code: int, payload: dict[str, Any]) -> None: + raw = json.dumps(payload).encode("utf-8") + self.send_response(status_code) + self._send_cors_headers() + self.send_header("Content-Type", "application/json; charset=utf-8") + self.send_header("Content-Length", str(len(raw))) + self.end_headers() + self.wfile.write(raw) + + def _send_bytes(self, status_code: int, content_type: str, data: bytes) -> None: + self.send_response(status_code) + self._send_cors_headers() + self.send_header("Content-Type", content_type) + self.send_header("Content-Length", str(len(data))) + self.end_headers() + self.wfile.write(data) + + def do_OPTIONS(self) -> None: # noqa: N802 + self.send_response(204) + self._send_cors_headers() + self.send_header("Content-Length", "0") + self.end_headers() + + def do_GET(self) -> None: # noqa: N802 + parsed = urlparse(self.path) + if parsed.path == "/ws/control" and self.headers.get("Upgrade", "").lower() == "websocket": + self._handle_websocket() + return + if parsed.path in ("/", "/index.html"): + self._send_bytes(200, "text/html; charset=utf-8", load_index_html()) + return + + srv: FallbackWebServer = self.server # type: ignore[assignment] + code, payload = dispatch_api_get(parsed.path, srv.controller, srv.deploy_service) + self._send_json(code, payload) + + def do_POST(self) -> None: # noqa: N802 + parsed = urlparse(self.path) + length = int(self.headers.get("Content-Length", "0") or "0") + if length < 0 or length > 160 * 1024 * 1024: + self._send_json(413, {"ok": False, "error": "Payload too large"}) + return + body = self.rfile.read(length) if length > 0 else b"" + content_type = self.headers.get("Content-Type", "") + query = parse_qs(parsed.query) + srv: FallbackWebServer = self.server # type: ignore[assignment] + code, payload = dispatch_api_post( + parsed.path, + body, + content_type, + query, + srv.controller, + srv.deploy_service, + ) + self._send_json(code, payload) + + def _handle_websocket(self) -> None: + key = self.headers.get("Sec-WebSocket-Key") + if not key: + self.send_error(400, "Missing Sec-WebSocket-Key") + return + accept = base64.b64encode(hashlib.sha1((key.strip() + WS_GUID).encode("ascii")).digest()).decode("ascii") + self.send_response(101, "Switching Protocols") + self.send_header("Upgrade", "websocket") + self.send_header("Connection", "Upgrade") + self.send_header("Sec-WebSocket-Accept", accept) + self.end_headers() + + srv: FallbackWebServer = self.server # type: ignore[assignment] + sock: socket.socket = self.connection + sock.settimeout(5.0) + write_lock = threading.Lock() + last_telemetry = 0.0 + + try: + while True: + now = time.monotonic() + if now - last_telemetry >= 0.1: + last_telemetry = now + msg = {"type": "telemetry", **build_status_payload(srv.controller, srv.deploy_service)} + frame = _ws_encode_frame(0x1, json.dumps(msg).encode("utf-8")) + with write_lock: + sock.sendall(frame) + + readable, _, _ = select.select([sock], [], [], 0.05) + if not readable: + continue + parsed = _ws_read_frame(sock) + if parsed is None: + break + opcode, payload = parsed + if opcode == 0x8: # close + with write_lock: + sock.sendall(_ws_encode_frame(0x8, b"")) + break + if opcode == 0x9: # ping -> pong + with write_lock: + sock.sendall(_ws_encode_frame(0xA, payload)) + continue + if opcode == 0x1: # text + try: + data = json.loads(payload.decode("utf-8")) + except Exception: + continue + ack = handle_ws_message(srv.controller, data) + if ack is not None: + with write_lock: + sock.sendall(_ws_encode_frame(0x1, json.dumps(ack).encode("utf-8"))) + except (BrokenPipeError, ConnectionResetError, TimeoutError, OSError): + pass + finally: + self.close_connection = True diff --git a/hardware/unoq/python/web/server.py b/hardware/unoq/python/web/server.py new file mode 100644 index 0000000..6152390 --- /dev/null +++ b/hardware/unoq/python/web/server.py @@ -0,0 +1,92 @@ +from __future__ import annotations + +import asyncio +import json +from typing import Any +from urllib.parse import parse_qs + +from core.controller import MicroduckController +from core.deploy_service import DeployService + +from .api import ( + build_status_payload, + dispatch_api_get, + dispatch_api_post, + handle_ws_message, + load_index_html, +) + + +def create_fastapi_app(controller: MicroduckController, deploy_service: DeployService) -> Any: + """Create FastAPI app backed by the shared web.api dispatcher.""" + from fastapi import FastAPI, Request, WebSocket, WebSocketDisconnect + from fastapi.middleware.cors import CORSMiddleware + from fastapi.responses import HTMLResponse, JSONResponse + + app = FastAPI(title="Microduck UNO Q Controller", version="0.1.0") + app.add_middleware( + CORSMiddleware, + allow_origins=["*"], + allow_methods=["*"], + allow_headers=["*"], + ) + + @app.middleware("http") + async def add_pna_header(request: Request, call_next: Any) -> Any: + resp = await call_next(request) + resp.headers["Access-Control-Allow-Private-Network"] = "true" + return resp + + @app.get("/", response_class=HTMLResponse) + async def index() -> HTMLResponse: + return HTMLResponse(content=load_index_html().decode("utf-8")) + + @app.get("/api/{subpath:path}") + async def api_get(subpath: str) -> JSONResponse: + code, payload = dispatch_api_get(f"/api/{subpath}", controller, deploy_service) + return JSONResponse(status_code=code, content=payload) + + @app.post("/api/{subpath:path}") + async def api_post(subpath: str, request: Request) -> JSONResponse: + body = await request.body() + content_type = request.headers.get("content-type", "") + query = parse_qs(request.url.query) + code, payload = dispatch_api_post( + f"/api/{subpath}", + body, + content_type, + query, + controller, + deploy_service, + ) + return JSONResponse(status_code=code, content=payload) + + @app.websocket("/ws/control") + async def ws_control(websocket: WebSocket) -> None: + await websocket.accept() + stop_event = asyncio.Event() + + async def telemetry_loop() -> None: + while not stop_event.is_set(): + msg = {"type": "telemetry", **build_status_payload(controller, deploy_service)} + await websocket.send_text(json.dumps(msg)) + await asyncio.sleep(0.1) + + sender = asyncio.create_task(telemetry_loop()) + try: + while True: + raw = await websocket.receive_text() + try: + data = json.loads(raw) + except Exception: + continue + ack = handle_ws_message(controller, data) + if ack is not None: + await websocket.send_text(json.dumps(ack)) + except WebSocketDisconnect: + pass + finally: + stop_event.set() + sender.cancel() + + return app diff --git a/hardware/unoq/python/web/static/index.html b/hardware/unoq/python/web/static/index.html new file mode 100644 index 0000000..6d2fdad --- /dev/null +++ b/hardware/unoq/python/web/static/index.html @@ -0,0 +1,619 @@ + + + + + + Microduck UNO Q Controller + + + +
+
MICRODUCK // UNO Q
+
+ IDLE + Idle + WS ... + IMU -- + --.-V + --C + --Hz +
+
+ + +
+
+ +
+ + + + +
+ +
+
+
+

Virtual Joystick (Hold & Drag)

+
+
Touch or drag anywhere here to walk / turn
+ + +
+
+ vx norm: 0.00 + wz norm: 0.00 +
+ + +
+ +
+

Skills & Modes

+
+ + + + + + + + +
+
Ready. Tap "Prepare Stand" when the robot is upright on the floor.
+
+
+ + +
+ + + + diff --git a/hardware/unoq/scripts/install_deps.sh b/hardware/unoq/scripts/install_deps.sh new file mode 100755 index 0000000..15e15c2 --- /dev/null +++ b/hardware/unoq/scripts/install_deps.sh @@ -0,0 +1,10 @@ +#!/usr/bin/env bash +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +REQ_FILE="${SCRIPT_DIR}/../python/requirements.txt" + +echo "[microduck-unoq] Installing Python dependencies from ${REQ_FILE}..." +python3 -m pip install --upgrade pip +python3 -m pip install -r "${REQ_FILE}" +echo "[microduck-unoq] Done." diff --git a/hardware/unoq/scripts/setup_ap.sh b/hardware/unoq/scripts/setup_ap.sh new file mode 100755 index 0000000..05b3986 --- /dev/null +++ b/hardware/unoq/scripts/setup_ap.sh @@ -0,0 +1,44 @@ +#!/usr/bin/env bash +set -euo pipefail + +# Configure an offline Wi-Fi Access Point on Arduino UNO Q (Debian + NetworkManager) +# Phone connects to this hotspot and opens http://192.168.4.1:8000 + +CON_NAME="${AP_CON_NAME:-microduck-ap}" +SSID="${AP_SSID:-Microduck-AP}" +PASS="${AP_PASS:-microduck123}" +IFACE="${AP_IFACE:-wlan0}" +IP_CIDR="${AP_IP_CIDR:-192.168.4.1/24}" + +if ! command -v nmcli >/dev/null 2>&1; then + echo "Error: nmcli (NetworkManager) not found. Configure hostapd + dnsmasq manually for ${IFACE} -> ${IP_CIDR}." >&2 + exit 1 +fi + +echo "[microduck-ap] Configuring Wi-Fi AP '${SSID}' on ${IFACE} (${IP_CIDR})..." + +if nmcli connection show "${CON_NAME}" >/dev/null 2>&1; then + nmcli connection delete "${CON_NAME}" +fi + +nmcli connection add \ + type wifi \ + ifname "${IFACE}" \ + con-name "${CON_NAME}" \ + autoconnect yes \ + ssid "${SSID}" + +nmcli connection modify "${CON_NAME}" \ + 802-11-wireless.mode ap \ + 802-11-wireless.band bg \ + ipv4.method shared \ + ipv4.addresses "${IP_CIDR}" \ + wifi-sec.key-mgmt wpa-psk \ + wifi-sec.psk "${PASS}" + +nmcli connection up "${CON_NAME}" + +echo "[microduck-ap] Hotspot active!" +echo " SSID: ${SSID}" +echo " Pass: ${PASS}" +echo " Control URL: http://${IP_CIDR%/*}:8000" diff --git a/hardware/unoq/scripts/usb_stage.py b/hardware/unoq/scripts/usb_stage.py new file mode 100644 index 0000000..1e60825 --- /dev/null +++ b/hardware/unoq/scripts/usb_stage.py @@ -0,0 +1,74 @@ +"""USB-only ONNX staging page; never loads policies or talks to actuators.""" +import hashlib +import json +import os +from pathlib import Path +import secrets +import tempfile +from http.server import BaseHTTPRequestHandler, HTTPServer + +ROOT = Path.home() / 'microduck-usb-staging' / 'models' +LIMIT = 16 * 1024 * 1024 +TOKEN = secrets.token_hex(24) +PAGE = '''Microduck USB · ONNX 写入 +

通过 USB 写入 ONNX

文件存入板上的 ~/microduck-usb-staging/models/,不会替换或启动当前策略。

+

+'''
+
+
+class Handler(BaseHTTPRequestHandler):
+    def allowed(self):
+        return self.headers.get('Host') == '127.0.0.1:18600'
+
+    def reply(self, data, kind):
+        self.send_response(200)
+        self.send_header('Content-Type', kind)
+        self.send_header('Content-Length', str(len(data)))
+        self.send_header('Cache-Control', 'no-store')
+        self.send_header('X-Frame-Options', 'DENY')
+        self.end_headers()
+        self.wfile.write(data)
+
+    def do_GET(self):
+        if not self.allowed() or self.path != '/':
+            return self.send_error(404)
+        self.reply(PAGE.replace('TOKEN_VALUE', TOKEN).encode(), 'text/html; charset=utf-8')
+
+    def do_POST(self):
+        if (not self.allowed() or self.path != '/stage' or self.headers.get('X-Stage-Token') != TOKEN
+                or self.headers.get('Origin') != 'http://127.0.0.1:18600'):
+            return self.send_error(403)
+        try:
+            size = int(self.headers.get('Content-Length', '0'))
+        except ValueError:
+            return self.send_error(400)
+        if not 0 < size <= LIMIT:
+            return self.send_error(413)
+        self.connection.settimeout(20)
+        data = self.rfile.read(size)
+        digest = hashlib.sha256(data).hexdigest()
+        if len(data) != size or digest != self.headers.get('X-SHA256'):
+            return self.send_error(400)
+        target = ROOT / (digest + '.onnx')
+        if not target.exists():
+            if sum(p.stat().st_size for p in ROOT.glob('*.onnx')) + size > 128 * 1024 * 1024:
+                return self.send_error(507)
+            with tempfile.NamedTemporaryFile(dir=ROOT, delete=False) as f:
+                tmp = Path(f.name)
+                try:
+                    f.write(data)
+                    f.flush()
+                    os.fsync(f.fileno())
+                    os.replace(tmp, target)
+                finally:
+                    tmp.unlink(missing_ok=True)
+        saved = hashlib.sha256(target.read_bytes()).hexdigest()
+        if saved != digest:
+            return self.send_error(500)
+        self.reply(json.dumps(dict(path=str(target), sha256=saved)).encode(), 'application/json')
+
+
+if __name__ == '__main__':
+    ROOT.mkdir(mode=0o700, parents=True, exist_ok=True)
+    HTTPServer(('127.0.0.1', 18600), Handler).serve_forever()
diff --git a/hardware/unoq/sketch/config.h b/hardware/unoq/sketch/config.h
new file mode 100644
index 0000000..c24435a
--- /dev/null
+++ b/hardware/unoq/sketch/config.h
@@ -0,0 +1,41 @@
+#pragma once
+
+#include 
+#include 
+
+// =============================================================================
+// Microduck UNO Q Hardware Pin & Bus Configuration
+// Centralizes physical wiring constants so schematic changes require one edit.
+// =============================================================================
+
+// Feetech HD-1910-C001 serial bus
+// On Arduino UNO Q, Serial1 maps to header D0 (RX / PB7) and D1 (TX / PB6).
+#define SERVO_SERIAL Serial1
+#define SERVO_BAUD 1000000UL
+
+// Set to a GPIO pin if using a 74HC126 / RS485 direction-controlled half-duplex
+// transceiver (HIGH = TX, LOW = RX). Leave -1 for resistor/diode auto-direction
+// single-wire boards (TX tied cleanly through hardware buffer) or Waveshare board.
+#define SERVO_DIR_PIN -1
+#define SERVO_DIR_TX_LEVEL HIGH
+#define SERVO_DIR_RX_LEVEL LOW
+
+// QMI8658 6-axis IMU on I2C
+// 0x6A when SA0/AD0 is LOW (default on many breakouts), 0x6B when HIGH.
+#define IMU_WIRE Wire
+#define IMU_I2C_ADDR 0x6A
+#define IMU_I2C_CLOCK_HZ 400000UL
+
+// IMU Mahony filter loop rate (runs non-blocking inside MCU loop())
+#define IMU_LOOP_INTERVAL_US 4000UL  // 250 Hz
+
+// Safety watchdog: disable servo torque if Linux stops sending hw_sync_step
+#define WATCHDOG_DEFAULT_MS 150UL
+
+// Servo feedback strategy:
+// 1 = Feetech SyncRead (0x82) followed by single-read fallback if bus drops packet
+// 0 = Sequential single Read (0x02) per joint (slower, simpler fallback for noisy lines)
+#define USE_FEETECH_SYNC_READ 1
+
+// Total joints on Microduck
+#define NUM_JOINTS 14
diff --git a/hardware/unoq/sketch/feetech_hls.cpp b/hardware/unoq/sketch/feetech_hls.cpp
new file mode 100644
index 0000000..dbded83
--- /dev/null
+++ b/hardware/unoq/sketch/feetech_hls.cpp
@@ -0,0 +1,349 @@
+#include "feetech_hls.h"
+
+#include 
+
+namespace {
+constexpr uint8_t kHeader = 0xFF;
+constexpr uint8_t kBroadcastId = 0xFE;
+
+constexpr uint8_t kInstPing = 0x01;
+constexpr uint8_t kInstRead = 0x02;
+constexpr uint8_t kInstWrite = 0x03;
+constexpr uint8_t kInstSyncRead = 0x82;
+constexpr uint8_t kInstSyncWrite = 0x83;
+
+constexpr uint8_t kRegTorqueEnable = 0x28;
+constexpr uint8_t kRegAcc = 0x29;
+constexpr uint8_t kRegPresentPos = 0x38;
+constexpr uint8_t kFeedbackReadLen = 8;  // pos(2), speed(2), load(2), volt(1), temp(1)
+}  // namespace
+
+void FeetechHlsBus::begin(HardwareSerial& serial, uint32_t baud, int dir_pin) {
+  serial_ = &serial;
+  dir_pin_ = dir_pin;
+  if (dir_pin_ >= 0) {
+    pinMode(dir_pin_, OUTPUT);
+    setTxMode(false);
+  }
+  serial_->begin(baud);
+  delay(5);
+  flushInput();
+}
+
+void FeetechHlsBus::setTxMode(bool tx) {
+  if (dir_pin_ >= 0) {
+    digitalWrite(dir_pin_, tx ? SERVO_DIR_TX_LEVEL : SERVO_DIR_RX_LEVEL);
+  }
+}
+
+void FeetechHlsBus::flushInput() {
+  if (!serial_) return;
+  while (serial_->available() > 0) {
+    (void)serial_->read();
+  }
+}
+
+uint8_t FeetechHlsBus::computeChecksum(const uint8_t* body_from_id, size_t body_len) {
+  uint32_t sum = 0;
+  for (size_t i = 0; i < body_len; ++i) {
+    sum += body_from_id[i];
+  }
+  return static_cast((~sum) & 0xFFU);
+}
+
+void FeetechHlsBus::sendRawPacket(const uint8_t* pkt, size_t len) {
+  if (!serial_ || len == 0 || len > sizeof(last_tx_)) return;
+  flushInput();
+  memcpy(last_tx_, pkt, len);
+  last_tx_len_ = len;
+  setTxMode(true);
+  serial_->write(pkt, len);
+  serial_->flush();
+  delayMicroseconds(8);
+  setTxMode(false);
+
+  // Strip local half-duplex TX echo if TX/RX share a single physical trace without gating
+  if (dir_pin_ < 0) {
+    uint32_t t0 = micros();
+    size_t matched = 0;
+    while (matched < len && (micros() - t0) < 400) {
+      if (serial_->available() > 0) {
+        int b = serial_->peek();
+        if (b == last_tx_[matched]) {
+          (void)serial_->read();
+          matched++;
+        } else {
+          break;
+        }
+      }
+    }
+  }
+}
+
+void FeetechHlsBus::sendPacket(uint8_t id, uint8_t inst, const uint8_t* params, uint8_t param_len) {
+  uint8_t pkt[128];
+  uint8_t length_field = static_cast(param_len + 2);
+  pkt[0] = kHeader;
+  pkt[1] = kHeader;
+  pkt[2] = id;
+  pkt[3] = length_field;
+  pkt[4] = inst;
+  for (uint8_t i = 0; i < param_len; ++i) {
+    pkt[5 + i] = params[i];
+  }
+  pkt[5 + param_len] = computeChecksum(&pkt[2], static_cast(3 + param_len));
+  sendRawPacket(pkt, static_cast(6 + param_len));
+}
+
+bool FeetechHlsBus::readPacketForId(
+    uint8_t expected_id,
+    uint8_t* out_err,
+    uint8_t* params,
+    uint8_t expected_len,
+    uint32_t timeout_us) {
+  if (!serial_) return false;
+  uint32_t t0 = micros();
+  uint8_t state = 0;
+  uint8_t id = 0, len_field = 0, err = 0;
+  uint8_t buf[64] = {};
+  uint8_t idx = 0;
+  uint8_t payload_len = 0;
+
+  while ((micros() - t0) < timeout_us) {
+    if (serial_->available() <= 0) {
+      continue;
+    }
+    uint8_t b = static_cast(serial_->read());
+    switch (state) {
+      case 0:
+        if (b == kHeader) state = 1;
+        break;
+      case 1:
+        if (b == kHeader) {
+          state = 2;
+        } else {
+          state = 0;
+        }
+        break;
+      case 2:
+        if (b == kHeader) break;  // extra 0xFF header alignment
+        id = b;
+        state = 3;
+        break;
+      case 3:
+        len_field = b;
+        if (len_field < 2 || len_field > 60) {
+          state = 0;
+        } else {
+          payload_len = static_cast(len_field - 2);
+          state = 4;
+        }
+        break;
+      case 4:
+        err = b;
+        idx = 0;
+        state = (payload_len == 0) ? 6 : 5;
+        break;
+      case 5:
+        buf[idx++] = b;
+        if (idx >= payload_len) state = 6;
+        break;
+      case 6: {
+        uint8_t check_buf[64];
+        check_buf[0] = id;
+        check_buf[1] = len_field;
+        check_buf[2] = err;
+        for (uint8_t i = 0; i < payload_len; ++i) check_buf[3 + i] = buf[i];
+        uint8_t chk = computeChecksum(check_buf, static_cast(3 + payload_len));
+        if (chk == b && id == expected_id && payload_len >= expected_len) {
+          if (out_err) *out_err = err;
+          if (params && expected_len > 0) memcpy(params, buf, expected_len);
+          return true;
+        }
+        state = 0;
+        break;
+      }
+      default:
+        state = 0;
+        break;
+    }
+  }
+  return false;
+}
+
+bool FeetechHlsBus::ping(uint8_t id, uint32_t timeout_us) {
+  sendPacket(id, kInstPing, nullptr, 0);
+  uint8_t err = 0;
+  return readPacketForId(id, &err, nullptr, 0, timeout_us);
+}
+
+int FeetechHlsBus::pingRange(uint8_t max_id, int32_t* out_ids, int max_out) {
+  int found = 0;
+  for (uint8_t id = 1; id <= max_id && found < max_out; ++id) {
+    if (ping(id, 2000)) {
+      out_ids[found++] = static_cast(id);
+    }
+  }
+  return found;
+}
+
+bool FeetechHlsBus::setTorqueBroadcast(bool enable) {
+  uint8_t params[2] = {kRegTorqueEnable, static_cast(enable ? 1 : 0)};
+  sendPacket(kBroadcastId, kInstWrite, params, 2);
+  delayMicroseconds(120);
+  return true;
+}
+
+bool FeetechHlsBus::setTorqueAll(const uint8_t* ids, int count, bool enable) {
+  setTorqueBroadcast(enable);
+  // Also sync-write individual entries cleanly in case broadcast is masked in servo EEPROM
+  uint8_t pkt[64];
+  if (count <= 0 || count > NUM_JOINTS) return false;
+  uint8_t param_len = static_cast(2 + count * 2);
+  pkt[0] = kHeader;
+  pkt[1] = kHeader;
+  pkt[2] = kBroadcastId;
+  pkt[3] = static_cast(param_len + 2);
+  pkt[4] = kInstSyncWrite;
+  pkt[5] = kRegTorqueEnable;
+  pkt[6] = 1;
+  uint8_t val = enable ? 1 : 0;
+  for (int i = 0; i < count; ++i) {
+    pkt[7 + i * 2] = ids[i];
+    pkt[8 + i * 2] = val;
+  }
+  pkt[5 + param_len] = computeChecksum(&pkt[2], static_cast(3 + param_len));
+  sendRawPacket(pkt, static_cast(6 + param_len));
+  return true;
+}
+
+bool FeetechHlsBus::syncWriteTargets(
+    const uint8_t* ids,
+    int count,
+    const int32_t* target_ticks,
+    uint8_t acc,
+    uint16_t speed_limit) {
+  if (!serial_ || count <= 0 || count > NUM_JOINTS) return false;
+  // 7 bytes starting at 0x29: [acc, pos_L, pos_H, time_L=0, time_H=0, spd_L, spd_H]
+  constexpr uint8_t kPerServo = 7;
+  uint8_t pkt[128];
+  uint8_t param_len = static_cast(2 + count * (1 + kPerServo));
+  pkt[0] = kHeader;
+  pkt[1] = kHeader;
+  pkt[2] = kBroadcastId;
+  pkt[3] = static_cast(param_len + 2);
+  pkt[4] = kInstSyncWrite;
+  pkt[5] = kRegAcc;
+  pkt[6] = kPerServo;
+
+  for (int i = 0; i < count; ++i) {
+    int32_t ticks = target_ticks[i];
+    if (ticks < 0) ticks = 0;
+    if (ticks > 4095) ticks = 4095;
+    uint16_t pos = static_cast(ticks);
+    size_t off = 7 + static_cast(i) * (1 + kPerServo);
+    pkt[off + 0] = ids[i];
+    pkt[off + 1] = acc;
+    pkt[off + 2] = static_cast(pos & 0xFFU);
+    pkt[off + 3] = static_cast((pos >> 8) & 0xFFU);
+    pkt[off + 4] = 0x00;  // moving time low
+    pkt[off + 5] = 0x00;  // moving time high
+    pkt[off + 6] = static_cast(speed_limit & 0xFFU);
+    pkt[off + 7] = static_cast((speed_limit >> 8) & 0xFFU);
+  }
+  pkt[5 + param_len] = computeChecksum(&pkt[2], static_cast(3 + param_len));
+  sendRawPacket(pkt, static_cast(6 + param_len));
+  return true;
+}
+
+bool FeetechHlsBus::readSingleJoint(
+    uint8_t id,
+    int32_t* out_tick,
+    int32_t* out_speed,
+    float* out_voltage_v,
+    float* out_temp_c) {
+  uint8_t params[2] = {kRegPresentPos, kFeedbackReadLen};
+  sendPacket(id, kInstRead, params, 2);
+  uint8_t err = 0;
+  uint8_t buf[kFeedbackReadLen] = {};
+  if (!readPacketForId(id, &err, buf, kFeedbackReadLen, 1600)) {
+    return false;
+  }
+  uint16_t pos_u16 = static_cast(buf[0]) | (static_cast(buf[1]) << 8);
+  uint16_t spd_u16 = static_cast(buf[2]) | (static_cast(buf[3]) << 8);
+  *out_tick = static_cast(pos_u16 & 0x7FFFU);
+  *out_speed = static_cast(spd_u16);
+  *out_voltage_v = static_cast(buf[6]) * 0.1f;
+  *out_temp_c = static_cast(buf[7]);
+  return err == 0;
+}
+
+uint32_t FeetechHlsBus::readFeedback(
+    const uint8_t* ids,
+    int count,
+    int32_t* out_ticks,
+    int32_t* out_speeds,
+    float* out_min_voltage_v,
+    float* out_max_temp_c) {
+  uint32_t err_mask = 0;
+  float min_v = 99.0f;
+  float max_t = -40.0f;
+  bool any_ok = false;
+
+#if USE_FEETECH_SYNC_READ
+  uint8_t sr_params[2 + NUM_JOINTS];
+  sr_params[0] = kRegPresentPos;
+  sr_params[1] = kFeedbackReadLen;
+  for (int i = 0; i < count; ++i) {
+    sr_params[2 + i] = ids[i];
+  }
+  sendPacket(kBroadcastId, kInstSyncRead, sr_params, static_cast(2 + count));
+
+  bool got[NUM_JOINTS] = {};
+  for (int i = 0; i < count; ++i) {
+    uint8_t err = 0;
+    uint8_t buf[kFeedbackReadLen] = {};
+    if (readPacketForId(ids[i], &err, buf, kFeedbackReadLen, 1200)) {
+      got[i] = true;
+      uint16_t pos_u16 = static_cast(buf[0]) | (static_cast(buf[1]) << 8);
+      uint16_t spd_u16 = static_cast(buf[2]) | (static_cast(buf[3]) << 8);
+      out_ticks[i] = static_cast(pos_u16 & 0x7FFFU);
+      out_speeds[i] = static_cast(spd_u16);
+      float v = static_cast(buf[6]) * 0.1f;
+      float t = static_cast(buf[7]);
+      if (v > 0.1f && v < min_v) min_v = v;
+      if (t > max_t) max_t = t;
+      any_ok = true;
+      if (err != 0) err_mask |= (1UL << i);
+    }
+  }
+  // Fallback single-read for any joints dropped during SyncRead
+  for (int i = 0; i < count; ++i) {
+    if (!got[i]) {
+      float v = 0.0f, t = 0.0f;
+      if (readSingleJoint(ids[i], &out_ticks[i], &out_speeds[i], &v, &t)) {
+        if (v > 0.1f && v < min_v) min_v = v;
+        if (t > max_t) max_t = t;
+        any_ok = true;
+      } else {
+        err_mask |= (1UL << i);
+      }
+    }
+  }
+#else
+  for (int i = 0; i < count; ++i) {
+    float v = 0.0f, t = 0.0f;
+    if (readSingleJoint(ids[i], &out_ticks[i], &out_speeds[i], &v, &t)) {
+      if (v > 0.1f && v < min_v) min_v = v;
+      if (t > max_t) max_t = t;
+      any_ok = true;
+    } else {
+      err_mask |= (1UL << i);
+    }
+  }
+#endif
+
+  *out_min_voltage_v = any_ok ? min_v : 0.0f;
+  *out_max_temp_c = any_ok ? max_t : 0.0f;
+  return err_mask;
+}
diff --git a/hardware/unoq/sketch/feetech_hls.h b/hardware/unoq/sketch/feetech_hls.h
new file mode 100644
index 0000000..cea4365
--- /dev/null
+++ b/hardware/unoq/sketch/feetech_hls.h
@@ -0,0 +1,50 @@
+#pragma once
+
+#include 
+#include "config.h"
+
+class FeetechHlsBus {
+ public:
+  void begin(HardwareSerial& serial, uint32_t baud, int dir_pin = SERVO_DIR_PIN);
+
+  bool ping(uint8_t id, uint32_t timeout_us = 1800);
+  int pingRange(uint8_t max_id, int32_t* out_ids, int max_out);
+
+  bool setTorqueBroadcast(bool enable);
+  bool setTorqueAll(const uint8_t* ids, int count, bool enable);
+
+  bool syncWriteTargets(
+      const uint8_t* ids,
+      int count,
+      const int32_t* target_ticks,
+      uint8_t acc,
+      uint16_t speed_limit);
+
+  // Returns bitmask of joints with comm/servo error (bit i set -> joint i fault/timeout)
+  uint32_t readFeedback(
+      const uint8_t* ids,
+      int count,
+      int32_t* out_ticks,
+      int32_t* out_speeds,
+      float* out_min_voltage_v,
+      float* out_max_temp_c);
+
+ private:
+  HardwareSerial* serial_ = nullptr;
+  int dir_pin_ = -1;
+  uint8_t last_tx_[128] = {};
+  size_t last_tx_len_ = 0;
+
+  void setTxMode(bool tx);
+  void flushInput();
+  void sendRawPacket(const uint8_t* pkt, size_t len);
+  void sendPacket(uint8_t id, uint8_t inst, const uint8_t* params, uint8_t param_len);
+  bool readPacketForId(uint8_t expected_id, uint8_t* out_err, uint8_t* params, uint8_t expected_len, uint32_t timeout_us);
+  bool readSingleJoint(
+      uint8_t id,
+      int32_t* out_tick,
+      int32_t* out_speed,
+      float* out_voltage_v,
+      float* out_temp_c);
+  static uint8_t computeChecksum(const uint8_t* body_from_id, size_t body_len);
+};
diff --git a/hardware/unoq/sketch/qmi8658.cpp b/hardware/unoq/sketch/qmi8658.cpp
new file mode 100644
index 0000000..62dc08d
--- /dev/null
+++ b/hardware/unoq/sketch/qmi8658.cpp
@@ -0,0 +1,216 @@
+#include "qmi8658.h"
+
+#include 
+
+namespace {
+constexpr uint8_t kRegWhoAmI = 0x00;
+constexpr uint8_t kRegCtrl1 = 0x02;
+constexpr uint8_t kRegCtrl2 = 0x03;
+constexpr uint8_t kRegCtrl3 = 0x04;
+constexpr uint8_t kRegCtrl5 = 0x06;
+constexpr uint8_t kRegCtrl7 = 0x08;
+constexpr uint8_t kRegAxL = 0x35;
+constexpr uint8_t kExpectedWhoAmI = 0x05;
+
+// CTRL2: aFS=±4g (1 << 4), aODR=250Hz (5) -> 8192 LSB/g
+constexpr uint8_t kCtrl2Val = 0x15;
+constexpr float kAccelScaleG = 4.0f / 32768.0f;
+
+// CTRL3: gFS=±1024 dps (6 << 4), gODR=448.4Hz (4) -> 32 LSB/dps
+constexpr uint8_t kCtrl3Val = 0x64;
+constexpr float kGyroScaleRadS = (1024.0f / 32768.0f) * (PI / 180.0f);
+
+// CTRL5: enable LPF mode 2 on both accel and gyro
+constexpr uint8_t kCtrl5Val = 0x55;
+
+constexpr float kTwoKp = 2.0f * 1.2f;  // Proportional gain
+constexpr float kTwoKi = 2.0f * 0.02f; // Integral gain
+}  // namespace
+
+bool Qmi8658Imu::writeReg(uint8_t reg, uint8_t val) {
+  wire_->beginTransmission(addr_);
+  wire_->write(reg);
+  wire_->write(val);
+  return wire_->endTransmission(true) == 0;
+}
+
+bool Qmi8658Imu::readRegs(uint8_t reg, uint8_t* buf, size_t len) {
+  wire_->beginTransmission(addr_);
+  wire_->write(reg);
+  if (wire_->endTransmission(false) != 0) {
+    return false;
+  }
+  size_t got = wire_->requestFrom(static_cast(addr_), static_cast(len));
+  if (got != len) {
+    while (wire_->available()) {
+      (void)wire_->read();
+    }
+    return false;
+  }
+  for (size_t i = 0; i < len; ++i) {
+    buf[i] = static_cast(wire_->read());
+  }
+  return true;
+}
+
+bool Qmi8658Imu::begin(TwoWire& wire, uint8_t addr) {
+  wire_ = &wire;
+  addr_ = addr;
+  healthy_ = false;
+  err_streak_ = 0;
+
+  uint8_t who = 0;
+  if (!readRegs(kRegWhoAmI, &who, 1) || who != kExpectedWhoAmI) {
+    return false;
+  }
+
+  // CTRL1 = 0x40 sets ADDR_AI (auto-increment for burst reads) and little-endian data
+  if (!writeReg(kRegCtrl7, 0x00)) return false;
+  delay(2);
+  if (!writeReg(kRegCtrl1, 0x40)) return false;
+  if (!writeReg(kRegCtrl2, kCtrl2Val)) return false;
+  if (!writeReg(kRegCtrl3, kCtrl3Val)) return false;
+  if (!writeReg(kRegCtrl5, kCtrl5Val)) return false;
+  if (!writeReg(kRegCtrl7, 0x03)) return false;  // Enable accel + gyro
+  delay(10);
+
+  // Align initial Mahony orientation from static gravity vector if available
+  q0_ = 1.0f;
+  q1_ = 0.0f;
+  q2_ = 0.0f;
+  q3_ = 0.0f;
+  ix_ = 0.0f;
+  iy_ = 0.0f;
+  iz_ = 0.0f;
+
+  if (readSampleRaw()) {
+    for (int i = 0; i < 40; ++i) {
+      mahonyUpdate(0.0f, 0.0f, 0.0f, raw_accel_[0], raw_accel_[1], raw_accel_[2], 0.01f);
+    }
+  }
+  healthy_ = true;
+  return true;
+}
+
+void Qmi8658Imu::setGyroBias(float bx, float by, float bz) {
+  gyro_bias_[0] = bx;
+  gyro_bias_[1] = by;
+  gyro_bias_[2] = bz;
+}
+
+bool Qmi8658Imu::readSampleRaw() {
+  if (!wire_) return false;
+  uint8_t buf[12];
+  if (!readRegs(kRegAxL, buf, sizeof(buf))) {
+    return false;
+  }
+  auto to_i16 = [](uint8_t lo, uint8_t hi) -> int16_t {
+    return static_cast((static_cast(hi) << 8) | lo);
+  };
+  raw_accel_[0] = static_cast(to_i16(buf[0], buf[1])) * kAccelScaleG;
+  raw_accel_[1] = static_cast(to_i16(buf[2], buf[3])) * kAccelScaleG;
+  raw_accel_[2] = static_cast(to_i16(buf[4], buf[5])) * kAccelScaleG;
+  raw_gyro_[0] = static_cast(to_i16(buf[6], buf[7])) * kGyroScaleRadS;
+  raw_gyro_[1] = static_cast(to_i16(buf[8], buf[9])) * kGyroScaleRadS;
+  raw_gyro_[2] = static_cast(to_i16(buf[10], buf[11])) * kGyroScaleRadS;
+  return true;
+}
+
+bool Qmi8658Imu::update(float dt_s) {
+  if (!readSampleRaw()) {
+    if (err_streak_ < 255) err_streak_++;
+    if (err_streak_ >= 10) healthy_ = false;
+    return false;
+  }
+  err_streak_ = 0;
+  healthy_ = true;
+
+  float gx = raw_gyro_[0] - gyro_bias_[0];
+  float gy = raw_gyro_[1] - gyro_bias_[1];
+  float gz = raw_gyro_[2] - gyro_bias_[2];
+  mahonyUpdate(gx, gy, gz, raw_accel_[0], raw_accel_[1], raw_accel_[2], dt_s);
+  return true;
+}
+
+void Qmi8658Imu::mahonyUpdate(float gx, float gy, float gz, float ax, float ay, float az, float dt_s) {
+  float norm = sqrtf(ax * ax + ay * ay + az * az);
+  if (norm > 0.25f) {
+    float inv = 1.0f / norm;
+    ax *= inv;
+    ay *= inv;
+    az *= inv;
+
+    // Estimated world-UP in body frame
+    float vx = 2.0f * (q1_ * q3_ - q0_ * q2_);
+    float vy = 2.0f * (q0_ * q1_ + q2_ * q3_);
+    float vz = 1.0f - 2.0f * (q1_ * q1_ + q2_ * q2_);
+
+    float ex = ay * vz - az * vy;
+    float ey = az * vx - ax * vz;
+    float ez = ax * vy - ay * vx;
+
+    if (kTwoKi > 0.0f) {
+      ix_ += kTwoKi * ex * dt_s;
+      iy_ += kTwoKi * ey * dt_s;
+      iz_ += kTwoKi * ez * dt_s;
+      gx += ix_;
+      gy += iy_;
+      gz += iz_;
+    }
+    gx += kTwoKp * ex;
+    gy += kTwoKp * ey;
+    gz += kTwoKp * ez;
+  }
+
+  float half_dt = 0.5f * dt_s;
+  float dq0 = (-q1_ * gx - q2_ * gy - q3_ * gz) * half_dt;
+  float dq1 = (q0_ * gx + q2_ * gz - q3_ * gy) * half_dt;
+  float dq2 = (q0_ * gy - q1_ * gz + q3_ * gx) * half_dt;
+  float dq3 = (q0_ * gz + q1_ * gy - q2_ * gx) * half_dt;
+  q0_ += dq0;
+  q1_ += dq1;
+  q2_ += dq2;
+  q3_ += dq3;
+
+  float qnorm = sqrtf(q0_ * q0_ + q1_ * q1_ + q2_ * q2_ + q3_ * q3_);
+  if (qnorm > 1e-6f) {
+    float inv_q = 1.0f / qnorm;
+    q0_ *= inv_q;
+    q1_ *= inv_q;
+    q2_ *= inv_q;
+    q3_ *= inv_q;
+  }
+}
+
+bool Qmi8658Imu::sampleStationaryGyroBias(int samples, float out_bias[3]) {
+  if (samples <= 0) samples = 1;
+  double sx = 0.0, sy = 0.0, sz = 0.0;
+  int ok_cnt = 0;
+  for (int i = 0; i < samples; ++i) {
+    if (readSampleRaw()) {
+      sx += raw_gyro_[0];
+      sy += raw_gyro_[1];
+      sz += raw_gyro_[2];
+      ok_cnt++;
+    }
+    delay(4);
+  }
+  if (ok_cnt == 0) return false;
+  out_bias[0] = static_cast(sx / ok_cnt);
+  out_bias[1] = static_cast(sy / ok_cnt);
+  out_bias[2] = static_cast(sz / ok_cnt);
+  return true;
+}
+
+void Qmi8658Imu::getRawGyro(float out[3]) const {
+  out[0] = raw_gyro_[0];
+  out[1] = raw_gyro_[1];
+  out[2] = raw_gyro_[2];
+}
+
+void Qmi8658Imu::getProjectedGravity(float out[3]) const {
+  // Gravity DOWN in world [0, 0, -1] rotated into body frame = -v_up
+  out[0] = -2.0f * (q1_ * q3_ - q0_ * q2_);
+  out[1] = -2.0f * (q0_ * q1_ + q2_ * q3_);
+  out[2] = -(1.0f - 2.0f * (q1_ * q1_ + q2_ * q2_));
+}
diff --git a/hardware/unoq/sketch/qmi8658.h b/hardware/unoq/sketch/qmi8658.h
new file mode 100644
index 0000000..38f5327
--- /dev/null
+++ b/hardware/unoq/sketch/qmi8658.h
@@ -0,0 +1,36 @@
+#pragma once
+
+#include 
+#include 
+
+class Qmi8658Imu {
+ public:
+  bool begin(TwoWire& wire, uint8_t addr);
+  void setGyroBias(float bx, float by, float bz);
+  bool update(float dt_s);
+  bool sampleStationaryGyroBias(int samples, float out_bias[3]);
+
+  bool isHealthy() const { return healthy_; }
+  void getRawGyro(float out[3]) const;
+  void getProjectedGravity(float out[3]) const;
+
+ private:
+  TwoWire* wire_ = nullptr;
+  uint8_t addr_ = 0x6A;
+  bool healthy_ = false;
+  uint8_t err_streak_ = 0;
+
+  // Bias subtracted strictly inside Mahony filter (raw gyro stays unbiased for Python HAL)
+  float gyro_bias_[3] = {0.0f, 0.0f, 0.0f};
+  float raw_gyro_[3] = {0.0f, 0.0f, 0.0f};
+  float raw_accel_[3] = {0.0f, 0.0f, 1.0f};
+
+  // Mahony quaternion (body -> world) and integral feedback
+  float q0_ = 1.0f, q1_ = 0.0f, q2_ = 0.0f, q3_ = 0.0f;
+  float ix_ = 0.0f, iy_ = 0.0f, iz_ = 0.0f;
+
+  bool writeReg(uint8_t reg, uint8_t val);
+  bool readRegs(uint8_t reg, uint8_t* buf, size_t len);
+  bool readSampleRaw();
+  void mahonyUpdate(float gx, float gy, float gz, float ax, float ay, float az, float dt_s);
+};
diff --git a/hardware/unoq/sketch/sketch.ino b/hardware/unoq/sketch/sketch.ino
new file mode 100644
index 0000000..2040dbc
--- /dev/null
+++ b/hardware/unoq/sketch/sketch.ino
@@ -0,0 +1,195 @@
+#include 
+#include 
+#include 
+
+#include "config.h"
+#include "feetech_hls.h"
+#include "qmi8658.h"
+
+namespace {
+Qmi8658Imu g_imu;
+FeetechHlsBus g_servo;
+
+uint8_t g_servo_ids[NUM_JOINTS] = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14};
+uint32_t g_watchdog_ms = WATCHDOG_DEFAULT_MS;
+uint32_t g_last_sync_ms = 0;
+uint32_t g_last_imu_us = 0;
+
+bool g_torque_enabled = false;
+bool g_imu_ready = false;
+
+int32_t g_last_ticks[NUM_JOINTS] = {2048, 2048, 2048, 2048, 2048, 2048, 2048, 2048, 2048, 2048, 2048, 2048, 2048, 2048};
+int32_t g_last_speeds[NUM_JOINTS] = {};
+}  // namespace
+
+int32_t hw_configure(std::vector payload) {
+  g_torque_enabled = false;
+  g_servo.setTorqueAll(g_servo_ids, NUM_JOINTS, false);
+
+  uint32_t baud = SERVO_BAUD;
+  uint8_t i2c_addr = IMU_I2C_ADDR;
+  if (payload.size() >= 3) {
+    baud = static_cast(payload[0] > 9600.0f ? payload[0] : SERVO_BAUD);
+    i2c_addr = static_cast(static_cast(payload[1] + 0.5f));
+    g_watchdog_ms = static_cast(payload[2] >= 40.0f ? payload[2] : WATCHDOG_DEFAULT_MS);
+  }
+  if (payload.size() >= 6) {
+    g_imu.setGyroBias(payload[3], payload[4], payload[5]);
+  }
+  if (payload.size() >= 6 + NUM_JOINTS) {
+    for (int i = 0; i < NUM_JOINTS; ++i) {
+      int sid = static_cast(payload[6 + i] + 0.5f);
+      if (sid >= 0 && sid <= 253) {
+        g_servo_ids[i] = static_cast(sid);
+      }
+    }
+  }
+
+  g_servo.begin(SERVO_SERIAL, baud, SERVO_DIR_PIN);
+  g_servo.setTorqueAll(g_servo_ids, NUM_JOINTS, false);
+
+  IMU_WIRE.begin();
+  IMU_WIRE.setClock(IMU_I2C_CLOCK_HZ);
+  g_imu_ready = g_imu.begin(IMU_WIRE, i2c_addr);
+  if (payload.size() >= 6) {
+    g_imu.setGyroBias(payload[3], payload[4], payload[5]);
+  }
+
+  g_last_sync_ms = millis();
+  g_last_imu_us = micros();
+  return g_imu_ready ? 1 : 0;
+}
+
+std::vector hw_sync_step(std::vector cmd_frame) {
+  std::vector res(38, 0.0f);
+  if (cmd_frame.size() < 3 + NUM_JOINTS) {
+    res[36] = g_imu.isHealthy() ? 1.0f : 0.0f;
+    res[37] = static_cast(0xFFFF);
+    return res;
+  }
+
+  bool want_torque = cmd_frame[0] != 0;
+  uint16_t speed_limit = static_cast(cmd_frame[1] > 0 ? cmd_frame[1] : 0);
+  uint8_t acc = static_cast(cmd_frame[2] >= 0 ? (cmd_frame[2] <= 254 ? cmd_frame[2] : 254) : 0);
+
+  int32_t target_ticks[NUM_JOINTS];
+  for (int i = 0; i < NUM_JOINTS; ++i) {
+    target_ticks[i] = cmd_frame[3 + i];
+  }
+
+  // Write targets BEFORE enabling torque on 0->1 transition to prevent initial snap
+  if (want_torque) {
+    g_servo.syncWriteTargets(g_servo_ids, NUM_JOINTS, target_ticks, acc, speed_limit);
+    if (!g_torque_enabled) {
+      g_servo.setTorqueAll(g_servo_ids, NUM_JOINTS, true);
+      g_torque_enabled = true;
+    }
+  } else {
+    if (g_torque_enabled) {
+      g_servo.setTorqueAll(g_servo_ids, NUM_JOINTS, false);
+      g_torque_enabled = false;
+    }
+  }
+
+  g_last_sync_ms = millis();
+
+  // Sample fresh IMU reading if at least 2ms elapsed since last 250Hz integration tick
+  uint32_t now_us = micros();
+  uint32_t dt_us = now_us - g_last_imu_us;
+  if (dt_us >= 2000UL && dt_us < 100000UL) {
+    g_imu.update(static_cast(dt_us) * 1e-6f);
+    g_last_imu_us = now_us;
+  }
+
+  float min_v = 0.0f;
+  float max_t = 0.0f;
+  uint32_t err_mask = g_servo.readFeedback(
+      g_servo_ids, NUM_JOINTS, g_last_ticks, g_last_speeds, &min_v, &max_t);
+
+  float gyro[3] = {0.0f, 0.0f, 0.0f};
+  float grav[3] = {0.0f, 0.0f, -1.0f};
+  g_imu.getRawGyro(gyro);
+  g_imu.getProjectedGravity(grav);
+
+  res[0] = gyro[0];
+  res[1] = gyro[1];
+  res[2] = gyro[2];
+  res[3] = grav[0];
+  res[4] = grav[1];
+  res[5] = grav[2];
+  for (int i = 0; i < NUM_JOINTS; ++i) {
+    res[6 + i] = static_cast(g_last_ticks[i]);
+    res[20 + i] = static_cast(g_last_speeds[i]);
+  }
+  res[34] = min_v;
+  res[35] = max_t;
+  res[36] = g_imu.isHealthy() ? 1.0f : 0.0f;
+  res[37] = static_cast(err_mask);
+  return res;
+}
+
+int32_t hw_set_torque(int32_t enable) {
+  bool on = enable != 0;
+  g_torque_enabled = on;
+  g_servo.setTorqueAll(g_servo_ids, NUM_JOINTS, on);
+  g_last_sync_ms = millis();
+  return 1;
+}
+
+std::vector hw_ping_servos(int32_t max_id) {
+  if (max_id < 1) max_id = 1;
+  if (max_id > 253) max_id = 253;
+  int32_t found[64];
+  int n = g_servo.pingRange(static_cast(max_id), found, 64);
+  return std::vector(found, found + n);
+}
+
+std::vector hw_calibrate_imu(int32_t samples) {
+  if (samples < 10) samples = 10;
+  if (samples > 1000) samples = 1000;
+  float bias[3] = {0.0f, 0.0f, 0.0f};
+  g_imu.sampleStationaryGyroBias(static_cast(samples), bias);
+  g_imu.setGyroBias(bias[0], bias[1], bias[2]);
+  return {bias[0], bias[1], bias[2]};
+}
+
+void setup() {
+  Bridge.begin();
+  Monitor.begin(115200);
+
+  g_servo.begin(SERVO_SERIAL, SERVO_BAUD, SERVO_DIR_PIN);
+  g_servo.setTorqueBroadcast(false);
+
+  IMU_WIRE.begin();
+  IMU_WIRE.setClock(IMU_I2C_CLOCK_HZ);
+  g_imu_ready = g_imu.begin(IMU_WIRE, IMU_I2C_ADDR);
+
+  Bridge.provide_safe("hw_configure", hw_configure);
+  Bridge.provide_safe("hw_sync_step", hw_sync_step);
+  Bridge.provide_safe("hw_set_torque", hw_set_torque);
+  Bridge.provide_safe("hw_ping_servos", hw_ping_servos);
+  Bridge.provide_safe("hw_calibrate_imu", hw_calibrate_imu);
+
+  g_last_sync_ms = millis();
+  g_last_imu_us = micros();
+  Monitor.println("Microduck UNO Q STM32 firmware initialized (torque off).");
+}
+
+void loop() {
+  uint32_t now_us = micros();
+  uint32_t dt_us = now_us - g_last_imu_us;
+  if (dt_us >= IMU_LOOP_INTERVAL_US) {
+    float dt_s = static_cast(dt_us) * 1e-6f;
+    if (dt_s > 0.05f) dt_s = 0.004f;
+    g_imu.update(dt_s);
+    g_last_imu_us = now_us;
+  }
+
+  // Hardware safety watchdog: disable servo torque if Linux 50Hz loop stalls
+  if (g_torque_enabled && (millis() - g_last_sync_ms) > g_watchdog_ms) {
+    g_torque_enabled = false;
+    g_servo.setTorqueBroadcast(false);
+    g_servo.setTorqueAll(g_servo_ids, NUM_JOINTS, false);
+    Monitor.println("WATCHDOG: Linux sync_step timeout -> servo torque disabled.");
+  }
+}
diff --git a/hardware/unoq/sketch/sketch.yaml b/hardware/unoq/sketch/sketch.yaml
new file mode 100644
index 0000000..e442ad0
--- /dev/null
+++ b/hardware/unoq/sketch/sketch.yaml
@@ -0,0 +1,12 @@
+profiles:
+  default:
+    fqbn: arduino:zephyr:unoq
+    platforms:
+      - platform: arduino:zephyr
+    libraries:
+      - Arduino_RouterBridge
+      - Arduino_RPClite
+      - ArxContainer
+      - ArxTypeTraits
+      - DebugLog
+      - MsgPack
diff --git a/hardware/xgoduck/README.md b/hardware/xgoduck/README.md
new file mode 100644
index 0000000..e923997
--- /dev/null
+++ b/hardware/xgoduck/README.md
@@ -0,0 +1,16 @@
+# Microduck / XGO Duck 原机 USB 热更新(影子推理)
+
+此适配器安装在 UNO Q 原机 Linux 控制进程内,不替换 STM32 固件。使用 `hardware/unoq/python/core` 中的 `state_machine.py`、`observations.py`、`config.py`(拷入板端 `python/shadow_core/`)。`install_hooks.py` 仅接受已核对哈希的原机 `controller.py` 和 `main.py`,在同目录保留 `.py.before-usb-shadow` 备份。原机标定、运动策略、急停和 MCU 协议不变。
+
+**使用**:当前板端应用目录为 `/home/arduino/ArduinoApps/xgoduck_runtime_arduino-master`。USB 重新插拔后,可在主机恢复转发:
+
+```sh
+adb -d forward tcp:18000 localfilesystem:/home/arduino/ArduinoApps/xgoduck_runtime_arduino-master/data/usb-shadow/api.sock
+adb -d forward tcp:19527 tcp:9527
+```
+
+使用 `http://127.0.0.1:5173/` 或 `http://localhost:5173/` 打开部署面板,高级 → USB (ADB) → 测试连接 → 推送。只有在原机 off/shadow、反馈新鲜且未使能时才接受部署;不接受标定文件。推送返回加载到**当前进程**的 ZIP SHA-256、技能列表与影子推理计数。重启后需重新推送。无效 ZIP/模型不替换已有版本。状态可只读查询 `http://127.0.0.1:18000/api/status`。
+
+在原机 shadow 模式,状态机按传感器帧生成 61 维观测,输出只作为下一帧历史,不进入电机目标。加载 `kickL` 后,“验证技能切换(仅推理)”会触发一次虚拟踢球,再回到行走。off 模式不会连续推理。原机 hold/policy 等模式**不使用**这些新模型。没有验证带扭矩运行。
+
+**撤回适配**:确认原机 off/shadow 且扭矩关闭,在板端 `python/` 内恢复 `controller.py.before-usb-shadow` → `controller.py`、`main.py.before-usb-shadow` → `main.py`,再重启原机 Linux 应用容器 `xgoduck_runtime_arduino-master-main-1`。不需要刷固件或恢复零位。原机升级后不要将旧备份套到新版本。
diff --git a/hardware/xgoduck/compare_walk.py b/hardware/xgoduck/compare_walk.py
new file mode 100644
index 0000000..1f952eb
--- /dev/null
+++ b/hardware/xgoduck/compare_walk.py
@@ -0,0 +1,76 @@
+"""Offline replay on UNO Q: no Bridge, network, controller construction or motor output.
+Run in the native app environment, PYTHONPATH=/app/python, with a captured trace JSON
+({frames:[native /api/status snapshots], version:deployed ZIP SHA256}) and ZIP arguments.
+"""
+import hashlib
+import io
+import json
+import sys
+import zipfile
+from pathlib import Path
+
+import numpy as np
+from policy import Policy, PYTHON_DIR
+from rl_core import HOME_DEG, MOTOR_IDS, POLICY_IDS, POLICY_INDEX
+from usb_shadow import Preview
+
+
+def replay(trace, raw):
+    if hashlib.sha256(raw).hexdigest() != trace['version']:
+        raise ValueError('Capture version differs from supplied deployment ZIP')
+    frames = trace['frames']
+    if not frames or len({s['mcu_seq'] for s in frames}) != len(frames):
+        raise ValueError('Empty or repeated frames')
+    with zipfile.ZipFile(io.BytesIO(raw)) as z:
+        manifest = json.loads(z.read('microduck_unoq/config/robot_manifest.json'))
+        walk = z.read('microduck_unoq/'+manifest['skills']['walk']['path'])
+    native = (PYTHON_DIR/'xgoduck_walk.onnx').read_bytes()
+
+    def preview(model, native_pose):
+        m = dict(manifest, skills={key: dict(manifest['skills'][key], path='policies/'+key+'.onnx') for key in ('walk','stand')}, custom_move=None)
+        if native_pose: m['default_pose'] = np.deg2rad(HOME_DEG).tolist()
+        # Stand is required by the parser, but never selected in this walking test.
+        buf = io.BytesIO()
+        with zipfile.ZipFile(buf,'w') as z:
+            z.writestr('microduck_unoq/config/robot_manifest.json',json.dumps(m))
+            for k in m['skills']: z.writestr('microduck_unoq/policies/'+k+'.onnx',model)
+        return Preview(buf.getvalue())
+
+    ref = Policy()
+    ok, message = ref.load()
+    if not ok: raise ValueError(message)
+    leg, head = (frames[0]['deployment'][k] for k in ('action_alpha','action_alpha_head'))
+    ref.set_action_alphas(leg=leg, head=head)
+    alpha = np.array([leg]*5+[head]*5+[leg]*4, np.float32)
+    cases = [('same_model_same_home',native,True),('imported_model_only',walk,True),('imported_model_and_home',walk,False)]
+    report = {}
+    for label, model, native_pose in cases:
+        p = preview(model,native_pose)
+        filtered = np.zeros(14,np.float32)
+        ref.reset_action_filter()
+        errors = []
+        for s in frames:
+            if s['enabled'] or not s['imu_ok'] or s['feedback_age_ms'] >= 150 or s['motors'] != list(MOTOR_IDS):
+                raise ValueError('Invalid sensor frame')
+            # Same snapshot to both implementations; status exposes raw gyro/velocity.
+            f = dict(grav=s['gravity'],gyro_rad=np.deg2rad(s['gyro']).tolist(),angles_deg=s['angles'],vels_deg=s['velocities'])
+            ref_target = np.asarray(ref.action_to_deg(ref.infer(**f)))[POLICY_INDEX]
+            p.step(f)
+            if p.skill != 'walk': raise ValueError(f'State machine selected {p.skill}')
+            # Shadow stops at raw actions. Reuse native smoothing semantics for an
+            # offline projection of model targets, not an MCU command or output clamp.
+            filtered *= alpha
+            filtered += p.last * (1-alpha)
+            target = np.rad2deg(p.pose + manifest['skills']['walk']['action_scale'] * filtered)
+            errors.append(target - ref_target)
+        e = np.stack(errors)
+        if not np.isfinite(e).all(): raise ValueError('Non-finite output')
+        frame, joint = np.unravel_index(np.abs(e).argmax(),e.shape)
+        report[label] = {'max_deg':float(np.abs(e[frame,joint])), 'id':int(POLICY_IDS[joint]), 'rms_deg':float(np.sqrt(np.mean(e**2)))}
+        if label == 'same_model_same_home' and report[label]['max_deg'] > 0.01:
+            raise AssertionError(f'Native parity failed: {report[label]}')
+    return {'frames':len(frames),'version':trace['version'],'model_sha256':{k:hashlib.sha256(v).hexdigest() for k,v in [('native',native),('imported',walk)]},'cases':report}
+
+
+if __name__ == '__main__':
+    print(json.dumps(replay(json.loads(Path(sys.argv[1]).read_text()), Path(sys.argv[2]).read_bytes()),indent=2))
diff --git a/hardware/xgoduck/control/index.html b/hardware/xgoduck/control/index.html
new file mode 100644
index 0000000..b42fc58
--- /dev/null
+++ b/hardware/xgoduck/control/index.html
@@ -0,0 +1,19 @@
+
+
+Microduck · 实机控制
+
ARDUINO UNO Q

Microduck 实机控制

+

正在读取板端状态…

+
反馈频率—
推理频率—
在线舵机—
IMU—
+

正在检查舵机 ID…

+

导入技能 · 真机联调 —

真机联调使能电机并先保持当前姿态;选择下方技能后,导入的 ONNX 才驱动舵机。仅推理预览不使能电机。

+ +
+

正在连接板端技能服务…

+

需要重新加载模型时:打开部署网页 → UNO Q 部署 → 高级 → USB (ADB)。

+

关节点动 · 模型与实机对照

先托稳机身、让关节悬空。按住并拖动滑块时,全机使能,其他关节保持起始角;松手断扭矩。此操作不修改标定。

正在读取模型…

—

整机 3D 模型
正在加载整机外观…

拖动模型旋转,滚轮或双指缩放,不会驱动舵机。橙色高亮所选关节的活动部件;无反馈轴按零角显示。每次最多偏离起点 12°、15°/秒,空格或顶部按钮可随时关闭扭矩。

未准备 · 只读

+

原机步态与起身 —

原机模型(非热更新版本)

—

相对直立姿态的倾角

—

状态计时

—

倾角超过 55° 持续 0.15 秒后进入起身流程;倾角低于 15° 持续 1 秒后恢复行走。

+

移动

vx 0.00 · vy 0.00

ωz 0.00

松开摇杆自动回中。W/S 前后,A/D 横移,Q/E 转向。空格关闭扭矩。

+

头部与嘴部

头部四轴范围为 ±1 弧度。嘴部使用 34 号舵机(0–30°),不属于策略的 14 维输出。

+

动作平滑系数

+

IMU 与舵机反馈

加速度—
角速度—
重力—
ID实测角度 °目标角度 °

等待读取指令回执…

+
diff --git a/hardware/xgoduck/control/joint-jog.js b/hardware/xgoduck/control/joint-jog.js new file mode 100644 index 0000000..c5537fa --- /dev/null +++ b/hardware/xgoduck/control/joint-jog.js @@ -0,0 +1,20 @@ +(() => { + const el=id=>document.getElementById('joint-'+id), select=el('select'), slider=el('range'), prep=el('prepare'), msg=t=>el('message').textContent=t; + const zh=['左髋偏航','左髋侧倾','左髋俯仰','左膝','左踝','右髋偏航','右髋侧倾','右髋俯仰','右膝','右踝','颈俯仰','头俯仰','头偏航','头侧倾']; + let rows=[],s=null,token=null,held=false,busy=false,epoch=0,timeout,last=0; + const row=()=>rows[select.selectedIndex], fresh=()=>s && performance.now()-last<850 && s.feedback_age_ms<150 && s.imu_ok; + const post=async body=>{const r=await fetch('/api/joint-jog',{method:'POST',headers:{'Content-Type':'application/json'},body:JSON.stringify(body),signal:AbortSignal.timeout(1000)});const x=await r.json();if(!r.ok)throw Error(x.detail||'设备拒绝点动');return x}; + function end(text){const old=token;token=null;held=false;epoch++;clearTimeout(timeout);slider.disabled=true;if(old)post({op:'end',token:old}).catch(()=>{});if(text)msg(text);render()} + function render(){const j=row();if(!j||!s)return;const i=s.motors.indexOf(j.id),ok=s.servo_ids.includes(j.id)&&fresh(),a=s.angles[i];for(const [i,r] of rows.entries())select.options[i].disabled=!s.servo_ids.includes(r.id);select.disabled=!!token||held;prep.disabled=!!token||!ok||s.enabled||!['shadow','off'].includes(s.mode)||aj.hi; + el('details').textContent=`${j.name} · ID ${j.id} · 局部轴 [${j.axis}] · 模型范围 ${j.lo}° 至 ${j.hi}°`; + el('angles').textContent=`实测 ${ok?a.toFixed(1)+'°':'无有效反馈'} | 滑块目标 ${token?(+slider.value).toFixed(1)+'°':'—'} | 已发送 ${s.mode==='joint_jog'?s.targets[i].toFixed(1)+'°':'—'}`+(ok&&(aj.hi)?' · 已超出模型范围,不可点动':'');draw()} + function draw(){window.dispatchEvent(new CustomEvent('joint-view-pose',{detail:{angles:Object.fromEntries(rows.filter(r=>s.servo_ids.includes(r.id)).map(r=>[r.name,s.angles[s.motors.indexOf(r.id)]])),selected:row().name,target:token?+slider.value:null,fresh:fresh()}}))} + window.addEventListener('duck-status',e=>{s=e.detail;last=performance.now();render()});select.onchange=()=>end('未准备 · 只读'); + prep.onclick=async()=>{const n=++epoch;prep.disabled=true;try{const a=await post({op:'prepare',id:row().id});if(n!==epoch||document.hidden){post({op:'end',token:a.token}).catch(()=>{});return}token=a.token;slider.min=a.lo;slider.max=a.hi;slider.value=a.base;slider.disabled=false;msg('已准备,无扭矩。按住并拖动,松手停止。');timeout=setTimeout(()=>end('准备已到期'),30000);render()}catch(e){msg(e.message);render()}}; + async function pulse(){if(!held||!token||busy)return;if(!fresh()){end('反馈中断,点动停止');return}const t=token;busy=true;try{await post({op:'move',token:t,deg:+slider.value})}catch(e){if(t===token)end(e.message)}finally{busy=false}} + slider.onpointerdown=e=>{if(!token||held||!e.isPrimary||e.button!==0)return;held=true;slider.setPointerCapture(e.pointerId);msg('点动中 · 松开即停');pulse()};slider.oninput=render; + for(const name of ['pointerup','pointercancel','lostpointercapture'])slider.addEventListener(name,()=>{if(held)end('已请求关闭扭矩;再次点动请点准备')}); + for(const name of ['blur','pagehide'])window.addEventListener(name,()=>end());document.addEventListener('visibilitychange',()=>{if(document.hidden)end()});document.getElementById('stop').addEventListener('click',()=>end('已停止')); + setInterval(()=>{if(held)pulse();if(token&&!fresh())end('反馈中断,已取消');if(!fresh())render()},100); + fetch('/api/joints').then(r=>r.json()).then(data=>{rows=data.joints;rows.forEach((r,i)=>select.add(new Option(`${zh[i]} · ${r.name} · ID ${r.id}`,r.id)));render()}).catch(e=>msg('关节模型读取失败:'+e.message)); +})(); diff --git a/hardware/xgoduck/control/localize_js.py b/hardware/xgoduck/control/localize_js.py new file mode 100644 index 0000000..e911fe2 --- /dev/null +++ b/hardware/xgoduck/control/localize_js.py @@ -0,0 +1,29 @@ +"""Generate the Chinese control JS from the reviewed native app.js; leave original intact.""" +import hashlib +import sys +from pathlib import Path + +src = Path(sys.argv[1]).read_bytes() +if hashlib.sha256(src).hexdigest() != 'ca5f5a72a1ebac094b439f02b90566d1c40730e499caba1c634fd6e33603ace0': + sys.exit('Unexpected native app.js version; review before installing.') +s = src.decode() +for old, new in [ + ('const modeLabel=', "window.dispatchEvent(new CustomEvent('duck-status',{detail:s}));const modeLabel="), + ('