feat: add Microduck UNO Q deployment and hotspot skill control

This commit is contained in:
2026-11-19 13:49:08 +08:00
parent 2b0c395508
commit 164a397726
99 changed files with 12488 additions and 4 deletions
+14
View File
@@ -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.
+3 -1
View File
@@ -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",
+54
View File
@@ -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 软插值,再开始步态测试。
+6
View File
@@ -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: []
@@ -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
}
}
@@ -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
}
+133
View File
@@ -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
}
]
}
@@ -0,0 +1 @@
"""Microduck Arduino UNO Q Python runtime package."""
@@ -0,0 +1 @@
"""Core configuration, observation pipeline, ONNX runner, and control loop."""
@@ -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
@@ -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()
@@ -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
@@ -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"])
@@ -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)
@@ -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,
)
@@ -0,0 +1 @@
"""Hardware abstraction layers for Microduck on Arduino UNO Q."""
@@ -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."""
@@ -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)
@@ -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])]
+125
View File
@@ -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())
@@ -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
@@ -0,0 +1 @@
"""Web HTTP/WebSocket control and deployment package for Microduck UNO Q."""
+184
View File
@@ -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"<html><body><h1>Microduck controller UI missing</h1></body></html>"
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}"}
@@ -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
@@ -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
@@ -0,0 +1,619 @@
<!doctype html>
<html lang="en">
<head>
<meta charset="utf-8" />
<meta name="viewport" content="width=device-width,initial-scale=1,maximum-scale=1,user-scalable=no" />
<title>Microduck UNO Q Controller</title>
<style>
:root {
--bg: #0f1117;
--panel: #191d27;
--ink: #f6f4ed;
--muted: #98a2b3;
--accent: #ff7a2f;
--accent-dark: #c95212;
--danger: #e53935;
--ok: #2ecc71;
--warn: #f39c12;
--border: #2a3142;
--mono: ui-monospace, SFMono-Regular, Menlo, Monaco, Consolas, monospace;
}
* { box-sizing: border-box; -webkit-tap-highlight-color: transparent; }
body {
margin: 0;
background: var(--bg);
color: var(--ink);
font-family: var(--mono);
overflow-x: hidden;
min-height: 100vh;
}
.topbar {
display: flex;
flex-wrap: wrap;
align-items: center;
justify-content: space-between;
gap: 8px;
padding: 10px 14px;
background: var(--panel);
border-bottom: 2px solid var(--border);
}
.brand { font-weight: 700; font-size: 15px; letter-spacing: 0.04em; color: var(--accent); }
.badges { display: flex; flex-wrap: wrap; gap: 6px; align-items: center; font-size: 12px; }
.badge {
padding: 3px 8px;
border-radius: 999px;
border: 1px solid var(--border);
background: #12151d;
}
.badge.ok { border-color: var(--ok); color: var(--ok); }
.badge.warn { border-color: var(--warn); color: var(--warn); }
.badge.danger { border-color: var(--danger); color: var(--danger); }
.tabs { display: flex; gap: 6px; }
.tab-btn {
padding: 6px 12px;
background: #12151d;
color: var(--muted);
border: 1px solid var(--border);
border-radius: 6px;
font-family: inherit;
font-size: 12px;
font-weight: 700;
cursor: pointer;
}
.tab-btn.active { background: var(--accent); color: #08080c; border-color: var(--accent); }
.safety-bar {
display: grid;
grid-template-columns: 1.3fr 1.2fr 1fr 1fr;
gap: 8px;
padding: 10px 14px;
background: #141821;
border-bottom: 1px solid var(--border);
align-items: center;
}
@media (max-width: 720px) {
.safety-bar { grid-template-columns: 1fr 1fr; }
}
button.btn {
font-family: inherit;
font-weight: 700;
font-size: 13px;
border-radius: 8px;
border: 1px solid var(--border);
padding: 10px 12px;
background: var(--panel);
color: var(--ink);
cursor: pointer;
}
button.btn:active { transform: translateY(1px); }
button.btn:disabled { opacity: 0.4; cursor: not-allowed; }
button.btn-estop {
background: var(--danger);
border-color: #ff6b6b;
color: #fff;
font-size: 15px;
letter-spacing: 0.06em;
}
button.btn-stand {
background: var(--accent);
border-color: #ffa573;
color: #08080c;
}
.toggle-box, .slider-box {
display: flex;
flex-direction: column;
gap: 4px;
font-size: 11px;
color: var(--muted);
background: var(--panel);
border: 1px solid var(--border);
border-radius: 8px;
padding: 6px 10px;
}
.toggle-row { display: flex; justify-content: space-between; align-items: center; gap: 8px; color: var(--ink); font-size: 12px; }
.main { padding: 12px 14px; }
.hidden { display: none !important; }
.control-grid {
display: grid;
grid-template-columns: minmax(240px, 1fr) minmax(240px, 1fr);
gap: 14px;
align-items: stretch;
}
@media (max-width: 700px) {
.control-grid { grid-template-columns: 1fr; }
}
.card {
background: var(--panel);
border: 1px solid var(--border);
border-radius: 10px;
padding: 12px;
}
.card-title { font-size: 12px; color: var(--muted); margin: 0 0 10px; text-transform: uppercase; letter-spacing: 0.06em; }
.stick-zone {
position: relative;
height: 250px;
border-radius: 10px;
background: radial-gradient(circle at center, #202636 0%, #11151f 75%);
border: 1px dashed #374151;
touch-action: none;
user-select: none;
overflow: hidden;
display: flex;
align-items: center;
justify-content: center;
}
.stick-hint { color: var(--muted); font-size: 12px; pointer-events: none; text-align: center; padding: 12px; }
.stick-base {
position: absolute;
width: 112px;
height: 112px;
border-radius: 50%;
border: 2px solid rgba(255, 122, 47, 0.75);
background: rgba(255, 122, 47, 0.08);
transform: translate(-50%, -50%);
pointer-events: none;
}
.stick-knob {
position: absolute;
width: 44px;
height: 44px;
border-radius: 50%;
background: var(--accent);
box-shadow: 0 0 16px rgba(255, 122, 47, 0.5);
transform: translate(-50%, -50%);
pointer-events: none;
}
.vel-readout { margin-top: 8px; font-size: 12px; color: var(--muted); display: flex; justify-content: space-between; }
.skills-grid {
display: grid;
grid-template-columns: repeat(2, minmax(0, 1fr));
gap: 8px;
}
.skill-btn.active { background: var(--accent); color: #08080c; border-color: #ffa573; }
.head-panel { margin-top: 10px; display: grid; grid-template-columns: 1fr 1fr; gap: 8px; }
.calib-actions { display: flex; flex-wrap: wrap; gap: 8px; margin-bottom: 10px; }
.joint-table { width: 100%; border-collapse: collapse; font-size: 12px; }
.joint-table th, .joint-table td { border-bottom: 1px solid var(--border); padding: 6px 4px; text-align: left; }
.joint-table input[type="number"] {
width: 64px;
background: #0f1117;
color: var(--ink);
border: 1px solid var(--border);
border-radius: 4px;
padding: 3px 5px;
font-family: inherit;
font-size: 12px;
}
.mini-btn {
padding: 4px 7px;
font-size: 11px;
border-radius: 5px;
border: 1px solid var(--border);
background: #12151d;
color: var(--ink);
font-family: inherit;
cursor: pointer;
}
.mini-btn:hover { border-color: var(--accent); }
.log-line { margin-top: 8px; font-size: 11px; color: var(--muted); word-break: break-word; }
</style>
</head>
<body>
<header class="topbar">
<div class="brand">MICRODUCK // UNO Q</div>
<div class="badges">
<span id="badge-life" class="badge warn">IDLE</span>
<span id="badge-mode" class="badge">Idle</span>
<span id="badge-ws" class="badge">WS ...</span>
<span id="badge-imu" class="badge">IMU --</span>
<span id="badge-batt" class="badge">--.-V</span>
<span id="badge-temp" class="badge">--C</span>
<span id="badge-hz" class="badge">--Hz</span>
</div>
<div class="tabs">
<button id="tab-ctrl" class="tab-btn active" type="button">Control</button>
<button id="tab-cal" class="tab-btn" type="button">Calibration</button>
</div>
</header>
<section class="safety-bar">
<button id="btn-estop" class="btn btn-estop" type="button">E-STOP (TORQUE OFF)</button>
<button id="btn-stand" class="btn btn-stand" type="button">Prepare Stand (1.5s)</button>
<label class="toggle-box">
<span>Recovery Policy</span>
<span class="toggle-row">
<span>Auto Stand on Fall</span>
<input id="chk-auto-stand" type="checkbox" />
</span>
</label>
<label class="slider-box">
<span>Speed Scale: <strong id="lbl-speed">50%</strong></span>
<input id="rng-speed" type="range" min="10" max="100" step="5" value="50" />
</label>
</section>
<main class="main">
<section id="view-control" class="control-grid">
<div class="card">
<h2 class="card-title">Virtual Joystick (Hold & Drag)</h2>
<div id="stick-zone" class="stick-zone">
<div id="stick-hint" class="stick-hint">Touch or drag anywhere here to walk / turn</div>
<div id="stick-base" class="stick-base hidden"></div>
<div id="stick-knob" class="stick-knob hidden"></div>
</div>
<div class="vel-readout">
<span id="lbl-vx">vx norm: 0.00</span>
<span id="lbl-wz">wz norm: 0.00</span>
</div>
<div id="head-panel" class="head-panel hidden">
<label class="slider-box">Neck Pitch <input id="hd-0" type="range" min="-1" max="1" step="0.05" value="0" /></label>
<label class="slider-box">Head Pitch <input id="hd-1" type="range" min="-1" max="1" step="0.05" value="0" /></label>
<label class="slider-box">Head Yaw <input id="hd-2" type="range" min="-1" max="1" step="0.05" value="0" /></label>
<label class="slider-box">Head Roll <input id="hd-3" type="range" min="-1" max="1" step="0.05" value="0" /></label>
</div>
</div>
<div class="card">
<h2 class="card-title">Skills & Modes</h2>
<div class="skills-grid">
<button class="btn skill-btn" data-action="sitStand" type="button">Sit / Stand</button>
<button class="btn skill-btn" data-action="head" id="btn-head" type="button">Head Mode</button>
<button class="btn skill-btn" data-action="kickL" type="button">Kick Left</button>
<button class="btn skill-btn" data-action="kickR" type="button">Kick Right</button>
<button class="btn skill-btn" data-action="groundPick" type="button">Ground Pick</button>
<button class="btn skill-btn" data-action="roll" id="btn-roll" type="button">Roll / Trick</button>
<button class="btn skill-btn" data-action="rollers" id="btn-rollers" type="button">Feet / Rollers</button>
<button class="btn skill-btn hidden" data-action="script" id="btn-script" type="button">Custom Script</button>
</div>
<div id="ctrl-log" class="log-line">Ready. Tap "Prepare Stand" when the robot is upright on the floor.</div>
</div>
</section>
<section id="view-calib" class="card hidden">
<h2 class="card-title">Hardware Profile & Servo Calibration (IDLE Required for Joint Test)</h2>
<div class="calib-actions">
<button id="btn-ping" class="btn" type="button">Ping Servos (1-20)</button>
<button id="btn-imu-zero" class="btn" type="button">IMU Bias Zero (Keep Still)</button>
<button id="btn-exit-cal" class="btn" type="button">Exit Calibration (Torque Off)</button>
<button id="btn-save-prof" class="btn btn-stand" type="button">Save Profile to Robot</button>
</div>
<div id="calib-status" class="log-line">No ping scan yet.</div>
<div style="overflow-x:auto; margin-top:10px;">
<table class="joint-table">
<thead>
<tr>
<th>Joint</th>
<th>ID</th>
<th>Sign</th>
<th>Offset</th>
<th>Live Tick</th>
<th>Live Rad</th>
<th>Actions</th>
</tr>
</thead>
<tbody id="joint-tbody"></tbody>
</table>
</div>
</section>
</main>
<script>
(() => {
const $ = (id) => document.getElementById(id);
let ws = null;
let reconnectTimer = null;
let profile = null;
let latestStatus = null;
const stickState = { active: false, pointerId: null, originX: 0, originY: 0, nx: 0, nwz: 0 };
const MAX_RADIUS = 56;
function log(msg) {
$("ctrl-log").textContent = msg;
}
async function apiPost(path, body) {
const res = await fetch(path, {
method: "POST",
headers: { "Content-Type": "application/json" },
body: JSON.stringify(body || {}),
});
const data = await res.json();
if (!res.ok || data.ok === false) {
throw new Error(data.error || `HTTP ${res.status}`);
}
return data;
}
function setTab(tab) {
const isCtrl = tab === "ctrl";
$("view-control").classList.toggle("hidden", !isCtrl);
$("view-calib").classList.toggle("hidden", isCtrl);
$("tab-ctrl").classList.toggle("active", isCtrl);
$("tab-cal").classList.toggle("active", !isCtrl);
if (!isCtrl) loadProfile();
}
$("tab-ctrl").onclick = () => setTab("ctrl");
$("tab-cal").onclick = () => setTab("cal");
function sendWs(obj) {
if (ws && ws.readyState === WebSocket.OPEN) {
ws.send(JSON.stringify(obj));
return true;
}
return false;
}
function updateStatusUI(st) {
latestStatus = st;
const lifeEl = $("badge-life");
lifeEl.textContent = st.lifecycle || "UNKNOWN";
lifeEl.className = "badge " + (st.lifecycle === "ACTIVE" ? "ok" : st.lifecycle === "FALL_SAFE" ? "danger" : "warn");
$("badge-mode").textContent = st.mode_label || st.skill_mode || "-";
$("badge-imu").textContent = st.imu_ok ? "IMU OK" : "IMU ERR";
$("badge-imu").className = "badge " + (st.imu_ok ? "ok" : "danger");
$("badge-batt").textContent = `${Number(st.voltage_v || 0).toFixed(2)}V`;
$("badge-temp").textContent = `${Number(st.max_temp_c || 0).toFixed(0)}C`;
$("badge-hz").textContent = `${Number(st.ctrl_hz || 0).toFixed(0)}Hz`;
$("chk-auto-stand").checked = !!st.auto_stand_on_fall;
const spdPct = Math.round((st.speed_scale || 0.5) * 100);
if (document.activeElement !== $("rng-speed")) {
$("rng-speed").value = String(spdPct);
$("lbl-speed").textContent = `${spdPct}%`;
}
$("btn-head").classList.toggle("active", !!st.head_mode);
$("head-panel").classList.toggle("hidden", !st.head_mode);
$("btn-rollers").disabled = !st.rollers_enabled;
$("btn-rollers").classList.toggle("active", st.loco === "rollers");
const hasScript = !!(st.custom_move && st.custom_move.slot === "script");
$("btn-script").classList.toggle("hidden", !hasScript);
if (st.error) log(`Error: ${st.error}`);
if (st.ticks && st.joint_pos_rad && profile) {
Object.keys(profile.joints).forEach((name, idx) => {
const tCell = document.getElementById(`live-tick-${name}`);
const rCell = document.getElementById(`live-rad-${name}`);
if (tCell) tCell.textContent = String(st.ticks[idx] ?? "-");
if (rCell) rCell.textContent = Number(st.joint_pos_rad[idx] ?? 0).toFixed(3);
});
}
}
function connectWs() {
if (reconnectTimer) clearTimeout(reconnectTimer);
const proto = location.protocol === "https:" ? "wss:" : "ws:";
ws = new WebSocket(`${proto}//${location.host}/ws/control`);
ws.onopen = () => {
$("badge-ws").textContent = "WS LIVE";
$("badge-ws").className = "badge ok";
};
ws.onclose = () => {
$("badge-ws").textContent = "WS OFF";
$("badge-ws").className = "badge danger";
reconnectTimer = setTimeout(connectWs, 1000);
};
ws.onmessage = (evt) => {
try {
const msg = JSON.parse(evt.data);
if (msg.type === "telemetry") updateStatusUI(msg);
} catch (_) {}
};
}
// Safety & Skill controls
$("btn-estop").onclick = async () => {
sendWs({ type: "estop" });
try { updateStatusUI(await apiPost("/api/estop")); log("E-STOP activated (torque off)"); } catch (e) { log(e.message); }
};
$("btn-stand").onclick = async () => {
try { updateStatusUI(await apiPost("/api/stand")); log("Soft-start stand engaged..."); } catch (e) { log(e.message); }
};
$("chk-auto-stand").onchange = async (e) => {
try { updateStatusUI(await apiPost("/api/settings", { auto_stand_on_fall: e.target.checked })); } catch (err) { log(err.message); }
};
$("rng-speed").oninput = (e) => {
$("lbl-speed").textContent = `${e.target.value}%`;
};
$("rng-speed").onchange = async (e) => {
try { updateStatusUI(await apiPost("/api/settings", { speed_scale: Number(e.target.value) / 100 })); } catch (err) { log(err.message); }
};
document.querySelectorAll(".skill-btn").forEach((btn) => {
btn.onclick = async () => {
const action = btn.dataset.action;
try {
const res = await apiPost("/api/action", { action });
if (res.status) updateStatusUI(res.status);
log(res.ok ? `Triggered ${action}` : `Action '${action}' rejected in current state`);
} catch (e) { log(e.message); }
};
});
const sendHead = () => {
sendWs({
type: "head",
neck_pitch: Number($("hd-0").value),
head_pitch: Number($("hd-1").value),
head_yaw: Number($("hd-2").value),
head_roll: Number($("hd-3").value),
});
};
["hd-0", "hd-1", "hd-2", "hd-3"].forEach((id) => { $(id).oninput = sendHead; });
// Floating virtual joystick
const zone = $("stick-zone");
const base = $("stick-base");
const knob = $("stick-knob");
const hint = $("stick-hint");
function updateStickVisual(x, y) {
if (!stickState.active) {
base.classList.add("hidden");
knob.classList.add("hidden");
hint.classList.remove("hidden");
return;
}
hint.classList.add("hidden");
base.classList.remove("hidden");
knob.classList.remove("hidden");
base.style.left = `${stickState.originX}px`;
base.style.top = `${stickState.originY}px`;
knob.style.left = `${x}px`;
knob.style.top = `${y}px`;
}
function applyStickCoords(clientX, clientY) {
const rect = zone.getBoundingClientRect();
const px = clientX - rect.left;
const py = clientY - rect.top;
const dx = px - stickState.originX;
const dy = py - stickState.originY;
const dist = Math.hypot(dx, dy);
const clamped = dist > MAX_RADIUS ? MAX_RADIUS / dist : 1.0;
const cx = stickState.originX + dx * clamped;
const cy = stickState.originY + dy * clamped;
stickState.nx = -((cy - stickState.originY) / MAX_RADIUS);
stickState.nwz = -((cx - stickState.originX) / MAX_RADIUS);
$("lbl-vx").textContent = `vx norm: ${stickState.nx.toFixed(2)}`;
$("lbl-wz").textContent = `wz norm: ${stickState.nwz.toFixed(2)}`;
updateStickVisual(cx, cy);
}
function releaseStick() {
stickState.active = false;
stickState.pointerId = null;
stickState.nx = 0;
stickState.nwz = 0;
$("lbl-vx").textContent = "vx norm: 0.00";
$("lbl-wz").textContent = "wz norm: 0.00";
updateStickVisual(0, 0);
sendWs({ type: "input", vx: 0, wz: 0, speed_scale: Number($("rng-speed").value) / 100 });
}
zone.addEventListener("pointerdown", (e) => {
const rect = zone.getBoundingClientRect();
stickState.active = true;
stickState.pointerId = e.pointerId;
stickState.originX = e.clientX - rect.left;
stickState.originY = e.clientY - rect.top;
zone.setPointerCapture(e.pointerId);
applyStickCoords(e.clientX, e.clientY);
});
zone.addEventListener("pointermove", (e) => {
if (!stickState.active || e.pointerId !== stickState.pointerId) return;
applyStickCoords(e.clientX, e.clientY);
});
zone.addEventListener("pointerup", (e) => {
if (e.pointerId === stickState.pointerId) releaseStick();
});
zone.addEventListener("pointercancel", releaseStick);
setInterval(() => {
if (stickState.active) {
sendWs({
type: "input",
vx: stickState.nx,
wz: stickState.nwz,
speed_scale: Number($("rng-speed").value) / 100,
});
}
}, 50);
// Calibration helpers
function renderJointTable() {
const tbody = $("joint-tbody");
tbody.innerHTML = "";
if (!profile || !profile.joints) return;
Object.entries(profile.joints).forEach(([name, cfg]) => {
const tr = document.createElement("tr");
tr.innerHTML = `
<td>${name}</td>
<td><input type="number" min="0" max="253" value="${cfg.id}" data-j="${name}" data-f="id" /></td>
<td><button class="mini-btn" type="button" data-flip="${name}">${cfg.sign > 0 ? "+1" : "-1"}</button></td>
<td><input type="number" min="-4096" max="4096" value="${cfg.zero_offset_ticks}" data-j="${name}" data-f="zero_offset_ticks" /></td>
<td id="live-tick-${name}">-</td>
<td id="live-rad-${name}">0.000</td>
<td>
<button class="mini-btn" type="button" data-zero="${name}">Zero</button>
<button class="mini-btn" type="button" data-test="${name}" data-rad="-0.2">-0.2</button>
<button class="mini-btn" type="button" data-test="${name}" data-rad="0">0</button>
<button class="mini-btn" type="button" data-test="${name}" data-rad="0.2">+0.2</button>
</td>
`;
tbody.appendChild(tr);
});
tbody.querySelectorAll("input[data-j]").forEach((inp) => {
inp.onchange = () => {
profile.joints[inp.dataset.j][inp.dataset.f] = Number(inp.value);
};
});
tbody.querySelectorAll("button[data-flip]").forEach((btn) => {
btn.onclick = () => {
const j = profile.joints[btn.dataset.flip];
j.sign = j.sign > 0 ? -1 : 1;
btn.textContent = j.sign > 0 ? "+1" : "-1";
};
});
tbody.querySelectorAll("button[data-zero]").forEach((btn) => {
btn.onclick = async () => {
try {
const res = await apiPost("/api/calibrate/joint_zero", { joint: btn.dataset.zero });
profile = res.profile;
renderJointTable();
$("calib-status").textContent = `Captured current tick as zero for ${btn.dataset.zero}`;
} catch (e) { $("calib-status").textContent = `Error: ${e.message}`; }
};
});
tbody.querySelectorAll("button[data-test]").forEach((btn) => {
btn.onclick = async () => {
try {
await apiPost("/api/calibrate/test_joint", { joint: btn.dataset.test, rad: Number(btn.dataset.rad) });
$("calib-status").textContent = `Commanded ${btn.dataset.test} -> ${btn.dataset.rad} rad`;
} catch (e) { $("calib-status").textContent = `Error: ${e.message}`; }
};
});
}
async function loadProfile() {
const res = await fetch("/api/profile");
const data = await res.json();
profile = data.profile;
renderJointTable();
}
$("btn-ping").onclick = async () => {
try {
const res = await apiPost("/api/calibrate/ping", { max_id: 20 });
$("calib-status").textContent = `Responding IDs (${res.ids.length}): [${res.ids.join(", ")}]`;
} catch (e) { $("calib-status").textContent = `Ping error: ${e.message}`; }
};
$("btn-imu-zero").onclick = async () => {
try {
$("calib-status").textContent = "Sampling IMU bias (keep robot stationary)...";
const res = await apiPost("/api/calibrate/imu_zero", { samples: 200 });
if (profile) profile.imu = res.imu;
$("calib-status").textContent = `IMU gyro bias updated: [${res.imu.gyro_bias_rad_s.map((x) => x.toFixed(4)).join(", ")}]`;
} catch (e) { $("calib-status").textContent = `IMU zero error: ${e.message}`; }
};
$("btn-exit-cal").onclick = async () => {
try { updateStatusUI(await apiPost("/api/calibrate/exit")); $("calib-status").textContent = "Exited calibration (torque off)"; } catch (e) { $("calib-status").textContent = e.message; }
};
$("btn-save-prof").onclick = async () => {
if (!profile) return;
try {
const res = await apiPost("/api/profile", profile);
profile = res.profile;
renderJointTable();
$("calib-status").textContent = "Saved hardware_profile.json to UNO Q.";
} catch (e) { $("calib-status").textContent = `Save error: ${e.message}`; }
};
fetch("/api/status").then((r) => r.json()).then(updateStatusUI).catch(() => {});
connectWs();
})();
</script>
</body>
</html>
@@ -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."
@@ -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"
+41
View File
@@ -0,0 +1,41 @@
#pragma once
#include <Arduino.h>
#include <Wire.h>
// =============================================================================
// 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
@@ -0,0 +1,349 @@
#include "feetech_hls.h"
#include <string.h>
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<uint8_t>((~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<uint8_t>(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<size_t>(3 + param_len));
sendRawPacket(pkt, static_cast<size_t>(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<uint8_t>(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<uint8_t>(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<size_t>(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<int32_t>(id);
}
}
return found;
}
bool FeetechHlsBus::setTorqueBroadcast(bool enable) {
uint8_t params[2] = {kRegTorqueEnable, static_cast<uint8_t>(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<uint8_t>(2 + count * 2);
pkt[0] = kHeader;
pkt[1] = kHeader;
pkt[2] = kBroadcastId;
pkt[3] = static_cast<uint8_t>(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<size_t>(3 + param_len));
sendRawPacket(pkt, static_cast<size_t>(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<uint8_t>(2 + count * (1 + kPerServo));
pkt[0] = kHeader;
pkt[1] = kHeader;
pkt[2] = kBroadcastId;
pkt[3] = static_cast<uint8_t>(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<uint16_t>(ticks);
size_t off = 7 + static_cast<size_t>(i) * (1 + kPerServo);
pkt[off + 0] = ids[i];
pkt[off + 1] = acc;
pkt[off + 2] = static_cast<uint8_t>(pos & 0xFFU);
pkt[off + 3] = static_cast<uint8_t>((pos >> 8) & 0xFFU);
pkt[off + 4] = 0x00; // moving time low
pkt[off + 5] = 0x00; // moving time high
pkt[off + 6] = static_cast<uint8_t>(speed_limit & 0xFFU);
pkt[off + 7] = static_cast<uint8_t>((speed_limit >> 8) & 0xFFU);
}
pkt[5 + param_len] = computeChecksum(&pkt[2], static_cast<size_t>(3 + param_len));
sendRawPacket(pkt, static_cast<size_t>(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<uint16_t>(buf[0]) | (static_cast<uint16_t>(buf[1]) << 8);
uint16_t spd_u16 = static_cast<uint16_t>(buf[2]) | (static_cast<uint16_t>(buf[3]) << 8);
*out_tick = static_cast<int32_t>(pos_u16 & 0x7FFFU);
*out_speed = static_cast<int32_t>(spd_u16);
*out_voltage_v = static_cast<float>(buf[6]) * 0.1f;
*out_temp_c = static_cast<float>(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<uint8_t>(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<uint16_t>(buf[0]) | (static_cast<uint16_t>(buf[1]) << 8);
uint16_t spd_u16 = static_cast<uint16_t>(buf[2]) | (static_cast<uint16_t>(buf[3]) << 8);
out_ticks[i] = static_cast<int32_t>(pos_u16 & 0x7FFFU);
out_speeds[i] = static_cast<int32_t>(spd_u16);
float v = static_cast<float>(buf[6]) * 0.1f;
float t = static_cast<float>(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;
}
@@ -0,0 +1,50 @@
#pragma once
#include <Arduino.h>
#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);
};
+216
View File
@@ -0,0 +1,216 @@
#include "qmi8658.h"
#include <math.h>
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<int>(addr_), static_cast<int>(len));
if (got != len) {
while (wire_->available()) {
(void)wire_->read();
}
return false;
}
for (size_t i = 0; i < len; ++i) {
buf[i] = static_cast<uint8_t>(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<int16_t>((static_cast<uint16_t>(hi) << 8) | lo);
};
raw_accel_[0] = static_cast<float>(to_i16(buf[0], buf[1])) * kAccelScaleG;
raw_accel_[1] = static_cast<float>(to_i16(buf[2], buf[3])) * kAccelScaleG;
raw_accel_[2] = static_cast<float>(to_i16(buf[4], buf[5])) * kAccelScaleG;
raw_gyro_[0] = static_cast<float>(to_i16(buf[6], buf[7])) * kGyroScaleRadS;
raw_gyro_[1] = static_cast<float>(to_i16(buf[8], buf[9])) * kGyroScaleRadS;
raw_gyro_[2] = static_cast<float>(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<float>(sx / ok_cnt);
out_bias[1] = static_cast<float>(sy / ok_cnt);
out_bias[2] = static_cast<float>(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_));
}
+36
View File
@@ -0,0 +1,36 @@
#pragma once
#include <Arduino.h>
#include <Wire.h>
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);
};
+195
View File
@@ -0,0 +1,195 @@
#include <Arduino_RouterBridge.h>
#include <Wire.h>
#include <vector>
#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<float> 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<uint32_t>(payload[0] > 9600.0f ? payload[0] : SERVO_BAUD);
i2c_addr = static_cast<uint8_t>(static_cast<int>(payload[1] + 0.5f));
g_watchdog_ms = static_cast<uint32_t>(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<int>(payload[6 + i] + 0.5f);
if (sid >= 0 && sid <= 253) {
g_servo_ids[i] = static_cast<uint8_t>(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<float> hw_sync_step(std::vector<int32_t> cmd_frame) {
std::vector<float> res(38, 0.0f);
if (cmd_frame.size() < 3 + NUM_JOINTS) {
res[36] = g_imu.isHealthy() ? 1.0f : 0.0f;
res[37] = static_cast<float>(0xFFFF);
return res;
}
bool want_torque = cmd_frame[0] != 0;
uint16_t speed_limit = static_cast<uint16_t>(cmd_frame[1] > 0 ? cmd_frame[1] : 0);
uint8_t acc = static_cast<uint8_t>(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<float>(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<float>(g_last_ticks[i]);
res[20 + i] = static_cast<float>(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<float>(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<int32_t> 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<uint8_t>(max_id), found, 64);
return std::vector<int32_t>(found, found + n);
}
std::vector<float> 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<int>(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<float>(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.");
}
}
@@ -0,0 +1,12 @@
profiles:
default:
fqbn: arduino:zephyr:unoq
platforms:
- platform: arduino:zephyr
libraries:
- Arduino_RouterBridge
- Arduino_RPClite
- ArxContainer
- ArxTypeTraits
- DebugLog
- MsgPack
+8
View File
@@ -0,0 +1,8 @@
<!doctype html><html lang="zh-CN"><meta charset="utf-8"><meta name="viewport" content="width=device-width, initial-scale=1"><title>Microduck 影子策略状态</title>
<style>body{margin:0;font:15px/1.5 system-ui;color:#16201f;background:#f4f8f6;padding:8px;border-radius:6px}p{margin:0;overflow-wrap:anywhere}</style>
<p id="summary">正在读取板端状态…</p><p id="details"></p><p id="skills"></p>
<script>
const names={walk:'行走',stand:'站立/起身',sitstand:'坐下/站起',kickL:'左脚踢球',kickR:'右脚踢球',groundpick:'拾取',roll:'翻滚',drive:'轮式行驶',crouch:'蹲伏'};
const text=(id,value)=>document.getElementById(id).textContent=value;
async function update(){try{if(document.hidden)return;const r=await fetch('http://127.0.0.1:18000/api/status',{cache:'no-store',signal:AbortSignal.timeout(1800)});if(!r.ok)throw Error();const s=await r.json();if(s.hal!=='xgoduck-shadow')throw Error();text('summary',s.version?'板端版本:'+s.version.slice(0,12):'尚未加载影子策略');text('details',s.shadow_error?'推理异常:'+s.shadow_error:s.native_mode==='shadow'?`当前影子策略:${names[s.shadow_skill]??s.shadow_skill??'无'} · 推理次数:${s.shadow_steps}`:'影子推理未运行 · 原机模式:'+({off:'扭矩已关闭',hold:'保持姿态',policy:'运动控制'}[s.native_mode]??s.native_mode));text('skills','已加载:'+(s.loaded_skills.map(k=>names[k]??k).join('、')||'无'));}catch{text('summary','影子服务未连接,请检查 USB 热更新转发(18000)。');text('skills','');text('details','');}finally{setTimeout(update,1200)}}update();
</script></html>
+73
View File
@@ -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/`,
);
}
+14 -2
View File
@@ -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() {
</>
)}
<Hud />
<SimulationToolbar />
<SimulationToolbar onOpenDeploy={() => setDeployOpen(true)} />
<ControlsHelp />
<RuntimeStatus />
<HardwareDeployDialog
open={deployOpen}
onClose={() => setDeployOpen(false)}
/>
</>
);
}
+136
View File
@@ -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;
}
+523
View File
@@ -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,
};
}
+801
View File
@@ -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 (
<Box
role="dialog"
aria-modal="true"
lang="zh-CN"
aria-label="将 Microduck 部署到 Arduino UNO Q"
sx={{
position: "fixed",
inset: 0,
zIndex: 40,
bgcolor: "rgba(8, 8, 12, 0.55)",
display: "flex",
alignItems: "center",
justifyContent: "center",
p: 2,
}}
onClick={(e) => {
if (e.target === e.currentTarget && !busy) onClose?.();
}}
>
<Box
sx={{
width: "min(680px, 100%)",
maxHeight: "90vh",
overflowY: "auto",
bgcolor: panelBg,
color: panelInk,
border: `2px solid ${borderColor}`,
boxShadow: neutral ? "none" : "6px 6px 0 #08080c",
fontFamily: "monospace",
fontSize: 12.5,
p: 2.2,
display: "flex",
flexDirection: "column",
gap: 1.4,
}}
>
<Box
sx={{
display: "flex",
justifyContent: "space-between",
alignItems: "center",
borderBottom: `1px solid ${borderColor}`,
pb: 1,
}}
>
<Box sx={{ fontWeight: 700, fontSize: 14 }}>
Microduck · UNO Q 部署
</Box>
<button type="button" disabled={busy} onClick={onClose}>
关闭
</button>
</Box>
<Box sx={{ p: 1, border: `1px solid ${borderColor}` }}>
<strong>Microduck · USB 实机</strong>
<p>
板端控制页显示的「XGO Duck」是这台 Microduck 的另一个名称。写入 ONNX
只会保存文件,不会切换当前运行的策略。
</p>
<a
href="http://127.0.0.1:18600/"
target="_blank"
rel="noopener noreferrer"
>
写入 ONNX 模型
</a>
{" · "}
<a
href="http://127.0.0.1:19527/"
target="_blank"
rel="noopener noreferrer"
>
打开实机控制页
</a>
</Box>
<details>
<summary>高级:导出部署包或热更新策略</summary>
<Box
sx={{ display: "flex", flexDirection: "column", gap: 1.4, mt: 1 }}
>
{/* Connection row */}
<Box sx={{ display: "flex", flexDirection: "column", gap: 0.7 }}>
<label htmlFor="unoq-url" style={{ fontWeight: 700 }}>
策略热更新服务地址(仅在板端已运行该服务时使用)
</label>
<Box sx={{ display: "flex", gap: 0.8, flexWrap: "wrap" }}>
<input
type="text"
id="unoq-url"
value={targetUrl}
onChange={(e) => rememberUrl(e.target.value)}
style={{
flex: "1 1 240px",
fontFamily: "monospace",
fontSize: 12,
padding: "5px 8px",
}}
/>
<button
type="button"
disabled={busy}
onClick={handleTestConnection}
>
测试连接
</button>
<button
type="button"
onClick={() =>
window.open(
boardStatus?.hal === "xgoduck-shadow"
? "http://127.0.0.1:19527/"
: `${cleanUrl}/`,
"_blank",
"noopener",
)
}
>
打开控制页
</button>
</Box>
<Box
sx={{
display: "flex",
gap: 0.6,
flexWrap: "wrap",
fontSize: 11,
}}
>
<span>预设地址:</span>
<button type="button" onClick={() => rememberUrl(USB_UNOQ_URL)}>
USB (ADB)
</button>
<button
type="button"
onClick={() => rememberUrl("http://192.168.4.1:8000")}
>
Wi-Fi 热点(192.168.4.1)
</button>
<button
type="button"
onClick={() => rememberUrl("http://microduck.local:8000")}
>
microduck.local
</button>
<button
type="button"
onClick={() => rememberUrl("http://127.0.0.1:8000")}
>
本机模拟(127.0.0.1)
</button>
</Box>
{cleanUrl === USB_UNOQ_URL && (
<span>
点击「测试连接」。原机接口仅更新影子推理策略,不覆盖标定、不使能舵机。
</span>
)}
{boardStatus && (
<Box
sx={{
p: 0.8,
border: `1px dashed ${borderColor}`,
fontSize: 11,
}}
>
硬件接口:
<b>{halNames[boardStatus.hal] ?? boardStatus.hal}</b>
|状态:
<b>
{states[boardStatus.lifecycle] ?? boardStatus.lifecycle}
</b>
|IMU:
<b>{boardStatus.imu_ok ? "正常" : "异常"}</b>|已加载技能:
{(boardStatus.loaded_skills || [])
.map((key) => skillNames[key] ?? key)
.join("、") || "无"}
{boardStatus.version && (
<div>
影子版本:{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") && (
<button
type="button"
disabled={busy}
onClick={async () => {
setBusy(true);
try {
const r = await fetch(
`${cleanUrl}/api/shadow/kickL`,
{ method: "POST" },
);
const result = await r.json();
if (!r.ok)
throw new Error(
result.error || `HTTP ${r.status}`,
);
setFeedback({
ok: true,
text: "已触发左脚踢球的影子状态切换,不会发送该动作到舵机。",
});
} catch (err) {
setFeedback({
ok: false,
text: errorDetail(err),
});
} finally {
setBusy(false);
}
}}
>
验证技能切换(仅推理)
</button>
)}
</div>
)}
</Box>
)}
</Box>
{/* Skills checklist */}
<Box sx={{ display: "flex", flexDirection: "column", gap: 0.6 }}>
<div style={{ fontWeight: 700 }}>
要部署的技能与策略(61 维观测,14 维动作)
</div>
<label style={{ display: "flex", alignItems: "center", gap: 6 }}>
<input
type="checkbox"
checked={includeRollers}
onChange={(e) => handleToggleRollers(e.target.checked)}
/>
包含轮式模式(行驶和蹲伏,可在控制页切换)
</label>
<Box
sx={{
display: "grid",
gridTemplateColumns: "repeat(auto-fill, minmax(180px, 1fr))",
gap: 0.5,
}}
>
{MICRODUCK_SKILL_CATALOG.map((item) => {
const disabled =
item.required ||
(item.variant === "rollers" && !includeRollers) ||
(item.key === "drive" && includeRollers);
return (
<label
key={item.key}
style={{
display: "flex",
alignItems: "center",
gap: 5,
opacity:
item.variant === "rollers" && !includeRollers
? 0.45
: 1,
}}
>
<input
type="checkbox"
checked={effectiveSkillSet.has(item.key)}
disabled={disabled}
onChange={() => toggleSkill(item.key)}
/>
<span>
{skillNames[item.key] ?? item.label}
{item.required ? "(必选)" : ""}
</span>
</label>
);
})}
</Box>
</Box>
{/* Custom move / local file import */}
<Box
sx={{
p: 1,
border: `1px solid ${borderColor}`,
display: "flex",
flexDirection: "column",
gap: 0.6,
}}
>
<div style={{ fontWeight: 700 }}>
自定义策略(.onnx)或动作脚本(.json),可选
</div>
{activeCustomPolicy?.status === "active" &&
activeCustomPolicy?.ref && (
<label
style={{ display: "flex", alignItems: "center", gap: 6 }}
>
<input
type="checkbox"
checked={useActiveSimulatorRef}
onChange={(e) => {
setUseActiveSimulatorRef(e.target.checked);
if (e.target.checked) setLocalCustom(null);
}}
/>
使用仿真器的当前动作:<b>{activeCustomPolicy.name}</b>
(类型:
{slotNames[activeCustomPolicy.slot] ??
activeCustomPolicy.slot}
)
</label>
)}
<Box
sx={{
display: "flex",
gap: 1,
alignItems: "center",
flexWrap: "wrap",
}}
>
<input
id="unoq-custom-file"
type="file"
accept=".onnx,.json"
hidden
onChange={handleUploadCustomFile}
/>
<button
type="button"
onClick={() =>
document.getElementById("unoq-custom-file")?.click()
}
>
选择本地文件
</button>
<label>
ONNX 用途:
<select
value={onnxSlot}
onChange={(e) => setOnnxSlot(e.target.value)}
>
<option value="walk">行走策略</option>
<option value="trick">特技/翻滚</option>
</select>
</label>
{onnxSlot === "trick" && (
<label>
持续时间(秒):
<input
type="number"
step="0.5"
min="0.5"
max="15"
value={trickDurationS}
onChange={(e) => setTrickDurationS(e.target.value)}
style={{ width: 56 }}
/>
</label>
)}
{localCustom && (
<button type="button" onClick={() => setLocalCustom(null)}>
移除「{localCustom.name}」
</button>
)}
</Box>
</Box>
{/* Calibration safety option */}
<label style={{ display: "flex", alignItems: "center", gap: 6 }}>
<input
type="checkbox"
disabled={boardStatus?.hal === "xgoduck-shadow"}
checked={overwriteProfile}
onChange={(e) => setOverwriteProfile(e.target.checked)}
/>
推送时覆盖板上的标定文件
hardware_profile.json(不勾选可保留舵机零位和 IMU 零偏)
</label>
{feedback && (
<Box
sx={{
p: 1,
border: `1.5px solid ${feedback.ok ? "#2e7d32" : "#c62828"}`,
color: feedback.ok
? neutral
? "#a5d6a7"
: "#1b5e20"
: "#ff5252",
fontWeight: 700,
wordBreak: "break-word",
}}
>
{feedback.text}
</Box>
)}
{/* Primary actions */}
<Box
sx={{
display: "flex",
gap: 1,
flexWrap: "wrap",
justifyContent: "flex-end",
pt: 0.5,
}}
>
<button type="button" disabled={busy} onClick={handleDownloadZip}>
{busy ? "处理中…" : "下载 UNO Q 完整部署包(.zip)"}
</button>
<button
type="button"
disabled={
busy ||
(cleanUrl === USB_UNOQ_URL &&
!usbInterfaces.includes(boardStatus?.hal))
}
title="热更新需要运行 Microduck 控制服务;使用 USB 时请先测试连接。"
onClick={handlePushDeploy}
style={{
background: "#ff7a2f",
color: "#08080c",
fontWeight: 700,
border: "1.5px solid #08080c",
padding: "6px 12px",
cursor: "pointer",
}}
>
{busy ? "处理中…" : "推送到 UNO Q(热更新)"}
</button>
</Box>
</Box>
</details>
</Box>
</Box>
);
}
+10 -1
View File
@@ -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() {
<button type="button" disabled={!ready} onClick={() => gameApi.reset?.()}>
Reset
</button>
{family === "microduck" && (
<button
type="button"
disabled={phase === "switching"}
onClick={() => onOpenDeploy?.()}
>
UNO Q 部署
</button>
)}
</Box>
);
}
+193
View File
@@ -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,
);
});
@@ -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<float> payload)`
- `hw_sync_step(std::vector<int32_t> cmd_frame) -> std::vector<float>`
- `hw_set_torque(int32_t enable) -> int32_t`
- `hw_ping_servos(int32_t max_id) -> std::vector<int32_t>`
- `hw_calibrate_imu(int32_t samples) -> std::vector<float>`
- 详细实现飞特 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<Uint8Array>`
- `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` 测试仍正常通过。
@@ -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 推理冒烟。
+33
View File
@@ -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|
+54
View File
@@ -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 软插值,再开始步态测试。
+6
View File
@@ -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: []
+147
View File
@@ -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
}
}
+127
View File
@@ -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
}
+1
View File
@@ -0,0 +1 @@
"""Microduck Arduino UNO Q Python runtime package."""
+1
View File
@@ -0,0 +1 @@
"""Core configuration, observation pipeline, ONNX runner, and control loop."""
+176
View File
@@ -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
+380
View File
@@ -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()
+252
View File
@@ -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
+85
View File
@@ -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"])
+123
View File
@@ -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)
+561
View File
@@ -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,
)
+1
View File
@@ -0,0 +1 @@
"""Hardware abstraction layers for Microduck on Arduino UNO Q."""
+57
View File
@@ -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."""
+82
View File
@@ -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)
+109
View File
@@ -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])]
+125
View File
@@ -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())
+5
View File
@@ -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
@@ -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")
+216
View File
@@ -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"
+197
View File
@@ -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()
+1
View File
@@ -0,0 +1 @@
"""Web HTTP/WebSocket control and deployment package for Microduck UNO Q."""
+184
View File
@@ -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"<html><body><h1>Microduck controller UI missing</h1></body></html>"
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}"}
+227
View File
@@ -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
+92
View File
@@ -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
+619
View File
@@ -0,0 +1,619 @@
<!doctype html>
<html lang="en">
<head>
<meta charset="utf-8" />
<meta name="viewport" content="width=device-width,initial-scale=1,maximum-scale=1,user-scalable=no" />
<title>Microduck UNO Q Controller</title>
<style>
:root {
--bg: #0f1117;
--panel: #191d27;
--ink: #f6f4ed;
--muted: #98a2b3;
--accent: #ff7a2f;
--accent-dark: #c95212;
--danger: #e53935;
--ok: #2ecc71;
--warn: #f39c12;
--border: #2a3142;
--mono: ui-monospace, SFMono-Regular, Menlo, Monaco, Consolas, monospace;
}
* { box-sizing: border-box; -webkit-tap-highlight-color: transparent; }
body {
margin: 0;
background: var(--bg);
color: var(--ink);
font-family: var(--mono);
overflow-x: hidden;
min-height: 100vh;
}
.topbar {
display: flex;
flex-wrap: wrap;
align-items: center;
justify-content: space-between;
gap: 8px;
padding: 10px 14px;
background: var(--panel);
border-bottom: 2px solid var(--border);
}
.brand { font-weight: 700; font-size: 15px; letter-spacing: 0.04em; color: var(--accent); }
.badges { display: flex; flex-wrap: wrap; gap: 6px; align-items: center; font-size: 12px; }
.badge {
padding: 3px 8px;
border-radius: 999px;
border: 1px solid var(--border);
background: #12151d;
}
.badge.ok { border-color: var(--ok); color: var(--ok); }
.badge.warn { border-color: var(--warn); color: var(--warn); }
.badge.danger { border-color: var(--danger); color: var(--danger); }
.tabs { display: flex; gap: 6px; }
.tab-btn {
padding: 6px 12px;
background: #12151d;
color: var(--muted);
border: 1px solid var(--border);
border-radius: 6px;
font-family: inherit;
font-size: 12px;
font-weight: 700;
cursor: pointer;
}
.tab-btn.active { background: var(--accent); color: #08080c; border-color: var(--accent); }
.safety-bar {
display: grid;
grid-template-columns: 1.3fr 1.2fr 1fr 1fr;
gap: 8px;
padding: 10px 14px;
background: #141821;
border-bottom: 1px solid var(--border);
align-items: center;
}
@media (max-width: 720px) {
.safety-bar { grid-template-columns: 1fr 1fr; }
}
button.btn {
font-family: inherit;
font-weight: 700;
font-size: 13px;
border-radius: 8px;
border: 1px solid var(--border);
padding: 10px 12px;
background: var(--panel);
color: var(--ink);
cursor: pointer;
}
button.btn:active { transform: translateY(1px); }
button.btn:disabled { opacity: 0.4; cursor: not-allowed; }
button.btn-estop {
background: var(--danger);
border-color: #ff6b6b;
color: #fff;
font-size: 15px;
letter-spacing: 0.06em;
}
button.btn-stand {
background: var(--accent);
border-color: #ffa573;
color: #08080c;
}
.toggle-box, .slider-box {
display: flex;
flex-direction: column;
gap: 4px;
font-size: 11px;
color: var(--muted);
background: var(--panel);
border: 1px solid var(--border);
border-radius: 8px;
padding: 6px 10px;
}
.toggle-row { display: flex; justify-content: space-between; align-items: center; gap: 8px; color: var(--ink); font-size: 12px; }
.main { padding: 12px 14px; }
.hidden { display: none !important; }
.control-grid {
display: grid;
grid-template-columns: minmax(240px, 1fr) minmax(240px, 1fr);
gap: 14px;
align-items: stretch;
}
@media (max-width: 700px) {
.control-grid { grid-template-columns: 1fr; }
}
.card {
background: var(--panel);
border: 1px solid var(--border);
border-radius: 10px;
padding: 12px;
}
.card-title { font-size: 12px; color: var(--muted); margin: 0 0 10px; text-transform: uppercase; letter-spacing: 0.06em; }
.stick-zone {
position: relative;
height: 250px;
border-radius: 10px;
background: radial-gradient(circle at center, #202636 0%, #11151f 75%);
border: 1px dashed #374151;
touch-action: none;
user-select: none;
overflow: hidden;
display: flex;
align-items: center;
justify-content: center;
}
.stick-hint { color: var(--muted); font-size: 12px; pointer-events: none; text-align: center; padding: 12px; }
.stick-base {
position: absolute;
width: 112px;
height: 112px;
border-radius: 50%;
border: 2px solid rgba(255, 122, 47, 0.75);
background: rgba(255, 122, 47, 0.08);
transform: translate(-50%, -50%);
pointer-events: none;
}
.stick-knob {
position: absolute;
width: 44px;
height: 44px;
border-radius: 50%;
background: var(--accent);
box-shadow: 0 0 16px rgba(255, 122, 47, 0.5);
transform: translate(-50%, -50%);
pointer-events: none;
}
.vel-readout { margin-top: 8px; font-size: 12px; color: var(--muted); display: flex; justify-content: space-between; }
.skills-grid {
display: grid;
grid-template-columns: repeat(2, minmax(0, 1fr));
gap: 8px;
}
.skill-btn.active { background: var(--accent); color: #08080c; border-color: #ffa573; }
.head-panel { margin-top: 10px; display: grid; grid-template-columns: 1fr 1fr; gap: 8px; }
.calib-actions { display: flex; flex-wrap: wrap; gap: 8px; margin-bottom: 10px; }
.joint-table { width: 100%; border-collapse: collapse; font-size: 12px; }
.joint-table th, .joint-table td { border-bottom: 1px solid var(--border); padding: 6px 4px; text-align: left; }
.joint-table input[type="number"] {
width: 64px;
background: #0f1117;
color: var(--ink);
border: 1px solid var(--border);
border-radius: 4px;
padding: 3px 5px;
font-family: inherit;
font-size: 12px;
}
.mini-btn {
padding: 4px 7px;
font-size: 11px;
border-radius: 5px;
border: 1px solid var(--border);
background: #12151d;
color: var(--ink);
font-family: inherit;
cursor: pointer;
}
.mini-btn:hover { border-color: var(--accent); }
.log-line { margin-top: 8px; font-size: 11px; color: var(--muted); word-break: break-word; }
</style>
</head>
<body>
<header class="topbar">
<div class="brand">MICRODUCK // UNO Q</div>
<div class="badges">
<span id="badge-life" class="badge warn">IDLE</span>
<span id="badge-mode" class="badge">Idle</span>
<span id="badge-ws" class="badge">WS ...</span>
<span id="badge-imu" class="badge">IMU --</span>
<span id="badge-batt" class="badge">--.-V</span>
<span id="badge-temp" class="badge">--C</span>
<span id="badge-hz" class="badge">--Hz</span>
</div>
<div class="tabs">
<button id="tab-ctrl" class="tab-btn active" type="button">Control</button>
<button id="tab-cal" class="tab-btn" type="button">Calibration</button>
</div>
</header>
<section class="safety-bar">
<button id="btn-estop" class="btn btn-estop" type="button">E-STOP (TORQUE OFF)</button>
<button id="btn-stand" class="btn btn-stand" type="button">Prepare Stand (1.5s)</button>
<label class="toggle-box">
<span>Recovery Policy</span>
<span class="toggle-row">
<span>Auto Stand on Fall</span>
<input id="chk-auto-stand" type="checkbox" />
</span>
</label>
<label class="slider-box">
<span>Speed Scale: <strong id="lbl-speed">50%</strong></span>
<input id="rng-speed" type="range" min="10" max="100" step="5" value="50" />
</label>
</section>
<main class="main">
<section id="view-control" class="control-grid">
<div class="card">
<h2 class="card-title">Virtual Joystick (Hold & Drag)</h2>
<div id="stick-zone" class="stick-zone">
<div id="stick-hint" class="stick-hint">Touch or drag anywhere here to walk / turn</div>
<div id="stick-base" class="stick-base hidden"></div>
<div id="stick-knob" class="stick-knob hidden"></div>
</div>
<div class="vel-readout">
<span id="lbl-vx">vx norm: 0.00</span>
<span id="lbl-wz">wz norm: 0.00</span>
</div>
<div id="head-panel" class="head-panel hidden">
<label class="slider-box">Neck Pitch <input id="hd-0" type="range" min="-1" max="1" step="0.05" value="0" /></label>
<label class="slider-box">Head Pitch <input id="hd-1" type="range" min="-1" max="1" step="0.05" value="0" /></label>
<label class="slider-box">Head Yaw <input id="hd-2" type="range" min="-1" max="1" step="0.05" value="0" /></label>
<label class="slider-box">Head Roll <input id="hd-3" type="range" min="-1" max="1" step="0.05" value="0" /></label>
</div>
</div>
<div class="card">
<h2 class="card-title">Skills & Modes</h2>
<div class="skills-grid">
<button class="btn skill-btn" data-action="sitStand" type="button">Sit / Stand</button>
<button class="btn skill-btn" data-action="head" id="btn-head" type="button">Head Mode</button>
<button class="btn skill-btn" data-action="kickL" type="button">Kick Left</button>
<button class="btn skill-btn" data-action="kickR" type="button">Kick Right</button>
<button class="btn skill-btn" data-action="groundPick" type="button">Ground Pick</button>
<button class="btn skill-btn" data-action="roll" id="btn-roll" type="button">Roll / Trick</button>
<button class="btn skill-btn" data-action="rollers" id="btn-rollers" type="button">Feet / Rollers</button>
<button class="btn skill-btn hidden" data-action="script" id="btn-script" type="button">Custom Script</button>
</div>
<div id="ctrl-log" class="log-line">Ready. Tap "Prepare Stand" when the robot is upright on the floor.</div>
</div>
</section>
<section id="view-calib" class="card hidden">
<h2 class="card-title">Hardware Profile & Servo Calibration (IDLE Required for Joint Test)</h2>
<div class="calib-actions">
<button id="btn-ping" class="btn" type="button">Ping Servos (1-20)</button>
<button id="btn-imu-zero" class="btn" type="button">IMU Bias Zero (Keep Still)</button>
<button id="btn-exit-cal" class="btn" type="button">Exit Calibration (Torque Off)</button>
<button id="btn-save-prof" class="btn btn-stand" type="button">Save Profile to Robot</button>
</div>
<div id="calib-status" class="log-line">No ping scan yet.</div>
<div style="overflow-x:auto; margin-top:10px;">
<table class="joint-table">
<thead>
<tr>
<th>Joint</th>
<th>ID</th>
<th>Sign</th>
<th>Offset</th>
<th>Live Tick</th>
<th>Live Rad</th>
<th>Actions</th>
</tr>
</thead>
<tbody id="joint-tbody"></tbody>
</table>
</div>
</section>
</main>
<script>
(() => {
const $ = (id) => document.getElementById(id);
let ws = null;
let reconnectTimer = null;
let profile = null;
let latestStatus = null;
const stickState = { active: false, pointerId: null, originX: 0, originY: 0, nx: 0, nwz: 0 };
const MAX_RADIUS = 56;
function log(msg) {
$("ctrl-log").textContent = msg;
}
async function apiPost(path, body) {
const res = await fetch(path, {
method: "POST",
headers: { "Content-Type": "application/json" },
body: JSON.stringify(body || {}),
});
const data = await res.json();
if (!res.ok || data.ok === false) {
throw new Error(data.error || `HTTP ${res.status}`);
}
return data;
}
function setTab(tab) {
const isCtrl = tab === "ctrl";
$("view-control").classList.toggle("hidden", !isCtrl);
$("view-calib").classList.toggle("hidden", isCtrl);
$("tab-ctrl").classList.toggle("active", isCtrl);
$("tab-cal").classList.toggle("active", !isCtrl);
if (!isCtrl) loadProfile();
}
$("tab-ctrl").onclick = () => setTab("ctrl");
$("tab-cal").onclick = () => setTab("cal");
function sendWs(obj) {
if (ws && ws.readyState === WebSocket.OPEN) {
ws.send(JSON.stringify(obj));
return true;
}
return false;
}
function updateStatusUI(st) {
latestStatus = st;
const lifeEl = $("badge-life");
lifeEl.textContent = st.lifecycle || "UNKNOWN";
lifeEl.className = "badge " + (st.lifecycle === "ACTIVE" ? "ok" : st.lifecycle === "FALL_SAFE" ? "danger" : "warn");
$("badge-mode").textContent = st.mode_label || st.skill_mode || "-";
$("badge-imu").textContent = st.imu_ok ? "IMU OK" : "IMU ERR";
$("badge-imu").className = "badge " + (st.imu_ok ? "ok" : "danger");
$("badge-batt").textContent = `${Number(st.voltage_v || 0).toFixed(2)}V`;
$("badge-temp").textContent = `${Number(st.max_temp_c || 0).toFixed(0)}C`;
$("badge-hz").textContent = `${Number(st.ctrl_hz || 0).toFixed(0)}Hz`;
$("chk-auto-stand").checked = !!st.auto_stand_on_fall;
const spdPct = Math.round((st.speed_scale || 0.5) * 100);
if (document.activeElement !== $("rng-speed")) {
$("rng-speed").value = String(spdPct);
$("lbl-speed").textContent = `${spdPct}%`;
}
$("btn-head").classList.toggle("active", !!st.head_mode);
$("head-panel").classList.toggle("hidden", !st.head_mode);
$("btn-rollers").disabled = !st.rollers_enabled;
$("btn-rollers").classList.toggle("active", st.loco === "rollers");
const hasScript = !!(st.custom_move && st.custom_move.slot === "script");
$("btn-script").classList.toggle("hidden", !hasScript);
if (st.error) log(`Error: ${st.error}`);
if (st.ticks && st.joint_pos_rad && profile) {
Object.keys(profile.joints).forEach((name, idx) => {
const tCell = document.getElementById(`live-tick-${name}`);
const rCell = document.getElementById(`live-rad-${name}`);
if (tCell) tCell.textContent = String(st.ticks[idx] ?? "-");
if (rCell) rCell.textContent = Number(st.joint_pos_rad[idx] ?? 0).toFixed(3);
});
}
}
function connectWs() {
if (reconnectTimer) clearTimeout(reconnectTimer);
const proto = location.protocol === "https:" ? "wss:" : "ws:";
ws = new WebSocket(`${proto}//${location.host}/ws/control`);
ws.onopen = () => {
$("badge-ws").textContent = "WS LIVE";
$("badge-ws").className = "badge ok";
};
ws.onclose = () => {
$("badge-ws").textContent = "WS OFF";
$("badge-ws").className = "badge danger";
reconnectTimer = setTimeout(connectWs, 1000);
};
ws.onmessage = (evt) => {
try {
const msg = JSON.parse(evt.data);
if (msg.type === "telemetry") updateStatusUI(msg);
} catch (_) {}
};
}
// Safety & Skill controls
$("btn-estop").onclick = async () => {
sendWs({ type: "estop" });
try { updateStatusUI(await apiPost("/api/estop")); log("E-STOP activated (torque off)"); } catch (e) { log(e.message); }
};
$("btn-stand").onclick = async () => {
try { updateStatusUI(await apiPost("/api/stand")); log("Soft-start stand engaged..."); } catch (e) { log(e.message); }
};
$("chk-auto-stand").onchange = async (e) => {
try { updateStatusUI(await apiPost("/api/settings", { auto_stand_on_fall: e.target.checked })); } catch (err) { log(err.message); }
};
$("rng-speed").oninput = (e) => {
$("lbl-speed").textContent = `${e.target.value}%`;
};
$("rng-speed").onchange = async (e) => {
try { updateStatusUI(await apiPost("/api/settings", { speed_scale: Number(e.target.value) / 100 })); } catch (err) { log(err.message); }
};
document.querySelectorAll(".skill-btn").forEach((btn) => {
btn.onclick = async () => {
const action = btn.dataset.action;
try {
const res = await apiPost("/api/action", { action });
if (res.status) updateStatusUI(res.status);
log(res.ok ? `Triggered ${action}` : `Action '${action}' rejected in current state`);
} catch (e) { log(e.message); }
};
});
const sendHead = () => {
sendWs({
type: "head",
neck_pitch: Number($("hd-0").value),
head_pitch: Number($("hd-1").value),
head_yaw: Number($("hd-2").value),
head_roll: Number($("hd-3").value),
});
};
["hd-0", "hd-1", "hd-2", "hd-3"].forEach((id) => { $(id).oninput = sendHead; });
// Floating virtual joystick
const zone = $("stick-zone");
const base = $("stick-base");
const knob = $("stick-knob");
const hint = $("stick-hint");
function updateStickVisual(x, y) {
if (!stickState.active) {
base.classList.add("hidden");
knob.classList.add("hidden");
hint.classList.remove("hidden");
return;
}
hint.classList.add("hidden");
base.classList.remove("hidden");
knob.classList.remove("hidden");
base.style.left = `${stickState.originX}px`;
base.style.top = `${stickState.originY}px`;
knob.style.left = `${x}px`;
knob.style.top = `${y}px`;
}
function applyStickCoords(clientX, clientY) {
const rect = zone.getBoundingClientRect();
const px = clientX - rect.left;
const py = clientY - rect.top;
const dx = px - stickState.originX;
const dy = py - stickState.originY;
const dist = Math.hypot(dx, dy);
const clamped = dist > MAX_RADIUS ? MAX_RADIUS / dist : 1.0;
const cx = stickState.originX + dx * clamped;
const cy = stickState.originY + dy * clamped;
stickState.nx = -((cy - stickState.originY) / MAX_RADIUS);
stickState.nwz = -((cx - stickState.originX) / MAX_RADIUS);
$("lbl-vx").textContent = `vx norm: ${stickState.nx.toFixed(2)}`;
$("lbl-wz").textContent = `wz norm: ${stickState.nwz.toFixed(2)}`;
updateStickVisual(cx, cy);
}
function releaseStick() {
stickState.active = false;
stickState.pointerId = null;
stickState.nx = 0;
stickState.nwz = 0;
$("lbl-vx").textContent = "vx norm: 0.00";
$("lbl-wz").textContent = "wz norm: 0.00";
updateStickVisual(0, 0);
sendWs({ type: "input", vx: 0, wz: 0, speed_scale: Number($("rng-speed").value) / 100 });
}
zone.addEventListener("pointerdown", (e) => {
const rect = zone.getBoundingClientRect();
stickState.active = true;
stickState.pointerId = e.pointerId;
stickState.originX = e.clientX - rect.left;
stickState.originY = e.clientY - rect.top;
zone.setPointerCapture(e.pointerId);
applyStickCoords(e.clientX, e.clientY);
});
zone.addEventListener("pointermove", (e) => {
if (!stickState.active || e.pointerId !== stickState.pointerId) return;
applyStickCoords(e.clientX, e.clientY);
});
zone.addEventListener("pointerup", (e) => {
if (e.pointerId === stickState.pointerId) releaseStick();
});
zone.addEventListener("pointercancel", releaseStick);
setInterval(() => {
if (stickState.active) {
sendWs({
type: "input",
vx: stickState.nx,
wz: stickState.nwz,
speed_scale: Number($("rng-speed").value) / 100,
});
}
}, 50);
// Calibration helpers
function renderJointTable() {
const tbody = $("joint-tbody");
tbody.innerHTML = "";
if (!profile || !profile.joints) return;
Object.entries(profile.joints).forEach(([name, cfg]) => {
const tr = document.createElement("tr");
tr.innerHTML = `
<td>${name}</td>
<td><input type="number" min="0" max="253" value="${cfg.id}" data-j="${name}" data-f="id" /></td>
<td><button class="mini-btn" type="button" data-flip="${name}">${cfg.sign > 0 ? "+1" : "-1"}</button></td>
<td><input type="number" min="-4096" max="4096" value="${cfg.zero_offset_ticks}" data-j="${name}" data-f="zero_offset_ticks" /></td>
<td id="live-tick-${name}">-</td>
<td id="live-rad-${name}">0.000</td>
<td>
<button class="mini-btn" type="button" data-zero="${name}">Zero</button>
<button class="mini-btn" type="button" data-test="${name}" data-rad="-0.2">-0.2</button>
<button class="mini-btn" type="button" data-test="${name}" data-rad="0">0</button>
<button class="mini-btn" type="button" data-test="${name}" data-rad="0.2">+0.2</button>
</td>
`;
tbody.appendChild(tr);
});
tbody.querySelectorAll("input[data-j]").forEach((inp) => {
inp.onchange = () => {
profile.joints[inp.dataset.j][inp.dataset.f] = Number(inp.value);
};
});
tbody.querySelectorAll("button[data-flip]").forEach((btn) => {
btn.onclick = () => {
const j = profile.joints[btn.dataset.flip];
j.sign = j.sign > 0 ? -1 : 1;
btn.textContent = j.sign > 0 ? "+1" : "-1";
};
});
tbody.querySelectorAll("button[data-zero]").forEach((btn) => {
btn.onclick = async () => {
try {
const res = await apiPost("/api/calibrate/joint_zero", { joint: btn.dataset.zero });
profile = res.profile;
renderJointTable();
$("calib-status").textContent = `Captured current tick as zero for ${btn.dataset.zero}`;
} catch (e) { $("calib-status").textContent = `Error: ${e.message}`; }
};
});
tbody.querySelectorAll("button[data-test]").forEach((btn) => {
btn.onclick = async () => {
try {
await apiPost("/api/calibrate/test_joint", { joint: btn.dataset.test, rad: Number(btn.dataset.rad) });
$("calib-status").textContent = `Commanded ${btn.dataset.test} -> ${btn.dataset.rad} rad`;
} catch (e) { $("calib-status").textContent = `Error: ${e.message}`; }
};
});
}
async function loadProfile() {
const res = await fetch("/api/profile");
const data = await res.json();
profile = data.profile;
renderJointTable();
}
$("btn-ping").onclick = async () => {
try {
const res = await apiPost("/api/calibrate/ping", { max_id: 20 });
$("calib-status").textContent = `Responding IDs (${res.ids.length}): [${res.ids.join(", ")}]`;
} catch (e) { $("calib-status").textContent = `Ping error: ${e.message}`; }
};
$("btn-imu-zero").onclick = async () => {
try {
$("calib-status").textContent = "Sampling IMU bias (keep robot stationary)...";
const res = await apiPost("/api/calibrate/imu_zero", { samples: 200 });
if (profile) profile.imu = res.imu;
$("calib-status").textContent = `IMU gyro bias updated: [${res.imu.gyro_bias_rad_s.map((x) => x.toFixed(4)).join(", ")}]`;
} catch (e) { $("calib-status").textContent = `IMU zero error: ${e.message}`; }
};
$("btn-exit-cal").onclick = async () => {
try { updateStatusUI(await apiPost("/api/calibrate/exit")); $("calib-status").textContent = "Exited calibration (torque off)"; } catch (e) { $("calib-status").textContent = e.message; }
};
$("btn-save-prof").onclick = async () => {
if (!profile) return;
try {
const res = await apiPost("/api/profile", profile);
profile = res.profile;
renderJointTable();
$("calib-status").textContent = "Saved hardware_profile.json to UNO Q.";
} catch (e) { $("calib-status").textContent = `Save error: ${e.message}`; }
};
fetch("/api/status").then((r) => r.json()).then(updateStatusUI).catch(() => {});
connectWs();
})();
</script>
</body>
</html>
+10
View File
@@ -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."
+44
View File
@@ -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"
+74
View File
@@ -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 = '''<!doctype html><html lang="zh"><meta charset="utf-8"><title>Microduck USB · ONNX 写入</title>
<h1>通过 USB 写入 ONNX</h1><p>文件存入板上的 ~/microduck-usb-staging/models/,不会替换或启动当前策略。</p>
<input id="file" type="file" accept=".onnx"><button id="send">写入板上测试目录</button><pre id="result"></pre>
<script>send.onclick=async()=>{const f=file.files[0];if(!f||!f.name.toLowerCase().endsWith('.onnx')||!f.size||f.size>16777216){result.textContent='请选择不超过 16 MiB 的 ONNX 文件';return}
send.disabled=true;try{result.textContent='正在写入并校验…';const hash=Array.from(new Uint8Array(await crypto.subtle.digest('SHA-256',await f.arrayBuffer())),x=>x.toString(16).padStart(2,'0')).join('');const r=await fetch('/stage',{method:'POST',headers:{'X-Stage-Token':'TOKEN_VALUE','X-SHA256':hash},body:f});if(!r.ok)throw new Error('HTTP '+r.status);const j=await r.json();if(j.sha256!==hash)throw new Error('校验失败');result.textContent='已写入,SHA-256 一致。未启用策略。\\n'+j.path+'\\n'+hash}catch(e){result.textContent=e.message}finally{send.disabled=false}}</script></html>'''
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()
+41
View File
@@ -0,0 +1,41 @@
#pragma once
#include <Arduino.h>
#include <Wire.h>
// =============================================================================
// 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
+349
View File
@@ -0,0 +1,349 @@
#include "feetech_hls.h"
#include <string.h>
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<uint8_t>((~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<uint8_t>(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<size_t>(3 + param_len));
sendRawPacket(pkt, static_cast<size_t>(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<uint8_t>(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<uint8_t>(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<size_t>(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<int32_t>(id);
}
}
return found;
}
bool FeetechHlsBus::setTorqueBroadcast(bool enable) {
uint8_t params[2] = {kRegTorqueEnable, static_cast<uint8_t>(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<uint8_t>(2 + count * 2);
pkt[0] = kHeader;
pkt[1] = kHeader;
pkt[2] = kBroadcastId;
pkt[3] = static_cast<uint8_t>(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<size_t>(3 + param_len));
sendRawPacket(pkt, static_cast<size_t>(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<uint8_t>(2 + count * (1 + kPerServo));
pkt[0] = kHeader;
pkt[1] = kHeader;
pkt[2] = kBroadcastId;
pkt[3] = static_cast<uint8_t>(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<uint16_t>(ticks);
size_t off = 7 + static_cast<size_t>(i) * (1 + kPerServo);
pkt[off + 0] = ids[i];
pkt[off + 1] = acc;
pkt[off + 2] = static_cast<uint8_t>(pos & 0xFFU);
pkt[off + 3] = static_cast<uint8_t>((pos >> 8) & 0xFFU);
pkt[off + 4] = 0x00; // moving time low
pkt[off + 5] = 0x00; // moving time high
pkt[off + 6] = static_cast<uint8_t>(speed_limit & 0xFFU);
pkt[off + 7] = static_cast<uint8_t>((speed_limit >> 8) & 0xFFU);
}
pkt[5 + param_len] = computeChecksum(&pkt[2], static_cast<size_t>(3 + param_len));
sendRawPacket(pkt, static_cast<size_t>(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<uint16_t>(buf[0]) | (static_cast<uint16_t>(buf[1]) << 8);
uint16_t spd_u16 = static_cast<uint16_t>(buf[2]) | (static_cast<uint16_t>(buf[3]) << 8);
*out_tick = static_cast<int32_t>(pos_u16 & 0x7FFFU);
*out_speed = static_cast<int32_t>(spd_u16);
*out_voltage_v = static_cast<float>(buf[6]) * 0.1f;
*out_temp_c = static_cast<float>(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<uint8_t>(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<uint16_t>(buf[0]) | (static_cast<uint16_t>(buf[1]) << 8);
uint16_t spd_u16 = static_cast<uint16_t>(buf[2]) | (static_cast<uint16_t>(buf[3]) << 8);
out_ticks[i] = static_cast<int32_t>(pos_u16 & 0x7FFFU);
out_speeds[i] = static_cast<int32_t>(spd_u16);
float v = static_cast<float>(buf[6]) * 0.1f;
float t = static_cast<float>(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;
}
+50
View File
@@ -0,0 +1,50 @@
#pragma once
#include <Arduino.h>
#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);
};
+216
View File
@@ -0,0 +1,216 @@
#include "qmi8658.h"
#include <math.h>
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<int>(addr_), static_cast<int>(len));
if (got != len) {
while (wire_->available()) {
(void)wire_->read();
}
return false;
}
for (size_t i = 0; i < len; ++i) {
buf[i] = static_cast<uint8_t>(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<int16_t>((static_cast<uint16_t>(hi) << 8) | lo);
};
raw_accel_[0] = static_cast<float>(to_i16(buf[0], buf[1])) * kAccelScaleG;
raw_accel_[1] = static_cast<float>(to_i16(buf[2], buf[3])) * kAccelScaleG;
raw_accel_[2] = static_cast<float>(to_i16(buf[4], buf[5])) * kAccelScaleG;
raw_gyro_[0] = static_cast<float>(to_i16(buf[6], buf[7])) * kGyroScaleRadS;
raw_gyro_[1] = static_cast<float>(to_i16(buf[8], buf[9])) * kGyroScaleRadS;
raw_gyro_[2] = static_cast<float>(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<float>(sx / ok_cnt);
out_bias[1] = static_cast<float>(sy / ok_cnt);
out_bias[2] = static_cast<float>(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_));
}
+36
View File
@@ -0,0 +1,36 @@
#pragma once
#include <Arduino.h>
#include <Wire.h>
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);
};
+195
View File
@@ -0,0 +1,195 @@
#include <Arduino_RouterBridge.h>
#include <Wire.h>
#include <vector>
#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<float> 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<uint32_t>(payload[0] > 9600.0f ? payload[0] : SERVO_BAUD);
i2c_addr = static_cast<uint8_t>(static_cast<int>(payload[1] + 0.5f));
g_watchdog_ms = static_cast<uint32_t>(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<int>(payload[6 + i] + 0.5f);
if (sid >= 0 && sid <= 253) {
g_servo_ids[i] = static_cast<uint8_t>(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<float> hw_sync_step(std::vector<int32_t> cmd_frame) {
std::vector<float> res(38, 0.0f);
if (cmd_frame.size() < 3 + NUM_JOINTS) {
res[36] = g_imu.isHealthy() ? 1.0f : 0.0f;
res[37] = static_cast<float>(0xFFFF);
return res;
}
bool want_torque = cmd_frame[0] != 0;
uint16_t speed_limit = static_cast<uint16_t>(cmd_frame[1] > 0 ? cmd_frame[1] : 0);
uint8_t acc = static_cast<uint8_t>(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<float>(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<float>(g_last_ticks[i]);
res[20 + i] = static_cast<float>(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<float>(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<int32_t> 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<uint8_t>(max_id), found, 64);
return std::vector<int32_t>(found, found + n);
}
std::vector<float> 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<int>(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<float>(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.");
}
}
+12
View File
@@ -0,0 +1,12 @@
profiles:
default:
fqbn: arduino:zephyr:unoq
platforms:
- platform: arduino:zephyr
libraries:
- Arduino_RouterBridge
- Arduino_RPClite
- ArxContainer
- ArxTypeTraits
- DebugLog
- MsgPack
+16
View File
@@ -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`。不需要刷固件或恢复零位。原机升级后不要将旧备份套到新版本。
+76
View File
@@ -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))
+19
View File
@@ -0,0 +1,19 @@
<!doctype html>
<html lang="zh-CN"><meta charset="utf-8"><meta name="viewport" content="width=device-width, initial-scale=1">
<title>Microduck · 实机控制</title><link rel="stylesheet" href="style.css">
<main><header><div><small>ARDUINO UNO Q</small><h1>Microduck 实机控制</h1></div><div class="header-actions"><nav class="nav"><a href="/">控制</a><a href="/calibrate.html">标定</a><a href="/servo.html">舵机设置</a></nav><button class="stop" id="stop">关闭扭矩(空格)</button></div></header>
<p id="message">正在读取板端状态…</p>
<section class="metrics"><article><small>反馈频率</small><strong id="rx">—</strong></article><article><small>推理频率</small><strong id="rl">—</strong></article><article><small>在线舵机</small><strong id="servos">—</strong></article><article><small>IMU</small><strong id="imu">—</strong></article></section>
<p id="missing">正在检查舵机 ID…</p>
<section id="usb-shadow"><h2>导入技能 · 真机联调 <span id="mode">—</span></h2><p>真机联调使能电机并先保持当前姿态;选择下方技能后,导入的 ONNX 才驱动舵机。仅推理预览不使能电机。</p>
<button id="live-start" disabled>开始真机联调(使能电机)</button> <button data-mode="shadow">仅推理预览(无扭矩)</button>
<div class="buttons" id="shadow-skills"><button data-skill="walk" disabled>行走</button><button data-skill="stand" disabled>站立/起身</button><button data-skill="sitstand" disabled>坐下/站起</button><button data-skill="kickL" disabled>左脚踢球</button><button data-skill="kickR" disabled>右脚踢球</button><button data-skill="groundpick" disabled>地面拾取</button><button data-skill="roll" disabled>翻滚</button></div>
<p id="shadow-status">正在连接板端技能服务…</p><p id="shadow-result" role="status"></p>
<p class="hint" id="skill-deploy-hint">需要重新加载模型时:<a href="http://127.0.0.1:5173/" target="_blank" rel="noopener">打开部署网页</a> → UNO Q 部署 → 高级 → USB (ADB)。</p></section>
<section id="joint-jog"><h2>关节点动 · 模型与实机对照</h2><p>先托稳机身、让关节悬空。按住并拖动滑块时,全机使能,其他关节保持起始角;松手断扭矩。此操作不修改标定。</p><label>模型关节 <select id="joint-select"></select></label> <button id="joint-prepare" disabled>准备点动(不使能)</button><p id="joint-details">正在读取模型…</p><label>模型目标角 <input id="joint-range" type="range" step="any" style="width:min(95%,480px);touch-action:none" disabled></label><p id="joint-angles">—</p><div><strong>整机 3D 模型</strong> <label>显示姿态 <select id="model-pose"><option value="actual">实机反馈角</option><option value="goal">滑块目标角</option></select></label> <button type="button" id="model-reset">复位视角</button></div><canvas id="joint-view" style="display:block;width:100%;height:clamp(320px,55vh,580px);touch-action:none;background:#e6edf3;border-radius:10px" aria-label="Microduck 整机实体模型,可旋转缩放"></canvas><small id="model-info" role="status">正在加载整机外观…</small><p class="hint">拖动模型旋转,滚轮或双指缩放,不会驱动舵机。橙色高亮所选关节的活动部件;无反馈轴按零角显示。每次最多偏离起点 12°、15°/秒,空格或顶部按钮可随时关闭扭矩。</p><p id="joint-message" aria-live="polite">未准备 · 只读</p></section>
<section><h2>原机步态与起身 <span id="recovery-phase">—</span></h2><div class="controls"><article><small>原机模型(非热更新版本)</small><p id="active-model">—</p></article><article><small>相对直立姿态的倾角</small><p id="tilt-angle">—</p></article><article><small>状态计时</small><p id="recovery-timer">—</p></article></div><p class="hint">倾角超过 55° 持续 0.15 秒后进入起身流程;倾角低于 15° 持续 1 秒后恢复行走。</p></section>
<section><h2>移动</h2><div class="joysticks"><div class="joystick-side"><label id="left-label">前后/横移</label><div class="joy" id="joy-left" role="group" aria-labelledby="left-label"><div class="knob"></div></div><p class="joy-value">vx <span id="vxValue">0.00</span> · vy <span id="vyValue">0.00</span></p></div><div class="joystick-side"><label id="right-label">转向</label><div class="joy" id="joy-right" role="group" aria-labelledby="right-label"><div class="knob"></div></div><p class="joy-value">ωz <span id="wzValue">0.00</span></p></div></div><button id="zero">速度归零</button><p class="hint">松开摇杆自动回中。W/S 前后,A/D 横移,Q/E 转向。空格关闭扭矩。</p></section>
<section><h2>头部与嘴部</h2><div class="controls" id="head-controls"></div><p class="hint">头部四轴范围为 ±1 弧度。嘴部使用 34 号舵机(0–30°),不属于策略的 14 维输出。</p></section>
<section><h2>动作平滑系数</h2><div class="controls"><label>腿部 α<input id="leg-alpha" type="range" min="0" max="0.95" value="0.45" step="0.05"></label><label>头部 α<input id="head-alpha" type="range" min="0" max="0.95" value="0.45" step="0.05"></label></div></section>
<section class="telemetry"><h2>IMU 与舵机反馈</h2><div class="telemetry-grid"><div class="imu-lines"><div><b>加速度</b><span id="acc-values">—</span></div><div><b>角速度</b><span id="gyro-values">—</span></div><div><b>重力</b><span id="gravity-values">—</span></div></div><div class="table"><table><thead><tr><th>ID</th><th>实测角度 °</th><th>目标角度 °</th></tr></thead><tbody id="joints"></tbody></table></div></div><p class="hint" id="command-health">等待读取指令回执…</p></section>
</main><script type="importmap">{"imports":{"three":"./model-view/three.module.min.js"}}</script><script type="module" src="model-view/view.js"></script><script src="joint-jog.js?v=full1"></script><script src="shadow-skills.js?v=hotspot1"></script><script src="microduck-control-skills-zh.js?v=live1"></script></html>
+20
View File
@@ -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)||a<j.lo||a>j.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&&(a<j.lo||a>j.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));
})();
+29
View File
@@ -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="),
('<label>head ', '<label>头部 '), ('<label>Mouth 34', '<label>嘴部 34'),
("'request failed'", "'请求失败'"), ("'Mode updated'", "'模式已更新'"),
('`Pick started · ', '`已触发拾取 · '), ('`Roulade · ', '`已触发翻滚 · '),
("'status read failed'", "'无法读取设备状态'"),
("{shadow:'inference only',off:'torque off',hold:'default pose',policy:'walk',calibrate:'calibrate',servo_debug:'servo setup'}", "{imported_live:'真机联调',joint_jog:'关节点动',shadow:'仅推理',off:'停止',hold:'保持姿态',policy:'行走',calibrate:'标定',servo_debug:'舵机设置'}"),
("?'online':'offline'", "?'在线':'离线'"), ("?' · enabled':' · torque off'", "?' · 已使能':' · 扭矩已关闭'"),
(' · state age ', ' · 反馈延迟 '), ("?'roulade':pick&&pick.active?'pick':({walk:'walk',default_pose:'get-up · action = 0',getup:'get-up'}", "?'翻滚':pick&&pick.active?'拾取':({walk:'行走',default_pose:'起身 · 动作为零',getup:'起身'}"),
("?phaseLabel:'idle'", "?phaseLabel:'空闲'"), ('`roulade ${', '`翻滚 ${'), ('`pick φ', '`拾取 φ'), ('`hold ${','`保持 ${'), (' s left`',' 秒剩余`'), ('`upright ${','`直立 ${'), ('`fallen ${','`倾倒 ${'),
('`sent ${','`已发送 ${'), (' · MCU echo ', ' · MCU 回执 '), (' · command age ', ' · 指令延迟 '), (' · gaps ', ' · 丢帧 '), (' · rejected ', ' · 拒绝指令 '),
("$('imu').textContent=", "const missing=Array.isArray(s.motors)&&Array.isArray(s.servo_ids)?s.motors.filter(id=>!s.servo_ids.includes(id)):null;$('missing').textContent=missing===null?'舵机列表不可用':missing.length?'未返回的舵机 ID:'+missing.join('、')+'。请先检查,再使能动作。':'全部舵机均有反馈';$('imu').textContent="),
]:
if s.count(old) != 1: raise ValueError(old)
s = s.replace(old, new)
# The imported skills card replaces the native pick/roll buttons; retain the
# native recovery telemetry, joystick handlers, heartbeat and stop behavior.
legacy_controls = ("$('pick').onclick=", "$('roulade').onclick=", "$('pick').disabled=", "$('roulade').disabled=", "const upright=", "document.querySelectorAll('[data-mode=\"hold\"],")
s = ''.join(line for line in s.splitlines(keepends=True) if not line.startswith(legacy_controls))
Path(sys.argv[2]).write_text(s,encoding='utf-8')
@@ -0,0 +1,64 @@
// Read-only viewer: no access to the controller's jog session or command endpoints.
import * as T from 'three';
import { GLTFLoader } from './loaders/GLTFLoader.js';
import { OrbitControls } from './controls/OrbitControls.js';
const canvas = document.getElementById('joint-view'), info = document.getElementById('model-info');
const source = document.getElementById('model-pose');
let pose, update;
window.addEventListener('joint-view-pose', e => { pose=e.detail; update?.(); });
const vec = (node, attr, fallback) => (node.getAttribute(attr)||fallback).trim().split(/\s+/).map(Number);
function transform(obj, node) {
obj.position.fromArray(vec(node,'pos','0 0 0'));
const [w,x,y,z] = vec(node,'quat','1 0 0 0'); obj.quaternion.set(x,y,z,w).normalize();
}
async function init() {
const renderer = new T.WebGLRenderer({canvas,antialias:true,alpha:false,powerPreference:'low-power'});
renderer.setPixelRatio(Math.min(devicePixelRatio,1.75)); renderer.setClearColor(0xe6edf3);
const [xml, library] = await Promise.all([
fetch('joint_model.xml').then(r=>{if(!r.ok)throw Error('模型 XML 不可用');return r.text()}),
new GLTFLoader().loadAsync(new URL('./microduck.glb',import.meta.url).href)
]);
const doc = new DOMParser().parseFromString(xml,'application/xml');
const geo = new Map();library.scene.traverse(o=>{if(o.isMesh){o.geometry.computeVertexNormals();geo.set(o.userData.meshFile,o.geometry)}});
const palette = new Map([...doc.querySelectorAll('asset > material')].map(m=>[m.getAttribute('name'),vec(m,'rgba','0.7 0.7 0.7 1')]));
const files = new Map([...doc.querySelectorAll('asset > mesh')].map(m=>{const f=m.getAttribute('file');return [m.getAttribute('name')||f.replace(/\.stl$/,''),f]}));
const scene=new T.Scene(), rig=new T.Group(), joints=new Map();scene.add(rig);let count=0;
function body(node,parent){
const base=new T.Group();transform(base,node);parent.add(base);
const pivot=new T.Group(), link=new T.Group();base.add(pivot);pivot.add(link);
const hinge=node.querySelector(':scope > joint');
if(hinge){pivot.position.fromArray(vec(hinge,'pos','0 0 0'));link.position.copy(pivot.position).negate();joints.set(hinge.getAttribute('name'),{pivot,axis:new T.Vector3(...vec(hinge,'axis','0 0 1')).normalize(),materials:[]});}
for(const g of node.querySelectorAll(':scope > geom[class="visual"]')){
const file=files.get(g.getAttribute('mesh')), shape=geo.get(file);if(!shape)throw Error(`缺少外观网格 ${file}`);
const [r,b1,b2]=palette.get(g.getAttribute('material'))||[.7,.7,.7];
const mat=new T.MeshStandardMaterial({color:new T.Color().setRGB(r,b1,b2,T.SRGBColorSpace),roughness:.72});
const part=new T.Mesh(shape,mat);transform(part,g);link.add(part);count++;
if(hinge)joints.get(hinge.getAttribute('name')).materials.push(mat);
}
node.querySelectorAll(':scope > body').forEach(b=>body(b,link));
}
body(doc.querySelector('worldbody > body'),rig);
scene.add(new T.HemisphereLight(0xffffff,0xc0c7d0,1.9));
for(const [p,n] of [[[2,-3,4],2],[[-2,2,1],1]]){const light=new T.DirectionalLight(0xffffff,n);light.position.set(...p);scene.add(light)}
const camera=new T.PerspectiveCamera(34,1,.002,5); camera.up.set(0,0,1);
const orbit=new OrbitControls(camera,canvas);orbit.minDistance=.045;orbit.maxDistance=1.65;
const render=()=>{if(!document.hidden)renderer.render(scene,camera)};
function size(){const w=canvas.clientWidth,h=canvas.clientHeight;if(!w||!h)return;camera.aspect=w/h;camera.updateProjectionMatrix();renderer.setSize(w,h,false)}
function fit(){size();const sphere=new T.Box3().setFromObject(rig,true).getBoundingSphere(new T.Sphere());const a=Math.atan(Math.tan(T.MathUtils.degToRad(17))*Math.min(camera.aspect,1));orbit.target.copy(sphere.center);camera.position.copy(sphere.center).addScaledVector(new T.Vector3(1,-1.65,.57).normalize(),sphere.radius/Math.sin(a)*1.06);orbit.update();render()}
let picked=null, framed=false;
update=()=>{
for(const [name,j] of joints){const isSelected=name===pose?.selected;
const deg=(isSelected&&source.value==='goal'&&pose.target!=null)?pose.target:(pose?.angles[name]??0);
j.pivot.quaternion.setFromAxisAngle(j.axis,T.MathUtils.degToRad(deg));
if(picked!==pose?.selected)for(const m of j.materials)m.emissive.setHex(isSelected?0xaa550e:0);
}
picked=pose?.selected; if(pose&&!framed){framed=true;fit()}
const text=`${count} 个外观件 · ${joints.size} 个关节 | `+(!pose?'等待读数':!pose.fresh?'反馈过期,保留最后姿态':source.value==='goal'&&pose.target!=null?'显示所选关节的滑块目标':'显示实机反馈角');
if(info.textContent!==text)info.textContent=text;render();
};
orbit.addEventListener('change',render);source.onchange=update;document.getElementById('model-reset').onclick=fit;
new ResizeObserver(()=>{size();render()}).observe(canvas);canvas.addEventListener('webglcontextlost',()=>info.textContent='3D 显示中断,请刷新页面');
fit();update();
}
init().catch(e=>{info.textContent=`整机模型显示失败:${e.message}。请使用支持 WebGL 的浏览器。`;console.error(e)});
+21
View File
@@ -0,0 +1,21 @@
(() => {
const hotspot=location.host==='10.42.0.1:9527';
if (!hotspot && (!['127.0.0.1','localhost'].includes(location.hostname) || location.port!=='19527')) return;
if(hotspot)document.getElementById('skill-deploy-hint').textContent='模型已保存在板上,无需电脑。更换模型仍通过 USB 部署。';
const panel=document.getElementById('usb-shadow'), buttons=[...panel.querySelectorAll('[data-skill]')];
const start=document.getElementById('live-start'),status=document.getElementById('shadow-status'),result=document.getElementById('shadow-result');
const names=Object.fromEntries(buttons.map(b=>[b.dataset.skill,b.textContent])),api=hotspot?'/api/skills':'http://127.0.0.1:18000/api';
let s=null,token=null,busy=false,epoch=0,beating=false;
async function live(body){const r=await fetch(api+'/live',{method:'POST',headers:{'Content-Type':'application/json'},body:JSON.stringify(body),signal:AbortSignal.timeout(1400)});const x=await r.json();if(!r.ok||!x.ok)throw Error(x.error||x.detail||'请求失败');return x}
function clear(){epoch++;const t=token;token=null;if(t)live({op:'end',token:t}).catch(()=>{});render()}
function render(){const motor=s?.native_mode==='imported_live', idle=s?.lifecycle==='IDLE'&&!s.torque_enabled&&s.imu_ok;
const shadow=idle&&s.native_mode==='shadow'&&!s.shadow_error;
start.disabled=busy||!!token||!idle||!s?.version||!s.live_supported||!!s.shadow_error;
for(const b of buttons)b.disabled=busy||!s?.loaded_skills?.includes(b.dataset.skill)||!(motor?(token&&s.torque_enabled):shadow&&!token);
status.textContent=!s?'板端技能服务未连接':!s.version?'未加载策略,请先推送部署包':`版本 ${s.version.slice(0,12)} · `+(motor?(s.torque_enabled?'电机已使能 · '+(names[s.live_skill]||'保持当前姿态'):'等待 MCU 使能回执'):shadow?'仅推理 · '+(names[s.shadow_skill]||'等待'):'未进入导入技能模式');}
async function poll(){try{if(!document.hidden){const r=await fetch(api+'/status',{cache:'no-store',signal:AbortSignal.timeout(1200)});s=r.ok?await r.json():null}}catch{s=null}finally{render();setTimeout(poll,250)}}
start.onclick=async()=>{if(start.disabled)return;const n=++epoch;busy=true;render();try{const x=await live({op:'start',version:s.version,generation:s.generation});if(n!==epoch||document.hidden){live({op:'end',token:x.token}).catch(()=>{});return}token=x.token;s=x.status;result.textContent='已请求使能;收到 MCU 回执后可选择技能。'}catch(e){result.textContent=e.message}finally{busy=false;render()}};
setInterval(async()=>{const t=token;if(!t||beating||document.hidden)return;beating=true;try{const x=await live({op:'beat',token:t});if(t===token)s=x.status}catch(e){if(t===token){clear();result.textContent='联调已中止:'+e.message}}finally{beating=false;render()}},180);
for(const b of buttons)b.onclick=async()=>{if(b.disabled)return;busy=true;render();const t=token;try{let x;if(t){x=await live({op:'skill',key:b.dataset.skill,token:t})}else{const r=await fetch(api+'/shadow/'+b.dataset.skill,{method:'POST',headers:{'Content-Type':'application/json'},signal:AbortSignal.timeout(1500)});x=await r.json();if(!r.ok||!x.ok)throw Error(x.error||x.detail||'未切换')}s=x.status;result.textContent=(t?'真机技能已接受:':'仅推理:')+b.textContent}catch(e){result.textContent=e.message}finally{busy=false;render()}};
document.getElementById('stop').addEventListener('click',clear);panel.querySelector('[data-mode="shadow"]').addEventListener('click',clear);window.addEventListener('pagehide',clear);document.addEventListener('visibilitychange',()=>{if(document.hidden)clear()});poll();
})();
@@ -0,0 +1,13 @@
#!/bin/sh
# Stage the standalone viewer assets into a directory named model-view for USB upload.
set -eu
here=$(cd "$(dirname "$0")" && pwd)
app="$here/../../../app"
three="$app/node_modules/three"
out=${1:?supply an output directory}
mkdir -p "$out/loaders" "$out/controls" "$out/utils"
cp "$here/model-view/view.js" "$out/view.js"
cp "$three/build/three.module.min.js" "$three/LICENSE" "$app/public/robot/mjlab/microduck.glb" "$out/"
for module in loaders/GLTFLoader controls/OrbitControls utils/BufferGeometryUtils; do
cp "$three/examples/jsm/$module.js" "$out/$module.js"
done
+35
View File
@@ -0,0 +1,35 @@
"""Read/control the in-process motor slot from the board's WPA hotspot; no deploy route."""
from fastapi import HTTPException, Request
from fastapi.responses import JSONResponse
HOST='10.42.0.1:9527'
def register(ui, slot):
def check(req, write=False):
if req.headers.get('host')!=HOST or (write and req.headers.get('origin')!='http://'+HOST):
raise HTTPException(403,'请从板子热点主页操作')
if write and req.headers.get('content-type','').split(';')[0]!='application/json':
raise HTTPException(415,'需要 JSON 请求')
def response(obj): return JSONResponse(obj,headers={'Cache-Control':'no-store'})
def status(request: Request):
check(request); return response(slot.status())
def live(request: Request, body: dict):
check(request,True)
if not slot.live: raise HTTPException(409,'未安装真机联调接口')
try: result=slot.live.request(body)
except (KeyError,TypeError,ValueError) as e: raise HTTPException(409,str(e))
return response({'ok':True,**result,'status':slot.status()})
def shadow(key: str, request: Request):
check(request,True)
try: slot.select_shadow(key)
except ValueError as e: raise HTTPException(409,str(e))
return response({'ok':True,'status':slot.status()})
ui.expose_api('GET','/api/skills/status',status)
ui.expose_api('POST','/api/skills/live',live)
ui.expose_api('POST','/api/skills/shadow/{key}',shadow)
+31
View File
@@ -0,0 +1,31 @@
"""Run on board with the reviewed original app's python/ path. Does not restart anything."""
import hashlib
import os
from pathlib import Path
import sys
def install(root):
edits = {
'controller.py': ('d982dd3b5025f29aa0503e85c30ab74aac552923793c94a6857d81c95e76aba5',
" if inferring and phase == 'default_pose':",
" if inferring and mode == 'shadow' and self.usb_shadow.tick(filt):\n pass # leave native target at DEFAULT_DEG, command remains DISARM\n elif inferring and phase == 'default_pose':"),
'main.py': ('176fad0332db4c00cdf803020aa7fd02700e34ee1727c9a9de5ab8c928aa11dc',
"worker = threading.Thread(target=controller.run", "from usb_shadow import attach\ncontroller.usb_shadow = attach(controller)\nworker = threading.Thread(target=controller.run"),
}
changed = {}
for name, (sha, before, after) in edits.items():
path = root / name
original = path.read_bytes()
if hashlib.sha256(original).hexdigest() != sha: raise ValueError(f'Unexpected original: {name}')
assert original.count(before.encode()) == 1
backup = path.with_suffix('.py.before-usb-shadow')
if backup.exists() and backup.read_bytes() != original: raise ValueError(f'Backup conflict: {backup}')
backup.write_bytes(original)
changed[path] = original.replace(before.encode(),after.encode())
compile(changed[path], str(path), 'exec')
for path, data in changed.items():
tmp = path.with_suffix('.py.usb-new'); tmp.write_bytes(data); os.replace(tmp,path)
if __name__ == '__main__': install(Path(sys.argv[1]))
@@ -0,0 +1,16 @@
"""Enable the hotspot routes on the reviewed joint-jog/live board entry point."""
import hashlib
import sys
from pathlib import Path
def install(path):
old=path.read_bytes()
if hashlib.sha256(old).hexdigest()!='9ae32ad1c26f3d75d11e775b18d7e4a95693c3e886671ada4d8a3a50ca874605':
raise ValueError('Unknown main.py; review before installing hotspot routes')
before=b'controller.usb_shadow = attach(controller)'
assert old.count(before)==1
new=old.replace(before,b'controller.usb_shadow = attach(controller,ui)')
compile(new,str(path),'exec'); path.write_bytes(new)
if __name__=='__main__':install(Path(sys.argv[1]))
+32
View File
@@ -0,0 +1,32 @@
"""Patch the reviewed shadow-enabled app (copy joint_jog.py & joint_model.xml first)."""
from pathlib import Path
import hashlib
import os
import sys
def install(root):
patches = {
'controller.py': ('26342f2ed8caf633338aec60d220c884c692eb866bd2bbbae7345304f93221e3', [
(' if mode == self.mode:', ''' if self.mode == 'joint_jog' and mode not in ('off','shadow'):
raise ValueError('请先停止关节点动')
self.jog.cancel()
if mode == self.mode:'''),
(' fb, filt, generation, mode = self.feedback', ''' if self.mode == 'joint_jog':
self.jog.tick()
return
fb, filt, generation, mode = self.feedback''')]),
'main.py': ('6cb7cf61625c5fee5da024c1888cf40f21e2ae2ad89c7acab830a54b9fe7eb93', [
("from usb_shadow import attach", "from joint_jog import attach as attach_jog\ncontroller.jog = attach_jog(controller,ui)\nfrom usb_shadow import attach")])}
changed = {}
for name,(sha, edits) in patches.items():
path = root/name; old = path.read_bytes(); assert hashlib.sha256(old).hexdigest()==sha, name
backup = root/(name+'.before-joint-jog'); assert not backup.exists() or backup.read_bytes()==old
s = old.decode()
for a,b in edits:
assert s.count(a)==1; s=s.replace(a,b)
compile(s, name, 'exec'); backup.write_bytes(old); changed[path]=s
for path,s in changed.items():
tmp = path.with_suffix('.jog-new'); tmp.write_text(s); os.replace(tmp,path)
if __name__=='__main__': install(Path(sys.argv[1]))
+18
View File
@@ -0,0 +1,18 @@
"""Patch the reviewed joint-jog controller only. Does not change the STM32."""
import hashlib
import os
from pathlib import Path
import sys
def install(root):
p=root/'controller.py'; old=p.read_bytes()
if hashlib.sha256(old).hexdigest()!='458d9d87f74b9724627bec814df29d4836248d3c7ba5c5558862654cf35dece0': raise ValueError('Unexpected controller revision')
s=old.decode()
for a,b in [('class Controller:', 'class Controller:\n has_live_hook = True'),
(" if self.mode == 'joint_jog' and mode", " if self.mode == 'imported_live':\n if mode not in ('off','shadow'): raise ValueError('请先停止导入策略')\n self.usb_shadow.live.stop()\n if mode == 'off' and self.mode == 'off': self.generation += 1 # invalidate delayed start\n if self.mode == 'joint_jog' and mode"),
(" if self.mode == 'joint_jog':\n self.jog.tick()", " if self.mode == 'imported_live':\n self.usb_shadow.live.tick()\n return\n if self.mode == 'joint_jog':\n self.jog.tick()")]:
assert s.count(a)==1; s=s.replace(a,b)
compile(s,str(p),'exec'); bkp=root/'controller.py.before-live-skills'; assert not bkp.exists() or bkp.read_bytes()==old; bkp.write_bytes(old)
tmp=root/'controller.py.live-new';tmp.write_text(s);os.replace(tmp,p)
if __name__=='__main__': install(Path(sys.argv[1]))
+97
View File
@@ -0,0 +1,97 @@
"""Momentary MJCF joint check, within the native controller lock and MCU command loop."""
import math
import secrets
import time
import xml.etree.ElementTree as ET
from pathlib import Path
from rl_core import MOTOR_IDS
from wire import ARM, HOLD, encode_command
NAMES = ('left_hip_yaw left_hip_roll left_hip_pitch left_knee left_ankle right_hip_yaw right_hip_roll right_hip_pitch right_knee right_ankle neck_pitch head_pitch head_yaw head_roll').split()
IDS = (10,11,12,13,14,20,21,22,23,24,30,31,32,33)
MODE = 'joint_jog'
class Jog:
def __init__(self, controller, xml):
self.c = controller
root = ET.parse(xml).getroot()
assert root.find('compiler').get('angle') == 'radian'
joints = {j.get('name'):j for j in root.findall('.//worldbody//joint')}
assert set(NAMES) == {a.get('joint') for a in root.findall('actuator/position')}
self.rows = {}
for id_, name in zip(IDS, NAMES):
j = joints[name]; lo, hi = map(math.degrees, map(float, j.get('range').split()))
self.rows[id_] = dict(id=id_, name=name, axis=j.get('axis'), lo=round(lo,5), hi=round(hi,5))
self.token = None
def cancel(self, error=''):
c = self.c
self.token = None
if c.mode == MODE:
c.mode = 'off'; c.generation += 1; c.last_error = error
c._disarm_now()
def handle(self, body):
c = self.c
with c.lock:
now = time.monotonic(); op = body.get('op')
if op == 'prepare':
if c.mode not in ('off','shadow'): raise ValueError('请先关闭扭矩并退出其他模式')
id_ = int(body['id']); row = self.rows[id_]; i = MOTOR_IDS.index(id_)
required = sum(1<<MOTOR_IDS.index(n) for n in set(IDS)-{31,33}) | (1<<i)
fb = c._require_feedback(required)
if fb.enabled or fb.calibrating or fb.servo_debugging: raise ValueError('请先退出原机带扭矩模式')
base = fb.angles[i]
if not row['lo'] <= base <= row['hi']: raise ValueError('实测角已超出模型限位,不能从此位置点动')
self.i, self.mask, self.base = i, fb.mask, base
self.lo = max(row['lo'], base-12); self.hi = min(row['hi'], base+12)
self.token = secrets.token_hex(16); self.gen = c.generation; self.until = now+30
return dict(token=self.token, base=base, lo=self.lo, hi=self.hi)
if not self.token or body.get('token') != self.token: raise ValueError('请重新选择并准备关节')
if op == 'end': self.cancel(); return {'ok':True}
if op != 'move': raise ValueError('未知操作')
try:
if now > self.until or c.generation != self.gen: raise ValueError('点动会话已结束')
goal = float(body['deg'])
if not self.lo <= goal <= self.hi: raise ValueError('目标超出本次点动范围')
fb = c._require_feedback(self.mask)
if fb.mask != self.mask or fb.calibrating or fb.servo_debugging: raise ValueError('舵机在线状态改变')
if c.mode != MODE:
if c.mode not in ('off','shadow') or fb.enabled or abs(fb.angles[self.i]-self.base)>2: raise ValueError('姿态或模式改变,请重新准备')
if not all(math.isfinite(x) for x in fb.angles): raise ValueError('无效角度')
self.targets = list(fb.angles); self.start = self.prev = now; self.first = True; self.armed = False
c.generation += 1; self.gen = c.generation; c.mode = MODE; c.arm_pending = False; c.last_error = ''
self.goal = goal; self.until = now+.45
return {'ok':True}
except (ValueError,TypeError,KeyError):
self.cancel(); raise
def tick(self):
# Called only under Controller.lock, only in MODE. Never starts on a GET or prepare.
c = self.c; now = time.monotonic()
try:
fb = c._require_feedback(self.mask)
if now > self.until or now-self.start > 15: raise ValueError('点动超时,扭矩已关闭')
if fb.mask != self.mask or fb.calibrating or fb.servo_debugging or fb.flags & 4: raise ValueError('反馈状态改变,点动停止')
if not self.first and not fb.enabled and (self.armed or now-self.start>.2): raise ValueError('使能未保持,点动停止')
if any(not math.isfinite(a) or abs(a-t)>9 for j,(a,t) in enumerate(zip(fb.angles,self.targets)) if self.mask & (1<<j)): raise ValueError('关节跟随偏差过大,点动停止')
self.armed |= fb.enabled
if self.armed:
cap = min(.025,now-self.prev)*15
self.targets[self.i] += max(-cap,min(cap,self.goal-self.targets[self.i]))
c.seq = (c.seq+1)&0xffffffff
cmd = encode_command(c.seq, fb.sample_us, ARM if self.first else HOLD, self.targets, ttl_us=100000)
c.bridge.notify('duck_command',cmd); c.sent += 1; c.last_action = self.targets.copy(); self.first=False; self.prev=now
except ValueError as exc: self.cancel(str(exc))
def attach(c, ui):
from fastapi import HTTPException
jog = Jog(c, Path(__file__).with_name('joint_model.xml'))
def info(): return {'joints':list(jog.rows.values())}
def action(body: dict):
try: return jog.handle(body)
except (ValueError,KeyError,TypeError) as exc: raise HTTPException(400,str(exc))
ui.expose_api('GET','/api/joints',info)
ui.expose_api('POST','/api/joint-jog',action)
return jog
+87
View File
@@ -0,0 +1,87 @@
"""Operator-owned motor session. Only the native 50 Hz worker sends motion frames."""
import secrets
import time
import numpy as np
from rl_core import MOTOR_IDS, POLICY_IDS, POLICY_INDEX
from wire import ARM, HOLD, POLICY, encode_command
from shadow_core.state_machine import MicroduckStateMachine, LifecycleState
from shadow_core.observations import build_observation
MODE='imported_live'
REQUIRED=sum(1<<MOTOR_IDS.index(n) for n in (10,11,12,13,14,20,21,22,23,24,30,32))
EVENT={'groundpick':'groundPick','sitstand':'sitToggle','kickL':'kickL','kickR':'kickR','roll':'roll'}
class LiveSkills:
def __init__(self,slot):
self.slot,self.c=slot,slot.c
self.token=None; self.skill=None
def stop(self, reason=''):
c=self.c; self.token=None; self.skill=None
if c.mode==MODE:
c.mode='off'; c.generation+=1; c.last_error=reason; c._disarm_now()
def request(self,b):
c=self.c
with c.lock:
now=time.monotonic(); op=b['op']
if op=='start':
p=self.slot.current
if not self.slot.idle() or not p or p.error or b.get('version')!=p.version or b.get('generation')!=c.generation: raise ValueError('设备状态已改变,请刷新状态再开始')
fb=c._require_feedback(REQUIRED)
if fb.calibrating or fb.servo_debugging or fb.flags&4: raise ValueError('请先退出标定或舵机维护')
self.sm=MicroduckStateMachine(p.sm.manifest,p.sm.custom_script)
self.sm.request_stand(p.pose,.02); self.sm.step(.02,-1.)
self.p=p; self.last=np.zeros(14,np.float32); self.filt_act=np.zeros_like(self.last); self.skill=None; self.prev_key=None; self.run_policy=False
self.targets=np.array(fb.angles,dtype=float); self.mask=fb.mask; self.first=True; self.acked=False; self.start=self.prev=now; self.until=now+.85
self.limits=np.array([[c.jog.rows[n]['lo'],c.jog.rows[n]['hi']] for n in POLICY_IDS]);self.online=np.array([bool(fb.mask&(1<<i)) for i in POLICY_INDEX])
c.jog.cancel(); c.last_error=''; c.arm_pending=False; c.generation+=1; c.mode=MODE
self.token=secrets.token_hex(16); return {'token':self.token}
if c.mode!=MODE or not self.token or b.get('token')!=self.token: raise ValueError('本页没有有效的电机会话')
if op=='end': self.stop(); return {}
if now>self.until: self.stop('联调心跳超时'); raise ValueError('联调会话已超时')
if op=='beat': self.until=now+.85; return {}
if op!='skill' or b.get('key') not in self.p.sessions or not self.acked: raise ValueError('请等待 MCU 使能回执后再选技能')
k=b['key']; sm=self.sm
if k=='walk':
sm.request_stand(self.p.pose,.02); sm.step(.02,-1.); self.prev_key=None
elif k=='stand':
if sm.recovery_state or sm.skill_mode!='walk': raise ValueError('请先切回行走再起身')
sm.recovery_state='recovering'; sm.recovery_steps=sm.recovery_upright_steps=0
elif k not in EVENT or (k=='sitstand' and sm.custom_slot in ('trick','script')) or not sm.trigger_action(EVENT[k]): raise ValueError('当前技能尚未结束,不能切换')
self.run_policy=True
return {}
def tick(self): # under c.lock: mutually exclusive with native targets and mode changes
c=self.c; now=time.monotonic()
try:
fb=c._require_feedback(self.mask)
if now>self.until or fb.mask!=self.mask or fb.calibrating or fb.servo_debugging or fb.flags&4: raise ValueError('联调心跳或反馈中断,已停机')
self.acked |= fb.enabled
if not self.first and not fb.enabled and (self.acked or now-self.start>.25): raise ValueError('电机使能未保持')
online=[i for i in range(len(MOTOR_IDS)) if fb.mask&(1<<i)]
if np.max(np.abs(np.asarray(fb.angles)[online]-self.targets[online]))>22: raise ValueError('关节跟随误差过大,已停机')
mode=ARM if self.first else HOLD
if self.acked and self.run_policy:
f=c.filtered; sm=self.sm; p=self.p
sm.vel_cmd[:]=c.twist; sm.head_target[:]=c.head
d=sm.step(.02,float(f['grav'][2])); k=d.policy_key
if not d.torque_enabled or not k or sm.lifecycle!=LifecycleState.ACTIVE: raise ValueError('技能状态机要求停机')
if d.zero_last_action or k!=self.prev_key: self.last.fill(0); self.filt_act.fill(0)
self.prev_key=self.skill=k
q,dq=[np.deg2rad(np.asarray(f[x])[POLICY_INDEX]) for x in ('angles_deg','vels_deg')]
obs=build_observation(f['gyro_rad'],f['grav'],q,dq,self.last,d.command_13,p.pose)
sess,name=p.sessions[k]; out=sess.run(None,{name:obs.reshape(1,61)})[0][0]
scale=float(sm.manifest['skills'][k].get('action_scale',sm.manifest.get('action_scale',1)))
alpha=np.array([c.policy.action_alpha]*5+[c.policy.action_alpha_head]*5+[c.policy.action_alpha]*4)
self.last[:]=out; self.filt_act[:]=alpha*self.filt_act+(1-alpha)*out
goal=np.rad2deg(p.pose+scale*self.filt_act)
if not np.isfinite(goal).all(): raise ValueError('策略目标非有限值')
# Starting outside an XML range never snaps to the limit; ramp from measured.
goal=np.clip(goal,self.limits[:,0],self.limits[:,1]); delta=min(.025,now-self.prev)*90
ix=POLICY_INDEX[self.online];self.targets[ix]+=np.clip(goal[self.online]-self.targets[ix],-delta,delta)
if fb.mask&(1<<14): self.targets[14]+=np.clip(np.clip(c.mouth,0,30)-self.targets[14],-delta,delta)
mode=POLICY
if time.monotonic()-c.rx_time>.1 or time.monotonic()>self.until: raise ValueError('反馈或策略计算超时')
c.seq=(c.seq+1)&0xffffffff; c.bridge.notify('duck_command',encode_command(c.seq,fb.sample_us,mode,self.targets,ttl_us=100000));c.sent+=1;c.last_action=self.targets.tolist();self.first=False;self.prev=now
except Exception as exc: self.stop(str(exc))
+29
View File
@@ -0,0 +1,29 @@
"""Actuator-free test: pass deployed ZIP, captured /api/status, and MJCF paths.
Requires native python/, shadow_core and this directory on PYTHONPATH. Never imports Bridge.
"""
import json
import sys
from pathlib import Path
from types import SimpleNamespace as NS
from unittest.mock import patch
from threading import RLock
import numpy as np
from usb_shadow import Slot,Preview
from joint_jog import Jog
from wire import ARM,HOLD,POLICY,COMMAND
from live_skills import REQUIRED,MODE
def run(bundle,s,model):
packets=[]; f=NS(enabled=False,angles=s['angles'],imu_ok=True,flags=1,calibrating=False,servo_debugging=False,mask=REQUIRED,sample_us=3)
c=NS(mode='off',feedback=f,lock=RLock(),generation=0,seq=0,sent=0,arm_pending=False,has_live_hook=True,twist=[0]*3,head=[0]*4,mouth=0,policy=NS(action_alpha=.45,action_alpha_head=.45),bridge=NS(notify=lambda _,b:packets.append(COMMAND.unpack(b))))
c.filtered=dict(angles_deg=f.angles,vels_deg=s['velocities'],grav=[0,0,-1],gyro_rad=[0,0,0]);c._require_feedback=lambda mask:f;c._disarm_now=lambda:packets.append('DISARM');c.jog=Jog(c,model);slot=Slot(c);slot.current=Preview(bundle);live=slot.live
with patch('time.monotonic') as clock:
for k in slot.current.sessions:
clock.return_value=c.rx_time=50.;t=live.request(dict(op='start',version=slot.current.version,generation=c.generation))['token']; live.tick();assert packets[-1][4]==ARM and np.allclose(packets[-1][-15:],f.angles)
f.enabled=True;clock.return_value=c.rx_time=50.02;live.tick();assert packets[-1][4]==HOLD
live.request(dict(op='skill',key=k,token=t));clock.return_value=c.rx_time=50.04; live.tick();assert c.mode==MODE and live.skill==k and packets[-1][4]==POLICY,(k,c.last_error,live.skill);assert np.max(np.abs(np.array(packets[-1][-15:])-f.angles))<=1.801
clock.return_value=c.rx_time=51.;live.tick();assert c.mode=='off' and packets[-1]=='DISARM'; f.enabled=False
print('PASS: measured ARM, hold, 7 ONNX-to-POLICY paths, first step slew bound, heartbeat disarm')
if __name__=='__main__':run(Path(sys.argv[1]).read_bytes(),json.loads(Path(sys.argv[2]).read_text()),sys.argv[3])
+231
View File
@@ -0,0 +1,231 @@
"""In-process, USB-only shadow policy slot for the original XGO Duck controller.
No Bridge imports, actuator writes, mode changes, calibration or firmware updates.
"""
import hashlib
import io
import json
import os
import re
import socketserver
import threading
import tempfile
import time
import zipfile
from http.server import BaseHTTPRequestHandler
from pathlib import Path
import numpy as np
import onnxruntime as ort
from rl_core import POLICY_INDEX
from shadow_core.config import validate_robot_manifest
from shadow_core.observations import build_observation
from shadow_core.state_machine import MicroduckStateMachine
DATA = Path(__file__).resolve().parents[1] / 'data' / 'usb-shadow'
SOCKET = DATA / 'api.sock'
ORIGINS = {'http://127.0.0.1:5173', 'http://localhost:5173'}
CONTROL_ORIGINS = {'http://127.0.0.1:19527', 'http://localhost:19527'}
ACTIONS = {'walk': None, 'stand': None, 'sitstand': 'sitToggle', 'kickL': 'kickL', 'kickR': 'kickR', 'roll': 'roll', 'groundpick': 'groundPick'}
LIMIT = 32 << 20
JOINTS = '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'.split()
class Preview:
def __init__(self, raw):
self.version = hashlib.sha256(raw).hexdigest()
self.sessions = {}
prefix = 'microduck_unoq/'
with zipfile.ZipFile(io.BytesIO(raw)) as z:
names = z.namelist()
if len(names) > 32 or len(names) != len(set(names)) or sum(i.file_size for i in z.infolist()) > LIMIT:
raise ValueError('部署包超出限制或有重名文件')
manifest = validate_robot_manifest(json.loads(z.read(prefix+'config/robot_manifest.json')))
if manifest['joint_names'] != JOINTS or manifest['command_size'] != 13:
raise ValueError('关节或指令布局不兼容')
self.pose = np.array(manifest['default_pose'], dtype=np.float32)
if not np.isfinite(self.pose).all():
raise ValueError('非有限默认关节角')
script_path = prefix+'config/custom_move_script.json'
script = json.loads(z.read(script_path)) if script_path in names else None
self.sm = MicroduckStateMachine(manifest, script)
allowed = {prefix+'config/robot_manifest.json', script_path}
for key, spec in manifest['skills'].items():
path = spec['path']
if not re.fullmatch(r'policies/[\w-]+\.onnx', path):
raise ValueError('策略文件路径不兼容')
allowed.add(prefix+path)
opts = ort.SessionOptions()
opts.intra_op_num_threads = opts.inter_op_num_threads = 1
session = ort.InferenceSession(z.read(prefix+path), sess_options=opts, providers=['CPUExecutionProvider'])
ins, outs = session.get_inputs(), session.get_outputs()
if len(ins)!=1 or len(outs)!=1 or ins[0].shape != [1,61] or outs[0].shape != [1,14] or ins[0].type != 'tensor(float)' or outs[0].type != 'tensor(float)':
raise ValueError(f'{key}: 需要 float32 [1,61] → [1,14]')
if not np.isfinite(session.run(None, {ins[0].name: np.zeros((1,61), np.float32)})[0]).all():
raise ValueError(f'{key}: 试推理失败')
self.sessions[key] = (session, ins[0].name)
if set(names) - allowed:
raise ValueError('仅支持策略热更新包,不接受固件或标定文件')
self.sm.request_stand(self.pose, soft_start_s=0.02) # virtual lifecycle only
self.last = np.zeros(14, np.float32)
self.steps, self.skill, self.error = 0, None, None
self.guard = threading.Lock()
self.stand_frames = 0
def select(self, key): # caller holds guard; this state is never used for motion
if key == 'walk':
self.stand_frames = 0
self.sm.request_stand(self.pose, soft_start_s=0.02)
return True
if key == 'stand':
if self.sm.skill_mode != 'walk' or self.sm.recovery_state: return False
self.last.fill(0)
self.stand_frames = 50 # preview the recovery model without forging an IMU fall
return True
if self.stand_frames or (key == 'sitstand' and self.sm.custom_slot in ('trick', 'script')): return False
return self.sm.trigger_action(ACTIONS[key])
def step(self, filt):
with self.guard:
d = self.sm.step(0.02, float(filt['grav'][2]))
key = d.policy_key
if self.stand_frames:
key = 'stand'
self.stand_frames -= 1
if d.zero_last_action or self.skill != key:
self.last.fill(0)
self.skill = key
if not key:
return
q, dq = [np.deg2rad(np.asarray(filt[k])[POLICY_INDEX]) for k in ('angles_deg','vels_deg')]
obs = build_observation(filt['gyro_rad'], filt['grav'], q, dq, self.last, d.command_13, self.pose)
sess, name = self.sessions[key]
out = sess.run(None, {name: obs.reshape(1,61)})[0][0]
if not np.isfinite(out).all():
raise ValueError('非有限策略输出')
self.last[:] = out # history only; deliberately never returned to the native target path
self.steps += 1
class Slot:
def __init__(self, controller):
self.c, self.current = controller, None
self.live = self.load_error = None
if getattr(controller,'has_live_hook',False):
from live_skills import LiveSkills
self.live = LiveSkills(self)
def idle(self): # caller holds native controller lock
c = self.c
return bool(c.mode in ('off','shadow') and c.feedback and not c.arm_pending and not c.feedback.enabled and time.monotonic()-c.rx_time < .15)
def replace(self, raw):
with self.c.lock:
if not self.idle(): raise ValueError('仅允许在反馈新鲜、扭矩关闭时更新')
generation = self.c.generation
candidate = Preview(raw) # never load while holding native control locks
with self.c.lock:
if not self.idle() or generation != self.c.generation: raise ValueError('更新期间设备状态已改变')
sample = self.c.filtered
if not sample: raise ValueError('缺少传感器反馈')
candidate.step(sample) # real observation before commit
with tempfile.NamedTemporaryFile(dir=DATA,delete=False) as f:
pending=Path(f.name)
try:
with pending.open('wb') as f:
f.write(raw); f.flush(); os.fsync(f.fileno())
with self.c.lock:
if not self.idle() or generation != self.c.generation: raise ValueError('更新期间设备状态已改变')
os.replace(pending,DATA/'active.zip'); self.current=candidate; self.load_error=None
finally: pending.unlink(missing_ok=True)
def select_shadow(self,key):
p=self.current
if not p or key not in ACTIONS: raise ValueError('尚未加载或未知技能')
with p.guard, self.c.lock:
if not (self.current is p and self.idle() and self.c.mode=='shadow' and self.c.feedback.imu_ok and not p.error and key in p.sessions):
raise ValueError('需要有效反馈和无扭矩预览模式')
if not p.select(key): raise ValueError('当前预览状态不能切换技能')
def status(self):
with self.c.lock:
p = self.current
return dict(hal='xgoduck-shadow', lifecycle='IDLE' if self.idle() else 'BUSY', native_mode=self.c.mode,
imu_ok=bool(self.c.feedback and self.c.feedback.imu_ok), torque_enabled=bool(not self.c.feedback or self.c.feedback.enabled),
live_supported=bool(self.live), generation=self.c.generation, live_skill=self.live.skill if self.live and self.c.mode=='imported_live' else None,
loaded_skills=sorted(p.sessions) if p else [], version=p.version if p else None,
shadow_skill=p.skill if p else None, shadow_steps=p.steps if p else 0, shadow_error=p.error if p else self.load_error)
def tick(self, filt):
p = self.current
if p and not p.error:
try: p.step(filt)
except Exception as exc: p.error = str(exc)
return p is not None
def attach(controller,ui=None):
slot = Slot(controller)
DATA.mkdir(mode=0o700,exist_ok=True)
saved=DATA/'active.zip'
if saved.exists():
try:
if not 0<saved.stat().st_size<=LIMIT: raise ValueError('本地策略包大小异常')
slot.current=Preview(saved.read_bytes()) # never restores motor enable/session
except Exception as exc: slot.load_error=str(exc)
if ui is not None:
from hotspot_skills import register
register(ui,slot)
class Handler(BaseHTTPRequestHandler):
def answer(self, code, obj):
body=json.dumps(obj).encode()
self.send_response(code)
if self.headers.get('Origin') in ORIGINS | CONTROL_ORIGINS:
self.send_header('Access-Control-Allow-Origin', self.headers['Origin'])
self.send_header('Access-Control-Allow-Methods', 'GET, POST, OPTIONS')
self.send_header('Access-Control-Allow-Headers', 'Content-Type')
self.send_header('Cache-Control','no-store')
self.send_header('Content-Type','application/json'); self.send_header('Content-Length',str(len(body)))
self.end_headers(); self.wfile.write(body)
def do_OPTIONS(self): self.answer(200,{})
def do_GET(self):
if self.path != '/api/status': return self.answer(404,{})
self.answer(200,slot.status())
def do_POST(self):
is_skill = self.path.startswith('/api/shadow/')
is_live = self.path == '/api/live'
permitted = CONTROL_ORIGINS if is_live else ORIGINS | CONTROL_ORIGINS if is_skill else ORIGINS
if self.headers.get('Origin') not in permitted or self.headers.get('Host') not in ('127.0.0.1:18000','localhost:18000'):
return self.answer(403,{'error':'来源不受信任'})
if is_live:
try:
if not slot.live or self.headers.get('Content-Type') != 'application/json': raise ValueError('不支持真机联调请求')
n=int(self.headers.get('Content-Length',0))
if not 0<n<=1024: raise ValueError('无效请求')
self.connection.settimeout(1)
data=slot.live.request(json.loads(self.rfile.read(n)))
except (KeyError,TypeError,ValueError,OSError) as exc: return self.answer(409,{'error':str(exc)})
return self.answer(200,{'ok':True,**data,'status':slot.status()})
if is_skill:
key = self.path.removeprefix('/api/shadow/')
if key not in ACTIONS: return self.answer(404,{'error':'未知技能'})
try: slot.select_shadow(key)
except ValueError as exc: return self.answer(409,{'error':str(exc)})
return self.answer(200,{'ok':True,'status':slot.status()})
if self.path != '/api/deploy?overwrite_profile=0' or self.headers.get('Content-Type') != 'application/zip':
return self.answer(400,{'error':'只接受策略部署包,不支持覆盖原机标定'})
try:
n=int(self.headers.get('Content-Length',0))
if not 0<n<=LIMIT: raise ValueError('部署包大小无效')
self.connection.settimeout(30)
raw=self.rfile.read(n)
if len(raw)!=n: raise ValueError('部署包不完整')
slot.replace(raw)
except Exception as exc: return self.answer(400,{'ok':False,'error':str(exc)})
self.answer(200,{'ok':True,'status':slot.status()})
def log_message(self,*args): pass
DATA.mkdir(mode=0o700,exist_ok=True)
SOCKET.unlink(missing_ok=True)
server=socketserver.UnixStreamServer(str(SOCKET),Handler)
os.chmod(SOCKET,0o600)
threading.Thread(target=server.serve_forever,name='usb-shadow-api',daemon=True).start()
return slot