feat: add Microduck UNO Q deployment and hotspot skill control
This commit is contained in:
@@ -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
@@ -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",
|
||||
|
||||
@@ -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 软插值,再开始步态测试。
|
||||
@@ -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
|
||||
}
|
||||
@@ -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])]
|
||||
@@ -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."""
|
||||
@@ -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"
|
||||
@@ -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);
|
||||
};
|
||||
@@ -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_));
|
||||
}
|
||||
@@ -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);
|
||||
};
|
||||
@@ -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
|
||||
@@ -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>
|
||||
@@ -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
@@ -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)}
|
||||
/>
|
||||
</>
|
||||
);
|
||||
}
|
||||
|
||||
@@ -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;
|
||||
}
|
||||
@@ -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,
|
||||
};
|
||||
}
|
||||
@@ -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>
|
||||
);
|
||||
}
|
||||
@@ -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>
|
||||
);
|
||||
}
|
||||
|
||||
@@ -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 推理冒烟。
|
||||
@@ -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|
|
||||
@@ -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 软插值,再开始步态测试。
|
||||
@@ -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
|
||||
}
|
||||
@@ -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])]
|
||||
@@ -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,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")
|
||||
@@ -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"
|
||||
@@ -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()
|
||||
@@ -0,0 +1 @@
|
||||
"""Web HTTP/WebSocket control and deployment package for Microduck UNO Q."""
|
||||
@@ -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>
|
||||
Executable
+10
@@ -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."
|
||||
Executable
+44
@@ -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"
|
||||
@@ -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()
|
||||
@@ -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);
|
||||
};
|
||||
@@ -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_));
|
||||
}
|
||||
@@ -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);
|
||||
};
|
||||
@@ -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
|
||||
@@ -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`。不需要刷固件或恢复零位。原机升级后不要将旧备份套到新版本。
|
||||
@@ -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))
|
||||
@@ -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>
|
||||
@@ -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));
|
||||
})();
|
||||
@@ -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)});
|
||||
@@ -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
|
||||
@@ -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)
|
||||
@@ -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]))
|
||||
@@ -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]))
|
||||
@@ -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]))
|
||||
@@ -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
|
||||
@@ -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))
|
||||
@@ -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])
|
||||
@@ -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
|
||||
Reference in New Issue
Block a user