2 Commits

197 changed files with 16365 additions and 14 deletions
+30 -2
View File
@@ -11,10 +11,18 @@ license: apache-2.0
# Browser robot simulation platform # Browser robot simulation platform
MuJoCo WASM + onnxruntime-web + three.js, with **Microduck** (legs / rollers), **Microban**, and **RP1** Robot Packages. Physics and ONNX inference run locally in the browser; no simulation backend is required. MuJoCo WASM + onnxruntime-web + three.js, with **Microduck** (legs / rollers), **Microban**, **RP1**, **Booster T1**, and **Apptronik Apollo** Robot Packages. Physics and ONNX inference run locally in the browser; no simulation backend is required.
The application opens directly into simulation, defaults to Microduck, and keeps pause, reset, robot selection and a collapsible keyboard reference on that same screen. There is no welcome / RESUME menu or BIOS animation. Anton is bundled locally, so the default interface does not wait for an external font service. The application opens directly into simulation, defaults to Microduck, and keeps pause, reset, robot selection and a collapsible keyboard reference on that same screen. There is no welcome / RESUME menu or BIOS animation. Anton is bundled locally, so the default interface does not wait for an external font service.
## v0.6
- Added Booster T1 and Apptronik Apollo packages with browser-native ONNX locomotion and WASD control.
- Apollo uses the explicitly selected upright-v3 `model_9999.pt`, including its calibrated stand pose, spawn height and fall threshold.
- Added body-frame linear-velocity observations and explicit World floor collision settings for imported models.
- Reproducible import scripts, source hashes and browser regression tests accompany both new robots.
- Verified: 68 unit tests, 19 targeted browser regression tests (all five robot families), package/hash validation, formatting and production build. See [v0.6 verification](docs/robots/v0.6-verification.md).
## v0.5 ## v0.5
- Brighter shared lighting and the original four-swatch **Line color** control for every robot. - Brighter shared lighting and the original four-swatch **Line color** control for every robot.
@@ -30,6 +38,8 @@ The application opens directly into simulation, defaults to Microduck, and keeps
| Microban | Shared robot-scaled 5.5 × 5.5 m arena, interactive geometries, textured per-body GLB visuals | Walking policy + independent head rotation | | Microban | Shared robot-scaled 5.5 × 5.5 m arena, interactive geometries, textured per-body GLB visuals | Walking policy + independent head rotation |
| RP1 | Shared robot-scaled 22 × 22 m arena, interactive geometries, user-supplied textured GLB visuals on original 24-DOF physics | Completed walking policy; trained 10 s dance segment (J) and full 4.36 s wave (H) | | RP1 | Shared robot-scaled 22 × 22 m arena, interactive geometries, user-supplied textured GLB visuals on original 24-DOF physics | Completed walking policy; trained 10 s dance segment (J) and full 4.36 s wave (H) |
**Booster T1** is also available in the Robot selector: W/S walks forward/backward, A/D turns, Space resets, Escape pauses, and C restores the chase camera. It uses the latest checkpoint at import (`model_6700.pt`, run `2026-11-02_15-03-39_t1_flat`) via ONNX, with original 23-DOF physics and STL visuals. See [T1 provenance, re-import and verification](app/public/robots/t1/SOURCE.md).
Switch robots using **Robot** in the toolbar. Returning to Microduck restores the session's scene-line colour and feet / rollers choice, but initializes a fresh physical state. Failed loads keep the previous robot available and show an inline retry action. Switch robots using **Robot** in the toolbar. Returning to Microduck restores the session's scene-line colour and feet / rollers choice, but initializes a fresh physical state. Failed loads keep the previous robot available and show an inline retry action.
Microduck policies / assets originate from [pollen-robotics/microduck](https://github.com/pollen-robotics/microduck) and [pollen-robotics/microduck_rl](https://github.com/pollen-robotics/microduck_rl). Microban physics / policy assets originate from [Rhoban/microban](https://github.com/Rhoban/microban); its textured visual replacement comes from the user-supplied `microban2.zip`. Exact hashes, dirty-worktree provenance and license caveats are included in its [Package source notes](app/public/robots/microban/SOURCE.md). Microduck policies / assets originate from [pollen-robotics/microduck](https://github.com/pollen-robotics/microduck) and [pollen-robotics/microduck_rl](https://github.com/pollen-robotics/microduck_rl). Microban physics / policy assets originate from [Rhoban/microban](https://github.com/Rhoban/microban); its textured visual replacement comes from the user-supplied `microban2.zip`. Exact hashes, dirty-worktree provenance and license caveats are included in its [Package source notes](app/public/robots/microban/SOURCE.md).
@@ -60,7 +70,7 @@ The football is removed. One browser session randomly generates **5–7 lightwei
The on-screen reference comes from the same Package bindings as the keyboard, and its skill entries are also clickable buttons. Text fields / selectors retain their normal keyboard behavior. Focused buttons retain native Space / Enter behavior without also resetting the robot. Losing focus, pausing, resetting or switching clears held movement keys. The on-screen reference comes from the same Package bindings as the keyboard, and its skill entries are also clickable buttons. Text fields / selectors retain their normal keyboard behavior. Focused buttons retain native Space / Enter behavior without also resetting the robot. Losing focus, pausing, resetting or switching clears held movement keys.
All bundled robots, and future Packages omitting `presentation.world`, use the same **robot-scaled arena template**. Microduck remains at **4.5 × 4.5 m**, with 0.25 m high / 0.05 m thick collision walls. World lengths scale by `dimensions.height / 0.27`: Microban gets **5.5 × 5.5 m**, walls ≈0.306 m high; RP1 gets **22 × 22 m**, walls ≈1.222 m high. Wall thickness, visual geometry, grid spacing and fade distances scale together, preserving Microduck's scene-to-body proportions. Nominal standing height is fixed per package, not inferred from the live pose or root spawn height; missing dimensions retain the reference scale. Interiors stay flat; no relief or automatic coordinate clamping is added. Explicit `world: "flat"` remains available for generic-runtime diagnostics. All robots start with the camera behind the body. Mouse orbit detaches chase mode; **C** restores rear following and the default zoom, including while paused (pressing C again does not detach). Dragging any robot or interactive geometry applies physical forces; dragging empty space orbits the camera, consistently for all three robots. Gamepad / touch are not part of this release's acceptance scope. All bundled robots, and future Packages omitting `presentation.world`, use the same **robot-scaled arena template**. Microduck remains at **4.5 × 4.5 m**, with 0.25 m high / 0.05 m thick collision walls. World lengths scale by `dimensions.height / 0.27`: Microban gets **5.5 × 5.5 m**, walls ≈0.306 m high; RP1 gets **22 × 22 m**, walls ≈1.222 m high. Wall thickness, visual geometry, grid spacing and fade distances scale together, preserving Microduck's scene-to-body proportions. Nominal standing height is fixed per package, not inferred from the live pose or root spawn height; missing dimensions retain the reference scale. Interiors stay flat; no relief or automatic coordinate clamping is added. Explicit `world: "flat"` remains available for generic-runtime diagnostics. All robots start with the camera behind the body. Mouse orbit detaches chase mode; **C** restores rear following and the default zoom, including while paused (pressing C again does not detach). Dragging any robot or interactive geometry applies physical forces; dragging empty space orbits the camera, consistently for all robots. Gamepad / touch are not part of this release's acceptance scope.
## Microban fidelity and limits ## Microban fidelity and limits
@@ -75,6 +85,10 @@ All bundled robots, and future Packages omitting `presentation.world`, use the s
BAM battery / friction / delay modeling, Placo squat, training, hardware control, ROS, camera observations, external Packages and online sharing are **not implemented** in this release. Ghost networking, stickers and celebration / entrance sequences are deferred. Audio retains the existing `SOUND_DISABLED=true` master setting; simulation never waits for an audio-unlock gesture. BAM battery / friction / delay modeling, Placo squat, training, hardware control, ROS, camera observations, external Packages and online sharing are **not implemented** in this release. Ghost networking, stickers and celebration / entrance sequences are deferred. Audio retains the existing `SOUND_DISABLED=true` master setting; simulation never waits for an audio-unlock gesture.
## Apptronik Apollo
Select **Apptronik Apollo** in the Robot menu. **W/S** walks forward/backward, **A/D** turns, **Space** resets, **Escape** pauses, and **C** restores the chase camera. Releasing movement keys stops the command. The package uses the explicitly selected checkpoint `apollo_flat_upright_v3/2026-11-02_17-39-21_apollo_upright_v3/model_9999.pt` (32 actions, 108 observations), its calibrated upright stand pose, original STL visuals and native position actuators. No scripted motion is substituted. See [Apollo source, re-import and verification](app/public/robots/apollo/SOURCE.md).
## RP1 trained skills ## RP1 trained skills
Walking uses final `model_9000.pt` from `rp1_24dof_flat/2026-10-28_15-47-49`, float32 `[1,243]` (three term-major history frames) → `[1,24]`. Dance uses resumed `2026-10-29_11-05-14_dance_resume/model_23998.pt`; wave retains its separate final `model_7999.pt`. Their actual actor branch is **student**, float32 `[1,126]` → `[1,24]`, with joint-reference position/velocity commands. All exports include the empirical normalizer and use their own reference defaults, named joint order and action history. Walking uses final `model_9000.pt` from `rp1_24dof_flat/2026-10-28_15-47-49`, float32 `[1,243]` (three term-major history frames) → `[1,24]`. Dance uses resumed `2026-10-29_11-05-14_dance_resume/model_23998.pt`; wave retains its separate final `model_7999.pt`. Their actual actor branch is **student**, float32 `[1,126]` → `[1,24]`, with joint-reference position/velocity commands. All exports include the empirical normalizer and use their own reference defaults, named joint order and action history.
@@ -136,3 +150,17 @@ See [Package authoring](docs/robots/package-authoring.md), [Microban details](do
## License ## 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). 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.
+117
View File
@@ -0,0 +1,117 @@
import { test, expect } from "@playwright/test";
test("Apollo: native MuJoCo/ONNX, WASD, stable locomotion and reset", async ({
page,
}, info) => {
const errors = [];
page.on("pageerror", (e) => errors.push(e.message));
await page.goto("/");
await expect(
page.getByRole("button", { name: "Pause", exact: true }),
).toBeEnabled();
await page.getByLabel("Robot", { exact: true }).selectOption("apollo");
await page.waitForFunction(
() =>
window.__store.getState().robotId === "apollo" ||
window.__store.getState().runtimeError,
);
expect(
await page.evaluate(() => window.__store.getState().runtimeError),
).toBeNull();
await page.evaluate(() => window.rl.pause());
expect(await page.evaluate(() => window.rl.buildObs().length)).toBe(108);
expect(await page.evaluate(() => window.rl.lastAction.length)).toBe(32);
// Apollo defaults disable visual-geom collisions: the World floor must
// explicitly override those defaults rather than inherit a ghost floor.
const floor = await page.evaluate(() => {
const m = window.rl.model,
id = m.geom("platform_floor").id;
return [m.geom_contype[id], m.geom_conaffinity[id], m.geom_group[id]];
});
expect(floor).toEqual([1, 1, 0]);
await page
.locator("canvas")
.first()
.click({ position: { x: 700, y: 600 } });
for (const [key, expected] of [
["KeyW", [0.5, 0, 0]],
["KeyS", [-0.3, 0, 0]],
["KeyA", [0, 0, 0.4]],
["KeyD", [0, 0, -0.4]],
]) {
await page.evaluate(async () => {
await window.rl.resetSim();
window.rl.resume();
});
await page.keyboard.down(key);
const command = await page.evaluate(() => Array.from(window.rl.command));
command.forEach((v, i) => expect(v).toBeCloseTo(expected[i], 5));
await page.keyboard.up(key);
expect(await page.evaluate(() => Array.from(window.rl.command))).toEqual([
0, 0, 0,
]);
await page.evaluate(() => window.rl.pause());
}
const trials = await page.evaluate(async () => {
const r = window.rl,
reports = [];
r.setPropsParked(true);
for (const [name, command] of [
["stand", [0, 0, 0]],
["forward", [0.5, 0, 0]],
["backward", [-0.3, 0, 0]],
["left", [0, 0, 0.4]],
["right", [0, 0, -0.4]],
]) {
await r.resetSim();
r.command.set(command);
for (let i = 0; i < 250; i++) {
await r.step(1);
if (
![...r.data.qpos, ...r.data.qvel, ...r.lastAction].every(
Number.isFinite,
)
)
throw new Error("Non-finite Apollo state");
if (r.fallen) break;
}
const [w, x, y, z] = r.data.qpos.slice(3, 7);
reports.push({
name,
position: Array.from(r.data.qpos.slice(0, 3)),
yaw: Math.atan2(2 * (w * z + x * y), 1 - 2 * (y * y + z * z)),
fallen: r.fallen,
time: r.data.time,
});
}
return reports;
});
console.log("Apollo trials", JSON.stringify(trials));
await info.attach("apollo-trials.json", {
body: JSON.stringify(trials, null, 2),
contentType: "application/json",
});
for (const trial of trials) {
expect(trial.fallen, trial.name).toBe(false);
expect(trial.position[2], `${trial.name}: upright height`).toBeGreaterThan(
1.04,
);
}
expect(trials[1].position[0]).toBeGreaterThan(1);
expect(trials[2].position[0]).toBeLessThan(-0.5);
expect(trials[3].yaw).toBeGreaterThan(0.8);
expect(trials[4].yaw).toBeLessThan(-0.8);
await page.keyboard.press("Space");
await expect.poll(() => page.evaluate(() => window.rl.data.time)).toBe(0);
await page.screenshot({ path: info.outputPath("apollo.png") });
for (const id of ["microban", "rp1", "t1", "apollo"]) {
await page.getByLabel("Robot", { exact: true }).selectOption(id);
await expect
.poll(() => page.evaluate(() => window.__store.getState().robotId))
.toBe(id);
expect(
await page.evaluate(() => window.__store.getState().runtimeError),
).toBeNull();
}
expect(errors).toEqual([]);
});
+1 -1
View File
@@ -23,7 +23,7 @@ test("third robot: real WASM/ONNX, keyboard command contract, trained skill bind
await boot(page); await boot(page);
expect( expect(
await page.getByLabel("Robot", { exact: true }).locator("option").count(), await page.getByLabel("Robot", { exact: true }).locator("option").count(),
).toBe(3); ).toBe(5);
expect(await page.evaluate(() => window.rl.buildObs().length)).toBe(243); expect(await page.evaluate(() => window.rl.buildObs().length)).toBe(243);
expect(await page.evaluate(() => window.rl.lastAction.length)).toBe(24); expect(await page.evaluate(() => window.rl.lastAction.length)).toBe(24);
await page await page
+101
View File
@@ -0,0 +1,101 @@
import { test, expect } from "@playwright/test";
test("T1: native MuJoCo/ONNX, WASD, stable locomotion and reset", async ({
page,
}, info) => {
const errors = [];
page.on("pageerror", (e) => errors.push(e.message));
await page.goto("/");
await expect(
page.getByRole("button", { name: "Pause", exact: true }),
).toBeEnabled();
await page.getByLabel("Robot", { exact: true }).selectOption("t1");
await page.waitForFunction(
() =>
window.__store.getState().robotId === "t1" ||
window.__store.getState().runtimeError,
);
expect(
await page.evaluate(() => window.__store.getState().runtimeError),
).toBeNull();
await page.evaluate(() => window.rl.pause());
expect(await page.evaluate(() => window.rl.buildObs().length)).toBe(81);
expect(await page.evaluate(() => window.rl.lastAction.length)).toBe(23);
await page
.locator("canvas")
.first()
.click({ position: { x: 700, y: 600 } });
for (const [key, expected] of [
["KeyW", [0.6, 0, 0]],
["KeyS", [-0.4, 0, 0]],
["KeyA", [0, 0, 0.6]],
["KeyD", [0, 0, -0.6]],
]) {
await page.evaluate(async () => {
await window.rl.resetSim();
window.rl.resume();
});
await page.keyboard.down(key);
const command = await page.evaluate(() => Array.from(window.rl.command));
command.forEach((v, i) => expect(v).toBeCloseTo(expected[i], 5));
await page.keyboard.up(key);
expect(await page.evaluate(() => Array.from(window.rl.command))).toEqual([
0, 0, 0,
]);
await page.evaluate(() => window.rl.pause());
}
const trials = await page.evaluate(async () => {
const r = window.rl,
reports = [];
r.setPropsParked(true);
for (const [name, command] of [
["stand", [0, 0, 0]],
["forward", [0.6, 0, 0]],
["backward", [-0.4, 0, 0]],
["left", [0, 0, 0.6]],
["right", [0, 0, -0.6]],
]) {
await r.resetSim();
r.command.set(command);
for (let i = 0; i < 250; i++) {
await r.step(1);
if (
![...r.data.qpos, ...r.data.qvel, ...r.lastAction].every(
Number.isFinite,
)
)
throw new Error("Non-finite T1 state");
if (r.fallen) break;
}
const [w, x, y, z] = r.data.qpos.slice(3, 7);
reports.push({
name,
position: Array.from(r.data.qpos.slice(0, 3)),
yaw: Math.atan2(2 * (w * z + x * y), 1 - 2 * (y * y + z * z)),
fallen: r.fallen,
time: r.data.time,
});
}
return reports;
});
console.log("T1 trials", JSON.stringify(trials));
await info.attach("t1-trials.json", {
body: JSON.stringify(trials, null, 2),
contentType: "application/json",
});
for (const trial of trials) expect(trial.fallen, trial.name).toBe(false);
expect(trials[1].position[0]).toBeGreaterThan(1);
expect(trials[2].position[0]).toBeLessThan(-0.5);
expect(trials[3].yaw).toBeGreaterThan(1);
expect(trials[4].yaw).toBeLessThan(-1);
await page.keyboard.press("Space");
await expect.poll(() => page.evaluate(() => window.rl.data.time)).toBe(0);
await page.screenshot({ path: info.outputPath("t1.png") });
for (const id of ["microban", "rp1", "t1"]) {
await page.getByLabel("Robot", { exact: true }).selectOption(id);
await expect
.poll(() => page.evaluate(() => window.__store.getState().robotId))
.toBe(id);
}
expect(errors).toEqual([]);
});
+2 -2
View File
@@ -1,12 +1,12 @@
{ {
"name": "microduck-sandbox", "name": "microduck-sandbox",
"version": "0.5.0", "version": "0.6.0",
"lockfileVersion": 3, "lockfileVersion": 3,
"requires": true, "requires": true,
"packages": { "packages": {
"": { "": {
"name": "microduck-sandbox", "name": "microduck-sandbox",
"version": "0.5.0", "version": "0.6.0",
"dependencies": { "dependencies": {
"@emotion/react": "^11.14.0", "@emotion/react": "^11.14.0",
"@emotion/styled": "^11.14.1", "@emotion/styled": "^11.14.1",
+4 -2
View File
@@ -1,18 +1,20 @@
{ {
"name": "microduck-sandbox", "name": "microduck-sandbox",
"private": true, "private": true,
"version": "0.5.0", "version": "0.6.0",
"type": "module", "type": "module",
"scripts": { "scripts": {
"dev": "vite", "dev": "vite",
"test": "node --test \"test/**/*.test.js\"", "test": "node --test \"test/**/*.test.js\"",
"sync:unoq": "node scripts/sync-unoq-bundle.mjs",
"prebuild": "node scripts/sync-unoq-bundle.mjs",
"build": "vite build", "build": "vite build",
"preview": "vite preview", "preview": "vite preview",
"test:e2e": "playwright test", "test:e2e": "playwright test",
"test:regression": "playwright test microduck-regression", "test:regression": "playwright test microduck-regression",
"baseline:record": "RECORD_BASELINE=1 playwright test microduck-regression", "baseline:record": "RECORD_BASELINE=1 playwright test microduck-regression",
"validate:packages": "node scripts/validate-packages.mjs", "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": { "dependencies": {
"@emotion/react": "^11.14.0", "@emotion/react": "^11.14.0",
+54
View File
@@ -0,0 +1,54 @@
# Microduck — Arduino UNO Q 实机控制与无硬件联调框架
该目录为 **Microduck** 在 **Arduino UNO Q**(QRB2210 Debian Linux MPU + STM32U585 Zephyr/Arduino MCU)上的完整板端控制架构,兼容 **Arduino App Lab** 目录规范。
## 目录结构
```text
hardware/unoq/
├── app.yaml # Arduino App Lab 清单
├── config/
│ ├── robot_manifest.json # 与网页仿真对齐的策略/关节/观测/状态机配置(可由网站导入覆盖)
│ └── hardware_profile.json # 14 路飞特 HD-1910-C001 与 QMI8658 标定参数(在线修改持久化)
├── policies/ # ONNX 策略存放目录(由网站导出包或 HTTP 推送填充)
├── python/
│ ├── main.py # 服务入口(支持 --hal mock 与 --hal router)
│ ├── core/ # 配置、61D 观测、50Hz 状态机、ONNX 推理、部署解包
│ ├── hal/ # RouterBridgeHal (实机) & MockHal (PC 无硬件联调)
│ └── web/ # FastAPI / Dev Fallback HTTP+WebSocket 服务与离线手机控制页
├── sketch/ # STM32U585 Arduino 固件 (QMI8658 + 飞特 HD-1910-C001 + Bridge)
└── scripts/
├── install_deps.sh # 板端 Python 依赖安装
└── setup_ap.sh # 一键开启 UNO Q Wi-Fi 热点 (Microduck-AP -> 192.168.4.1)
```
## 1. 无硬件 PC 端预联调(立即运行)
即使暂时未连接 Arduino UNO Q 硬件,也可以在电脑本地启动完整服务测试:
```bash
cd hardware/unoq
python3 python/main.py --hal mock --policy-backend dummy --port 8000
```
> 若环境已安装 `onnxruntime`、`fastapi`、`uvicorn`,可直接省略 `--policy-backend dummy`(默认 `auto` 会优先使用真实 `onnxruntime` 与 `FastAPI`)。
随后:
1. 在浏览器启动 Microduck 仿真网站(`cd app && npm run dev`),点击顶部工具栏 **Hardware (UNO Q)**,输入 `http://127.0.0.1:8000` 并点击 **推送到 UNO Q**,即可测试在线导入控制文件与热重载。
2. 用浏览器打开 `http://127.0.0.1:8000`(或模拟手机视图),可测试虚拟摇杆、技能按钮、E-STOP 急停、准备站立软启动与舵机/IMU 标定页。
## 2. 上电连接 Arduino UNO Q 调试流程
1. **导出并拷入项目**:在仿真网站点击 **Hardware (UNO Q) -> 下载完整部署包 (.zip)**,解压后得到完整 `microduck_unoq/` 目录(已包含你选中的 `.onnx` 策略与配置),放入 UNO Q `/home/arduino/ArduinoApps/microduck_unoq/`(或通过 Arduino App Lab 打开)。
2. **确认 STM32 引脚与总线宏**([sketch/config.h](sketch/config.h)):
- `SERVO_SERIAL`(默认 `Serial1`)、`SERVO_BAUD`(默认 `1000000`)、`SERVO_DIR_PIN`(`-1` 为硬件自动流向/单线,使用 74HC126 等方向引脚时填对应 GPIO)。
- `IMU_WIRE`(`Wire` 或 UNO Q Qwiic 接口对应的 `Wire1`)、`IMU_I2C_ADDR`(`0x6A` 或 `0x6B`)。
3. **烧录 STM32 并启动 App Lab 应用**:
- 使用 Arduino App Lab 或 `arduino-app-cli` 编译上传 `sketch/` 并启动 `python/main.py`(默认 `--hal router`)。
- 如需开启 Wi-Fi 热点:在 UNO Q Linux 终端执行 `sudo bash scripts/setup_ap.sh`,手机连接 `Microduck-AP` 后访问 `http://192.168.4.1:8000`。
4. **首次上机标定(务必先悬空/架起机身!)**:
- 打开手机页面 **标定调试** 页签:
- 点击 **扫描舵机 ID** 确认 14 个飞特 HD-1910-C001 全部在线;
- 点击 **IMU 静止零偏校准**,观察站立放置时 `projected_gravity` 接近 `[0, 0, -1]`;若不一致调整 `imu.axis_map`;
- 逐关节小幅测试并校准 `sign`(`+1`/`-1`)与 `zero_offset_ticks`(一键设为当前物理中位);
- 确认无误后回到 **控制** 页签,拉低 **速度比例滑条(如 30%)**,点击 **准备站立** 完成 1.5s 软插值,再开始步态测试。
+6
View File
@@ -0,0 +1,6 @@
name: microduck-unoq
description: Microduck RL policy runtime, mobile Wi-Fi AP controller, and STM32 HAL for Arduino UNO Q (QMI8658 + Feetech HD-1910-C001)
version: 0.1.0
ports:
- 8000
bricks: []
@@ -0,0 +1,147 @@
{
"schema": 1,
"board": "arduino_uno_q",
"servo": {
"model": "FEETECH_HD-1910-C001",
"protocol": "hls_sts_half_duplex",
"baudrate": 1000000,
"ticks_per_rev": 4096,
"neutral_tick": 2048,
"speed_limit": 0,
"acc": 50,
"torque_limit": 1000,
"vel_source": "fd",
"vel_lpf_alpha": 0.4,
"vel_scale_rad_s_per_unit": 0.0015339807878856412
},
"imu": {
"model": "QMI8658",
"i2c_addr": "0x6A",
"axis_map": [
[
1,
0,
0
],
[
0,
1,
0
],
[
0,
0,
1
]
],
"gyro_bias_rad_s": [0.0, 0.0, 0.0],
"accel_bias_g": [0.0, 0.0, 0.0]
},
"joints": {
"left_hip_yaw": {
"id": 1,
"sign": 1,
"zero_offset_ticks": 0,
"min_rad": -0.4363323129985824,
"max_rad": 0.5235987755982988
},
"left_hip_roll": {
"id": 2,
"sign": 1,
"zero_offset_ticks": 0,
"min_rad": -0.3839724354387525,
"max_rad": 0.3839724354387525
},
"left_hip_pitch": {
"id": 3,
"sign": 1,
"zero_offset_ticks": 0,
"min_rad": -1.570796326795005,
"max_rad": 1.5707963267947882
},
"left_knee": {
"id": 4,
"sign": 1,
"zero_offset_ticks": 0,
"min_rad": -1.570796326795012,
"max_rad": 1.570796326794781
},
"left_ankle": {
"id": 5,
"sign": 1,
"zero_offset_ticks": 0,
"min_rad": -1.5707963267949019,
"max_rad": 1.5707963267948912
},
"neck_pitch": {
"id": 6,
"sign": 1,
"zero_offset_ticks": 0,
"min_rad": -1.5707963267948966,
"max_rad": 1.0471975511965976
},
"head_pitch": {
"id": 7,
"sign": 1,
"zero_offset_ticks": 0,
"min_rad": -1.5707963267948974,
"max_rad": 1.5707963267948957
},
"head_yaw": {
"id": 8,
"sign": 1,
"zero_offset_ticks": 0,
"min_rad": -2.967059728390373,
"max_rad": 2.967059728390348
},
"head_roll": {
"id": 9,
"sign": 1,
"zero_offset_ticks": 0,
"min_rad": -0.43633231299859127,
"max_rad": 0.4363323129985735
},
"right_hip_yaw": {
"id": 10,
"sign": 1,
"zero_offset_ticks": 0,
"min_rad": -0.5235987755982988,
"max_rad": 0.4363323129985824
},
"right_hip_roll": {
"id": 11,
"sign": 1,
"zero_offset_ticks": 0,
"min_rad": -0.3839724354387525,
"max_rad": 0.3839724354387525
},
"right_hip_pitch": {
"id": 12,
"sign": 1,
"zero_offset_ticks": 0,
"min_rad": -1.5707963267949268,
"max_rad": 1.5707963267948664
},
"right_knee": {
"id": 13,
"sign": 1,
"zero_offset_ticks": 0,
"min_rad": -1.570796326794932,
"max_rad": 1.570796326794861
},
"right_ankle": {
"id": 14,
"sign": 1,
"zero_offset_ticks": 0,
"min_rad": -1.5707963267949054,
"max_rad": 1.5707963267948877
}
},
"safety": {
"soft_start_s": 1.5,
"watchdog_ms": 150,
"ws_timeout_ms": 500,
"auto_stand_on_fall": false,
"cmd_vel_scale": 0.5
}
}
@@ -0,0 +1,127 @@
{
"schema": 1,
"robot_id": "microduck-legs",
"family": "microduck",
"sim": {
"timestep": 0.005,
"decimation": 4,
"ctrl_dt": 0.02,
"hz": 50
},
"obs_size": 61,
"action_size": 14,
"command_size": 13,
"joint_names": [
"left_hip_yaw",
"left_hip_roll",
"left_hip_pitch",
"left_knee",
"left_ankle",
"neck_pitch",
"head_pitch",
"head_yaw",
"head_roll",
"right_hip_yaw",
"right_hip_roll",
"right_hip_pitch",
"right_knee",
"right_ankle"
],
"default_pose": [
0.0,
-0.0872664600610733,
-0.4579240083694458,
-0.004939999897032976,
0.4529840052127838,
0.3490658402442932,
0.3490658402442932,
0.0,
0.0,
0.0,
0.0872664600610733,
0.4579240083694458,
0.004939999897032976,
-0.4529840052127838
],
"action_scale": 1.0,
"variants": {
"legs": {
"limits": [0.25, -0.2, 1.0],
"enabled": true
},
"rollers": {
"limits": [0.6, -0.5, 0.3],
"enabled": false
}
},
"legacy": {
"crouch_period": 5.0,
"crouch_end": 0.7,
"groundpick_period": 4.0,
"groundpick_end": 0.7
},
"head": {
"max_rad": 2.5,
"alpha": 0.2,
"signs": [1, -1, 1, -1]
},
"safety": {
"tilt_z": -0.5,
"upright_z": -0.85,
"fall_debounce_steps": 10,
"fall_settle_steps": 15,
"recover_upright_steps": 50,
"recover_giveup_steps": 300,
"kick_steps": 25,
"post_kick_lock_steps": 20,
"roll_max_steps": 150,
"sit_delay_s": 0.8,
"stand_delay_s": 2.0
},
"skills": {
"walk": {
"path": "policies/walk.onnx",
"kind": "perpetual",
"slot": "walk",
"action_scale": 1.0
},
"stand": {
"path": "policies/stand.onnx",
"kind": "recovery",
"slot": "stand",
"action_scale": 1.0
},
"sitstand": {
"path": "policies/sitstand.onnx",
"kind": "perpetual",
"slot": "sitstand",
"action_scale": 1.0
},
"kickL": {
"path": "policies/kickL.onnx",
"kind": "episodic",
"slot": "kickL",
"action_scale": 1.0
},
"kickR": {
"path": "policies/kickR.onnx",
"kind": "episodic",
"slot": "kickR",
"action_scale": 1.0
},
"groundpick": {
"path": "policies/groundpick.onnx",
"kind": "episodic",
"slot": "groundpick",
"action_scale": 1.0
},
"roll": {
"path": "policies/roll.onnx",
"kind": "episodic",
"slot": "roll",
"action_scale": 1.0,
"duration_s": 3.0
}
},
"custom_move": null
}
+133
View File
@@ -0,0 +1,133 @@
{
"schema": 1,
"files": [
{
"path": "app.yaml",
"executable": false
},
{
"path": "config/hardware_profile.json",
"executable": false
},
{
"path": "config/robot_manifest.json",
"executable": false
},
{
"path": "python/__init__.py",
"executable": false
},
{
"path": "python/core/__init__.py",
"executable": false
},
{
"path": "python/core/config.py",
"executable": false
},
{
"path": "python/core/controller.py",
"executable": false
},
{
"path": "python/core/deploy_service.py",
"executable": false
},
{
"path": "python/core/observations.py",
"executable": false
},
{
"path": "python/core/policy_runner.py",
"executable": false
},
{
"path": "python/core/state_machine.py",
"executable": false
},
{
"path": "python/hal/__init__.py",
"executable": false
},
{
"path": "python/hal/base.py",
"executable": false
},
{
"path": "python/hal/mock_hal.py",
"executable": false
},
{
"path": "python/hal/router_hal.py",
"executable": false
},
{
"path": "python/main.py",
"executable": false
},
{
"path": "python/requirements.txt",
"executable": false
},
{
"path": "python/web/__init__.py",
"executable": false
},
{
"path": "python/web/api.py",
"executable": false
},
{
"path": "python/web/fallback_server.py",
"executable": false
},
{
"path": "python/web/server.py",
"executable": false
},
{
"path": "python/web/static/index.html",
"executable": false
},
{
"path": "README.md",
"executable": false
},
{
"path": "scripts/install_deps.sh",
"executable": true
},
{
"path": "scripts/setup_ap.sh",
"executable": true
},
{
"path": "sketch/config.h",
"executable": false
},
{
"path": "sketch/feetech_hls.cpp",
"executable": false
},
{
"path": "sketch/feetech_hls.h",
"executable": false
},
{
"path": "sketch/qmi8658.cpp",
"executable": false
},
{
"path": "sketch/qmi8658.h",
"executable": false
},
{
"path": "sketch/sketch.ino",
"executable": false
},
{
"path": "sketch/sketch.yaml",
"executable": false
}
]
}
@@ -0,0 +1 @@
"""Microduck Arduino UNO Q Python runtime package."""
@@ -0,0 +1 @@
"""Core configuration, observation pipeline, ONNX runner, and control loop."""
@@ -0,0 +1,176 @@
from __future__ import annotations
import json
import math
from pathlib import Path
from typing import Any
import numpy as np
TWO_PI = 2.0 * math.pi
def validate_robot_manifest(manifest: dict[str, Any]) -> dict[str, Any]:
if not isinstance(manifest, dict):
raise ValueError("robot_manifest must be a JSON object")
if manifest.get("obs_size") != 61:
raise ValueError(f"Unsupported obs_size {manifest.get('obs_size')}; expected 61")
if manifest.get("action_size") != 14:
raise ValueError(f"Unsupported action_size {manifest.get('action_size')}; expected 14")
if manifest.get("command_size") != 13:
raise ValueError(f"Unsupported command_size {manifest.get('command_size')}; expected 13")
joint_names = manifest.get("joint_names")
default_pose = manifest.get("default_pose")
if not isinstance(joint_names, list) or len(joint_names) != 14:
raise ValueError("robot_manifest.joint_names must contain 14 joints")
if not isinstance(default_pose, list) or len(default_pose) != 14:
raise ValueError("robot_manifest.default_pose must contain 14 floats")
if not all(isinstance(v, (int, float)) and math.isfinite(float(v)) for v in default_pose):
raise ValueError("robot_manifest.default_pose contains non-finite value")
skills = manifest.get("skills")
if not isinstance(skills, dict):
raise ValueError("robot_manifest.skills must be an object")
for required in ("walk", "stand"):
if required not in skills or not isinstance(skills[required], dict) or not skills[required].get("path"):
raise ValueError(f"robot_manifest.skills missing required '{required}' policy")
return manifest
def load_robot_manifest(path: str | Path) -> dict[str, Any]:
with Path(path).open("r", encoding="utf-8") as f:
data = json.load(f)
return validate_robot_manifest(data)
def validate_hardware_profile(profile: dict[str, Any], joint_names: list[str] | None = None) -> dict[str, Any]:
if not isinstance(profile, dict):
raise ValueError("hardware_profile must be a JSON object")
servo = profile.get("servo")
if not isinstance(servo, dict):
raise ValueError("hardware_profile.servo missing")
ticks_per_rev = int(servo.get("ticks_per_rev", 0))
if ticks_per_rev <= 0:
raise ValueError("hardware_profile.servo.ticks_per_rev must be > 0")
imu = profile.get("imu")
if not isinstance(imu, dict):
raise ValueError("hardware_profile.imu missing")
axis_map = np.asarray(imu.get("axis_map", []), dtype=np.float64)
if axis_map.shape != (3, 3) or not np.all(np.isfinite(axis_map)):
raise ValueError("hardware_profile.imu.axis_map must be a 3x3 finite matrix")
joints = profile.get("joints")
if not isinstance(joints, dict):
raise ValueError("hardware_profile.joints missing")
expected = joint_names or list(joints.keys())
if len(expected) != 14:
raise ValueError("hardware_profile.joints must configure all 14 joints")
seen_ids: set[int] = set()
for name in expected:
if name not in joints:
raise ValueError(f"hardware_profile.joints missing '{name}'")
cfg = joints[name]
jid = int(cfg["id"])
if not (0 <= jid <= 253) or jid in seen_ids:
raise ValueError(f"Invalid or duplicate servo id {jid} for joint {name}")
seen_ids.add(jid)
if int(cfg["sign"]) not in (-1, 1):
raise ValueError(f"Joint {name} sign must be -1 or 1")
if float(cfg["min_rad"]) > float(cfg["max_rad"]):
raise ValueError(f"Joint {name} min_rad > max_rad")
return profile
def load_hardware_profile(path: str | Path, joint_names: list[str] | None = None) -> dict[str, Any]:
with Path(path).open("r", encoding="utf-8") as f:
data = json.load(f)
return validate_hardware_profile(data, joint_names=joint_names)
def save_hardware_profile(path: str | Path, profile: dict[str, Any], joint_names: list[str] | None = None) -> dict[str, Any]:
validated = validate_hardware_profile(profile, joint_names=joint_names)
target = Path(path)
target.parent.mkdir(parents=True, exist_ok=True)
tmp = target.with_suffix(".json.tmp")
with tmp.open("w", encoding="utf-8") as f:
json.dump(validated, f, indent=2, ensure_ascii=False)
f.write("\n")
tmp.replace(target)
return validated
def rad_to_ticks(q_rad: np.ndarray | list[float], joint_names: list[str], profile: dict[str, Any]) -> list[int]:
servo = profile["servo"]
ticks_per_rev = int(servo["ticks_per_rev"])
neutral = int(servo["neutral_tick"])
rad_per_tick = TWO_PI / ticks_per_rev
joints_cfg = profile["joints"]
max_tick = ticks_per_rev - 1
out: list[int] = []
for idx, name in enumerate(joint_names):
cfg = joints_cfg[name]
angle = float(q_rad[idx])
if not math.isfinite(angle):
raise ValueError(f"Non-finite angle for joint {name}")
angle = max(float(cfg["min_rad"]), min(float(cfg["max_rad"]), angle))
tick = round(neutral + int(cfg["zero_offset_ticks"]) + int(cfg["sign"]) * (angle / rad_per_tick))
tick = max(0, min(max_tick, tick))
out.append(int(tick))
return out
def ticks_to_rad(ticks: np.ndarray | list[int], joint_names: list[str], profile: dict[str, Any]) -> np.ndarray:
servo = profile["servo"]
ticks_per_rev = int(servo["ticks_per_rev"])
neutral = int(servo["neutral_tick"])
rad_per_tick = TWO_PI / ticks_per_rev
joints_cfg = profile["joints"]
out = np.zeros(len(joint_names), dtype=np.float64)
for idx, name in enumerate(joint_names):
cfg = joints_cfg[name]
zero = neutral + int(cfg["zero_offset_ticks"])
out[idx] = int(cfg["sign"]) * (float(ticks[idx]) - zero) * rad_per_tick
return out
def raw_vel_to_rad_s(raw_vel: list[int], joint_names: list[str], profile: dict[str, Any]) -> np.ndarray:
"""Decode Feetech signed-magnitude speed ticks (bit 15 = negative) to rad/s."""
servo = profile["servo"]
scale = float(servo.get("vel_scale_rad_s_per_unit", TWO_PI / int(servo["ticks_per_rev"])))
joints_cfg = profile["joints"]
out = np.zeros(len(joint_names), dtype=np.float64)
for idx, name in enumerate(joint_names):
raw = int(raw_vel[idx])
mag = raw & 0x7FFF
signed = -mag if (raw & 0x8000) else mag
out[idx] = int(joints_cfg[name]["sign"]) * signed * scale
return out
def apply_imu_calibration(
gyro_raw: np.ndarray | list[float],
grav_raw: np.ndarray | list[float],
profile: dict[str, Any],
) -> tuple[np.ndarray, np.ndarray]:
imu = profile["imu"]
axis_map = np.asarray(imu["axis_map"], dtype=np.float64)
gyro_bias = np.asarray(imu.get("gyro_bias_rad_s", [0.0, 0.0, 0.0]), dtype=np.float64)
g_in = np.asarray(gyro_raw, dtype=np.float64)
p_in = np.asarray(grav_raw, dtype=np.float64)
if g_in.shape != (3,) or p_in.shape != (3,):
raise ValueError("IMU vectors must be 3D")
if not (np.all(np.isfinite(g_in)) and np.all(np.isfinite(p_in))):
raise ValueError("IMU readings contain non-finite values")
gyro = axis_map @ (g_in - gyro_bias)
grav = axis_map @ p_in
norm = float(np.linalg.norm(grav))
if norm > 1e-6:
grav = grav / norm
return gyro, grav
@@ -0,0 +1,380 @@
from __future__ import annotations
import threading
import time
from pathlib import Path
from typing import Any
import numpy as np
from hal.base import HardwareBridge, HardwareFrame
from .config import (
apply_imu_calibration,
rad_to_ticks,
raw_vel_to_rad_s,
save_hardware_profile,
ticks_to_rad,
)
from .observations import build_observation, decode_action
from .policy_runner import PolicySet
from .state_machine import LifecycleState, MicroduckStateMachine
class MicroduckController:
"""50Hz real-time control loop coordinating HAL, 61D observations, ONNX policies, and safety."""
def __init__(
self,
base_dir: str | Path,
manifest: dict[str, Any],
profile: dict[str, Any],
hal: HardwareBridge,
policies: PolicySet,
custom_script: dict[str, Any] | None = None,
profile_path: str | Path | None = None,
) -> None:
self.base_dir = Path(base_dir)
self.profile_path = Path(profile_path) if profile_path else self.base_dir / "config" / "hardware_profile.json"
self.manifest = manifest
self.profile = profile
self.hal = hal
self.policies = policies
self.joint_names: list[str] = list(manifest["joint_names"])
self.default_pose = np.asarray(manifest["default_pose"], dtype=np.float32)
self.ctrl_dt = float(manifest.get("sim", {}).get("ctrl_dt", 0.02))
self.state_machine = MicroduckStateMachine(manifest, custom_script=custom_script)
self._lock = threading.RLock()
self._running = False
self._thread: threading.Thread | None = None
self.last_action = np.zeros(14, dtype=np.float32)
self._last_q_rad: np.ndarray | None = None
self._vel_lpf = np.zeros(14, dtype=np.float64)
self._last_remote_input_s: float = 0.0
self._last_frame: HardwareFrame | None = None
self._last_gyro = np.zeros(3, dtype=np.float64)
self._last_grav = np.array([0.0, 0.0, -1.0], dtype=np.float64)
self._last_q = self.default_pose.astype(np.float64).copy()
self._last_targets = self.default_pose.astype(np.float64).copy()
self._ctrl_hz = 0.0
self._last_error: str | None = None
self._calib_target_ticks: list[int] | None = None
self._calib_torque_on = False
def connect(self) -> None:
with self._lock:
self.hal.connect()
self.hal.configure(self.joint_names, self.profile)
self._last_frame = self.hal.sync_step(
enable_torque=False,
target_ticks=rad_to_ticks(self.default_pose, self.joint_names, self.profile),
)
self._last_gyro, self._last_grav = apply_imu_calibration(
self._last_frame.gyro_raw, self._last_frame.proj_gravity_raw, self.profile
)
self._last_q = ticks_to_rad(self._last_frame.ticks, self.joint_names, self.profile)
def start(self) -> None:
if self._running:
return
self.connect()
self._running = True
self._thread = threading.Thread(target=self._loop, name="microduck-50hz", daemon=True)
self._thread.start()
def stop(self) -> None:
self._running = False
if self._thread is not None:
self._thread.join(timeout=2.0)
self._thread = None
with self._lock:
try:
self.state_machine.emergency_stop()
self.hal.set_torque(False)
finally:
self.hal.close()
def _loop(self) -> None:
next_t = time.monotonic()
hz_t0 = next_t
ticks = 0
while self._running:
now = time.monotonic()
try:
self.step_once(dt=self.ctrl_dt, now_s=now)
self._last_error = None
except Exception as exc:
self._last_error = str(exc)
with self._lock:
self.state_machine.emergency_stop()
try:
self.hal.set_torque(False)
except Exception:
pass
ticks += 1
if now - hz_t0 >= 0.5:
self._ctrl_hz = ticks / max(now - hz_t0, 1e-6)
ticks = 0
hz_t0 = now
next_t += self.ctrl_dt
sleep_s = next_t - time.monotonic()
if sleep_s > 0:
time.sleep(sleep_s)
else:
next_t = time.monotonic()
def set_remote_input(
self,
norm_x: float,
norm_wz: float,
speed_scale: float | None = None,
now_s: float | None = None,
) -> None:
with self._lock:
scale = (
float(speed_scale)
if speed_scale is not None
else float(self.profile.get("safety", {}).get("cmd_vel_scale", 0.5))
)
self.state_machine.set_velocity_cmd(norm_x, norm_wz, speed_scale=scale)
self._last_remote_input_s = time.monotonic() if now_s is None else float(now_s)
def set_head_target(self, neck_pitch: float, head_pitch: float, head_yaw: float, head_roll: float) -> None:
with self._lock:
self.state_machine.set_head_target(neck_pitch, head_pitch, head_yaw, head_roll)
def set_auto_stand_on_fall(self, enabled: bool) -> None:
with self._lock:
self.profile.setdefault("safety", {})["auto_stand_on_fall"] = bool(enabled)
save_hardware_profile(self.profile_path, self.profile, self.joint_names)
def set_speed_scale(self, scale: float) -> None:
with self._lock:
clamped = max(0.1, min(1.0, float(scale)))
self.profile.setdefault("safety", {})["cmd_vel_scale"] = clamped
save_hardware_profile(self.profile_path, self.profile, self.joint_names)
def request_stand(self) -> None:
with self._lock:
if "walk" not in self.policies.policies:
raise RuntimeError("No control policies loaded yet — deploy a bundle from the website first")
self._calib_torque_on = False
self._calib_target_ticks = None
soft_start_s = float(self.profile.get("safety", {}).get("soft_start_s", 1.5))
self.state_machine.request_stand(self._last_q, soft_start_s=soft_start_s)
def emergency_stop(self) -> None:
with self._lock:
self._calib_torque_on = False
self._calib_target_ticks = None
self.state_machine.emergency_stop()
self.last_action.fill(0)
self.hal.set_torque(False)
def trigger_action(self, action: str) -> bool:
with self._lock:
return self.state_machine.trigger_action(action)
def reload_runtime(
self,
manifest: dict[str, Any],
policies: PolicySet,
profile: dict[str, Any] | None = None,
custom_script: dict[str, Any] | None = None,
) -> None:
with self._lock:
self.state_machine.emergency_stop()
self.hal.set_torque(False)
self.manifest = manifest
self.policies = policies
self.joint_names = list(manifest["joint_names"])
self.default_pose = np.asarray(manifest["default_pose"], dtype=np.float32)
self.ctrl_dt = float(manifest.get("sim", {}).get("ctrl_dt", 0.02))
self.state_machine.update_manifest(manifest, custom_script=custom_script)
if profile is not None:
self.profile = profile
self.hal.configure(self.joint_names, self.profile)
self.last_action.fill(0)
self._last_error = None
def update_profile(self, profile: dict[str, Any]) -> dict[str, Any]:
with self._lock:
self.profile = save_hardware_profile(self.profile_path, profile, self.joint_names)
self.hal.configure(self.joint_names, self.profile)
return self.profile
def capture_joint_zero(self, joint_name: str) -> dict[str, Any]:
with self._lock:
if joint_name not in self.joint_names:
raise ValueError(f"Unknown joint '{joint_name}'")
idx = self.joint_names.index(joint_name)
if self._last_frame is None:
raise RuntimeError("No hardware frame captured yet")
neutral = int(self.profile["servo"]["neutral_tick"])
current_tick = int(self._last_frame.ticks[idx])
self.profile["joints"][joint_name]["zero_offset_ticks"] = current_tick - neutral
return self.update_profile(self.profile)
def test_single_joint(self, joint_name: str, target_rad: float) -> list[int]:
with self._lock:
if joint_name not in self.joint_names:
raise ValueError(f"Unknown joint '{joint_name}'")
if self.state_machine.lifecycle not in (LifecycleState.IDLE, LifecycleState.CALIBRATION):
raise RuntimeError("Emergency stop first before testing individual joints")
self.state_machine.enter_calibration()
idx = self.joint_names.index(joint_name)
pose = self._last_q.copy()
pose[idx] = float(target_rad)
ticks = rad_to_ticks(pose, self.joint_names, self.profile)
self._calib_target_ticks = ticks
self._calib_torque_on = True
return ticks
def exit_calibration(self) -> None:
with self._lock:
self._calib_torque_on = False
self._calib_target_ticks = None
self.state_machine.exit_calibration()
self.hal.set_torque(False)
def calibrate_imu_zero(self, samples: int = 200) -> dict[str, Any]:
with self._lock:
bias = self.hal.calibrate_imu_raw(samples=samples)
self.profile["imu"]["gyro_bias_rad_s"] = [float(b) for b in bias]
self.update_profile(self.profile)
return self.profile["imu"]
def ping_servos(self, max_id: int = 20) -> list[int]:
with self._lock:
return self.hal.ping_servos(max_id=max_id)
def _compute_joint_vel(self, q_rad: np.ndarray, raw_speeds: list[int], dt: float) -> np.ndarray:
servo_cfg = self.profile["servo"]
if servo_cfg.get("vel_source") == "servo":
return raw_vel_to_rad_s(raw_speeds, self.joint_names, self.profile)
if self._last_q_rad is None:
self._last_q_rad = q_rad.copy()
self._vel_lpf.fill(0.0)
return self._vel_lpf.copy()
fd = (q_rad - self._last_q_rad) / max(dt, 1e-4)
self._last_q_rad = q_rad.copy()
alpha = float(servo_cfg.get("vel_lpf_alpha", 0.4))
self._vel_lpf = (1.0 - alpha) * self._vel_lpf + alpha * fd
return self._vel_lpf.copy()
def step_once(self, dt: float | None = None, now_s: float | None = None) -> dict[str, Any]:
with self._lock:
dt_s = self.ctrl_dt if dt is None else float(dt)
now = time.monotonic() if now_s is None else float(now_s)
# Enforce mobile WS heartbeat timeout while walking
ws_timeout_s = float(self.profile.get("safety", {}).get("ws_timeout_ms", 500)) / 1000.0
if self._last_remote_input_s > 0.0 and (now - self._last_remote_input_s) > ws_timeout_s:
self.state_machine.set_velocity_cmd(0.0, 0.0)
servo_cfg = self.profile["servo"]
speed_limit = int(servo_cfg.get("speed_limit", 0))
acc = int(servo_cfg.get("acc", 50))
# Determine command to send for THIS cycle from current state + previous sensor state
auto_stand = bool(self.profile.get("safety", {}).get("auto_stand_on_fall", False))
if self.state_machine.lifecycle == LifecycleState.CALIBRATION:
enable_torque = self._calib_torque_on
cmd_ticks = (
list(self._calib_target_ticks)
if self._calib_target_ticks is not None
else rad_to_ticks(self._last_q, self.joint_names, self.profile)
)
decision = self.state_machine.step(dt_s, float(self._last_grav[2]), auto_stand)
else:
decision = self.state_machine.step(dt_s, float(self._last_grav[2]), auto_stand)
if decision.zero_last_action:
self.last_action.fill(0)
if decision.lifecycle == LifecycleState.SOFT_START:
q_interp = (
1.0 - decision.soft_start_alpha
) * self.state_machine.soft_start_q0 + decision.soft_start_alpha * self.default_pose
self._last_targets = q_interp
cmd_ticks = rad_to_ticks(q_interp, self.joint_names, self.profile)
enable_torque = True
elif decision.lifecycle == LifecycleState.ACTIVE and decision.policy_key is not None:
policy = self.policies.policies.get(decision.policy_key) or self.policies.policies["walk"]
q_vel = self._vel_lpf
obs = build_observation(
self._last_gyro,
self._last_grav,
self._last_q,
q_vel,
self.last_action,
decision.command_13,
self.default_pose,
)
raw_act = policy.infer(obs)
self.last_action = raw_act
targets = decode_action(raw_act, self.default_pose, policy.action_scale)
self._last_targets = targets.astype(np.float64)
cmd_ticks = rad_to_ticks(targets, self.joint_names, self.profile)
enable_torque = True
elif decision.lifecycle == LifecycleState.ACTIVE and decision.recovery_state == "fallen":
cmd_ticks = rad_to_ticks(self._last_targets, self.joint_names, self.profile)
enable_torque = True
else:
cmd_ticks = rad_to_ticks(self._last_q, self.joint_names, self.profile)
enable_torque = False
frame = self.hal.sync_step(
enable_torque=enable_torque,
target_ticks=cmd_ticks,
speed_limit=speed_limit,
acc=acc,
)
self._last_frame = frame
self._last_gyro, self._last_grav = apply_imu_calibration(
frame.gyro_raw, frame.proj_gravity_raw, self.profile
)
self._last_q = ticks_to_rad(frame.ticks, self.joint_names, self.profile)
self._compute_joint_vel(self._last_q, frame.raw_speeds, dt_s)
return self._build_status_unlocked(decision, enable_torque)
def _build_status_unlocked(self, decision: Any | None = None, torque_enabled: bool | None = None) -> dict[str, Any]:
frame = self._last_frame
life = self.state_machine.lifecycle.value
active_policy = decision.policy_key if decision else None
if torque_enabled is None:
torque_enabled = life in (LifecycleState.SOFT_START.value, LifecycleState.ACTIVE.value) or self._calib_torque_on
return {
"hal": self.hal.name,
"policy_backend": self.policies.backend,
"lifecycle": life,
"mode_label": decision.mode_label if decision else self.state_machine._mode_label(),
"skill_mode": self.state_machine.skill_mode,
"active_policy": active_policy,
"recovery_state": self.state_machine.recovery_state,
"loco": self.state_machine.loco,
"rollers_enabled": self.state_machine.rollers_enabled,
"head_mode": self.state_machine.head_mode,
"torque_enabled": bool(torque_enabled),
"auto_stand_on_fall": bool(self.profile.get("safety", {}).get("auto_stand_on_fall", False)),
"speed_scale": float(self.profile.get("safety", {}).get("cmd_vel_scale", 0.5)),
"ctrl_hz": round(self._ctrl_hz, 1),
"voltage_v": round(frame.voltage_v, 2) if frame else 0.0,
"max_temp_c": round(frame.max_temp_c, 1) if frame else 0.0,
"imu_ok": frame.imu_ok if frame else False,
"servo_err_mask": frame.servo_err_mask if frame else 0,
"gyro": [round(float(x), 4) for x in self._last_gyro],
"projected_gravity": [round(float(x), 4) for x in self._last_grav],
"ticks": list(frame.ticks) if frame else [2048] * 14,
"joint_pos_rad": [round(float(x), 4) for x in self._last_q],
"loaded_skills": sorted(self.policies.policies.keys()),
"custom_move": self.manifest.get("custom_move"),
"error": self._last_error,
}
def get_status(self) -> dict[str, Any]:
with self._lock:
return self._build_status_unlocked()
@@ -0,0 +1,252 @@
from __future__ import annotations
import io
import json
import shutil
import stat
import tempfile
import threading
import time
import zipfile
from pathlib import Path, PurePosixPath
from typing import Any
from .config import save_hardware_profile, validate_hardware_profile, validate_robot_manifest
from .controller import MicroduckController
from .policy_runner import PolicySet
MAX_ZIP_FILES = 512
MAX_ZIP_UNCOMPRESSED_BYTES = 128 * 1024 * 1024
def _safe_rel_path(member_name: str) -> PurePosixPath:
if not member_name or "\\" in member_name:
raise ValueError(f"Unsafe zip member path: {member_name!r}")
if member_name.startswith("/") or ":" in member_name:
raise ValueError(f"Absolute or drive path forbidden in zip: {member_name!r}")
parts = PurePosixPath(member_name).parts
if not parts or any(part in ("..", ".", "") for part in parts):
raise ValueError(f"Path traversal forbidden in zip: {member_name!r}")
return PurePosixPath(*parts)
class DeployService:
"""Atomic, validated hot-reload deployment of Microduck manifest + ONNX policies."""
def __init__(
self,
base_dir: str | Path,
controller: MicroduckController,
policy_backend: str = "auto",
) -> None:
self.base_dir = Path(base_dir)
self.controller = controller
self.policy_backend = policy_backend
self._lock = threading.Lock()
self.last_deploy: dict[str, Any] | None = self._read_last_deploy()
def _read_last_deploy(self) -> dict[str, Any] | None:
meta_path = self.base_dir / "config" / ".last_deploy.json"
if not meta_path.is_file():
return None
try:
with meta_path.open("r", encoding="utf-8") as f:
return json.load(f)
except Exception:
return None
def _extract_bundle_to_stage(self, raw_bytes: bytes, stage_dir: Path) -> None:
if not raw_bytes:
raise ValueError("Empty deployment bundle")
try:
zf = zipfile.ZipFile(io.BytesIO(raw_bytes), "r")
except zipfile.BadZipFile as exc:
raise ValueError("Deployment payload is not a valid ZIP archive") from exc
with zf:
infos = zf.infolist()
if len(infos) > MAX_ZIP_FILES:
raise ValueError(f"Too many files in deployment zip ({len(infos)} > {MAX_ZIP_FILES})")
total_size = 0
entries: list[tuple[zipfile.ZipInfo, PurePosixPath]] = []
for info in infos:
if info.is_dir():
if info.filename.rstrip("/"):
_safe_rel_path(info.filename.rstrip("/"))
continue
mode = (info.external_attr >> 16) & 0o170000
if mode == stat.S_IFLNK:
raise ValueError(f"Symlinks are forbidden in deployment zip: {info.filename!r}")
total_size += max(0, int(info.file_size))
if total_size > MAX_ZIP_UNCOMPRESSED_BYTES:
raise ValueError("Deployment zip exceeds maximum uncompressed size (128 MB)")
entries.append((info, _safe_rel_path(info.filename)))
if not entries:
raise ValueError("Deployment zip contains no files")
top_dirs = {p.parts[0] for _, p in entries if len(p.parts) > 1}
has_root_manifest = any(
str(p) in ("config/robot_manifest.json", "robot_manifest.json") for _, p in entries
)
strip_prefix: str | None = None
if not has_root_manifest and len(top_dirs) == 1:
only_top = next(iter(top_dirs))
if any(
str(p) in (f"{only_top}/config/robot_manifest.json", f"{only_top}/robot_manifest.json")
for _, p in entries
):
strip_prefix = only_top
for info, rel in entries:
if strip_prefix and rel.parts[0] == strip_prefix:
if len(rel.parts) == 1:
continue
rel = PurePosixPath(*rel.parts[1:])
dest = (stage_dir / Path(*rel.parts)).resolve()
if not dest.is_relative_to(stage_dir.resolve()):
raise ValueError(f"Escaping path rejected: {info.filename!r}")
dest.parent.mkdir(parents=True, exist_ok=True)
with zf.open(info, "r") as src, dest.open("wb") as dst:
shutil.copyfileobj(src, dst)
def apply_zip_bytes(self, raw_bytes: bytes, overwrite_profile: bool = False) -> dict[str, Any]:
with self._lock, tempfile.TemporaryDirectory(prefix="microduck_deploy_") as tmp:
stage = Path(tmp) / "stage"
backup = Path(tmp) / "backup"
stage.mkdir(parents=True, exist_ok=True)
backup.mkdir(parents=True, exist_ok=True)
self._extract_bundle_to_stage(raw_bytes, stage)
manifest_src = stage / "config" / "robot_manifest.json"
if not manifest_src.is_file():
alt = stage / "robot_manifest.json"
if alt.is_file():
manifest_src = alt
else:
raise ValueError("Deployment zip missing config/robot_manifest.json")
try:
manifest = validate_robot_manifest(json.loads(manifest_src.read_text(encoding="utf-8")))
except json.JSONDecodeError as exc:
raise ValueError(f"Invalid robot_manifest.json: {exc}") from exc
# Normalize into stage/config & verify every referenced ONNX exists & is non-empty
(stage / "config").mkdir(parents=True, exist_ok=True)
(stage / "config" / "robot_manifest.json").write_text(
json.dumps(manifest, indent=2) + "\n", encoding="utf-8"
)
stage_policies: dict[str, Path] = {}
for key, spec in manifest["skills"].items():
rel = spec.get("path")
if not rel:
continue
rel_posix = _safe_rel_path(str(rel))
pol_file = (stage / Path(*rel_posix.parts)).resolve()
if not pol_file.is_relative_to(stage.resolve()) or not pol_file.is_file():
raise ValueError(f"Deployment zip missing policy '{key}' at '{rel}'")
if pol_file.stat().st_size == 0:
raise ValueError(f"Policy '{key}' at '{rel}' is empty")
stage_policies[key] = pol_file
# Optional custom move script
custom_script: dict[str, Any] | None = None
script_src = stage / "config" / "custom_move_script.json"
if not script_src.is_file():
alt_script = stage / "custom_move_script.json"
if alt_script.is_file():
script_src = alt_script
if script_src.is_file():
try:
custom_script = json.loads(script_src.read_text(encoding="utf-8"))
except json.JSONDecodeError as exc:
raise ValueError(f"Invalid custom_move_script.json: {exc}") from exc
if not isinstance(custom_script, dict) or not isinstance(custom_script.get("keyframes"), list):
raise ValueError("custom_move_script.json must contain a 'keyframes' list")
elif isinstance(manifest.get("custom_move"), dict) and manifest["custom_move"].get("slot") == "script":
raise ValueError("Manifest references custom_move script, but config/custom_move_script.json is missing")
# Optional hardware profile
target_profile_path = self.base_dir / "config" / "hardware_profile.json"
incoming_profile: dict[str, Any] | None = None
profile_src = stage / "config" / "hardware_profile.json"
if not profile_src.is_file():
alt_prof = stage / "hardware_profile.json"
if alt_prof.is_file():
profile_src = alt_prof
if (overwrite_profile or not target_profile_path.is_file()) and profile_src.is_file():
try:
incoming_profile = validate_hardware_profile(
json.loads(profile_src.read_text(encoding="utf-8")),
joint_names=manifest["joint_names"],
)
except json.JSONDecodeError as exc:
raise ValueError(f"Invalid hardware_profile.json: {exc}") from exc
# Pre-validate ONNX graph & metadata inside stage before touching live files
PolicySet.load_from_manifest(stage, manifest, backend=self.policy_backend)
# Backup existing config + policies for atomic rollback
live_config = self.base_dir / "config"
live_policies = self.base_dir / "policies"
live_config.mkdir(parents=True, exist_ok=True)
live_policies.mkdir(parents=True, exist_ok=True)
for sub in ("config", "policies"):
src_dir = self.base_dir / sub
if src_dir.is_dir():
shutil.copytree(src_dir, backup / sub, dirs_exist_ok=True)
try:
shutil.copy2(stage / "config" / "robot_manifest.json", live_config / "robot_manifest.json")
target_script = live_config / "custom_move_script.json"
if custom_script is not None:
target_script.write_text(json.dumps(custom_script, indent=2) + "\n", encoding="utf-8")
elif target_script.is_file():
target_script.unlink()
for _, src_pol in stage_policies.items():
rel_to_stage = src_pol.relative_to(stage)
dst_pol = self.base_dir / rel_to_stage
dst_pol.parent.mkdir(parents=True, exist_ok=True)
shutil.copy2(src_pol, dst_pol)
if incoming_profile is not None:
save_hardware_profile(target_profile_path, incoming_profile, joint_names=manifest["joint_names"])
live_policies_set = PolicySet.load_from_manifest(
self.base_dir, manifest, backend=self.policy_backend
)
self.controller.reload_runtime(
manifest=manifest,
policies=live_policies_set,
profile=incoming_profile,
custom_script=custom_script,
)
except Exception:
# Roll back live config + policies on reload failure
for sub in ("config", "policies"):
bak_dir = backup / sub
live_dir = self.base_dir / sub
if bak_dir.is_dir():
shutil.copytree(bak_dir, live_dir, dirs_exist_ok=True)
raise
deploy_meta = {
"ok": True,
"deployed_at_s": round(time.time(), 3),
"robot_id": manifest.get("robot_id", "microduck"),
"loaded_skills": sorted(live_policies_set.policies.keys()),
"profile_overwritten": incoming_profile is not None,
"custom_move": manifest.get("custom_move"),
}
(live_config / ".last_deploy.json").write_text(
json.dumps(deploy_meta, indent=2) + "\n", encoding="utf-8"
)
self.last_deploy = deploy_meta
return deploy_meta
@@ -0,0 +1,85 @@
from __future__ import annotations
from typing import Any
import numpy as np
def build_observation(
gyro: np.ndarray,
projected_gravity: np.ndarray,
joint_pos_rad: np.ndarray,
joint_vel_rad_s: np.ndarray,
last_action: np.ndarray,
command_13: np.ndarray,
default_pose: np.ndarray,
) -> np.ndarray:
"""Assemble the exact 61D float32 observation expected by Microduck ONNX policies.
Layout:
[0:3] base_ang_vel (gyro, rad/s)
[3:6] projected_gravity (trunk_base frame, unit vector pointing down: [0,0,-1] upright)
[6:20] joint_pos_rel (joint_pos_rad - default_pose)
[20:34] joint_vel (rad/s)
[34:48] last_action (previous raw actor output)
[48:61] command (13D: [vx, vy, wz, head(4), body(6)])
"""
obs = np.empty(61, dtype=np.float32)
obs[0:3] = np.asarray(gyro, dtype=np.float32)
obs[3:6] = np.asarray(projected_gravity, dtype=np.float32)
obs[6:20] = np.asarray(joint_pos_rad, dtype=np.float32) - np.asarray(default_pose, dtype=np.float32)
obs[20:34] = np.asarray(joint_vel_rad_s, dtype=np.float32)
obs[34:48] = np.asarray(last_action, dtype=np.float32)
obs[48:61] = np.asarray(command_13, dtype=np.float32)
if not np.all(np.isfinite(obs)):
raise ValueError("Observation vector contains non-finite values")
return obs
def decode_action(
raw_action: np.ndarray,
default_pose: np.ndarray,
action_scale: float = 1.0,
) -> np.ndarray:
act = np.asarray(raw_action, dtype=np.float32)
if act.shape != (14,) or not np.all(np.isfinite(act)):
raise ValueError("raw_action must be a finite 14D vector")
return np.asarray(default_pose, dtype=np.float32) + np.float32(action_scale) * act
def script_command_at(script: dict[str, Any], t_s: float, cmd_size: int = 13) -> np.ndarray:
"""Linearly interpolate a custom move_script.json timeline at time t_s."""
keyframes = script.get("keyframes", [])
out = np.zeros(cmd_size, dtype=np.float32)
if not keyframes:
return out
duration_s = float(script.get("durationS") or script.get("duration_s") or 0.0)
loop = script.get("loop", True) is not False
tt = float(t_s)
if loop and duration_s > 0.0:
tt = ((tt % duration_s) + duration_s) % duration_s
def _pad(raw_cmd: list[float]) -> np.ndarray:
arr = np.zeros(cmd_size, dtype=np.float32)
n = min(cmd_size, len(raw_cmd))
if n > 0:
arr[:n] = np.asarray(raw_cmd[:n], dtype=np.float32)
return arr
if tt <= float(keyframes[0]["t"]):
return _pad(keyframes[0]["cmd"])
for i in range(len(keyframes) - 1):
a = keyframes[i]
b = keyframes[i + 1]
ta = float(a["t"])
tb = float(b["t"])
if ta <= tt <= tb:
u = (tt - ta) / max(tb - ta, 1e-6)
ca = _pad(a["cmd"])
cb = _pad(b["cmd"])
return (1.0 - u) * ca + u * cb
return _pad(keyframes[-1]["cmd"])
@@ -0,0 +1,123 @@
from __future__ import annotations
from dataclasses import dataclass
from pathlib import Path
from typing import Any
import numpy as np
@dataclass
class LoadedPolicy:
key: str
action_scale: float
backend: str
_session: Any | None = None
_input_name: str = "obs"
_output_name: str = "actions"
def infer(self, obs_61: np.ndarray) -> np.ndarray:
if obs_61.shape != (61,):
raise ValueError(f"Expected 61D observation, got {obs_61.shape}")
if self.backend == "dummy":
# Deterministic zero-action dummy policy for PC dry-run without onnxruntime.
return np.zeros(14, dtype=np.float32)
inp = np.asarray(obs_61, dtype=np.float32).reshape(1, 61)
out = self._session.run([self._output_name], {self._input_name: inp})[0]
act = np.asarray(out, dtype=np.float32).reshape(14)
if not np.all(np.isfinite(act)):
raise ValueError(f"Policy '{self.key}' returned non-finite action")
return act
class PolicySet:
def __init__(self, policies: dict[str, LoadedPolicy], backend: str) -> None:
self.policies = policies
self.backend = backend
@classmethod
def resolve_backend(cls, requested: str) -> str:
if requested in ("onnx", "dummy"):
return requested
if requested != "auto":
raise ValueError(f"Unknown policy backend: {requested}")
try:
import onnxruntime # noqa: F401
return "onnx"
except ImportError as exc:
raise RuntimeError(
"onnxruntime is not installed. Run 'bash scripts/install_deps.sh' on UNO Q, "
"or pass '--policy-backend dummy' for pre-hardware UI/protocol dry-run."
) from exc
@classmethod
def load_from_manifest(
cls,
base_dir: str | Path,
manifest: dict[str, Any],
backend: str = "auto",
) -> "PolicySet":
resolved_backend = cls.resolve_backend(backend)
ort = None
if resolved_backend == "onnx":
try:
import onnxruntime as ort_mod
ort = ort_mod
except ImportError as exc:
raise RuntimeError("onnxruntime is required for backend='onnx'") from exc
root = Path(base_dir)
default_scale = float(manifest.get("action_scale", 1.0))
skills = manifest.get("skills", {})
loaded: dict[str, LoadedPolicy] = {}
for key, spec in skills.items():
rel_path = spec.get("path")
if not rel_path:
continue
scale = float(spec.get("action_scale", default_scale))
if resolved_backend == "dummy":
loaded[key] = LoadedPolicy(key=key, action_scale=scale, backend="dummy")
continue
model_path = (root / rel_path).resolve()
if not model_path.is_file():
raise FileNotFoundError(f"Missing ONNX policy for '{key}': {model_path}")
sess_opts = ort.SessionOptions()
sess_opts.intra_op_num_threads = 2
sess = ort.InferenceSession(
str(model_path),
sess_options=sess_opts,
providers=["CPUExecutionProvider"],
)
inps = sess.get_inputs()
outs = sess.get_outputs()
if len(inps) != 1 or len(outs) != 1:
raise ValueError(f"Policy '{key}' must have 1 input / 1 output")
# Smoke validation mirroring custom-policy.js
smoke_obs = np.zeros((1, 61), dtype=np.float32)
smoke_out = sess.run([outs[0].name], {inps[0].name: smoke_obs})[0]
if tuple(np.asarray(smoke_out).shape) != (1, 14):
raise ValueError(
f"Policy '{key}' output shape {np.asarray(smoke_out).shape} != (1, 14)"
)
if not np.all(np.isfinite(smoke_out)):
raise ValueError(f"Policy '{key}' smoke output is non-finite")
loaded[key] = LoadedPolicy(
key=key,
action_scale=scale,
backend="onnx",
_session=sess,
_input_name=inps[0].name,
_output_name=outs[0].name,
)
if "walk" not in loaded or "stand" not in loaded:
raise ValueError("PolicySet must load at least 'walk' and 'stand'")
return cls(policies=loaded, backend=resolved_backend)
@@ -0,0 +1,561 @@
from __future__ import annotations
import math
from dataclasses import dataclass
from enum import Enum
from typing import Any
import numpy as np
from .observations import script_command_at
class LifecycleState(str, Enum):
IDLE = "IDLE"
SOFT_START = "SOFT_START"
ACTIVE = "ACTIVE"
CALIBRATION = "CALIBRATION"
FALL_SAFE = "FALL_SAFE"
@dataclass
class StepDecision:
lifecycle: LifecycleState
skill_mode: str
policy_key: str | None
torque_enabled: bool
soft_start_alpha: float
command_13: np.ndarray
zero_last_action: bool
mode_label: str
recovery_state: str | None
loco: str
head_mode: bool
class MicroduckStateMachine:
"""Hardware-safe 50Hz state machine mirroring app/src/robots/microduck/runtime.js."""
def __init__(self, manifest: dict[str, Any], custom_script: dict[str, Any] | None = None) -> None:
self.manifest = manifest
self.custom_script = custom_script
self._load_params(manifest)
self.lifecycle: LifecycleState = LifecycleState.IDLE
self.loco: str = "legs" # "legs" | "rollers"
self.skill_mode: str = "walk" # "walk"|"sitstand"|"roll"|"kickL"|"kickR"|"crouch"|"groundpick"
self.sit_flag: int = 0
self.head_mode: bool = False
self.vel_cmd = np.zeros(3, dtype=np.float32) # [vx, 0, wz]
self.head_target = np.zeros(4, dtype=np.float32)
self.head_smooth = np.zeros(4, dtype=np.float32)
self.soft_start_q0 = np.zeros(14, dtype=np.float64)
self.soft_start_elapsed = 0.0
self.soft_start_duration = 1.5
self.sit_delay_rem: float | None = None
self.stand_delay_rem: float | None = None
self.kick_steps: int | None = None
self.post_kick_lock: int = 0
self.crouch_phase: float | None = None
self.pick_phase: float | None = None
self.roll_steps: int | None = None
self.roll_tipped: bool = False
self.script_t: float | None = None
self.fall_debounce: int = 0
self.recovery_state: str | None = None # None | "fallen" | "recovering"
self.recovery_steps: int = 0
self.recovery_upright_steps: int = 0
def _load_params(self, manifest: dict[str, Any]) -> None:
safety = manifest.get("safety", {})
head = manifest.get("head", {})
legacy = manifest.get("legacy", {})
variants = manifest.get("variants", {})
self.tilt_z = float(safety.get("tilt_z", -0.5))
self.upright_z = float(safety.get("upright_z", -0.85))
self.fall_debounce_steps = int(safety.get("fall_debounce_steps", 10))
self.fall_settle_steps = int(safety.get("fall_settle_steps", 15))
self.recover_upright_steps_req = int(safety.get("recover_upright_steps", 50))
self.recover_giveup_steps = int(safety.get("recover_giveup_steps", 300))
self.kick_total_steps = int(safety.get("kick_steps", 25))
self.post_kick_lock_total = int(safety.get("post_kick_lock_steps", 20))
self.roll_max_steps = int(safety.get("roll_max_steps", 150))
self.sit_delay_s = float(safety.get("sit_delay_s", 0.8))
self.stand_delay_s = float(safety.get("stand_delay_s", 2.0))
self.head_max = float(head.get("max_rad", 2.5))
self.head_alpha = float(head.get("alpha", 0.2))
self.head_signs = np.asarray(head.get("signs", [1, -1, 1, -1]), dtype=np.float32)
self.crouch_period_s = float(legacy.get("crouch_period", 5.0))
self.crouch_end_phase = float(legacy.get("crouch_end", 0.7))
self.groundpick_period_s = float(legacy.get("groundpick_period", 4.0))
self.groundpick_end_phase = float(legacy.get("groundpick_end", 0.7))
self.legs_limits = list(variants.get("legs", {}).get("limits", [0.25, -0.2, 1.0]))
self.rollers_limits = list(variants.get("rollers", {}).get("limits", [0.6, -0.5, 0.3]))
self.rollers_enabled = bool(variants.get("rollers", {}).get("enabled", False))
custom_move = manifest.get("custom_move")
self.custom_slot = custom_move.get("slot") if isinstance(custom_move, dict) else None
roll_skill = manifest.get("skills", {}).get("roll", {})
ctrl_dt = float(manifest.get("sim", {}).get("ctrl_dt", 0.02))
if self.custom_slot == "trick" and roll_skill.get("duration_s"):
self.roll_max_steps = max(10, round(float(roll_skill["duration_s"]) / ctrl_dt))
def update_manifest(self, manifest: dict[str, Any], custom_script: dict[str, Any] | None = None) -> None:
self.manifest = manifest
self.custom_script = custom_script
self._load_params(manifest)
if not self.rollers_enabled and self.loco == "rollers":
self.loco = "legs"
def _clear_skill_runs(self) -> None:
self.sit_delay_rem = None
self.stand_delay_rem = None
self.kick_steps = None
self.post_kick_lock = 0
self.crouch_phase = None
self.pick_phase = None
self.roll_steps = None
self.roll_tipped = False
self.script_t = None
self.fall_debounce = 0
self.recovery_state = None
self.recovery_steps = 0
self.recovery_upright_steps = 0
def emergency_stop(self) -> None:
self._clear_skill_runs()
self.skill_mode = "walk"
self.sit_flag = 0
self.head_mode = False
self.vel_cmd.fill(0)
self.head_target.fill(0)
self.head_smooth.fill(0)
self.lifecycle = LifecycleState.IDLE
def enter_calibration(self) -> None:
self.emergency_stop()
self.lifecycle = LifecycleState.CALIBRATION
def exit_calibration(self) -> None:
if self.lifecycle == LifecycleState.CALIBRATION:
self.lifecycle = LifecycleState.IDLE
def request_stand(self, current_q_rad: np.ndarray, soft_start_s: float = 1.5) -> None:
self._clear_skill_runs()
self.skill_mode = "walk"
self.sit_flag = 0
self.head_mode = False
self.vel_cmd.fill(0)
self.head_target.fill(0)
self.head_smooth.fill(0)
self.soft_start_q0 = np.asarray(current_q_rad, dtype=np.float64).copy()
self.soft_start_elapsed = 0.0
self.soft_start_duration = max(0.02, float(soft_start_s))
self.lifecycle = LifecycleState.SOFT_START
def set_velocity_cmd(self, norm_x: float, norm_wz: float, speed_scale: float = 1.0) -> None:
lim_f, lim_b, lim_a = self.rollers_limits if self.loco == "rollers" else self.legs_limits
nx = max(-1.0, min(1.0, float(norm_x)))
nwz = max(-1.0, min(1.0, float(norm_wz)))
scale = max(0.05, min(1.0, float(speed_scale)))
vx = (nx * lim_f if nx >= 0.0 else nx * (-lim_b)) * scale
wz = nwz * lim_a * scale
self.vel_cmd[0] = vx
self.vel_cmd[1] = 0.0
self.vel_cmd[2] = wz
def set_head_target(self, neck_pitch: float, head_pitch: float, head_yaw: float, head_roll: float) -> None:
raw = np.array([neck_pitch, head_pitch, head_yaw, head_roll], dtype=np.float32)
raw = np.clip(raw, -1.0, 1.0)
self.head_target = self.head_signs * raw * self.head_max
def _is_one_shot_busy(self) -> bool:
return (
(self.skill_mode == "roll" and self.roll_steps is not None)
or (self.skill_mode in ("kickL", "kickR") and self.kick_steps is not None)
or (self.skill_mode == "crouch" and self.crouch_phase is not None)
or (self.skill_mode == "groundpick" and self.pick_phase is not None)
)
def trigger_action(self, action: str) -> bool:
if self.lifecycle != LifecycleState.ACTIVE or self.recovery_state is not None:
return False
if action == "headToggle":
if self.head_mode:
self.head_mode = False
return True
if (
self.skill_mode in ("walk", "sitstand")
and self.post_kick_lock == 0
and self.stand_delay_rem is None
):
self.head_mode = True
return True
return False
if action == "locoToggle":
if not self.rollers_enabled:
return False
if self._is_one_shot_busy() or self.stand_delay_rem is not None:
return False
self._clear_skill_runs()
self.skill_mode = "walk"
self.sit_flag = 0
self.head_mode = False
self.loco = "rollers" if self.loco == "legs" else "legs"
return True
if action == "sitToggle":
if self.loco != "legs":
return self.trigger_action("crouch")
if self.custom_slot == "trick":
return self.trigger_action("roll")
if self.custom_slot == "script" and self.custom_script is not None:
return self.trigger_action("scriptToggle")
if self._is_one_shot_busy():
return False
self.head_mode = False
if self.skill_mode == "sitstand" and self.sit_flag == 1:
self.sit_flag = 0
self.sit_delay_rem = None
self.stand_delay_rem = self.stand_delay_s
return True
if self.skill_mode == "walk" and self.stand_delay_rem is None:
self.skill_mode = "sitstand"
self.sit_flag = 0
self.sit_delay_rem = self.sit_delay_s
return True
return False
if action == "scriptToggle":
if self.custom_script is None or self.loco != "legs":
return False
if self.script_t is not None:
self.script_t = None
return True
if self.skill_mode == "walk" and self.stand_delay_rem is None:
self.head_mode = False
self.script_t = 0.0
return True
return False
if action in ("kickL", "kickR"):
if self.loco != "legs" or self.skill_mode != "walk" or self.stand_delay_rem is not None:
return False
self.head_mode = False
self.sit_delay_rem = None
self.skill_mode = action
self.sit_flag = 0
self.kick_steps = 0
return True
if action == "groundPick":
if self.loco != "legs" or self.skill_mode != "walk" or self.stand_delay_rem is not None:
return False
self.head_mode = False
self.sit_delay_rem = None
self.skill_mode = "groundpick"
self.sit_flag = 0
self.pick_phase = 0.0
return True
if action == "roll":
if self.loco == "rollers":
return self.trigger_action("crouch")
if self.skill_mode != "walk" or self.stand_delay_rem is not None:
return False
self.head_mode = False
self.sit_delay_rem = None
self.skill_mode = "roll"
self.sit_flag = 0
self.roll_steps = 0
self.roll_tipped = False
return True
if action == "crouch":
if self.loco != "rollers" or self.skill_mode != "walk":
return False
self.head_mode = False
self.skill_mode = "crouch"
self.crouch_phase = 0.0
return True
return False
def _active_policy_key(self) -> str | None:
if self.recovery_state == "fallen":
return None
if self.recovery_state == "recovering":
return "stand"
if self.loco == "rollers" and self.skill_mode == "walk":
return "drive"
return self.skill_mode
def _mode_label(self) -> str:
if self.lifecycle == LifecycleState.IDLE:
return "Idle (Torque Off)"
if self.lifecycle == LifecycleState.CALIBRATION:
return "Calibration"
if self.lifecycle == LifecycleState.FALL_SAFE:
return "Fall Safe"
if self.lifecycle == LifecycleState.SOFT_START:
return "Soft Start"
if self.recovery_state:
return "Recovery"
if self.skill_mode == "roll":
return "Trick" if self.custom_slot == "trick" else "Roll"
if self.script_t is not None:
return "Script"
if self.skill_mode == "crouch":
return "Crouch"
if self.skill_mode == "groundpick":
return "Pick"
if self.skill_mode in ("kickL", "kickR"):
return "Kick"
if self.head_mode:
return "Head"
if self.skill_mode == "sitstand" and self.sit_flag == 1:
return "Sit"
return "Drive" if self.loco == "rollers" else "Run"
def _build_command_13(self) -> np.ndarray:
cmd = np.zeros(13, dtype=np.float32)
if self.skill_mode == "sitstand":
cmd[0] = float(self.sit_flag)
elif self.skill_mode == "crouch" and self.crouch_phase is not None:
a = 2.0 * math.pi * self.crouch_phase
cmd[0] = math.cos(a)
cmd[1] = math.sin(a)
elif self.skill_mode == "groundpick" and self.pick_phase is not None:
a = 2.0 * math.pi * self.pick_phase
cmd[0] = math.cos(a)
cmd[1] = math.sin(a)
elif not (
self.head_mode
or self.skill_mode == "roll"
or self.skill_mode in ("kickL", "kickR")
or self.post_kick_lock > 0
or self.recovery_state is not None
):
cmd[0:3] = self.vel_cmd
for h in range(4):
self.head_smooth[h] += self.head_alpha * (self.head_target[h] - self.head_smooth[h])
gp_zero = self.skill_mode == "groundpick" or self.recovery_state is not None
if not gp_zero:
cmd[3:7] = self.head_smooth
if (
self.script_t is not None
and self.skill_mode == "walk"
and self.recovery_state is None
and self.custom_script is not None
):
cmd = script_command_at(self.custom_script, self.script_t, 13)
return cmd
def step(
self,
dt: float,
proj_grav_z: float,
auto_stand_on_fall: bool = False,
) -> StepDecision:
zero_last_action = False
if self.lifecycle in (LifecycleState.IDLE, LifecycleState.CALIBRATION, LifecycleState.FALL_SAFE):
return StepDecision(
lifecycle=self.lifecycle,
skill_mode=self.skill_mode,
policy_key=None,
torque_enabled=False,
soft_start_alpha=0.0,
command_13=np.zeros(13, dtype=np.float32),
zero_last_action=False,
mode_label=self._mode_label(),
recovery_state=self.recovery_state,
loco=self.loco,
head_mode=self.head_mode,
)
if self.lifecycle == LifecycleState.SOFT_START:
self.soft_start_elapsed += dt
u = min(1.0, self.soft_start_elapsed / self.soft_start_duration)
alpha = 0.5 * (1.0 - math.cos(math.pi * u))
if u < 1.0 - 1e-6:
return StepDecision(
lifecycle=self.lifecycle,
skill_mode="soft_start",
policy_key=None,
torque_enabled=True,
soft_start_alpha=alpha,
command_13=np.zeros(13, dtype=np.float32),
zero_last_action=True,
mode_label=self._mode_label(),
recovery_state=None,
loco=self.loco,
head_mode=False,
)
self.lifecycle = LifecycleState.ACTIVE
self.skill_mode = "walk"
zero_last_action = True
# Active lifecycle: advance timers before fall/recovery and command build
if self.sit_delay_rem is not None:
self.sit_delay_rem -= dt
if self.sit_delay_rem <= 0.0:
self.sit_delay_rem = None
if self.skill_mode == "sitstand":
self.sit_flag = 1
if self.stand_delay_rem is not None:
self.stand_delay_rem -= dt
if self.stand_delay_rem <= 0.0:
self.stand_delay_rem = None
self.skill_mode = "walk"
zero_last_action = True
# Fall detection & recovery state machine
fallen_now = (not math.isfinite(proj_grav_z)) or (proj_grav_z > self.tilt_z)
if self.recovery_state == "fallen":
self.recovery_steps += 1
if self.recovery_steps >= self.fall_settle_steps:
self.recovery_state = "recovering"
self.recovery_steps = 0
self.recovery_upright_steps = 0
zero_last_action = True
elif self.recovery_state == "recovering":
self.recovery_steps += 1
if proj_grav_z < self.upright_z:
self.recovery_upright_steps += 1
else:
self.recovery_upright_steps = 0
if self.recovery_upright_steps >= self.recover_upright_steps_req:
self.recovery_state = None
self.recovery_steps = 0
self.recovery_upright_steps = 0
self.skill_mode = "walk"
zero_last_action = True
elif self.recovery_steps >= self.recover_giveup_steps:
self._clear_skill_runs()
self.lifecycle = LifecycleState.FALL_SAFE
return StepDecision(
lifecycle=self.lifecycle,
skill_mode="walk",
policy_key=None,
torque_enabled=False,
soft_start_alpha=0.0,
command_13=np.zeros(13, dtype=np.float32),
zero_last_action=True,
mode_label=self._mode_label(),
recovery_state=None,
loco=self.loco,
head_mode=False,
)
elif fallen_now:
recoverable = (
auto_stand_on_fall
and self.loco == "legs"
and self.skill_mode == "walk"
and self.post_kick_lock == 0
and self.stand_delay_rem is None
)
self.fall_debounce += 1
if self.fall_debounce >= self.fall_debounce_steps:
self.fall_debounce = 0
self.head_mode = False
if recoverable:
self.recovery_state = "fallen"
self.recovery_steps = 0
else:
self._clear_skill_runs()
self.lifecycle = LifecycleState.FALL_SAFE
return StepDecision(
lifecycle=self.lifecycle,
skill_mode="walk",
policy_key=None,
torque_enabled=False,
soft_start_alpha=0.0,
command_13=np.zeros(13, dtype=np.float32),
zero_last_action=True,
mode_label=self._mode_label(),
recovery_state=None,
loco=self.loco,
head_mode=False,
)
else:
self.fall_debounce = 0
policy_key = self._active_policy_key()
command_13 = self._build_command_13()
# Advance step-counted skills AFTER choosing this tick's policy/command
if self.post_kick_lock > 0 and self.skill_mode == "walk":
self.post_kick_lock -= 1
if self.skill_mode in ("kickL", "kickR") and self.kick_steps is not None:
self.kick_steps += 1
if self.kick_steps >= self.kick_total_steps:
self.kick_steps = None
self.skill_mode = "walk"
self.post_kick_lock = self.post_kick_lock_total
if self.skill_mode == "crouch" and self.crouch_phase is not None:
self.crouch_phase += dt / self.crouch_period_s
if self.crouch_phase >= self.crouch_end_phase:
self.crouch_phase = None
self.skill_mode = "walk"
if self.skill_mode == "groundpick" and self.pick_phase is not None:
self.pick_phase += dt / self.groundpick_period_s
if self.pick_phase >= self.groundpick_end_phase:
self.pick_phase = None
self.skill_mode = "walk"
if self.skill_mode == "roll" and self.roll_steps is not None:
self.roll_steps += 1
if proj_grav_z > -0.3:
self.roll_tipped = True
upright = proj_grav_z < self.upright_z
done = self.roll_tipped and upright and self.roll_steps >= 40
expired = self.roll_steps >= self.roll_max_steps
if done or expired:
self.roll_steps = None
self.roll_tipped = False
self.skill_mode = "walk"
zero_last_action = True
if not upright:
self.lifecycle = LifecycleState.FALL_SAFE
if self.script_t is not None:
self.script_t += dt
if self.custom_script is None:
self.script_t = None
else:
dur = float(self.custom_script.get("durationS") or self.custom_script.get("duration_s") or 0.0)
loop = self.custom_script.get("loop", True) is not False
if not loop and self.script_t >= dur:
self.script_t = None
return StepDecision(
lifecycle=self.lifecycle,
skill_mode=self.skill_mode,
policy_key=policy_key,
torque_enabled=self.lifecycle == LifecycleState.ACTIVE,
soft_start_alpha=1.0,
command_13=command_13,
zero_last_action=zero_last_action,
mode_label=self._mode_label(),
recovery_state=self.recovery_state,
loco=self.loco,
head_mode=self.head_mode,
)
@@ -0,0 +1 @@
"""Hardware abstraction layers for Microduck on Arduino UNO Q."""
@@ -0,0 +1,57 @@
from __future__ import annotations
from abc import ABC, abstractmethod
from dataclasses import dataclass, field
from typing import Any
@dataclass
class HardwareFrame:
gyro_raw: list[float]
proj_gravity_raw: list[float]
ticks: list[int]
raw_speeds: list[int]
voltage_v: float = 7.4
max_temp_c: float = 35.0
imu_ok: bool = True
servo_err_mask: int = 0
timestamp_s: float = 0.0
extra: dict[str, Any] = field(default_factory=dict)
class HardwareBridge(ABC):
name: str = "abstract"
@abstractmethod
def connect(self) -> None:
"""Initialize connection to the MCU / servo bus."""
@abstractmethod
def close(self) -> None:
"""Release hardware resources safely."""
@abstractmethod
def configure(self, joint_names: list[str], profile: dict[str, Any]) -> None:
"""Push servo IDs, I2C address, watchdog, and IMU bias config to HAL."""
@abstractmethod
def sync_step(
self,
enable_torque: bool,
target_ticks: list[int],
speed_limit: int = 0,
acc: int = 50,
) -> HardwareFrame:
"""Send target joint ticks and read fresh IMU + servo feedback."""
@abstractmethod
def set_torque(self, enable: bool) -> None:
"""Immediately enable or disable torque on all configured servos."""
@abstractmethod
def ping_servos(self, max_id: int = 20) -> list[int]:
"""Scan and return responding servo IDs."""
@abstractmethod
def calibrate_imu_raw(self, samples: int = 200) -> list[float]:
"""Collect stationary gyro readings (rad/s in sensor frame) and return mean bias."""
@@ -0,0 +1,82 @@
from __future__ import annotations
import math
import time
from typing import Any
from .base import HardwareBridge, HardwareFrame
class MockHal(HardwareBridge):
"""PC-runnable hardware simulator for pre-hardware verification."""
name = "mock"
def __init__(self, initial_tick: int = 2048) -> None:
self.connected = False
self.torque_enabled = False
self.servo_ids = list(range(1, 15))
self.ticks = [int(initial_tick)] * 14
self.raw_speeds = [0] * 14
self._gyro = [0.0, 0.0, 0.0]
self._gravity = [0.0, 0.0, -1.0]
self._alpha = 0.45
def connect(self) -> None:
self.connected = True
def close(self) -> None:
self.torque_enabled = False
self.connected = False
def configure(self, joint_names: list[str], profile: dict[str, Any]) -> None:
joints = profile.get("joints", {})
self.servo_ids = [int(joints[n]["id"]) for n in joint_names]
def set_mock_gravity(self, grav: list[float]) -> None:
gx, gy, gz = float(grav[0]), float(grav[1]), float(grav[2])
norm = math.sqrt(gx * gx + gy * gy + gz * gz) or 1.0
self._gravity = [gx / norm, gy / norm, gz / norm]
def set_mock_gyro(self, gyro: list[float]) -> None:
self._gyro = [float(gyro[0]), float(gyro[1]), float(gyro[2])]
def sync_step(
self,
enable_torque: bool,
target_ticks: list[int],
speed_limit: int = 0,
acc: int = 50,
) -> HardwareFrame:
self.torque_enabled = bool(enable_torque)
if self.torque_enabled:
for i in range(14):
prev = self.ticks[i]
nxt = int(round(prev + self._alpha * (int(target_ticks[i]) - prev)))
delta = nxt - prev
self.ticks[i] = max(0, min(4095, nxt))
mag = min(0x7FFF, abs(delta) * 50)
self.raw_speeds[i] = (mag | 0x8000) if delta < 0 else mag
else:
self.raw_speeds = [0] * 14
return HardwareFrame(
gyro_raw=list(self._gyro),
proj_gravity_raw=list(self._gravity),
ticks=list(self.ticks),
raw_speeds=list(self.raw_speeds),
voltage_v=7.8,
max_temp_c=34.0,
imu_ok=True,
servo_err_mask=0,
timestamp_s=time.monotonic(),
)
def set_torque(self, enable: bool) -> None:
self.torque_enabled = bool(enable)
def ping_servos(self, max_id: int = 20) -> list[int]:
return [sid for sid in self.servo_ids if 0 <= sid <= max_id]
def calibrate_imu_raw(self, samples: int = 200) -> list[float]:
return list(self._gyro)
@@ -0,0 +1,109 @@
from __future__ import annotations
import time
from typing import Any
from .base import HardwareBridge, HardwareFrame
class RouterBridgeHal(HardwareBridge):
"""Arduino UNO Q HAL communicating with STM32U585 over Arduino_RouterBridge."""
name = "router"
def __init__(self) -> None:
self._bridge = None
self._servo_ids = list(range(1, 15))
def connect(self) -> None:
try:
from arduino.app_utils import Bridge # type: ignore
except ImportError as exc:
raise RuntimeError(
"arduino.app_utils.Bridge is not available. Run on Arduino UNO Q via App Lab / "
"arduino-app-cli, or pass '--hal mock' for PC pre-hardware debugging."
) from exc
self._bridge = Bridge
def close(self) -> None:
if self._bridge is not None:
try:
self.set_torque(False)
except Exception:
pass
self._bridge = None
def _require_bridge(self) -> Any:
if self._bridge is None:
raise RuntimeError("RouterBridgeHal is not connected")
return self._bridge
def configure(self, joint_names: list[str], profile: dict[str, Any]) -> None:
bridge = self._require_bridge()
joints = profile["joints"]
servo = profile["servo"]
imu = profile["imu"]
safety = profile.get("safety", {})
self._servo_ids = [int(joints[name]["id"]) for name in joint_names]
addr_raw = imu.get("i2c_addr", "0x6A")
i2c_addr = int(addr_raw, 16) if isinstance(addr_raw, str) else int(addr_raw)
bias = [float(x) for x in imu.get("gyro_bias_rad_s", [0.0, 0.0, 0.0])]
payload: list[float] = [
float(servo.get("baudrate", 1000000)),
float(i2c_addr),
float(safety.get("watchdog_ms", 150)),
bias[0],
bias[1],
bias[2],
*[float(sid) for sid in self._servo_ids],
]
bridge.call("hw_configure", payload)
def sync_step(
self,
enable_torque: bool,
target_ticks: list[int],
speed_limit: int = 0,
acc: int = 50,
) -> HardwareFrame:
bridge = self._require_bridge()
if len(target_ticks) != 14:
raise ValueError("Expected 14 target_ticks")
cmd_frame = [
1 if enable_torque else 0,
int(speed_limit),
int(acc),
*[int(t) for t in target_ticks],
]
res = bridge.call("hw_sync_step", cmd_frame)
if not isinstance(res, (list, tuple)) or len(res) < 38:
raise RuntimeError(f"Invalid hw_sync_step response from MCU: {res!r}")
return HardwareFrame(
gyro_raw=[float(res[0]), float(res[1]), float(res[2])],
proj_gravity_raw=[float(res[3]), float(res[4]), float(res[5])],
ticks=[int(round(res[6 + i])) for i in range(14)],
raw_speeds=[int(round(res[20 + i])) for i in range(14)],
voltage_v=float(res[34]),
max_temp_c=float(res[35]),
imu_ok=bool(int(round(res[36]))),
servo_err_mask=int(round(res[37])),
timestamp_s=time.monotonic(),
)
def set_torque(self, enable: bool) -> None:
bridge = self._require_bridge()
bridge.call("hw_set_torque", 1 if enable else 0)
def ping_servos(self, max_id: int = 20) -> list[int]:
bridge = self._require_bridge()
res = bridge.call("hw_ping_servos", int(max_id))
return [int(x) for x in res] if isinstance(res, (list, tuple)) else []
def calibrate_imu_raw(self, samples: int = 200) -> list[float]:
bridge = self._require_bridge()
res = bridge.call("hw_calibrate_imu", int(samples))
if not isinstance(res, (list, tuple)) or len(res) != 3:
raise RuntimeError(f"Invalid hw_calibrate_imu response: {res!r}")
return [float(res[0]), float(res[1]), float(res[2])]
+125
View File
@@ -0,0 +1,125 @@
from __future__ import annotations
import argparse
import os
import signal
import sys
from pathlib import Path
PYTHON_ROOT = Path(__file__).resolve().parent
UNOQ_ROOT = PYTHON_ROOT.parent
if str(PYTHON_ROOT) not in sys.path:
sys.path.insert(0, str(PYTHON_ROOT))
from core.config import load_hardware_profile, load_robot_manifest # noqa: E402
from core.controller import MicroduckController # noqa: E402
from core.deploy_service import DeployService # noqa: E402
from core.policy_runner import PolicySet # noqa: E402
from hal.base import HardwareBridge # noqa: E402
from hal.mock_hal import MockHal # noqa: E402
from hal.router_hal import RouterBridgeHal # noqa: E402
from web.fallback_server import FallbackWebServer # noqa: E402
def _select_hal(hal_mode: str) -> HardwareBridge:
if hal_mode == "mock":
return MockHal()
if hal_mode == "router":
return RouterBridgeHal()
# auto: prefer RouterBridge when running on UNO Q App Lab, otherwise ask explicitly for --hal mock
try:
from arduino.app_utils import Bridge # type: ignore # noqa: F401
return RouterBridgeHal()
except ImportError as exc:
raise RuntimeError(
"arduino.app_utils is not available in this Python environment. "
"Launch inside UNO Q App Lab, or pass '--hal mock' for PC pre-hardware testing."
) from exc
def main(argv: list[str] | None = None) -> int:
parser = argparse.ArgumentParser(description="Microduck UNO Q 50Hz Controller & Deploy Server")
parser.add_argument("--base-dir", type=Path, default=UNOQ_ROOT, help="UNO Q config/policies directory")
parser.add_argument(
"--hal",
choices=("auto", "router", "mock"),
default=os.environ.get("MICRODUCK_HAL", "auto"),
help="Hardware backend: 'router' (UNO Q STM32), 'mock' (PC simulator), or 'auto'",
)
parser.add_argument(
"--policy-backend",
choices=("auto", "onnx", "dummy"),
default=os.environ.get("MICRODUCK_POLICY_BACKEND", "auto"),
help="Inference backend: 'onnx' / 'auto', or explicit 'dummy' dry-run",
)
parser.add_argument("--host", default="0.0.0.0")
parser.add_argument("--port", type=int, default=8000)
args = parser.parse_args(argv)
base_dir = Path(args.base_dir).resolve()
manifest = load_robot_manifest(base_dir / "config" / "robot_manifest.json")
profile = load_hardware_profile(base_dir / "config" / "hardware_profile.json", joint_names=manifest["joint_names"])
custom_script = None
script_path = base_dir / "config" / "custom_move_script.json"
if script_path.is_file():
import json
custom_script = json.loads(script_path.read_text(encoding="utf-8"))
hal = _select_hal(args.hal)
boot_error: str | None = None
try:
policies = PolicySet.load_from_manifest(base_dir, manifest, backend=args.policy_backend)
except FileNotFoundError as exc:
# Allow web server & /api/deploy to boot on a fresh board before first push from website
boot_error = f"Awaiting first policy deploy: {exc}"
policies = PolicySet({}, backend="awaiting_deploy")
controller = MicroduckController(
base_dir=base_dir,
manifest=manifest,
profile=profile,
hal=hal,
policies=policies,
custom_script=custom_script,
)
if boot_error:
controller._last_error = boot_error
deploy_backend = args.policy_backend
deploy_service = DeployService(base_dir=base_dir, controller=controller, policy_backend=deploy_backend)
controller.start()
def _shutdown(*_args: object) -> None:
raise SystemExit(0)
signal.signal(signal.SIGTERM, _shutdown)
srv: FallbackWebServer | None = None
try:
try:
import uvicorn # type: ignore
from web.server import create_fastapi_app
app = create_fastapi_app(controller, deploy_service)
uvicorn.run(app, host=args.host, port=args.port, log_level="info")
except ImportError:
srv = FallbackWebServer(args.host, args.port, controller, deploy_service)
print(f"[microduck-unoq] FastAPI/uvicorn not installed; serving stdlib HTTP/WS on {args.host}:{srv.port}")
srv.serve_forever()
except (KeyboardInterrupt, SystemExit):
pass
finally:
if srv is not None:
srv.server_close()
controller.stop()
return 0
if __name__ == "__main__":
raise SystemExit(main())
@@ -0,0 +1,5 @@
numpy>=1.26.0
onnxruntime>=1.18.0
fastapi>=0.115.0
uvicorn[standard]>=0.30.0
python-multipart>=0.0.9
@@ -0,0 +1 @@
"""Web HTTP/WebSocket control and deployment package for Microduck UNO Q."""
+184
View File
@@ -0,0 +1,184 @@
from __future__ import annotations
import copy
import json
from email.parser import BytesParser
from email.policy import default as email_default_policy
from pathlib import Path
from typing import Any
from core.controller import MicroduckController
from core.deploy_service import DeployService
STATIC_DIR = Path(__file__).resolve().parent / "static"
def load_index_html() -> bytes:
path = STATIC_DIR / "index.html"
if not path.is_file():
return b"<html><body><h1>Microduck controller UI missing</h1></body></html>"
return path.read_bytes()
def parse_bool_flag(raw: str | None) -> bool:
if raw is None:
return False
return raw.strip().lower() in ("1", "true", "yes", "on")
def extract_deploy_payload(
content_type: str,
body: bytes,
query_overwrite: str | None = None,
) -> tuple[bytes, bool]:
overwrite = parse_bool_flag(query_overwrite)
ctype = (content_type or "").lower()
if "multipart/form-data" in ctype:
raw_msg = f"Content-Type: {content_type}\r\nMIME-Version: 1.0\r\n\r\n".encode("latin1") + body
msg = BytesParser(policy=email_default_policy).parsebytes(raw_msg)
if not msg.is_multipart():
raise ValueError("Malformed multipart/form-data upload")
zip_bytes: bytes | None = None
for part in msg.iter_parts():
field = part.get_param("name", header="content-disposition")
payload = part.get_payload(decode=True) or b""
if field in ("bundle", "file", "zip") and payload:
zip_bytes = payload
elif field == "overwrite_profile":
overwrite = parse_bool_flag(payload.decode("utf-8", errors="ignore"))
if not zip_bytes:
raise ValueError("Multipart upload missing 'bundle' zip field")
return zip_bytes, overwrite
return body, overwrite
def build_status_payload(controller: MicroduckController, deploy_service: DeployService) -> dict[str, Any]:
st = controller.get_status()
st["last_deploy"] = copy.deepcopy(deploy_service.last_deploy) if deploy_service.last_deploy else None
return st
def handle_ws_message(controller: MicroduckController, msg: dict[str, Any]) -> dict[str, Any] | None:
kind = msg.get("type")
if kind == "input":
controller.set_remote_input(
norm_x=float(msg.get("vx", 0.0)),
norm_wz=float(msg.get("wz", 0.0)),
speed_scale=float(msg["speed_scale"]) if "speed_scale" in msg else None,
)
return None
if kind == "head":
controller.set_head_target(
float(msg.get("neck_pitch", 0.0)),
float(msg.get("head_pitch", 0.0)),
float(msg.get("head_yaw", 0.0)),
float(msg.get("head_roll", 0.0)),
)
return None
if kind == "stand":
try:
controller.request_stand()
return {"type": "ack", "event": "stand"}
except RuntimeError as exc:
return {"type": "error", "error": str(exc)}
if kind == "estop":
controller.emergency_stop()
return {"type": "ack", "event": "estop"}
if kind == "action":
action = str(msg.get("action", ""))
ok = controller.trigger_action(action)
return {"type": "ack", "event": "action", "action": action, "ok": ok}
if kind == "settings":
if "auto_stand_on_fall" in msg:
controller.set_auto_stand_on_fall(bool(msg["auto_stand_on_fall"]))
if "speed_scale" in msg:
controller.set_speed_scale(float(msg["speed_scale"]))
return {"type": "ack", "event": "settings"}
if kind == "ping":
return {"type": "pong", "t": msg.get("t")}
return None
def dispatch_api_get(
path: str,
controller: MicroduckController,
deploy_service: DeployService,
) -> tuple[int, dict[str, Any]]:
if path == "/api/status":
return 200, build_status_payload(controller, deploy_service)
if path == "/api/profile":
return 200, {"ok": True, "profile": copy.deepcopy(controller.profile)}
return 404, {"ok": False, "error": f"Unknown GET endpoint: {path}"}
def dispatch_api_post(
path: str,
body_bytes: bytes,
content_type: str,
query_params: dict[str, list[str]],
controller: MicroduckController,
deploy_service: DeployService,
) -> tuple[int, dict[str, Any]]:
if path == "/api/deploy":
try:
q_ow = query_params.get("overwrite_profile", [None])[0]
raw_zip, overwrite = extract_deploy_payload(content_type, body_bytes, q_ow)
res = dict(deploy_service.apply_zip_bytes(raw_zip, overwrite_profile=overwrite))
res["status"] = build_status_payload(controller, deploy_service)
return 200, res
except ValueError as exc:
return 400, {"ok": False, "error": str(exc)}
except Exception as exc:
return 500, {"ok": False, "error": str(exc)}
try:
payload = json.loads(body_bytes.decode("utf-8")) if body_bytes.strip() else {}
except Exception:
return 400, {"ok": False, "error": "Invalid JSON body"}
try:
if path == "/api/stand":
controller.request_stand()
return 200, build_status_payload(controller, deploy_service)
if path == "/api/estop":
controller.emergency_stop()
return 200, build_status_payload(controller, deploy_service)
if path == "/api/action":
action = str(payload.get("action", ""))
ok = controller.trigger_action(action)
return 200, {"ok": ok, "status": build_status_payload(controller, deploy_service)}
if path == "/api/settings":
if "auto_stand_on_fall" in payload:
controller.set_auto_stand_on_fall(bool(payload["auto_stand_on_fall"]))
if "speed_scale" in payload:
controller.set_speed_scale(float(payload["speed_scale"]))
return 200, build_status_payload(controller, deploy_service)
if path == "/api/profile":
profile = controller.update_profile(payload)
return 200, {"ok": True, "profile": profile}
if path == "/api/calibrate/ping":
max_id = int(payload.get("max_id", 20))
return 200, {"ok": True, "ids": controller.ping_servos(max_id=max_id)}
if path == "/api/calibrate/joint_zero":
joint = str(payload.get("joint", ""))
profile = controller.capture_joint_zero(joint)
return 200, {"ok": True, "profile": profile}
if path == "/api/calibrate/test_joint":
joint = str(payload.get("joint", ""))
rad = float(payload.get("rad", 0.0))
ticks = controller.test_single_joint(joint, rad)
return 200, {"ok": True, "ticks": ticks}
if path == "/api/calibrate/exit":
controller.exit_calibration()
return 200, build_status_payload(controller, deploy_service)
if path == "/api/calibrate/imu_zero":
samples = int(payload.get("samples", 200))
imu = controller.calibrate_imu_zero(samples=samples)
return 200, {"ok": True, "imu": imu}
except (ValueError, RuntimeError) as exc:
return 400, {"ok": False, "error": str(exc)}
except Exception as exc:
return 500, {"ok": False, "error": str(exc)}
return 404, {"ok": False, "error": f"Unknown POST endpoint: {path}"}
@@ -0,0 +1,227 @@
from __future__ import annotations
import base64
import hashlib
import json
import select
import socket
import struct
import threading
import time
from http.server import BaseHTTPRequestHandler, ThreadingHTTPServer
from typing import Any
from urllib.parse import parse_qs, urlparse
from core.controller import MicroduckController
from core.deploy_service import DeployService
from .api import (
build_status_payload,
dispatch_api_get,
dispatch_api_post,
handle_ws_message,
load_index_html,
)
WS_GUID = "258EAFA5-E914-47DA-95CA-C5AB0DC85B11"
def _ws_encode_frame(opcode: int, payload: bytes) -> bytes:
header = bytearray([0x80 | (opcode & 0x0F)])
n = len(payload)
if n < 126:
header.append(n)
elif n < 65536:
header.append(126)
header.extend(struct.pack("!H", n))
else:
header.append(127)
header.extend(struct.pack("!Q", n))
return bytes(header) + payload
def _recv_exact(sock: socket.socket, n: int) -> bytes | None:
chunks = bytearray()
while len(chunks) < n:
chunk = sock.recv(n - len(chunks))
if not chunk:
return None
chunks.extend(chunk)
return bytes(chunks)
def _ws_read_frame(sock: socket.socket) -> tuple[int, bytes] | None:
head = _recv_exact(sock, 2)
if head is None:
return None
b1, b2 = head[0], head[1]
opcode = b1 & 0x0F
masked = bool(b2 & 0x80)
length = b2 & 0x7F
if length == 126:
ext = _recv_exact(sock, 2)
if ext is None:
return None
length = struct.unpack("!H", ext)[0]
elif length == 127:
ext = _recv_exact(sock, 8)
if ext is None:
return None
length = struct.unpack("!Q", ext)[0]
if length > 1024 * 1024:
return None
mask = _recv_exact(sock, 4) if masked else b""
if masked and mask is None:
return None
payload = _recv_exact(sock, length) if length > 0 else b""
if payload is None:
return None
if masked:
payload = bytes(b ^ mask[i % 4] for i, b in enumerate(payload))
return opcode, payload
class FallbackWebServer(ThreadingHTTPServer):
"""Zero-dependency HTTP + RFC6455 WebSocket server (runs on stock Python 3.12)."""
allow_reuse_address = True
daemon_threads = True
def __init__(
self,
host: str,
port: int,
controller: MicroduckController,
deploy_service: DeployService,
) -> None:
self.controller = controller
self.deploy_service = deploy_service
super().__init__((host, port), _RequestHandler)
self.port = int(self.server_address[1])
class _RequestHandler(BaseHTTPRequestHandler):
protocol_version = "HTTP/1.1"
def log_message(self, format: str, *args: Any) -> None: # noqa: A002
return
def _send_cors_headers(self) -> None:
self.send_header("Access-Control-Allow-Origin", "*")
self.send_header("Access-Control-Allow-Methods", "GET, POST, OPTIONS")
self.send_header("Access-Control-Allow-Headers", "Content-Type")
self.send_header("Access-Control-Allow-Private-Network", "true")
def _send_json(self, status_code: int, payload: dict[str, Any]) -> None:
raw = json.dumps(payload).encode("utf-8")
self.send_response(status_code)
self._send_cors_headers()
self.send_header("Content-Type", "application/json; charset=utf-8")
self.send_header("Content-Length", str(len(raw)))
self.end_headers()
self.wfile.write(raw)
def _send_bytes(self, status_code: int, content_type: str, data: bytes) -> None:
self.send_response(status_code)
self._send_cors_headers()
self.send_header("Content-Type", content_type)
self.send_header("Content-Length", str(len(data)))
self.end_headers()
self.wfile.write(data)
def do_OPTIONS(self) -> None: # noqa: N802
self.send_response(204)
self._send_cors_headers()
self.send_header("Content-Length", "0")
self.end_headers()
def do_GET(self) -> None: # noqa: N802
parsed = urlparse(self.path)
if parsed.path == "/ws/control" and self.headers.get("Upgrade", "").lower() == "websocket":
self._handle_websocket()
return
if parsed.path in ("/", "/index.html"):
self._send_bytes(200, "text/html; charset=utf-8", load_index_html())
return
srv: FallbackWebServer = self.server # type: ignore[assignment]
code, payload = dispatch_api_get(parsed.path, srv.controller, srv.deploy_service)
self._send_json(code, payload)
def do_POST(self) -> None: # noqa: N802
parsed = urlparse(self.path)
length = int(self.headers.get("Content-Length", "0") or "0")
if length < 0 or length > 160 * 1024 * 1024:
self._send_json(413, {"ok": False, "error": "Payload too large"})
return
body = self.rfile.read(length) if length > 0 else b""
content_type = self.headers.get("Content-Type", "")
query = parse_qs(parsed.query)
srv: FallbackWebServer = self.server # type: ignore[assignment]
code, payload = dispatch_api_post(
parsed.path,
body,
content_type,
query,
srv.controller,
srv.deploy_service,
)
self._send_json(code, payload)
def _handle_websocket(self) -> None:
key = self.headers.get("Sec-WebSocket-Key")
if not key:
self.send_error(400, "Missing Sec-WebSocket-Key")
return
accept = base64.b64encode(hashlib.sha1((key.strip() + WS_GUID).encode("ascii")).digest()).decode("ascii")
self.send_response(101, "Switching Protocols")
self.send_header("Upgrade", "websocket")
self.send_header("Connection", "Upgrade")
self.send_header("Sec-WebSocket-Accept", accept)
self.end_headers()
srv: FallbackWebServer = self.server # type: ignore[assignment]
sock: socket.socket = self.connection
sock.settimeout(5.0)
write_lock = threading.Lock()
last_telemetry = 0.0
try:
while True:
now = time.monotonic()
if now - last_telemetry >= 0.1:
last_telemetry = now
msg = {"type": "telemetry", **build_status_payload(srv.controller, srv.deploy_service)}
frame = _ws_encode_frame(0x1, json.dumps(msg).encode("utf-8"))
with write_lock:
sock.sendall(frame)
readable, _, _ = select.select([sock], [], [], 0.05)
if not readable:
continue
parsed = _ws_read_frame(sock)
if parsed is None:
break
opcode, payload = parsed
if opcode == 0x8: # close
with write_lock:
sock.sendall(_ws_encode_frame(0x8, b""))
break
if opcode == 0x9: # ping -> pong
with write_lock:
sock.sendall(_ws_encode_frame(0xA, payload))
continue
if opcode == 0x1: # text
try:
data = json.loads(payload.decode("utf-8"))
except Exception:
continue
ack = handle_ws_message(srv.controller, data)
if ack is not None:
with write_lock:
sock.sendall(_ws_encode_frame(0x1, json.dumps(ack).encode("utf-8")))
except (BrokenPipeError, ConnectionResetError, TimeoutError, OSError):
pass
finally:
self.close_connection = True
@@ -0,0 +1,92 @@
from __future__ import annotations
import asyncio
import json
from typing import Any
from urllib.parse import parse_qs
from core.controller import MicroduckController
from core.deploy_service import DeployService
from .api import (
build_status_payload,
dispatch_api_get,
dispatch_api_post,
handle_ws_message,
load_index_html,
)
def create_fastapi_app(controller: MicroduckController, deploy_service: DeployService) -> Any:
"""Create FastAPI app backed by the shared web.api dispatcher."""
from fastapi import FastAPI, Request, WebSocket, WebSocketDisconnect
from fastapi.middleware.cors import CORSMiddleware
from fastapi.responses import HTMLResponse, JSONResponse
app = FastAPI(title="Microduck UNO Q Controller", version="0.1.0")
app.add_middleware(
CORSMiddleware,
allow_origins=["*"],
allow_methods=["*"],
allow_headers=["*"],
)
@app.middleware("http")
async def add_pna_header(request: Request, call_next: Any) -> Any:
resp = await call_next(request)
resp.headers["Access-Control-Allow-Private-Network"] = "true"
return resp
@app.get("/", response_class=HTMLResponse)
async def index() -> HTMLResponse:
return HTMLResponse(content=load_index_html().decode("utf-8"))
@app.get("/api/{subpath:path}")
async def api_get(subpath: str) -> JSONResponse:
code, payload = dispatch_api_get(f"/api/{subpath}", controller, deploy_service)
return JSONResponse(status_code=code, content=payload)
@app.post("/api/{subpath:path}")
async def api_post(subpath: str, request: Request) -> JSONResponse:
body = await request.body()
content_type = request.headers.get("content-type", "")
query = parse_qs(request.url.query)
code, payload = dispatch_api_post(
f"/api/{subpath}",
body,
content_type,
query,
controller,
deploy_service,
)
return JSONResponse(status_code=code, content=payload)
@app.websocket("/ws/control")
async def ws_control(websocket: WebSocket) -> None:
await websocket.accept()
stop_event = asyncio.Event()
async def telemetry_loop() -> None:
while not stop_event.is_set():
msg = {"type": "telemetry", **build_status_payload(controller, deploy_service)}
await websocket.send_text(json.dumps(msg))
await asyncio.sleep(0.1)
sender = asyncio.create_task(telemetry_loop())
try:
while True:
raw = await websocket.receive_text()
try:
data = json.loads(raw)
except Exception:
continue
ack = handle_ws_message(controller, data)
if ack is not None:
await websocket.send_text(json.dumps(ack))
except WebSocketDisconnect:
pass
finally:
stop_event.set()
sender.cancel()
return app
@@ -0,0 +1,619 @@
<!doctype html>
<html lang="en">
<head>
<meta charset="utf-8" />
<meta name="viewport" content="width=device-width,initial-scale=1,maximum-scale=1,user-scalable=no" />
<title>Microduck UNO Q Controller</title>
<style>
:root {
--bg: #0f1117;
--panel: #191d27;
--ink: #f6f4ed;
--muted: #98a2b3;
--accent: #ff7a2f;
--accent-dark: #c95212;
--danger: #e53935;
--ok: #2ecc71;
--warn: #f39c12;
--border: #2a3142;
--mono: ui-monospace, SFMono-Regular, Menlo, Monaco, Consolas, monospace;
}
* { box-sizing: border-box; -webkit-tap-highlight-color: transparent; }
body {
margin: 0;
background: var(--bg);
color: var(--ink);
font-family: var(--mono);
overflow-x: hidden;
min-height: 100vh;
}
.topbar {
display: flex;
flex-wrap: wrap;
align-items: center;
justify-content: space-between;
gap: 8px;
padding: 10px 14px;
background: var(--panel);
border-bottom: 2px solid var(--border);
}
.brand { font-weight: 700; font-size: 15px; letter-spacing: 0.04em; color: var(--accent); }
.badges { display: flex; flex-wrap: wrap; gap: 6px; align-items: center; font-size: 12px; }
.badge {
padding: 3px 8px;
border-radius: 999px;
border: 1px solid var(--border);
background: #12151d;
}
.badge.ok { border-color: var(--ok); color: var(--ok); }
.badge.warn { border-color: var(--warn); color: var(--warn); }
.badge.danger { border-color: var(--danger); color: var(--danger); }
.tabs { display: flex; gap: 6px; }
.tab-btn {
padding: 6px 12px;
background: #12151d;
color: var(--muted);
border: 1px solid var(--border);
border-radius: 6px;
font-family: inherit;
font-size: 12px;
font-weight: 700;
cursor: pointer;
}
.tab-btn.active { background: var(--accent); color: #08080c; border-color: var(--accent); }
.safety-bar {
display: grid;
grid-template-columns: 1.3fr 1.2fr 1fr 1fr;
gap: 8px;
padding: 10px 14px;
background: #141821;
border-bottom: 1px solid var(--border);
align-items: center;
}
@media (max-width: 720px) {
.safety-bar { grid-template-columns: 1fr 1fr; }
}
button.btn {
font-family: inherit;
font-weight: 700;
font-size: 13px;
border-radius: 8px;
border: 1px solid var(--border);
padding: 10px 12px;
background: var(--panel);
color: var(--ink);
cursor: pointer;
}
button.btn:active { transform: translateY(1px); }
button.btn:disabled { opacity: 0.4; cursor: not-allowed; }
button.btn-estop {
background: var(--danger);
border-color: #ff6b6b;
color: #fff;
font-size: 15px;
letter-spacing: 0.06em;
}
button.btn-stand {
background: var(--accent);
border-color: #ffa573;
color: #08080c;
}
.toggle-box, .slider-box {
display: flex;
flex-direction: column;
gap: 4px;
font-size: 11px;
color: var(--muted);
background: var(--panel);
border: 1px solid var(--border);
border-radius: 8px;
padding: 6px 10px;
}
.toggle-row { display: flex; justify-content: space-between; align-items: center; gap: 8px; color: var(--ink); font-size: 12px; }
.main { padding: 12px 14px; }
.hidden { display: none !important; }
.control-grid {
display: grid;
grid-template-columns: minmax(240px, 1fr) minmax(240px, 1fr);
gap: 14px;
align-items: stretch;
}
@media (max-width: 700px) {
.control-grid { grid-template-columns: 1fr; }
}
.card {
background: var(--panel);
border: 1px solid var(--border);
border-radius: 10px;
padding: 12px;
}
.card-title { font-size: 12px; color: var(--muted); margin: 0 0 10px; text-transform: uppercase; letter-spacing: 0.06em; }
.stick-zone {
position: relative;
height: 250px;
border-radius: 10px;
background: radial-gradient(circle at center, #202636 0%, #11151f 75%);
border: 1px dashed #374151;
touch-action: none;
user-select: none;
overflow: hidden;
display: flex;
align-items: center;
justify-content: center;
}
.stick-hint { color: var(--muted); font-size: 12px; pointer-events: none; text-align: center; padding: 12px; }
.stick-base {
position: absolute;
width: 112px;
height: 112px;
border-radius: 50%;
border: 2px solid rgba(255, 122, 47, 0.75);
background: rgba(255, 122, 47, 0.08);
transform: translate(-50%, -50%);
pointer-events: none;
}
.stick-knob {
position: absolute;
width: 44px;
height: 44px;
border-radius: 50%;
background: var(--accent);
box-shadow: 0 0 16px rgba(255, 122, 47, 0.5);
transform: translate(-50%, -50%);
pointer-events: none;
}
.vel-readout { margin-top: 8px; font-size: 12px; color: var(--muted); display: flex; justify-content: space-between; }
.skills-grid {
display: grid;
grid-template-columns: repeat(2, minmax(0, 1fr));
gap: 8px;
}
.skill-btn.active { background: var(--accent); color: #08080c; border-color: #ffa573; }
.head-panel { margin-top: 10px; display: grid; grid-template-columns: 1fr 1fr; gap: 8px; }
.calib-actions { display: flex; flex-wrap: wrap; gap: 8px; margin-bottom: 10px; }
.joint-table { width: 100%; border-collapse: collapse; font-size: 12px; }
.joint-table th, .joint-table td { border-bottom: 1px solid var(--border); padding: 6px 4px; text-align: left; }
.joint-table input[type="number"] {
width: 64px;
background: #0f1117;
color: var(--ink);
border: 1px solid var(--border);
border-radius: 4px;
padding: 3px 5px;
font-family: inherit;
font-size: 12px;
}
.mini-btn {
padding: 4px 7px;
font-size: 11px;
border-radius: 5px;
border: 1px solid var(--border);
background: #12151d;
color: var(--ink);
font-family: inherit;
cursor: pointer;
}
.mini-btn:hover { border-color: var(--accent); }
.log-line { margin-top: 8px; font-size: 11px; color: var(--muted); word-break: break-word; }
</style>
</head>
<body>
<header class="topbar">
<div class="brand">MICRODUCK // UNO Q</div>
<div class="badges">
<span id="badge-life" class="badge warn">IDLE</span>
<span id="badge-mode" class="badge">Idle</span>
<span id="badge-ws" class="badge">WS ...</span>
<span id="badge-imu" class="badge">IMU --</span>
<span id="badge-batt" class="badge">--.-V</span>
<span id="badge-temp" class="badge">--C</span>
<span id="badge-hz" class="badge">--Hz</span>
</div>
<div class="tabs">
<button id="tab-ctrl" class="tab-btn active" type="button">Control</button>
<button id="tab-cal" class="tab-btn" type="button">Calibration</button>
</div>
</header>
<section class="safety-bar">
<button id="btn-estop" class="btn btn-estop" type="button">E-STOP (TORQUE OFF)</button>
<button id="btn-stand" class="btn btn-stand" type="button">Prepare Stand (1.5s)</button>
<label class="toggle-box">
<span>Recovery Policy</span>
<span class="toggle-row">
<span>Auto Stand on Fall</span>
<input id="chk-auto-stand" type="checkbox" />
</span>
</label>
<label class="slider-box">
<span>Speed Scale: <strong id="lbl-speed">50%</strong></span>
<input id="rng-speed" type="range" min="10" max="100" step="5" value="50" />
</label>
</section>
<main class="main">
<section id="view-control" class="control-grid">
<div class="card">
<h2 class="card-title">Virtual Joystick (Hold & Drag)</h2>
<div id="stick-zone" class="stick-zone">
<div id="stick-hint" class="stick-hint">Touch or drag anywhere here to walk / turn</div>
<div id="stick-base" class="stick-base hidden"></div>
<div id="stick-knob" class="stick-knob hidden"></div>
</div>
<div class="vel-readout">
<span id="lbl-vx">vx norm: 0.00</span>
<span id="lbl-wz">wz norm: 0.00</span>
</div>
<div id="head-panel" class="head-panel hidden">
<label class="slider-box">Neck Pitch <input id="hd-0" type="range" min="-1" max="1" step="0.05" value="0" /></label>
<label class="slider-box">Head Pitch <input id="hd-1" type="range" min="-1" max="1" step="0.05" value="0" /></label>
<label class="slider-box">Head Yaw <input id="hd-2" type="range" min="-1" max="1" step="0.05" value="0" /></label>
<label class="slider-box">Head Roll <input id="hd-3" type="range" min="-1" max="1" step="0.05" value="0" /></label>
</div>
</div>
<div class="card">
<h2 class="card-title">Skills & Modes</h2>
<div class="skills-grid">
<button class="btn skill-btn" data-action="sitStand" type="button">Sit / Stand</button>
<button class="btn skill-btn" data-action="head" id="btn-head" type="button">Head Mode</button>
<button class="btn skill-btn" data-action="kickL" type="button">Kick Left</button>
<button class="btn skill-btn" data-action="kickR" type="button">Kick Right</button>
<button class="btn skill-btn" data-action="groundPick" type="button">Ground Pick</button>
<button class="btn skill-btn" data-action="roll" id="btn-roll" type="button">Roll / Trick</button>
<button class="btn skill-btn" data-action="rollers" id="btn-rollers" type="button">Feet / Rollers</button>
<button class="btn skill-btn hidden" data-action="script" id="btn-script" type="button">Custom Script</button>
</div>
<div id="ctrl-log" class="log-line">Ready. Tap "Prepare Stand" when the robot is upright on the floor.</div>
</div>
</section>
<section id="view-calib" class="card hidden">
<h2 class="card-title">Hardware Profile & Servo Calibration (IDLE Required for Joint Test)</h2>
<div class="calib-actions">
<button id="btn-ping" class="btn" type="button">Ping Servos (1-20)</button>
<button id="btn-imu-zero" class="btn" type="button">IMU Bias Zero (Keep Still)</button>
<button id="btn-exit-cal" class="btn" type="button">Exit Calibration (Torque Off)</button>
<button id="btn-save-prof" class="btn btn-stand" type="button">Save Profile to Robot</button>
</div>
<div id="calib-status" class="log-line">No ping scan yet.</div>
<div style="overflow-x:auto; margin-top:10px;">
<table class="joint-table">
<thead>
<tr>
<th>Joint</th>
<th>ID</th>
<th>Sign</th>
<th>Offset</th>
<th>Live Tick</th>
<th>Live Rad</th>
<th>Actions</th>
</tr>
</thead>
<tbody id="joint-tbody"></tbody>
</table>
</div>
</section>
</main>
<script>
(() => {
const $ = (id) => document.getElementById(id);
let ws = null;
let reconnectTimer = null;
let profile = null;
let latestStatus = null;
const stickState = { active: false, pointerId: null, originX: 0, originY: 0, nx: 0, nwz: 0 };
const MAX_RADIUS = 56;
function log(msg) {
$("ctrl-log").textContent = msg;
}
async function apiPost(path, body) {
const res = await fetch(path, {
method: "POST",
headers: { "Content-Type": "application/json" },
body: JSON.stringify(body || {}),
});
const data = await res.json();
if (!res.ok || data.ok === false) {
throw new Error(data.error || `HTTP ${res.status}`);
}
return data;
}
function setTab(tab) {
const isCtrl = tab === "ctrl";
$("view-control").classList.toggle("hidden", !isCtrl);
$("view-calib").classList.toggle("hidden", isCtrl);
$("tab-ctrl").classList.toggle("active", isCtrl);
$("tab-cal").classList.toggle("active", !isCtrl);
if (!isCtrl) loadProfile();
}
$("tab-ctrl").onclick = () => setTab("ctrl");
$("tab-cal").onclick = () => setTab("cal");
function sendWs(obj) {
if (ws && ws.readyState === WebSocket.OPEN) {
ws.send(JSON.stringify(obj));
return true;
}
return false;
}
function updateStatusUI(st) {
latestStatus = st;
const lifeEl = $("badge-life");
lifeEl.textContent = st.lifecycle || "UNKNOWN";
lifeEl.className = "badge " + (st.lifecycle === "ACTIVE" ? "ok" : st.lifecycle === "FALL_SAFE" ? "danger" : "warn");
$("badge-mode").textContent = st.mode_label || st.skill_mode || "-";
$("badge-imu").textContent = st.imu_ok ? "IMU OK" : "IMU ERR";
$("badge-imu").className = "badge " + (st.imu_ok ? "ok" : "danger");
$("badge-batt").textContent = `${Number(st.voltage_v || 0).toFixed(2)}V`;
$("badge-temp").textContent = `${Number(st.max_temp_c || 0).toFixed(0)}C`;
$("badge-hz").textContent = `${Number(st.ctrl_hz || 0).toFixed(0)}Hz`;
$("chk-auto-stand").checked = !!st.auto_stand_on_fall;
const spdPct = Math.round((st.speed_scale || 0.5) * 100);
if (document.activeElement !== $("rng-speed")) {
$("rng-speed").value = String(spdPct);
$("lbl-speed").textContent = `${spdPct}%`;
}
$("btn-head").classList.toggle("active", !!st.head_mode);
$("head-panel").classList.toggle("hidden", !st.head_mode);
$("btn-rollers").disabled = !st.rollers_enabled;
$("btn-rollers").classList.toggle("active", st.loco === "rollers");
const hasScript = !!(st.custom_move && st.custom_move.slot === "script");
$("btn-script").classList.toggle("hidden", !hasScript);
if (st.error) log(`Error: ${st.error}`);
if (st.ticks && st.joint_pos_rad && profile) {
Object.keys(profile.joints).forEach((name, idx) => {
const tCell = document.getElementById(`live-tick-${name}`);
const rCell = document.getElementById(`live-rad-${name}`);
if (tCell) tCell.textContent = String(st.ticks[idx] ?? "-");
if (rCell) rCell.textContent = Number(st.joint_pos_rad[idx] ?? 0).toFixed(3);
});
}
}
function connectWs() {
if (reconnectTimer) clearTimeout(reconnectTimer);
const proto = location.protocol === "https:" ? "wss:" : "ws:";
ws = new WebSocket(`${proto}//${location.host}/ws/control`);
ws.onopen = () => {
$("badge-ws").textContent = "WS LIVE";
$("badge-ws").className = "badge ok";
};
ws.onclose = () => {
$("badge-ws").textContent = "WS OFF";
$("badge-ws").className = "badge danger";
reconnectTimer = setTimeout(connectWs, 1000);
};
ws.onmessage = (evt) => {
try {
const msg = JSON.parse(evt.data);
if (msg.type === "telemetry") updateStatusUI(msg);
} catch (_) {}
};
}
// Safety & Skill controls
$("btn-estop").onclick = async () => {
sendWs({ type: "estop" });
try { updateStatusUI(await apiPost("/api/estop")); log("E-STOP activated (torque off)"); } catch (e) { log(e.message); }
};
$("btn-stand").onclick = async () => {
try { updateStatusUI(await apiPost("/api/stand")); log("Soft-start stand engaged..."); } catch (e) { log(e.message); }
};
$("chk-auto-stand").onchange = async (e) => {
try { updateStatusUI(await apiPost("/api/settings", { auto_stand_on_fall: e.target.checked })); } catch (err) { log(err.message); }
};
$("rng-speed").oninput = (e) => {
$("lbl-speed").textContent = `${e.target.value}%`;
};
$("rng-speed").onchange = async (e) => {
try { updateStatusUI(await apiPost("/api/settings", { speed_scale: Number(e.target.value) / 100 })); } catch (err) { log(err.message); }
};
document.querySelectorAll(".skill-btn").forEach((btn) => {
btn.onclick = async () => {
const action = btn.dataset.action;
try {
const res = await apiPost("/api/action", { action });
if (res.status) updateStatusUI(res.status);
log(res.ok ? `Triggered ${action}` : `Action '${action}' rejected in current state`);
} catch (e) { log(e.message); }
};
});
const sendHead = () => {
sendWs({
type: "head",
neck_pitch: Number($("hd-0").value),
head_pitch: Number($("hd-1").value),
head_yaw: Number($("hd-2").value),
head_roll: Number($("hd-3").value),
});
};
["hd-0", "hd-1", "hd-2", "hd-3"].forEach((id) => { $(id).oninput = sendHead; });
// Floating virtual joystick
const zone = $("stick-zone");
const base = $("stick-base");
const knob = $("stick-knob");
const hint = $("stick-hint");
function updateStickVisual(x, y) {
if (!stickState.active) {
base.classList.add("hidden");
knob.classList.add("hidden");
hint.classList.remove("hidden");
return;
}
hint.classList.add("hidden");
base.classList.remove("hidden");
knob.classList.remove("hidden");
base.style.left = `${stickState.originX}px`;
base.style.top = `${stickState.originY}px`;
knob.style.left = `${x}px`;
knob.style.top = `${y}px`;
}
function applyStickCoords(clientX, clientY) {
const rect = zone.getBoundingClientRect();
const px = clientX - rect.left;
const py = clientY - rect.top;
const dx = px - stickState.originX;
const dy = py - stickState.originY;
const dist = Math.hypot(dx, dy);
const clamped = dist > MAX_RADIUS ? MAX_RADIUS / dist : 1.0;
const cx = stickState.originX + dx * clamped;
const cy = stickState.originY + dy * clamped;
stickState.nx = -((cy - stickState.originY) / MAX_RADIUS);
stickState.nwz = -((cx - stickState.originX) / MAX_RADIUS);
$("lbl-vx").textContent = `vx norm: ${stickState.nx.toFixed(2)}`;
$("lbl-wz").textContent = `wz norm: ${stickState.nwz.toFixed(2)}`;
updateStickVisual(cx, cy);
}
function releaseStick() {
stickState.active = false;
stickState.pointerId = null;
stickState.nx = 0;
stickState.nwz = 0;
$("lbl-vx").textContent = "vx norm: 0.00";
$("lbl-wz").textContent = "wz norm: 0.00";
updateStickVisual(0, 0);
sendWs({ type: "input", vx: 0, wz: 0, speed_scale: Number($("rng-speed").value) / 100 });
}
zone.addEventListener("pointerdown", (e) => {
const rect = zone.getBoundingClientRect();
stickState.active = true;
stickState.pointerId = e.pointerId;
stickState.originX = e.clientX - rect.left;
stickState.originY = e.clientY - rect.top;
zone.setPointerCapture(e.pointerId);
applyStickCoords(e.clientX, e.clientY);
});
zone.addEventListener("pointermove", (e) => {
if (!stickState.active || e.pointerId !== stickState.pointerId) return;
applyStickCoords(e.clientX, e.clientY);
});
zone.addEventListener("pointerup", (e) => {
if (e.pointerId === stickState.pointerId) releaseStick();
});
zone.addEventListener("pointercancel", releaseStick);
setInterval(() => {
if (stickState.active) {
sendWs({
type: "input",
vx: stickState.nx,
wz: stickState.nwz,
speed_scale: Number($("rng-speed").value) / 100,
});
}
}, 50);
// Calibration helpers
function renderJointTable() {
const tbody = $("joint-tbody");
tbody.innerHTML = "";
if (!profile || !profile.joints) return;
Object.entries(profile.joints).forEach(([name, cfg]) => {
const tr = document.createElement("tr");
tr.innerHTML = `
<td>${name}</td>
<td><input type="number" min="0" max="253" value="${cfg.id}" data-j="${name}" data-f="id" /></td>
<td><button class="mini-btn" type="button" data-flip="${name}">${cfg.sign > 0 ? "+1" : "-1"}</button></td>
<td><input type="number" min="-4096" max="4096" value="${cfg.zero_offset_ticks}" data-j="${name}" data-f="zero_offset_ticks" /></td>
<td id="live-tick-${name}">-</td>
<td id="live-rad-${name}">0.000</td>
<td>
<button class="mini-btn" type="button" data-zero="${name}">Zero</button>
<button class="mini-btn" type="button" data-test="${name}" data-rad="-0.2">-0.2</button>
<button class="mini-btn" type="button" data-test="${name}" data-rad="0">0</button>
<button class="mini-btn" type="button" data-test="${name}" data-rad="0.2">+0.2</button>
</td>
`;
tbody.appendChild(tr);
});
tbody.querySelectorAll("input[data-j]").forEach((inp) => {
inp.onchange = () => {
profile.joints[inp.dataset.j][inp.dataset.f] = Number(inp.value);
};
});
tbody.querySelectorAll("button[data-flip]").forEach((btn) => {
btn.onclick = () => {
const j = profile.joints[btn.dataset.flip];
j.sign = j.sign > 0 ? -1 : 1;
btn.textContent = j.sign > 0 ? "+1" : "-1";
};
});
tbody.querySelectorAll("button[data-zero]").forEach((btn) => {
btn.onclick = async () => {
try {
const res = await apiPost("/api/calibrate/joint_zero", { joint: btn.dataset.zero });
profile = res.profile;
renderJointTable();
$("calib-status").textContent = `Captured current tick as zero for ${btn.dataset.zero}`;
} catch (e) { $("calib-status").textContent = `Error: ${e.message}`; }
};
});
tbody.querySelectorAll("button[data-test]").forEach((btn) => {
btn.onclick = async () => {
try {
await apiPost("/api/calibrate/test_joint", { joint: btn.dataset.test, rad: Number(btn.dataset.rad) });
$("calib-status").textContent = `Commanded ${btn.dataset.test} -> ${btn.dataset.rad} rad`;
} catch (e) { $("calib-status").textContent = `Error: ${e.message}`; }
};
});
}
async function loadProfile() {
const res = await fetch("/api/profile");
const data = await res.json();
profile = data.profile;
renderJointTable();
}
$("btn-ping").onclick = async () => {
try {
const res = await apiPost("/api/calibrate/ping", { max_id: 20 });
$("calib-status").textContent = `Responding IDs (${res.ids.length}): [${res.ids.join(", ")}]`;
} catch (e) { $("calib-status").textContent = `Ping error: ${e.message}`; }
};
$("btn-imu-zero").onclick = async () => {
try {
$("calib-status").textContent = "Sampling IMU bias (keep robot stationary)...";
const res = await apiPost("/api/calibrate/imu_zero", { samples: 200 });
if (profile) profile.imu = res.imu;
$("calib-status").textContent = `IMU gyro bias updated: [${res.imu.gyro_bias_rad_s.map((x) => x.toFixed(4)).join(", ")}]`;
} catch (e) { $("calib-status").textContent = `IMU zero error: ${e.message}`; }
};
$("btn-exit-cal").onclick = async () => {
try { updateStatusUI(await apiPost("/api/calibrate/exit")); $("calib-status").textContent = "Exited calibration (torque off)"; } catch (e) { $("calib-status").textContent = e.message; }
};
$("btn-save-prof").onclick = async () => {
if (!profile) return;
try {
const res = await apiPost("/api/profile", profile);
profile = res.profile;
renderJointTable();
$("calib-status").textContent = "Saved hardware_profile.json to UNO Q.";
} catch (e) { $("calib-status").textContent = `Save error: ${e.message}`; }
};
fetch("/api/status").then((r) => r.json()).then(updateStatusUI).catch(() => {});
connectWs();
})();
</script>
</body>
</html>
@@ -0,0 +1,10 @@
#!/usr/bin/env bash
set -euo pipefail
SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
REQ_FILE="${SCRIPT_DIR}/../python/requirements.txt"
echo "[microduck-unoq] Installing Python dependencies from ${REQ_FILE}..."
python3 -m pip install --upgrade pip
python3 -m pip install -r "${REQ_FILE}"
echo "[microduck-unoq] Done."
@@ -0,0 +1,44 @@
#!/usr/bin/env bash
set -euo pipefail
# Configure an offline Wi-Fi Access Point on Arduino UNO Q (Debian + NetworkManager)
# Phone connects to this hotspot and opens http://192.168.4.1:8000
CON_NAME="${AP_CON_NAME:-microduck-ap}"
SSID="${AP_SSID:-Microduck-AP}"
PASS="${AP_PASS:-microduck123}"
IFACE="${AP_IFACE:-wlan0}"
IP_CIDR="${AP_IP_CIDR:-192.168.4.1/24}"
if ! command -v nmcli >/dev/null 2>&1; then
echo "Error: nmcli (NetworkManager) not found. Configure hostapd + dnsmasq manually for ${IFACE} -> ${IP_CIDR}." >&2
exit 1
fi
echo "[microduck-ap] Configuring Wi-Fi AP '${SSID}' on ${IFACE} (${IP_CIDR})..."
if nmcli connection show "${CON_NAME}" >/dev/null 2>&1; then
nmcli connection delete "${CON_NAME}"
fi
nmcli connection add \
type wifi \
ifname "${IFACE}" \
con-name "${CON_NAME}" \
autoconnect yes \
ssid "${SSID}"
nmcli connection modify "${CON_NAME}" \
802-11-wireless.mode ap \
802-11-wireless.band bg \
ipv4.method shared \
ipv4.addresses "${IP_CIDR}" \
wifi-sec.key-mgmt wpa-psk \
wifi-sec.psk "${PASS}"
nmcli connection up "${CON_NAME}"
echo "[microduck-ap] Hotspot active!"
echo " SSID: ${SSID}"
echo " Pass: ${PASS}"
echo " Control URL: http://${IP_CIDR%/*}:8000"
+41
View File
@@ -0,0 +1,41 @@
#pragma once
#include <Arduino.h>
#include <Wire.h>
// =============================================================================
// Microduck UNO Q Hardware Pin & Bus Configuration
// Centralizes physical wiring constants so schematic changes require one edit.
// =============================================================================
// Feetech HD-1910-C001 serial bus
// On Arduino UNO Q, Serial1 maps to header D0 (RX / PB7) and D1 (TX / PB6).
#define SERVO_SERIAL Serial1
#define SERVO_BAUD 1000000UL
// Set to a GPIO pin if using a 74HC126 / RS485 direction-controlled half-duplex
// transceiver (HIGH = TX, LOW = RX). Leave -1 for resistor/diode auto-direction
// single-wire boards (TX tied cleanly through hardware buffer) or Waveshare board.
#define SERVO_DIR_PIN -1
#define SERVO_DIR_TX_LEVEL HIGH
#define SERVO_DIR_RX_LEVEL LOW
// QMI8658 6-axis IMU on I2C
// 0x6A when SA0/AD0 is LOW (default on many breakouts), 0x6B when HIGH.
#define IMU_WIRE Wire
#define IMU_I2C_ADDR 0x6A
#define IMU_I2C_CLOCK_HZ 400000UL
// IMU Mahony filter loop rate (runs non-blocking inside MCU loop())
#define IMU_LOOP_INTERVAL_US 4000UL // 250 Hz
// Safety watchdog: disable servo torque if Linux stops sending hw_sync_step
#define WATCHDOG_DEFAULT_MS 150UL
// Servo feedback strategy:
// 1 = Feetech SyncRead (0x82) followed by single-read fallback if bus drops packet
// 0 = Sequential single Read (0x02) per joint (slower, simpler fallback for noisy lines)
#define USE_FEETECH_SYNC_READ 1
// Total joints on Microduck
#define NUM_JOINTS 14
@@ -0,0 +1,349 @@
#include "feetech_hls.h"
#include <string.h>
namespace {
constexpr uint8_t kHeader = 0xFF;
constexpr uint8_t kBroadcastId = 0xFE;
constexpr uint8_t kInstPing = 0x01;
constexpr uint8_t kInstRead = 0x02;
constexpr uint8_t kInstWrite = 0x03;
constexpr uint8_t kInstSyncRead = 0x82;
constexpr uint8_t kInstSyncWrite = 0x83;
constexpr uint8_t kRegTorqueEnable = 0x28;
constexpr uint8_t kRegAcc = 0x29;
constexpr uint8_t kRegPresentPos = 0x38;
constexpr uint8_t kFeedbackReadLen = 8; // pos(2), speed(2), load(2), volt(1), temp(1)
} // namespace
void FeetechHlsBus::begin(HardwareSerial& serial, uint32_t baud, int dir_pin) {
serial_ = &serial;
dir_pin_ = dir_pin;
if (dir_pin_ >= 0) {
pinMode(dir_pin_, OUTPUT);
setTxMode(false);
}
serial_->begin(baud);
delay(5);
flushInput();
}
void FeetechHlsBus::setTxMode(bool tx) {
if (dir_pin_ >= 0) {
digitalWrite(dir_pin_, tx ? SERVO_DIR_TX_LEVEL : SERVO_DIR_RX_LEVEL);
}
}
void FeetechHlsBus::flushInput() {
if (!serial_) return;
while (serial_->available() > 0) {
(void)serial_->read();
}
}
uint8_t FeetechHlsBus::computeChecksum(const uint8_t* body_from_id, size_t body_len) {
uint32_t sum = 0;
for (size_t i = 0; i < body_len; ++i) {
sum += body_from_id[i];
}
return static_cast<uint8_t>((~sum) & 0xFFU);
}
void FeetechHlsBus::sendRawPacket(const uint8_t* pkt, size_t len) {
if (!serial_ || len == 0 || len > sizeof(last_tx_)) return;
flushInput();
memcpy(last_tx_, pkt, len);
last_tx_len_ = len;
setTxMode(true);
serial_->write(pkt, len);
serial_->flush();
delayMicroseconds(8);
setTxMode(false);
// Strip local half-duplex TX echo if TX/RX share a single physical trace without gating
if (dir_pin_ < 0) {
uint32_t t0 = micros();
size_t matched = 0;
while (matched < len && (micros() - t0) < 400) {
if (serial_->available() > 0) {
int b = serial_->peek();
if (b == last_tx_[matched]) {
(void)serial_->read();
matched++;
} else {
break;
}
}
}
}
}
void FeetechHlsBus::sendPacket(uint8_t id, uint8_t inst, const uint8_t* params, uint8_t param_len) {
uint8_t pkt[128];
uint8_t length_field = static_cast<uint8_t>(param_len + 2);
pkt[0] = kHeader;
pkt[1] = kHeader;
pkt[2] = id;
pkt[3] = length_field;
pkt[4] = inst;
for (uint8_t i = 0; i < param_len; ++i) {
pkt[5 + i] = params[i];
}
pkt[5 + param_len] = computeChecksum(&pkt[2], static_cast<size_t>(3 + param_len));
sendRawPacket(pkt, static_cast<size_t>(6 + param_len));
}
bool FeetechHlsBus::readPacketForId(
uint8_t expected_id,
uint8_t* out_err,
uint8_t* params,
uint8_t expected_len,
uint32_t timeout_us) {
if (!serial_) return false;
uint32_t t0 = micros();
uint8_t state = 0;
uint8_t id = 0, len_field = 0, err = 0;
uint8_t buf[64] = {};
uint8_t idx = 0;
uint8_t payload_len = 0;
while ((micros() - t0) < timeout_us) {
if (serial_->available() <= 0) {
continue;
}
uint8_t b = static_cast<uint8_t>(serial_->read());
switch (state) {
case 0:
if (b == kHeader) state = 1;
break;
case 1:
if (b == kHeader) {
state = 2;
} else {
state = 0;
}
break;
case 2:
if (b == kHeader) break; // extra 0xFF header alignment
id = b;
state = 3;
break;
case 3:
len_field = b;
if (len_field < 2 || len_field > 60) {
state = 0;
} else {
payload_len = static_cast<uint8_t>(len_field - 2);
state = 4;
}
break;
case 4:
err = b;
idx = 0;
state = (payload_len == 0) ? 6 : 5;
break;
case 5:
buf[idx++] = b;
if (idx >= payload_len) state = 6;
break;
case 6: {
uint8_t check_buf[64];
check_buf[0] = id;
check_buf[1] = len_field;
check_buf[2] = err;
for (uint8_t i = 0; i < payload_len; ++i) check_buf[3 + i] = buf[i];
uint8_t chk = computeChecksum(check_buf, static_cast<size_t>(3 + payload_len));
if (chk == b && id == expected_id && payload_len >= expected_len) {
if (out_err) *out_err = err;
if (params && expected_len > 0) memcpy(params, buf, expected_len);
return true;
}
state = 0;
break;
}
default:
state = 0;
break;
}
}
return false;
}
bool FeetechHlsBus::ping(uint8_t id, uint32_t timeout_us) {
sendPacket(id, kInstPing, nullptr, 0);
uint8_t err = 0;
return readPacketForId(id, &err, nullptr, 0, timeout_us);
}
int FeetechHlsBus::pingRange(uint8_t max_id, int32_t* out_ids, int max_out) {
int found = 0;
for (uint8_t id = 1; id <= max_id && found < max_out; ++id) {
if (ping(id, 2000)) {
out_ids[found++] = static_cast<int32_t>(id);
}
}
return found;
}
bool FeetechHlsBus::setTorqueBroadcast(bool enable) {
uint8_t params[2] = {kRegTorqueEnable, static_cast<uint8_t>(enable ? 1 : 0)};
sendPacket(kBroadcastId, kInstWrite, params, 2);
delayMicroseconds(120);
return true;
}
bool FeetechHlsBus::setTorqueAll(const uint8_t* ids, int count, bool enable) {
setTorqueBroadcast(enable);
// Also sync-write individual entries cleanly in case broadcast is masked in servo EEPROM
uint8_t pkt[64];
if (count <= 0 || count > NUM_JOINTS) return false;
uint8_t param_len = static_cast<uint8_t>(2 + count * 2);
pkt[0] = kHeader;
pkt[1] = kHeader;
pkt[2] = kBroadcastId;
pkt[3] = static_cast<uint8_t>(param_len + 2);
pkt[4] = kInstSyncWrite;
pkt[5] = kRegTorqueEnable;
pkt[6] = 1;
uint8_t val = enable ? 1 : 0;
for (int i = 0; i < count; ++i) {
pkt[7 + i * 2] = ids[i];
pkt[8 + i * 2] = val;
}
pkt[5 + param_len] = computeChecksum(&pkt[2], static_cast<size_t>(3 + param_len));
sendRawPacket(pkt, static_cast<size_t>(6 + param_len));
return true;
}
bool FeetechHlsBus::syncWriteTargets(
const uint8_t* ids,
int count,
const int32_t* target_ticks,
uint8_t acc,
uint16_t speed_limit) {
if (!serial_ || count <= 0 || count > NUM_JOINTS) return false;
// 7 bytes starting at 0x29: [acc, pos_L, pos_H, time_L=0, time_H=0, spd_L, spd_H]
constexpr uint8_t kPerServo = 7;
uint8_t pkt[128];
uint8_t param_len = static_cast<uint8_t>(2 + count * (1 + kPerServo));
pkt[0] = kHeader;
pkt[1] = kHeader;
pkt[2] = kBroadcastId;
pkt[3] = static_cast<uint8_t>(param_len + 2);
pkt[4] = kInstSyncWrite;
pkt[5] = kRegAcc;
pkt[6] = kPerServo;
for (int i = 0; i < count; ++i) {
int32_t ticks = target_ticks[i];
if (ticks < 0) ticks = 0;
if (ticks > 4095) ticks = 4095;
uint16_t pos = static_cast<uint16_t>(ticks);
size_t off = 7 + static_cast<size_t>(i) * (1 + kPerServo);
pkt[off + 0] = ids[i];
pkt[off + 1] = acc;
pkt[off + 2] = static_cast<uint8_t>(pos & 0xFFU);
pkt[off + 3] = static_cast<uint8_t>((pos >> 8) & 0xFFU);
pkt[off + 4] = 0x00; // moving time low
pkt[off + 5] = 0x00; // moving time high
pkt[off + 6] = static_cast<uint8_t>(speed_limit & 0xFFU);
pkt[off + 7] = static_cast<uint8_t>((speed_limit >> 8) & 0xFFU);
}
pkt[5 + param_len] = computeChecksum(&pkt[2], static_cast<size_t>(3 + param_len));
sendRawPacket(pkt, static_cast<size_t>(6 + param_len));
return true;
}
bool FeetechHlsBus::readSingleJoint(
uint8_t id,
int32_t* out_tick,
int32_t* out_speed,
float* out_voltage_v,
float* out_temp_c) {
uint8_t params[2] = {kRegPresentPos, kFeedbackReadLen};
sendPacket(id, kInstRead, params, 2);
uint8_t err = 0;
uint8_t buf[kFeedbackReadLen] = {};
if (!readPacketForId(id, &err, buf, kFeedbackReadLen, 1600)) {
return false;
}
uint16_t pos_u16 = static_cast<uint16_t>(buf[0]) | (static_cast<uint16_t>(buf[1]) << 8);
uint16_t spd_u16 = static_cast<uint16_t>(buf[2]) | (static_cast<uint16_t>(buf[3]) << 8);
*out_tick = static_cast<int32_t>(pos_u16 & 0x7FFFU);
*out_speed = static_cast<int32_t>(spd_u16);
*out_voltage_v = static_cast<float>(buf[6]) * 0.1f;
*out_temp_c = static_cast<float>(buf[7]);
return err == 0;
}
uint32_t FeetechHlsBus::readFeedback(
const uint8_t* ids,
int count,
int32_t* out_ticks,
int32_t* out_speeds,
float* out_min_voltage_v,
float* out_max_temp_c) {
uint32_t err_mask = 0;
float min_v = 99.0f;
float max_t = -40.0f;
bool any_ok = false;
#if USE_FEETECH_SYNC_READ
uint8_t sr_params[2 + NUM_JOINTS];
sr_params[0] = kRegPresentPos;
sr_params[1] = kFeedbackReadLen;
for (int i = 0; i < count; ++i) {
sr_params[2 + i] = ids[i];
}
sendPacket(kBroadcastId, kInstSyncRead, sr_params, static_cast<uint8_t>(2 + count));
bool got[NUM_JOINTS] = {};
for (int i = 0; i < count; ++i) {
uint8_t err = 0;
uint8_t buf[kFeedbackReadLen] = {};
if (readPacketForId(ids[i], &err, buf, kFeedbackReadLen, 1200)) {
got[i] = true;
uint16_t pos_u16 = static_cast<uint16_t>(buf[0]) | (static_cast<uint16_t>(buf[1]) << 8);
uint16_t spd_u16 = static_cast<uint16_t>(buf[2]) | (static_cast<uint16_t>(buf[3]) << 8);
out_ticks[i] = static_cast<int32_t>(pos_u16 & 0x7FFFU);
out_speeds[i] = static_cast<int32_t>(spd_u16);
float v = static_cast<float>(buf[6]) * 0.1f;
float t = static_cast<float>(buf[7]);
if (v > 0.1f && v < min_v) min_v = v;
if (t > max_t) max_t = t;
any_ok = true;
if (err != 0) err_mask |= (1UL << i);
}
}
// Fallback single-read for any joints dropped during SyncRead
for (int i = 0; i < count; ++i) {
if (!got[i]) {
float v = 0.0f, t = 0.0f;
if (readSingleJoint(ids[i], &out_ticks[i], &out_speeds[i], &v, &t)) {
if (v > 0.1f && v < min_v) min_v = v;
if (t > max_t) max_t = t;
any_ok = true;
} else {
err_mask |= (1UL << i);
}
}
}
#else
for (int i = 0; i < count; ++i) {
float v = 0.0f, t = 0.0f;
if (readSingleJoint(ids[i], &out_ticks[i], &out_speeds[i], &v, &t)) {
if (v > 0.1f && v < min_v) min_v = v;
if (t > max_t) max_t = t;
any_ok = true;
} else {
err_mask |= (1UL << i);
}
}
#endif
*out_min_voltage_v = any_ok ? min_v : 0.0f;
*out_max_temp_c = any_ok ? max_t : 0.0f;
return err_mask;
}
@@ -0,0 +1,50 @@
#pragma once
#include <Arduino.h>
#include "config.h"
class FeetechHlsBus {
public:
void begin(HardwareSerial& serial, uint32_t baud, int dir_pin = SERVO_DIR_PIN);
bool ping(uint8_t id, uint32_t timeout_us = 1800);
int pingRange(uint8_t max_id, int32_t* out_ids, int max_out);
bool setTorqueBroadcast(bool enable);
bool setTorqueAll(const uint8_t* ids, int count, bool enable);
bool syncWriteTargets(
const uint8_t* ids,
int count,
const int32_t* target_ticks,
uint8_t acc,
uint16_t speed_limit);
// Returns bitmask of joints with comm/servo error (bit i set -> joint i fault/timeout)
uint32_t readFeedback(
const uint8_t* ids,
int count,
int32_t* out_ticks,
int32_t* out_speeds,
float* out_min_voltage_v,
float* out_max_temp_c);
private:
HardwareSerial* serial_ = nullptr;
int dir_pin_ = -1;
uint8_t last_tx_[128] = {};
size_t last_tx_len_ = 0;
void setTxMode(bool tx);
void flushInput();
void sendRawPacket(const uint8_t* pkt, size_t len);
void sendPacket(uint8_t id, uint8_t inst, const uint8_t* params, uint8_t param_len);
bool readPacketForId(uint8_t expected_id, uint8_t* out_err, uint8_t* params, uint8_t expected_len, uint32_t timeout_us);
bool readSingleJoint(
uint8_t id,
int32_t* out_tick,
int32_t* out_speed,
float* out_voltage_v,
float* out_temp_c);
static uint8_t computeChecksum(const uint8_t* body_from_id, size_t body_len);
};
+216
View File
@@ -0,0 +1,216 @@
#include "qmi8658.h"
#include <math.h>
namespace {
constexpr uint8_t kRegWhoAmI = 0x00;
constexpr uint8_t kRegCtrl1 = 0x02;
constexpr uint8_t kRegCtrl2 = 0x03;
constexpr uint8_t kRegCtrl3 = 0x04;
constexpr uint8_t kRegCtrl5 = 0x06;
constexpr uint8_t kRegCtrl7 = 0x08;
constexpr uint8_t kRegAxL = 0x35;
constexpr uint8_t kExpectedWhoAmI = 0x05;
// CTRL2: aFS=±4g (1 << 4), aODR=250Hz (5) -> 8192 LSB/g
constexpr uint8_t kCtrl2Val = 0x15;
constexpr float kAccelScaleG = 4.0f / 32768.0f;
// CTRL3: gFS=±1024 dps (6 << 4), gODR=448.4Hz (4) -> 32 LSB/dps
constexpr uint8_t kCtrl3Val = 0x64;
constexpr float kGyroScaleRadS = (1024.0f / 32768.0f) * (PI / 180.0f);
// CTRL5: enable LPF mode 2 on both accel and gyro
constexpr uint8_t kCtrl5Val = 0x55;
constexpr float kTwoKp = 2.0f * 1.2f; // Proportional gain
constexpr float kTwoKi = 2.0f * 0.02f; // Integral gain
} // namespace
bool Qmi8658Imu::writeReg(uint8_t reg, uint8_t val) {
wire_->beginTransmission(addr_);
wire_->write(reg);
wire_->write(val);
return wire_->endTransmission(true) == 0;
}
bool Qmi8658Imu::readRegs(uint8_t reg, uint8_t* buf, size_t len) {
wire_->beginTransmission(addr_);
wire_->write(reg);
if (wire_->endTransmission(false) != 0) {
return false;
}
size_t got = wire_->requestFrom(static_cast<int>(addr_), static_cast<int>(len));
if (got != len) {
while (wire_->available()) {
(void)wire_->read();
}
return false;
}
for (size_t i = 0; i < len; ++i) {
buf[i] = static_cast<uint8_t>(wire_->read());
}
return true;
}
bool Qmi8658Imu::begin(TwoWire& wire, uint8_t addr) {
wire_ = &wire;
addr_ = addr;
healthy_ = false;
err_streak_ = 0;
uint8_t who = 0;
if (!readRegs(kRegWhoAmI, &who, 1) || who != kExpectedWhoAmI) {
return false;
}
// CTRL1 = 0x40 sets ADDR_AI (auto-increment for burst reads) and little-endian data
if (!writeReg(kRegCtrl7, 0x00)) return false;
delay(2);
if (!writeReg(kRegCtrl1, 0x40)) return false;
if (!writeReg(kRegCtrl2, kCtrl2Val)) return false;
if (!writeReg(kRegCtrl3, kCtrl3Val)) return false;
if (!writeReg(kRegCtrl5, kCtrl5Val)) return false;
if (!writeReg(kRegCtrl7, 0x03)) return false; // Enable accel + gyro
delay(10);
// Align initial Mahony orientation from static gravity vector if available
q0_ = 1.0f;
q1_ = 0.0f;
q2_ = 0.0f;
q3_ = 0.0f;
ix_ = 0.0f;
iy_ = 0.0f;
iz_ = 0.0f;
if (readSampleRaw()) {
for (int i = 0; i < 40; ++i) {
mahonyUpdate(0.0f, 0.0f, 0.0f, raw_accel_[0], raw_accel_[1], raw_accel_[2], 0.01f);
}
}
healthy_ = true;
return true;
}
void Qmi8658Imu::setGyroBias(float bx, float by, float bz) {
gyro_bias_[0] = bx;
gyro_bias_[1] = by;
gyro_bias_[2] = bz;
}
bool Qmi8658Imu::readSampleRaw() {
if (!wire_) return false;
uint8_t buf[12];
if (!readRegs(kRegAxL, buf, sizeof(buf))) {
return false;
}
auto to_i16 = [](uint8_t lo, uint8_t hi) -> int16_t {
return static_cast<int16_t>((static_cast<uint16_t>(hi) << 8) | lo);
};
raw_accel_[0] = static_cast<float>(to_i16(buf[0], buf[1])) * kAccelScaleG;
raw_accel_[1] = static_cast<float>(to_i16(buf[2], buf[3])) * kAccelScaleG;
raw_accel_[2] = static_cast<float>(to_i16(buf[4], buf[5])) * kAccelScaleG;
raw_gyro_[0] = static_cast<float>(to_i16(buf[6], buf[7])) * kGyroScaleRadS;
raw_gyro_[1] = static_cast<float>(to_i16(buf[8], buf[9])) * kGyroScaleRadS;
raw_gyro_[2] = static_cast<float>(to_i16(buf[10], buf[11])) * kGyroScaleRadS;
return true;
}
bool Qmi8658Imu::update(float dt_s) {
if (!readSampleRaw()) {
if (err_streak_ < 255) err_streak_++;
if (err_streak_ >= 10) healthy_ = false;
return false;
}
err_streak_ = 0;
healthy_ = true;
float gx = raw_gyro_[0] - gyro_bias_[0];
float gy = raw_gyro_[1] - gyro_bias_[1];
float gz = raw_gyro_[2] - gyro_bias_[2];
mahonyUpdate(gx, gy, gz, raw_accel_[0], raw_accel_[1], raw_accel_[2], dt_s);
return true;
}
void Qmi8658Imu::mahonyUpdate(float gx, float gy, float gz, float ax, float ay, float az, float dt_s) {
float norm = sqrtf(ax * ax + ay * ay + az * az);
if (norm > 0.25f) {
float inv = 1.0f / norm;
ax *= inv;
ay *= inv;
az *= inv;
// Estimated world-UP in body frame
float vx = 2.0f * (q1_ * q3_ - q0_ * q2_);
float vy = 2.0f * (q0_ * q1_ + q2_ * q3_);
float vz = 1.0f - 2.0f * (q1_ * q1_ + q2_ * q2_);
float ex = ay * vz - az * vy;
float ey = az * vx - ax * vz;
float ez = ax * vy - ay * vx;
if (kTwoKi > 0.0f) {
ix_ += kTwoKi * ex * dt_s;
iy_ += kTwoKi * ey * dt_s;
iz_ += kTwoKi * ez * dt_s;
gx += ix_;
gy += iy_;
gz += iz_;
}
gx += kTwoKp * ex;
gy += kTwoKp * ey;
gz += kTwoKp * ez;
}
float half_dt = 0.5f * dt_s;
float dq0 = (-q1_ * gx - q2_ * gy - q3_ * gz) * half_dt;
float dq1 = (q0_ * gx + q2_ * gz - q3_ * gy) * half_dt;
float dq2 = (q0_ * gy - q1_ * gz + q3_ * gx) * half_dt;
float dq3 = (q0_ * gz + q1_ * gy - q2_ * gx) * half_dt;
q0_ += dq0;
q1_ += dq1;
q2_ += dq2;
q3_ += dq3;
float qnorm = sqrtf(q0_ * q0_ + q1_ * q1_ + q2_ * q2_ + q3_ * q3_);
if (qnorm > 1e-6f) {
float inv_q = 1.0f / qnorm;
q0_ *= inv_q;
q1_ *= inv_q;
q2_ *= inv_q;
q3_ *= inv_q;
}
}
bool Qmi8658Imu::sampleStationaryGyroBias(int samples, float out_bias[3]) {
if (samples <= 0) samples = 1;
double sx = 0.0, sy = 0.0, sz = 0.0;
int ok_cnt = 0;
for (int i = 0; i < samples; ++i) {
if (readSampleRaw()) {
sx += raw_gyro_[0];
sy += raw_gyro_[1];
sz += raw_gyro_[2];
ok_cnt++;
}
delay(4);
}
if (ok_cnt == 0) return false;
out_bias[0] = static_cast<float>(sx / ok_cnt);
out_bias[1] = static_cast<float>(sy / ok_cnt);
out_bias[2] = static_cast<float>(sz / ok_cnt);
return true;
}
void Qmi8658Imu::getRawGyro(float out[3]) const {
out[0] = raw_gyro_[0];
out[1] = raw_gyro_[1];
out[2] = raw_gyro_[2];
}
void Qmi8658Imu::getProjectedGravity(float out[3]) const {
// Gravity DOWN in world [0, 0, -1] rotated into body frame = -v_up
out[0] = -2.0f * (q1_ * q3_ - q0_ * q2_);
out[1] = -2.0f * (q0_ * q1_ + q2_ * q3_);
out[2] = -(1.0f - 2.0f * (q1_ * q1_ + q2_ * q2_));
}
+36
View File
@@ -0,0 +1,36 @@
#pragma once
#include <Arduino.h>
#include <Wire.h>
class Qmi8658Imu {
public:
bool begin(TwoWire& wire, uint8_t addr);
void setGyroBias(float bx, float by, float bz);
bool update(float dt_s);
bool sampleStationaryGyroBias(int samples, float out_bias[3]);
bool isHealthy() const { return healthy_; }
void getRawGyro(float out[3]) const;
void getProjectedGravity(float out[3]) const;
private:
TwoWire* wire_ = nullptr;
uint8_t addr_ = 0x6A;
bool healthy_ = false;
uint8_t err_streak_ = 0;
// Bias subtracted strictly inside Mahony filter (raw gyro stays unbiased for Python HAL)
float gyro_bias_[3] = {0.0f, 0.0f, 0.0f};
float raw_gyro_[3] = {0.0f, 0.0f, 0.0f};
float raw_accel_[3] = {0.0f, 0.0f, 1.0f};
// Mahony quaternion (body -> world) and integral feedback
float q0_ = 1.0f, q1_ = 0.0f, q2_ = 0.0f, q3_ = 0.0f;
float ix_ = 0.0f, iy_ = 0.0f, iz_ = 0.0f;
bool writeReg(uint8_t reg, uint8_t val);
bool readRegs(uint8_t reg, uint8_t* buf, size_t len);
bool readSampleRaw();
void mahonyUpdate(float gx, float gy, float gz, float ax, float ay, float az, float dt_s);
};
+195
View File
@@ -0,0 +1,195 @@
#include <Arduino_RouterBridge.h>
#include <Wire.h>
#include <vector>
#include "config.h"
#include "feetech_hls.h"
#include "qmi8658.h"
namespace {
Qmi8658Imu g_imu;
FeetechHlsBus g_servo;
uint8_t g_servo_ids[NUM_JOINTS] = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14};
uint32_t g_watchdog_ms = WATCHDOG_DEFAULT_MS;
uint32_t g_last_sync_ms = 0;
uint32_t g_last_imu_us = 0;
bool g_torque_enabled = false;
bool g_imu_ready = false;
int32_t g_last_ticks[NUM_JOINTS] = {2048, 2048, 2048, 2048, 2048, 2048, 2048, 2048, 2048, 2048, 2048, 2048, 2048, 2048};
int32_t g_last_speeds[NUM_JOINTS] = {};
} // namespace
int32_t hw_configure(std::vector<float> payload) {
g_torque_enabled = false;
g_servo.setTorqueAll(g_servo_ids, NUM_JOINTS, false);
uint32_t baud = SERVO_BAUD;
uint8_t i2c_addr = IMU_I2C_ADDR;
if (payload.size() >= 3) {
baud = static_cast<uint32_t>(payload[0] > 9600.0f ? payload[0] : SERVO_BAUD);
i2c_addr = static_cast<uint8_t>(static_cast<int>(payload[1] + 0.5f));
g_watchdog_ms = static_cast<uint32_t>(payload[2] >= 40.0f ? payload[2] : WATCHDOG_DEFAULT_MS);
}
if (payload.size() >= 6) {
g_imu.setGyroBias(payload[3], payload[4], payload[5]);
}
if (payload.size() >= 6 + NUM_JOINTS) {
for (int i = 0; i < NUM_JOINTS; ++i) {
int sid = static_cast<int>(payload[6 + i] + 0.5f);
if (sid >= 0 && sid <= 253) {
g_servo_ids[i] = static_cast<uint8_t>(sid);
}
}
}
g_servo.begin(SERVO_SERIAL, baud, SERVO_DIR_PIN);
g_servo.setTorqueAll(g_servo_ids, NUM_JOINTS, false);
IMU_WIRE.begin();
IMU_WIRE.setClock(IMU_I2C_CLOCK_HZ);
g_imu_ready = g_imu.begin(IMU_WIRE, i2c_addr);
if (payload.size() >= 6) {
g_imu.setGyroBias(payload[3], payload[4], payload[5]);
}
g_last_sync_ms = millis();
g_last_imu_us = micros();
return g_imu_ready ? 1 : 0;
}
std::vector<float> hw_sync_step(std::vector<int32_t> cmd_frame) {
std::vector<float> res(38, 0.0f);
if (cmd_frame.size() < 3 + NUM_JOINTS) {
res[36] = g_imu.isHealthy() ? 1.0f : 0.0f;
res[37] = static_cast<float>(0xFFFF);
return res;
}
bool want_torque = cmd_frame[0] != 0;
uint16_t speed_limit = static_cast<uint16_t>(cmd_frame[1] > 0 ? cmd_frame[1] : 0);
uint8_t acc = static_cast<uint8_t>(cmd_frame[2] >= 0 ? (cmd_frame[2] <= 254 ? cmd_frame[2] : 254) : 0);
int32_t target_ticks[NUM_JOINTS];
for (int i = 0; i < NUM_JOINTS; ++i) {
target_ticks[i] = cmd_frame[3 + i];
}
// Write targets BEFORE enabling torque on 0->1 transition to prevent initial snap
if (want_torque) {
g_servo.syncWriteTargets(g_servo_ids, NUM_JOINTS, target_ticks, acc, speed_limit);
if (!g_torque_enabled) {
g_servo.setTorqueAll(g_servo_ids, NUM_JOINTS, true);
g_torque_enabled = true;
}
} else {
if (g_torque_enabled) {
g_servo.setTorqueAll(g_servo_ids, NUM_JOINTS, false);
g_torque_enabled = false;
}
}
g_last_sync_ms = millis();
// Sample fresh IMU reading if at least 2ms elapsed since last 250Hz integration tick
uint32_t now_us = micros();
uint32_t dt_us = now_us - g_last_imu_us;
if (dt_us >= 2000UL && dt_us < 100000UL) {
g_imu.update(static_cast<float>(dt_us) * 1e-6f);
g_last_imu_us = now_us;
}
float min_v = 0.0f;
float max_t = 0.0f;
uint32_t err_mask = g_servo.readFeedback(
g_servo_ids, NUM_JOINTS, g_last_ticks, g_last_speeds, &min_v, &max_t);
float gyro[3] = {0.0f, 0.0f, 0.0f};
float grav[3] = {0.0f, 0.0f, -1.0f};
g_imu.getRawGyro(gyro);
g_imu.getProjectedGravity(grav);
res[0] = gyro[0];
res[1] = gyro[1];
res[2] = gyro[2];
res[3] = grav[0];
res[4] = grav[1];
res[5] = grav[2];
for (int i = 0; i < NUM_JOINTS; ++i) {
res[6 + i] = static_cast<float>(g_last_ticks[i]);
res[20 + i] = static_cast<float>(g_last_speeds[i]);
}
res[34] = min_v;
res[35] = max_t;
res[36] = g_imu.isHealthy() ? 1.0f : 0.0f;
res[37] = static_cast<float>(err_mask);
return res;
}
int32_t hw_set_torque(int32_t enable) {
bool on = enable != 0;
g_torque_enabled = on;
g_servo.setTorqueAll(g_servo_ids, NUM_JOINTS, on);
g_last_sync_ms = millis();
return 1;
}
std::vector<int32_t> hw_ping_servos(int32_t max_id) {
if (max_id < 1) max_id = 1;
if (max_id > 253) max_id = 253;
int32_t found[64];
int n = g_servo.pingRange(static_cast<uint8_t>(max_id), found, 64);
return std::vector<int32_t>(found, found + n);
}
std::vector<float> hw_calibrate_imu(int32_t samples) {
if (samples < 10) samples = 10;
if (samples > 1000) samples = 1000;
float bias[3] = {0.0f, 0.0f, 0.0f};
g_imu.sampleStationaryGyroBias(static_cast<int>(samples), bias);
g_imu.setGyroBias(bias[0], bias[1], bias[2]);
return {bias[0], bias[1], bias[2]};
}
void setup() {
Bridge.begin();
Monitor.begin(115200);
g_servo.begin(SERVO_SERIAL, SERVO_BAUD, SERVO_DIR_PIN);
g_servo.setTorqueBroadcast(false);
IMU_WIRE.begin();
IMU_WIRE.setClock(IMU_I2C_CLOCK_HZ);
g_imu_ready = g_imu.begin(IMU_WIRE, IMU_I2C_ADDR);
Bridge.provide_safe("hw_configure", hw_configure);
Bridge.provide_safe("hw_sync_step", hw_sync_step);
Bridge.provide_safe("hw_set_torque", hw_set_torque);
Bridge.provide_safe("hw_ping_servos", hw_ping_servos);
Bridge.provide_safe("hw_calibrate_imu", hw_calibrate_imu);
g_last_sync_ms = millis();
g_last_imu_us = micros();
Monitor.println("Microduck UNO Q STM32 firmware initialized (torque off).");
}
void loop() {
uint32_t now_us = micros();
uint32_t dt_us = now_us - g_last_imu_us;
if (dt_us >= IMU_LOOP_INTERVAL_US) {
float dt_s = static_cast<float>(dt_us) * 1e-6f;
if (dt_s > 0.05f) dt_s = 0.004f;
g_imu.update(dt_s);
g_last_imu_us = now_us;
}
// Hardware safety watchdog: disable servo torque if Linux 50Hz loop stalls
if (g_torque_enabled && (millis() - g_last_sync_ms) > g_watchdog_ms) {
g_torque_enabled = false;
g_servo.setTorqueBroadcast(false);
g_servo.setTorqueAll(g_servo_ids, NUM_JOINTS, false);
Monitor.println("WATCHDOG: Linux sync_step timeout -> servo torque disabled.");
}
}
@@ -0,0 +1,12 @@
profiles:
default:
fqbn: arduino:zephyr:unoq
platforms:
- platform: arduino:zephyr
libraries:
- Arduino_RouterBridge
- Arduino_RPClite
- ArxContainer
- ArxTypeTraits
- DebugLog
- MsgPack
+8
View File
@@ -0,0 +1,8 @@
<!doctype html><html lang="zh-CN"><meta charset="utf-8"><meta name="viewport" content="width=device-width, initial-scale=1"><title>Microduck 影子策略状态</title>
<style>body{margin:0;font:15px/1.5 system-ui;color:#16201f;background:#f4f8f6;padding:8px;border-radius:6px}p{margin:0;overflow-wrap:anywhere}</style>
<p id="summary">正在读取板端状态…</p><p id="details"></p><p id="skills"></p>
<script>
const names={walk:'行走',stand:'站立/起身',sitstand:'坐下/站起',kickL:'左脚踢球',kickR:'右脚踢球',groundpick:'拾取',roll:'翻滚',drive:'轮式行驶',crouch:'蹲伏'};
const text=(id,value)=>document.getElementById(id).textContent=value;
async function update(){try{if(document.hidden)return;const r=await fetch('http://127.0.0.1:18000/api/status',{cache:'no-store',signal:AbortSignal.timeout(1800)});if(!r.ok)throw Error();const s=await r.json();if(s.hal!=='xgoduck-shadow')throw Error();text('summary',s.version?'板端版本:'+s.version.slice(0,12):'尚未加载影子策略');text('details',s.shadow_error?'推理异常:'+s.shadow_error:s.native_mode==='shadow'?`当前影子策略:${names[s.shadow_skill]??s.shadow_skill??'无'} · 推理次数:${s.shadow_steps}`:'影子推理未运行 · 原机模式:'+({off:'扭矩已关闭',hold:'保持姿态',policy:'运动控制'}[s.native_mode]??s.native_mode));text('skills','已加载:'+(s.loaded_skills.map(k=>names[k]??k).join('、')||'无'));}catch{text('summary','影子服务未连接,请检查 USB 热更新转发(18000)。');text('skills','');text('details','');}finally{setTimeout(update,1200)}}update();
</script></html>
@@ -0,0 +1,202 @@
Apache License
Version 2.0, January 2004
http://www.apache.org/licenses/
TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
1. Definitions.
"License" shall mean the terms and conditions for use, reproduction,
and distribution as defined by Sections 1 through 9 of this document.
"Licensor" shall mean the copyright owner or entity authorized by
the copyright owner that is granting the License.
"Legal Entity" shall mean the union of the acting entity and all
other entities that control, are controlled by, or are under common
control with that entity. For the purposes of this definition,
"control" means (i) the power, direct or indirect, to cause the
direction or management of such entity, whether by contract or
otherwise, or (ii) ownership of fifty percent (50%) or more of the
outstanding shares, or (iii) beneficial ownership of such entity.
"You" (or "Your") shall mean an individual or Legal Entity
exercising permissions granted by this License.
"Source" form shall mean the preferred form for making modifications,
including but not limited to software source code, documentation
source, and configuration files.
"Object" form shall mean any form resulting from mechanical
transformation or translation of a Source form, including but
not limited to compiled object code, generated documentation,
and conversions to other media types.
"Work" shall mean the work of authorship, whether in Source or
Object form, made available under the License, as indicated by a
copyright notice that is included in or attached to the work
(an example is provided in the Appendix below).
"Derivative Works" shall mean any work, whether in Source or Object
form, that is based on (or derived from) the Work and for which the
editorial revisions, annotations, elaborations, or other modifications
represent, as a whole, an original work of authorship. For the purposes
of this License, Derivative Works shall not include works that remain
separable from, or merely link (or bind by name) to the interfaces of,
the Work and Derivative Works thereof.
"Contribution" shall mean any work of authorship, including
the original version of the Work and any modifications or additions
to that Work or Derivative Works thereof, that is intentionally
submitted to Licensor for inclusion in the Work by the copyright owner
or by an individual or Legal Entity authorized to submit on behalf of
the copyright owner. For the purposes of this definition, "submitted"
means any form of electronic, verbal, or written communication sent
to the Licensor or its representatives, including but not limited to
communication on electronic mailing lists, source code control systems,
and issue tracking systems that are managed by, or on behalf of, the
Licensor for the purpose of discussing and improving the Work, but
excluding communication that is conspicuously marked or otherwise
designated in writing by the copyright owner as "Not a Contribution."
"Contributor" shall mean Licensor and any individual or Legal Entity
on behalf of whom a Contribution has been received by Licensor and
subsequently incorporated within the Work.
2. Grant of Copyright License. Subject to the terms and conditions of
this License, each Contributor hereby grants to You a perpetual,
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
copyright license to reproduce, prepare Derivative Works of,
publicly display, publicly perform, sublicense, and distribute the
Work and such Derivative Works in Source or Object form.
3. Grant of Patent License. Subject to the terms and conditions of
this License, each Contributor hereby grants to You a perpetual,
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
(except as stated in this section) patent license to make, have made,
use, offer to sell, sell, import, and otherwise transfer the Work,
where such license applies only to those patent claims licensable
by such Contributor that are necessarily infringed by their
Contribution(s) alone or by combination of their Contribution(s)
with the Work to which such Contribution(s) was submitted. If You
institute patent litigation against any entity (including a
cross-claim or counterclaim in a lawsuit) alleging that the Work
or a Contribution incorporated within the Work constitutes direct
or contributory patent infringement, then any patent licenses
granted to You under this License for that Work shall terminate
as of the date such litigation is filed.
4. Redistribution. You may reproduce and distribute copies of the
Work or Derivative Works thereof in any medium, with or without
modifications, and in Source or Object form, provided that You
meet the following conditions:
(a) You must give any other recipients of the Work or
Derivative Works a copy of this License; and
(b) You must cause any modified files to carry prominent notices
stating that You changed the files; and
(c) You must retain, in the Source form of any Derivative Works
that You distribute, all copyright, patent, trademark, and
attribution notices from the Source form of the Work,
excluding those notices that do not pertain to any part of
the Derivative Works; and
(d) If the Work includes a "NOTICE" text file as part of its
distribution, then any Derivative Works that You distribute must
include a readable copy of the attribution notices contained
within such NOTICE file, excluding those notices that do not
pertain to any part of the Derivative Works, in at least one
of the following places: within a NOTICE text file distributed
as part of the Derivative Works; within the Source form or
documentation, if provided along with the Derivative Works; or,
within a display generated by the Derivative Works, if and
wherever such third-party notices normally appear. The contents
of the NOTICE file are for informational purposes only and
do not modify the License. You may add Your own attribution
notices within Derivative Works that You distribute, alongside
or as an addendum to the NOTICE text from the Work, provided
that such additional attribution notices cannot be construed
as modifying the License.
You may add Your own copyright statement to Your modifications and
may provide additional or different license terms and conditions
for use, reproduction, or distribution of Your modifications, or
for any such Derivative Works as a whole, provided Your use,
reproduction, and distribution of the Work otherwise complies with
the conditions stated in this License.
5. Submission of Contributions. Unless You explicitly state otherwise,
any Contribution intentionally submitted for inclusion in the Work
by You to the Licensor shall be under the terms and conditions of
this License, without any additional terms or conditions.
Notwithstanding the above, nothing herein shall supersede or modify
the terms of any separate license agreement you may have executed
with Licensor regarding such Contributions.
6. Trademarks. This License does not grant permission to use the trade
names, trademarks, service marks, or product names of the Licensor,
except as required for reasonable and customary use in describing the
origin of the Work and reproducing the content of the NOTICE file.
7. Disclaimer of Warranty. Unless required by applicable law or
agreed to in writing, Licensor provides the Work (and each
Contributor provides its Contributions) on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
implied, including, without limitation, any warranties or conditions
of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
PARTICULAR PURPOSE. You are solely responsible for determining the
appropriateness of using or redistributing the Work and assume any
risks associated with Your exercise of permissions under this License.
8. Limitation of Liability. In no event and under no legal theory,
whether in tort (including negligence), contract, or otherwise,
unless required by applicable law (such as deliberate and grossly
negligent acts) or agreed to in writing, shall any Contributor be
liable to You for damages, including any direct, indirect, special,
incidental, or consequential damages of any character arising as a
result of this License or out of the use or inability to use the
Work (including but not limited to damages for loss of goodwill,
work stoppage, computer failure or malfunction, or any and all
other commercial damages or losses), even if such Contributor
has been advised of the possibility of such damages.
9. Accepting Warranty or Additional Liability. While redistributing
the Work or Derivative Works thereof, You may choose to offer,
and charge a fee for, acceptance of support, warranty, indemnity,
or other liability obligations and/or rights consistent with this
License. However, in accepting such obligations, You may act only
on Your own behalf and on Your sole responsibility, not on behalf
of any other Contributor, and only if You agree to indemnify,
defend, and hold each Contributor harmless for any liability
incurred by, or claims asserted against, such Contributor by reason
of your accepting any such warranty or additional liability.
END OF TERMS AND CONDITIONS
APPENDIX: How to apply the Apache License to your work.
To apply the Apache License to your work, attach the following
boilerplate notice, with the fields enclosed by brackets "[]"
replaced with your own identifying information. (Don't include
the brackets!) The text should be enclosed in the appropriate
comment syntax for the file format. We also recommend that a
file or class name and description of purpose be included on the
same "printed page" as the copyright notice for easier
identification within third-party archives.
Copyright [2024] [Apptronik]
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at
http://www.apache.org/licenses/LICENSE-2.0
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
+202
View File
@@ -0,0 +1,202 @@
Apache License
Version 2.0, January 2004
http://www.apache.org/licenses/
TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
1. Definitions.
"License" shall mean the terms and conditions for use, reproduction,
and distribution as defined by Sections 1 through 9 of this document.
"Licensor" shall mean the copyright owner or entity authorized by
the copyright owner that is granting the License.
"Legal Entity" shall mean the union of the acting entity and all
other entities that control, are controlled by, or are under common
control with that entity. For the purposes of this definition,
"control" means (i) the power, direct or indirect, to cause the
direction or management of such entity, whether by contract or
otherwise, or (ii) ownership of fifty percent (50%) or more of the
outstanding shares, or (iii) beneficial ownership of such entity.
"You" (or "Your") shall mean an individual or Legal Entity
exercising permissions granted by this License.
"Source" form shall mean the preferred form for making modifications,
including but not limited to software source code, documentation
source, and configuration files.
"Object" form shall mean any form resulting from mechanical
transformation or translation of a Source form, including but
not limited to compiled object code, generated documentation,
and conversions to other media types.
"Work" shall mean the work of authorship, whether in Source or
Object form, made available under the License, as indicated by a
copyright notice that is included in or attached to the work
(an example is provided in the Appendix below).
"Derivative Works" shall mean any work, whether in Source or Object
form, that is based on (or derived from) the Work and for which the
editorial revisions, annotations, elaborations, or other modifications
represent, as a whole, an original work of authorship. For the purposes
of this License, Derivative Works shall not include works that remain
separable from, or merely link (or bind by name) to the interfaces of,
the Work and Derivative Works thereof.
"Contribution" shall mean any work of authorship, including
the original version of the Work and any modifications or additions
to that Work or Derivative Works thereof, that is intentionally
submitted to Licensor for inclusion in the Work by the copyright owner
or by an individual or Legal Entity authorized to submit on behalf of
the copyright owner. For the purposes of this definition, "submitted"
means any form of electronic, verbal, or written communication sent
to the Licensor or its representatives, including but not limited to
communication on electronic mailing lists, source code control systems,
and issue tracking systems that are managed by, or on behalf of, the
Licensor for the purpose of discussing and improving the Work, but
excluding communication that is conspicuously marked or otherwise
designated in writing by the copyright owner as "Not a Contribution."
"Contributor" shall mean Licensor and any individual or Legal Entity
on behalf of whom a Contribution has been received by Licensor and
subsequently incorporated within the Work.
2. Grant of Copyright License. Subject to the terms and conditions of
this License, each Contributor hereby grants to You a perpetual,
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
copyright license to reproduce, prepare Derivative Works of,
publicly display, publicly perform, sublicense, and distribute the
Work and such Derivative Works in Source or Object form.
3. Grant of Patent License. Subject to the terms and conditions of
this License, each Contributor hereby grants to You a perpetual,
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
(except as stated in this section) patent license to make, have made,
use, offer to sell, sell, import, and otherwise transfer the Work,
where such license applies only to those patent claims licensable
by such Contributor that are necessarily infringed by their
Contribution(s) alone or by combination of their Contribution(s)
with the Work to which such Contribution(s) was submitted. If You
institute patent litigation against any entity (including a
cross-claim or counterclaim in a lawsuit) alleging that the Work
or a Contribution incorporated within the Work constitutes direct
or contributory patent infringement, then any patent licenses
granted to You under this License for that Work shall terminate
as of the date such litigation is filed.
4. Redistribution. You may reproduce and distribute copies of the
Work or Derivative Works thereof in any medium, with or without
modifications, and in Source or Object form, provided that You
meet the following conditions:
(a) You must give any other recipients of the Work or
Derivative Works a copy of this License; and
(b) You must cause any modified files to carry prominent notices
stating that You changed the files; and
(c) You must retain, in the Source form of any Derivative Works
that You distribute, all copyright, patent, trademark, and
attribution notices from the Source form of the Work,
excluding those notices that do not pertain to any part of
the Derivative Works; and
(d) If the Work includes a "NOTICE" text file as part of its
distribution, then any Derivative Works that You distribute must
include a readable copy of the attribution notices contained
within such NOTICE file, excluding those notices that do not
pertain to any part of the Derivative Works, in at least one
of the following places: within a NOTICE text file distributed
as part of the Derivative Works; within the Source form or
documentation, if provided along with the Derivative Works; or,
within a display generated by the Derivative Works, if and
wherever such third-party notices normally appear. The contents
of the NOTICE file are for informational purposes only and
do not modify the License. You may add Your own attribution
notices within Derivative Works that You distribute, alongside
or as an addendum to the NOTICE text from the Work, provided
that such additional attribution notices cannot be construed
as modifying the License.
You may add Your own copyright statement to Your modifications and
may provide additional or different license terms and conditions
for use, reproduction, or distribution of Your modifications, or
for any such Derivative Works as a whole, provided Your use,
reproduction, and distribution of the Work otherwise complies with
the conditions stated in this License.
5. Submission of Contributions. Unless You explicitly state otherwise,
any Contribution intentionally submitted for inclusion in the Work
by You to the Licensor shall be under the terms and conditions of
this License, without any additional terms or conditions.
Notwithstanding the above, nothing herein shall supersede or modify
the terms of any separate license agreement you may have executed
with Licensor regarding such Contributions.
6. Trademarks. This License does not grant permission to use the trade
names, trademarks, service marks, or product names of the Licensor,
except as required for reasonable and customary use in describing the
origin of the Work and reproducing the content of the NOTICE file.
7. Disclaimer of Warranty. Unless required by applicable law or
agreed to in writing, Licensor provides the Work (and each
Contributor provides its Contributions) on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
implied, including, without limitation, any warranties or conditions
of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
PARTICULAR PURPOSE. You are solely responsible for determining the
appropriateness of using or redistributing the Work and assume any
risks associated with Your exercise of permissions under this License.
8. Limitation of Liability. In no event and under no legal theory,
whether in tort (including negligence), contract, or otherwise,
unless required by applicable law (such as deliberate and grossly
negligent acts) or agreed to in writing, shall any Contributor be
liable to You for damages, including any direct, indirect, special,
incidental, or consequential damages of any character arising as a
result of this License or out of the use or inability to use the
Work (including but not limited to damages for loss of goodwill,
work stoppage, computer failure or malfunction, or any and all
other commercial damages or losses), even if such Contributor
has been advised of the possibility of such damages.
9. Accepting Warranty or Additional Liability. While redistributing
the Work or Derivative Works thereof, You may choose to offer,
and charge a fee for, acceptance of support, warranty, indemnity,
or other liability obligations and/or rights consistent with this
License. However, in accepting such obligations, You may act only
on Your own behalf and on Your sole responsibility, not on behalf
of any other Contributor, and only if You agree to indemnify,
defend, and hold each Contributor harmless for any liability
incurred by, or claims asserted against, such Contributor by reason
of your accepting any such warranty or additional liability.
END OF TERMS AND CONDITIONS
APPENDIX: How to apply the Apache License to your work.
To apply the Apache License to your work, attach the following
boilerplate notice, with the fields enclosed by brackets "[]"
replaced with your own identifying information. (Don't include
the brackets!) The text should be enclosed in the appropriate
comment syntax for the file format. We also recommend that a
file or class name and description of purpose be included on the
same "printed page" as the copyright notice for easier
identification within third-party archives.
Copyright 2025, The mjlab Developers
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at
http://www.apache.org/licenses/LICENSE-2.0
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
+40
View File
@@ -0,0 +1,40 @@
# Apptronik Apollo
## Source and checkpoint
- Model: `/home/cen/Embodied_Workspace/PlatformProject/apptronik_apollo/apptronik_apollo.xml` and its STL assets (Apache-2.0, copyright Apptronik; see `LICENSES/apollo.txt`).
- Training: `/home/cen/Embodied_Workspace/Mujoco_Projects/mjlab` (see `LICENSES/mjlab.txt`).
- Explicitly selected v0.6 checkpoint: `logs/rsl_rl/apollo_flat_upright_v3/2026-11-02_17-39-21_apollo_upright_v3/model_9999.pt`.
- Browser policy: corresponding run's ONNX, copied to `policies/walk.onnx`. Import verifies exported MLP weights, observation mean and folded normalization standard deviation against the checkpoint, not just its file timestamp.
- `source-manifest.json` records checkpoint SHA-256, copied file hashes and training configuration hashes. No runtime access to mjlab is required.
## Contract and modifications
The 108 observations are body-frame linear velocity (base IMU velocimeter), angular velocity (gyro), projected gravity, 32 relative joint positions, 32 joint velocities, 32 previous actions and the three-component velocity command, in that order. Normalization is embedded in ONNX. Action targets are stand-pose defaults plus per-joint residual scales. Head/arm residual scales are zero, as in the training configuration; their original actuators hold the pose.
`import-apollo.py` applies mjlab Entity's collision editors and preserves native actuator gains, damping, armature, force limits and stand defaults. Generated `model.xml` is modified from upstream: named visual geoms, base-aligned IMU, foot sites, training collision filtering, no standalone lights/keyframes, explicit task solver options (5 ms timestep, four substeps per control tick, Newton/pyramidal/implicitfast, 20 iterations). The newer native MuJoCo serializer's `input="pos"` actuator keyword is omitted for WASM compatibility; the equivalent general actuator's position gain/bias is retained. Stand defaults are read from mjlab's calibrated upright-v3 keyframe, not the original deeply flexed asset pose or rounded ONNX metadata. The importer cross-checks the pose against ONNX metadata. Knees default to 0.35 rad, spawn root height is 1.1008385 m (15 mm clearance), grounded target height is 1.0858385 m, and the fall threshold is 0.98 m, matching upright-v3 training.
The shared platform composer creates the floor with explicit collision bits and group, overriding Apollo's visual-only global geom defaults. The arena uses the platform's normal robot-height scaling, interactive objects and camera controls. No pose animation or kinematic walking is used.
Controls: W/S = +0.5/-0.3 m/s forward/backward; A/D = +/-0.4 rad/s yaw command; arrows are aliases. Key release zeros the command. Space resets; Escape pauses; C restores chase camera.
## Re-import
From the mjlab directory:
```sh
uv run python /home/cen/Embodied_Workspace/Mujoco_Projects/microduck-simulator/app/scripts/import-apollo.py . \
--checkpoint-file logs/rsl_rl/apollo_flat_upright_v3/2026-11-02_17-39-21_apollo_upright_v3/model_9999.pt
```
Use `--checkpoint-file` to reproduce this release exactly. Without it, the importer selects the newest checkpoint in `apollo_flat_upright_v3` by modification time (iteration breaks ties). Its run must already contain a matching ONNX export newer than the checkpoint; otherwise import fails rather than silently using stale weights. Set `APOLLO_ASSETS_ROOT` if the source model lives elsewhere.
## Verification
```sh
cd app
npm run validate:packages
npx playwright test apollo.spec.js
```
Real browser MuJoCo WASM and ONNX inference test: 108 observations / 32 actions, WASD commands and key release, floor collision bits, five independent five-second finite-state rollouts, reset, and switching between Apollo/T1/RP1/Microban. Upright-v3 rollouts with props parked: no falls; forward x +2.258 m, backward x -1.174 m, left yaw +1.158 rad, right yaw -1.725 rad, standing height 1.086 m. Tests also require final root height above 1.04 m in every trial. This is browser simulation acceptance, not a claim of perfect velocity tracking or hardware equivalence. Screenshot is generated in Playwright's test-results directory.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
+365
View File
@@ -0,0 +1,365 @@
<mujoco model="apptronik_apollo">
<!-- Modified for browser simulation by import-apollo.py; see SOURCE.md. -->
<compiler angle="radian" meshdir="assets" texturedir="assets" />
<default>
<joint limited="true" />
<geom contype="0" conaffinity="0" condim="1" group="1" solref="0.005" margin="0.0005" />
<light diffuse="0.8 0.8 0.8" />
<equality solref="0.005" solimp="0.99 0.999 1e-05" />
<default class="active_adhesion">
<geom margin="0.01" gap="0.01" rgba="0.8 0.5 0.5 1" />
</default>
<default class="visual">
<geom type="mesh" />
<default class="visual_dark">
<geom rgba="0.0980392 0.0980392 0.0980392 1" />
</default>
<default class="visual_light">
<geom rgba="0.960784 0.960784 0.952941 1" />
</default>
</default>
<default class="collision">
<geom group="3" rgba="0 1 0 0.4" />
</default>
</default>
<asset>
<mesh name="neck_mount_fix_link" file="neck_mount_fix_link.stl" />
<mesh name="neck_yaw_link" file="neck_yaw_link.stl" />
<mesh name="afh_2_1_link" file="afh_2_1_link.stl" />
<mesh name="torso_link" content_type="model/stl" file="torso_link.stl" />
<mesh name="battery_mount_fix" file="battery_mount_fix.stl" />
<mesh name="torso_pitch_link" file="torso_pitch_link.stl" />
<mesh name="torso_roll_link" file="torso_roll_link.stl" />
<mesh name="pelvis_link" file="pelvis_link.stl" />
<mesh name="l_shoulder_aa_link" file="l_shoulder_aa_link.stl" />
<mesh name="l_shoulder_ie_link" file="l_shoulder_ie_link.stl" />
<mesh name="l_shoulder_fe_link" file="l_shoulder_fe_link.stl" />
<mesh name="l_elbow_fe_link" file="l_elbow_fe_link.stl" />
<mesh name="l_wrist_roll_link" file="l_wrist_roll_link.stl" />
<mesh name="l_wrist_yaw_link" file="l_wrist_yaw_link.stl" />
<mesh name="l_wrist_pitch_link" file="l_wrist_pitch_link.stl" />
<mesh name="wrist_adapter" file="ability_hand/wrist_adapter.STL" />
<mesh name="wristmesh" file="ability_hand/wristmesh.STL" />
<mesh name="FB_palm_ref" file="ability_hand/FB_palm_ref.STL" />
<mesh name="idx-F1" file="ability_hand/idx-F1.STL" />
<mesh name="idx-F2" file="ability_hand/idx-F2.STL" />
<mesh name="thumb-F1" file="ability_hand/thumb-F1.STL" />
<mesh name="thumb-F2-left" file="ability_hand/thumb-F2-left.STL" />
<mesh name="r_shoulder_aa_link" file="r_shoulder_aa_link.stl" />
<mesh name="r_shoulder_ie_link" file="r_shoulder_ie_link.stl" />
<mesh name="r_shoulder_fe_link" file="r_shoulder_fe_link.stl" />
<mesh name="r_elbow_fe_link" file="r_elbow_fe_link.stl" />
<mesh name="r_wrist_roll_link" file="r_wrist_roll_link.stl" />
<mesh name="r_wrist_yaw_link" file="r_wrist_yaw_link.stl" />
<mesh name="r_wrist_pitch_link" file="r_wrist_pitch_link.stl" />
<mesh name="FB_palm_ref_MIR" file="ability_hand/FB_palm_ref_MIR.STL" />
<mesh name="thumb-F1-MIR" file="ability_hand/thumb-F1-MIR.STL" />
<mesh name="thumb-F2-right" file="ability_hand/thumb-F2-right.STL" />
<mesh name="l_hip_ie_link" file="l_hip_ie_link.stl" />
<mesh name="l_hip_aa_link" file="l_hip_aa_link.stl" />
<mesh name="l_hip_fe_link" file="l_hip_fe_link.stl" />
<mesh name="l_knee_fe_link" file="l_knee_fe_link.stl" />
<mesh name="l_ankle_ie_link" file="l_ankle_ie_link.stl" />
<mesh name="l_foot_link" file="l_foot_link.stl" />
<mesh name="r_hip_ie_link" file="r_hip_ie_link.stl" />
<mesh name="r_hip_aa_link" file="r_hip_aa_link.stl" />
<mesh name="r_hip_fe_link" file="r_hip_fe_link.stl" />
<mesh name="r_knee_fe_link" file="r_knee_fe_link.stl" />
<mesh name="r_ankle_ie_link" file="r_ankle_ie_link.stl" />
<mesh name="r_foot_link" file="r_foot_link.stl" />
</asset>
<worldbody>
<body name="base_link" pos="0 0 1.0813">
<inertial pos="-0.0466459 -2.50736e-05 -0.0721416" quat="0.661991 0.662086 0.248682 -0.248127" mass="7.43582" diaginertia="0.0633318 0.0512326 0.0281854" />
<joint name="floating_base" type="free" />
<geom name="base_link_visual_0" class="visual_light" pos="0 0 0" quat="1 0 0 0" mesh="pelvis_link" />
<geom name="collision_capsule_body_pelvis" class="collision" size="0.1 0.09" pos="-0.06 0 -0.08" quat="0.707388 0.706825 0 0" type="capsule" conaffinity="1" friction="0.8" margin="0" />
<site name="base_imu" size="0.01" />
<camera name="track" quat="0.791164 0.554139 0.148485 0.211997" pos="1.69 -2.93 1.232" mode="trackcom" />
<body name="torso_oak_d_pro_w_rear_frame_link" pos="-0.164306 5e-08 0.0140886" quat="0.000689634 -0.500011 0.000398172 0.866019">
<inertial pos="0 0 0" mass="0.115" diaginertia="0.000144423 3.89011e-05 0.000126395" />
<camera name="torso_oak_d_pro_w_rear" quat="0.500398 0.5 -0.499602 -0.5" fovy="70" />
</body>
<body name="torso_oak_d_pro_w_front_frame_link" pos="0.087306 -5e-08 0.0480886" quat="0.866025 0 0.500001 0">
<inertial pos="0 0 0" mass="0.115" diaginertia="0.000144423 3.89011e-05 0.000126395" />
<camera name="torso_oak_d_pro_w_front" quat="0.500398 0.5 -0.499602 -0.5" fovy="70" />
</body>
<body name="torso_roll_link" pos="0.03 0 0.03125">
<inertial pos="-0.0337439 7.47864e-09 -0.0199137" quat="0.5297 0.511275 -0.470427 0.486532" mass="0.824114" diaginertia="0.00159405 0.00123171 0.000760001" />
<joint name="torso_yaw" range="-0.829031 0.829031" actuatorfrcrange="-120 120" armature="0.231424" damping="200" frictionloss="0.7" axis="0 0 -1" />
<geom name="torso_roll_link_visual_0" class="visual_dark" quat="1 0 0 0" mesh="torso_roll_link" />
<body name="torso_pitch_link">
<inertial pos="-0.0120797 5.39687e-05 -0.00065376" quat="0.49026 0.489868 -0.50995 0.509534" mass="0.313137" diaginertia="0.000415493 0.000345069 9.17604e-05" />
<joint name="torso_roll" range="-0.20944 0.20944" actuatorfrcrange="-414 414" armature="1.56493" damping="165" frictionloss="4.53" axis="1 0 0" />
<geom name="torso_pitch_link_visual_0" class="visual_dark" quat="1 0 0 0" mesh="torso_pitch_link" />
<body name="torso_link">
<inertial pos="-0.0484952 0.00143204 0.185535" quat="0.702792 0.0157426 -6.68569e-05 0.711221" mass="19.3408" diaginertia="0.303595 0.272287 0.22035" />
<joint name="torso_pitch" range="-0.305433 1.35263" actuatorfrcrange="-315 315" armature="0.970692" damping="142" frictionloss="13.15" axis="0 1 0" />
<geom name="torso_link_visual_0" class="visual_light" pos="0 0 0" quat="1 0 0 0" mesh="torso_link" />
<geom name="collision_cylinder_torso" class="collision" size="0.16 0.05" pos="-0.045 0 0.2" type="capsule" conaffinity="1" friction="0.8" margin="0" />
<geom name="torso_link_visual_2" class="visual_dark" pos="0 0 0" quat="1 0 0 0" mesh="neck_mount_fix_link" />
<geom name="torso_link_visual_3" class="visual_dark" pos="0 0 0" quat="1 0 0 0" mesh="battery_mount_fix" />
<site name="imu" pos="0.067 0 0.124874" quat="9.38184e-07 0.707105 9.38187e-07 -0.707108" />
<body name="neck_yaw_link" pos="-0.0303298 0 0.32841" quat="0.991445 0 0.130526 0">
<inertial pos="-0.0110616 0.000438589 0.066541" quat="0.715974 -0.0707674 -0.0795701 0.689958" mass="0.707753" diaginertia="0.00278981 0.002738 0.00071974" />
<joint name="neck_yaw" range="-1.65806 1.65806" actuatorfrcrange="-10.6 10.6" armature="0.0368" damping="15" />
<geom name="neck_yaw_link_visual_0" class="visual_dark" quat="1 0 0 0" mesh="neck_yaw_link" />
<body name="neck_roll_link" pos="-0.025 0 0.2" quat="1.32679e-06 -1 0 0">
<inertial pos="9.66662e-05 -0.000233736 -0.000463806" quat="0.000426355 0.703652 0.00178423 0.710542" mass="0.0189001" diaginertia="1.89727e-06 1.24254e-06 1.0322e-06" />
<joint name="neck_roll" range="-0.785398 0.785398" actuatorfrcrange="-34.2 34.2" armature="0.00305531" damping="3" axis="1 0 0" />
<body name="neck_pitch_link" quat="1.17527e-06 0.885799 6.15724e-07 -0.464069">
<inertial pos="0.0412474 -0.00081671 0.0152493" quat="0.350337 0.613318 0.617311 0.346457" mass="1.78084" diaginertia="0.0135957 0.0109432 0.00697581" />
<joint name="neck_pitch" range="-0.261799 0.523599" actuatorfrcrange="-34.2 34.2" armature="0.00256298" damping="3" axis="0 1 0" />
<geom name="neck_pitch_link_visual_0" class="visual_light" pos="0 0 0" quat="0.938794 0 0.344479 0" mesh="afh_2_1_link" />
</body>
</body>
</body>
<body name="l_shoulder_aa_link" pos="-0.05 0.2 0.32">
<inertial pos="0.000169922 0.0025177 -0.0190065" quat="0.710165 -0.0144759 0.023056 0.703508" mass="0.0980048" diaginertia="4.61576e-05 3.97046e-05 1.43504e-05" />
<joint name="l_shoulder_aa" range="-0.122173 1.6057" actuatorfrcrange="-78 78" armature="0.0982771" damping="26" frictionloss="2" axis="1 0 0" />
<geom name="l_shoulder_aa_link_visual_0" class="visual_dark" quat="1 0 0 0" mesh="l_shoulder_aa_link" />
<body name="l_shoulder_ie_link">
<inertial pos="-0.00799772 0.0154729 -0.0231746" quat="0.0914838 0.586016 -0.671384 0.444364" mass="0.451281" diaginertia="0.00069896 0.000628486 0.000278066" />
<joint name="l_shoulder_ie" range="-0.471239 0.471239" actuatorfrcrange="-67 67" armature="0.0823104" damping="35" frictionloss="1.48" />
<geom name="l_shoulder_ie_link_visual_0" class="visual_dark" quat="1 0 0 0" mesh="l_shoulder_ie_link" />
<body name="l_shoulder_fe_link" pos="0.0104822 0.03912 0" quat="0.991445 0 0 -0.130526">
<inertial pos="0.00453031 0.00134632 -0.143556" quat="0.707095 0.0140891 -0.00274087 0.706973" mass="3.51271" diaginertia="0.0284589 0.0232812 0.00763948" />
<joint name="l_shoulder_fe" range="-2.18166 0.610865" actuatorfrcrange="-114 114" armature="0.112768" damping="18" frictionloss="1.94" axis="0 1 0" />
<geom name="l_shoulder_fe_link_visual_0" class="visual_light" pos="0 0 0" quat="1 0 0 0" mesh="l_shoulder_fe_link" />
<geom name="collision_capsule_body_l_arm_1" class="collision" size="0.055 0.14" pos="-0.005 0.01 -0.16" type="capsule" contype="8" conaffinity="7" friction="0.8" margin="0" />
<geom name="collision_capsule_body_l_arm_2" class="collision" size="0.055 0.055" pos="0.05 0.005 -0.125" type="capsule" contype="8" conaffinity="7" friction="0.8" margin="0" />
<geom name="collision_capsule_body_l_arm_3" class="collision" size="0.055 0.02" pos="-0.06 0.0025 -0.13" type="capsule" contype="8" conaffinity="7" friction="0.8" margin="0" />
<body name="l_elbow_fe_link" pos="0.025 0 -0.315">
<inertial pos="-0.0407636 -0.000100019 -0.0277184" quat="0.446501 0.557407 0.539502 0.445951" mass="0.947642" diaginertia="0.00105876 0.000944065 0.00076124" />
<joint name="l_elbow_fe" range="-2.61799 0.174533" actuatorfrcrange="-114 114" armature="0.102645" damping="17" frictionloss="2.55" axis="0 1 0" />
<geom name="l_elbow_fe_link_visual_0" class="visual_dark" pos="0 0 0" quat="1 0 0 0" mesh="l_elbow_fe_link" />
<body name="l_wrist_roll_link" pos="-0.04 0 -0.06" quat="0.707105 0 0 0.707108">
<inertial pos="0.0088493 0.000434657 -0.071867" quat="0.701782 -0.0503482 -0.0435035 0.709278" mass="0.694478" diaginertia="0.00294512 0.00284275 0.000723985" />
<joint name="l_wrist_roll" range="-1.65806 1.65806" actuatorfrcrange="-10.6 10.6" armature="0.0368" damping="15" />
<geom name="l_wrist_roll_link_visual_0" class="visual_light" pos="0 0 0" quat="1 0 0 0" mesh="l_wrist_roll_link" />
<geom name="collision_cylinder_l_arm" class="collision" size="0.04755 0.1076" pos="0 0 -0.1" type="capsule" contype="8" conaffinity="7" friction="0.8" margin="0" />
<body name="l_wrist_yaw_link" pos="0.025 0 -0.2">
<inertial pos="-9.66662e-05 0.000233736 -0.000463806" quat="-0.00178423 0.710542 -0.000426355 0.703652" mass="0.0189001" diaginertia="1.89727e-06 1.24254e-06 1.0322e-06" />
<joint name="l_wrist_yaw" range="-0.698 0.698" actuatorfrcrange="-34.2 34.2" armature="0.00329371" damping="3" axis="1 0 0" />
<geom name="l_wrist_yaw_link_visual_0" class="visual_dark" quat="1 0 0 0" mesh="l_wrist_yaw_link" />
<body name="l_wrist_pitch_link" quat="0.707105 0 0 -0.707108">
<inertial pos="0.0071177 -0.038442 -0.0669519" quat="0.614861 0.00892233 0.0323416 0.787921" mass="0.806883" diaginertia="0.00459474 0.00452816 0.00206219" />
<joint name="l_wrist_pitch" range="-0.75 1.588" actuatorfrcrange="-34.2 34.2" armature="0.00288074" damping="3" axis="1 0 0" />
<geom name="l_wrist_pitch_link_visual_0" class="visual_dark" quat="1 0 0 0" mesh="l_wrist_pitch_link" />
<geom name="wrist_adapter_mesh" class="visual_dark" pos="0 -0.0297676 -0.0277946" quat="1 0 0 0" mesh="wrist_adapter" />
<geom name="wrist_mesh" class="visual_dark" pos="0 -0.0343657 -0.0392954" quat="0.130526 0.991445 0 0" mesh="wristmesh" />
<geom name="palm_mesh" class="visual_dark" pos="0.0240477 -0.0463856 -0.0695448" quat="0.00516808 0.0439309 0.130372 0.990478" mesh="FB_palm_ref" />
<geom name="index_mesh_1" class="visual_dark" pos="0.0279216 -0.0502244 -0.134299" quat="0.0355504 -0.725304 0.0429902 0.686165" mesh="idx-F1" />
<frame pos="0.0296624 -0.053984 -0.172686" quat="-0.873305 0.487175 0 0">
<geom name="index_mesh_2" class="visual_dark" quat="0.00654576 -0.722842 0.0735774 0.687054" mesh="idx-F2" />
</frame>
<geom name="middle_mesh_1" class="visual_dark" pos="0.00841718 -0.0488804 -0.137964" quat="0.0114639 -0.705931 0.0210022 0.707876" mesh="idx-F1" />
<frame pos="0.00814998 -0.0526507 -0.176389" quat="-0.873305 0.487175 0 0">
<geom name="middle_mesh_2" class="visual_dark" quat="-0.018436 -0.704415 0.0507911 0.707729" mesh="idx-F2" />
</frame>
<geom name="ring_mesh_1" class="visual_dark" pos="-0.0117529 -0.049354 -0.135394" quat="-0.0126044 -0.685391 -0.0036555 0.728057" mesh="idx-F1" />
<frame pos="-0.0139914 -0.0531239 -0.173755" quat="-0.873305 0.487175 0 0">
<geom name="ring_mesh_2" class="visual_dark" quat="-0.0433349 -0.684934 0.025288 0.726875" mesh="idx-F2" />
</frame>
<geom name="pinky_mesh_1" class="visual_dark" pos="-0.0308633 -0.0509308 -0.128792" quat="-0.0350419 -0.683018 -0.0274385 0.729045" mesh="idx-F1" />
<frame pos="-0.0330569 -0.0546993 -0.167156" quat="-0.873305 0.487175 0 0">
<geom name="pinky_mesh_2" class="visual_dark" quat="-0.0657942 -0.683567 0.00142601 0.726915" mesh="idx-F2" />
</frame>
<geom name="thumb_mesh_1" class="visual_dark" pos="0.0240477 -0.0463856 -0.0695448" quat="-0.986553 0.125649 -0.0560269 -0.0882391" mesh="thumb-F1" />
<geom name="thumb_mesh_2" class="visual_dark" pos="0.0499644 -0.0457352 -0.0874305" quat="0.500891 -0.859176 -0.0358978 0.0981657" mesh="thumb-F2-left" />
<geom name="collision_l_hand_plate" class="collision" size="0.041 0.02 0.083" pos="0.00749988 -0.025 -0.112" type="box" contype="8" conaffinity="7" friction="0.8" margin="0" />
</body>
</body>
</body>
</body>
</body>
</body>
</body>
<body name="r_shoulder_aa_link" pos="-0.05 -0.2 0.32">
<inertial pos="0.000169922 0.0025177 -0.0190065" quat="0.710165 -0.0144759 0.023056 0.703508" mass="0.0980048" diaginertia="4.61576e-05 3.97046e-05 1.43504e-05" />
<joint name="r_shoulder_aa" range="-1.6057 0.122173" actuatorfrcrange="-78 78" armature="0.098338" damping="26" frictionloss="2" axis="1 0 0" />
<geom name="r_shoulder_aa_link_visual_0" class="visual_dark" quat="1 0 0 0" mesh="r_shoulder_aa_link" />
<body name="r_shoulder_ie_link">
<inertial pos="-0.00800045 -0.0154894 -0.0231814" quat="0.583857 0.0885889 -0.445504 0.672897" mass="0.451079" diaginertia="0.000700361 0.00062725 0.000277886" />
<joint name="r_shoulder_ie" range="-0.471239 0.471239" actuatorfrcrange="-67 67" armature="0.0823191" damping="35" frictionloss="1.48" />
<geom name="r_shoulder_ie_link_visual_0" class="visual_dark" quat="1 0 0 0" mesh="r_shoulder_ie_link" />
<body name="r_shoulder_fe_link" pos="0.0104822 -0.03912 0" quat="0.991445 0 0 0.130526">
<inertial pos="0.00420186 -0.00125575 -0.142983" quat="0.7032 -0.00225527 0.011754 0.710892" mass="3.50359" diaginertia="0.0282519 0.0230724 0.00763457" />
<joint name="r_shoulder_fe" range="-2.18166 0.610865" actuatorfrcrange="-114 114" armature="0.112768" damping="18" frictionloss="1.94" axis="0 1 0" />
<geom name="r_shoulder_fe_link_visual_0" class="visual_light" pos="0 0 0" quat="1 0 0 0" mesh="r_shoulder_fe_link" />
<geom name="collision_capsule_body_r_arm_1" class="collision" size="0.055 0.14" pos="-0.005 -0.01 -0.16" type="capsule" contype="8" conaffinity="7" friction="0.8" margin="0" />
<geom name="collision_capsule_body_r_arm_2" class="collision" size="0.055 0.055" pos="0.05 -0.005 -0.125" type="capsule" contype="8" conaffinity="7" friction="0.8" margin="0" />
<geom name="collision_capsule_body_r_arm_3" class="collision" size="0.055 0.02" pos="-0.06 -0.0025 -0.13" type="capsule" contype="8" conaffinity="7" friction="0.8" margin="0" />
<body name="r_elbow_fe_link" pos="0.025 0 -0.315">
<inertial pos="-0.0407591 0.000134025 -0.0277185" quat="0.458533 0.527215 0.568785 0.433907" mass="0.947653" diaginertia="0.00105948 0.000943838 0.000761313" />
<joint name="r_elbow_fe" range="-2.61799 0.174533" actuatorfrcrange="-114 114" armature="0.102645" damping="17" frictionloss="2.55" axis="0 1 0" />
<geom name="r_elbow_fe_link_visual_0" class="visual_dark" pos="0 0 0" quat="1 0 0 0" mesh="r_elbow_fe_link" />
<body name="r_wrist_roll_link" pos="-0.04 0 -0.06" quat="0.707105 0 0 -0.707108">
<inertial pos="0.00887138 -0.000263913 -0.0718159" quat="0.698707 -0.0449538 -0.0486448 0.712336" mass="0.695137" diaginertia="0.00294733 0.00284457 0.000724756" />
<joint name="r_wrist_roll" range="-1.65806 1.65806" actuatorfrcrange="-10.6 10.6" armature="0.0368" damping="15" />
<geom name="r_wrist_roll_link_visual_0" class="visual_light" pos="0 0 0" quat="1 0 0 0" mesh="r_wrist_roll_link" />
<geom name="collision_cylinder_r_arm" class="collision" size="0.04755 0.1076" pos="0 0 -0.1" type="capsule" contype="8" conaffinity="7" friction="0.8" margin="0" />
<body name="r_wrist_yaw_link" pos="0.025 0 -0.2" quat="1.32679e-06 0 0 -1">
<inertial pos="9.66662e-05 0.000233736 -0.000463806" quat="-0.00044002 0.703652 -0.00179716 0.710542" mass="0.0189001" diaginertia="1.89727e-06 1.24254e-06 1.0322e-06" />
<joint name="r_wrist_yaw" range="-0.698 0.698" actuatorfrcrange="-34.2 34.2" armature="0.0032937" damping="3" axis="1 0 0" />
<geom name="r_wrist_yaw_link_visual_0" class="visual_dark" quat="1 0 0 0" mesh="r_wrist_yaw_link" />
<body name="r_wrist_pitch_link" quat="0.707105 0 0 -0.707108">
<inertial pos="0.0181202 0.0388729 -0.0688824" quat="0.630263 -0.0221958 -0.0501406 0.774443" mass="0.806683" diaginertia="0.00526208 0.00452156 0.0026899" />
<joint name="r_wrist_pitch" range="-1.588 0.75" actuatorfrcrange="-34.2 34.2" armature="0.00288075" damping="3" axis="1 0 0" />
<geom name="r_wrist_pitch_link_visual_0" class="visual_dark" quat="1 0 0 0" mesh="r_wrist_pitch_link" />
<geom name="r_wrist_pitch_link_visual_1" class="visual_dark" pos="0 0.0297676 -0.0277946" quat="1.32679e-06 0 0 1" mesh="wrist_adapter" />
<geom name="r_wrist_pitch_link_visual_2" class="visual_dark" pos="1.22016e-08 0.0343657 -0.0392954" quat="1.73181e-07 1.31544e-06 0.991445 0.130526" mesh="wristmesh" />
<geom name="r_wrist_pitch_link_visual_3" class="visual_dark" pos="0.0240477 0.0463855 -0.0695448" quat="0.990478 0.130372 0.0439307 0.00516677" mesh="FB_palm_ref_MIR" />
<geom name="r_wrist_pitch_link_visual_4" class="visual_dark" pos="0.0279216 0.0502243 -0.134299" quat="0.725304 -0.0355495 0.686165 0.0429892" mesh="idx-F1" />
<frame pos="0.0296625 0.053984 -0.172686" quat="-0.873305 -0.487175 0 0">
<geom name="r_wrist_pitch_link_visual_5" class="visual_dark" quat="0.722842 -0.00654484 0.687054 0.0735764" mesh="idx-F2" />
</frame>
<geom name="r_wrist_pitch_link_visual_6" class="visual_dark" pos="0.00841723 0.0488804 -0.137964" quat="0.705931 -0.0114629 0.707876 0.0210013" mesh="idx-F1" />
<frame pos="0.00815005 0.0526507 -0.176389" quat="-0.873305 -0.487175 0 0">
<geom name="r_wrist_pitch_link_visual_7" class="visual_dark" quat="0.704415 0.018437 0.707729 0.0507901" mesh="idx-F2" />
</frame>
<geom name="r_wrist_pitch_link_visual_8" class="visual_dark" pos="-0.0117529 0.0493541 -0.135394" quat="0.685391 0.0126053 0.728057 -0.00365641" mesh="idx-F1" />
<frame pos="-0.0139913 0.053124 -0.173755" quat="-0.873305 -0.487175 0 0">
<geom name="r_wrist_pitch_link_visual_9" class="visual_dark" quat="0.684934 0.0433359 0.726875 0.0252871" mesh="idx-F2" />
</frame>
<geom name="r_wrist_pitch_link_visual_10" class="visual_dark" pos="-0.0308632 0.0509308 -0.128792" quat="0.683018 0.0350429 0.729045 -0.0274394" mesh="idx-F1" />
<frame pos="-0.0330568 0.0546993 -0.167156" quat="-0.873305 -0.487175 0 0">
<geom name="r_wrist_pitch_link_visual_11" class="visual_dark" quat="0.683567 0.0657951 0.726915 0.00142511" mesh="idx-F2" />
</frame>
<geom name="r_wrist_pitch_link_visual_12" class="visual_dark" pos="0.0240477 0.0463855 -0.0695448" quat="-0.125649 0.986553 -0.0882404 -0.0560268" mesh="thumb-F1-MIR" />
<geom name="r_wrist_pitch_link_visual_13" class="visual_dark" pos="0.0499644 0.0457351 -0.0874305" quat="-0.859175 0.500891 -0.0981665 0.0358989" mesh="thumb-F2-right" />
<geom name="collision_r_hand_plate" class="collision" size="0.041 0.02 0.083" pos="0.00749988 0.025 -0.112" type="box" contype="8" conaffinity="7" friction="0.8" margin="0" />
</body>
</body>
</body>
</body>
</body>
</body>
</body>
</body>
</body>
</body>
<body name="l_hip_ie_link" pos="-0.02 0.11 -0.16875" quat="0.957662 0.126078 -0.256605 0.0337826">
<inertial pos="-0.0275726 0.000155527 0.0265849" quat="-0.0182885 0.43376 -0.029543 0.900358" mass="1.49972" diaginertia="0.00446385 0.00343018 0.00205254" />
<joint name="l_hip_ie" range="-0.567232 1.09083" actuatorfrcrange="-120 120" armature="0.231424" damping="171" frictionloss="7.95" />
<geom name="l_hip_ie_link_visual_0" class="visual_dark" quat="1 0 0 0" mesh="l_hip_ie_link" />
<body name="l_hip_aa_link">
<inertial pos="0.000193883 0.00068757 0.00240456" quat="0.488822 0.400587 -0.484645 0.604733" mass="0.429858" diaginertia="0.000370659 0.000322451 0.000129015" />
<joint name="l_hip_aa" range="-0.218166 0.741765" actuatorfrcrange="-494 494" armature="0.399395" damping="153" frictionloss="7" axis="1 0 0" />
<geom name="l_hip_aa_link_visual_0" class="visual_dark" quat="1 0 0 0" mesh="l_hip_aa_link" />
<body name="l_hip_fe_link" quat="0.957662 -0.126079 0.256605 -0.0337828">
<inertial pos="0.0065559 0.00346415 -0.207233" quat="0.864334 0.00804176 0.0489557 0.500466" mass="10.7833" diaginertia="0.124113 0.120556 0.0499696" />
<joint name="l_hip_fe" range="-1.85005 0.476475" actuatorfrcrange="-342 342" armature="0.187096" damping="92" frictionloss="21.06" axis="0 1 0" />
<geom name="l_hip_fe_link_visual_0" class="visual_light" pos="0 0 0" quat="1 0 0 0" mesh="l_hip_fe_link" />
<geom name="collision_capsule_body_l_thigh" class="collision" size="0.105 0.1" pos="0 0.005 -0.215" type="capsule" contype="2" conaffinity="13" friction="0.8" margin="0" />
<body name="l_knee_fe_link" pos="-0.05 0 -0.425">
<inertial pos="0.043534 0.00505054 -0.152417" quat="0.917057 0.00954505 0.0113608 0.398479" mass="4.57515" diaginertia="0.0639569 0.0630895 0.0108036" />
<joint name="l_knee_fe" range="0 2.61799" actuatorfrcrange="-336 336" armature="0.292264" damping="46" frictionloss="7.7" axis="0 1 0" />
<geom name="l_knee_fe_link_visual_0" class="visual_light" pos="0 0 0" quat="1 0 0 0" mesh="l_knee_fe_link" />
<geom name="collision_capsule_body_l_shin" class="collision" size="0.071 0.15" pos="0.0476 0.0061 -0.19" quat="0.998048 0 0.0624593 0" type="capsule" contype="2" conaffinity="13" friction="0.8" margin="0" />
<body name="l_ankle_ie_link" pos="0.05 0 -0.425" quat="0.987672 0.0864102 -0.130029 0.0113761">
<inertial pos="0.00995038 9.93144e-05 0.000832075" quat="0.692933 0.691803 0.145013 -0.14221" mass="0.12478" diaginertia="4.80038e-05 4.18024e-05 1.66067e-05" />
<joint name="l_ankle_ie" range="-0.654498 0.305433" actuatorfrcrange="-120 120" armature="0.121874" damping="11" frictionloss="0.89" axis="1 0 0" />
<geom name="l_ankle_ie_link_visual_0" class="visual_dark" quat="1 0 0 0" mesh="l_ankle_ie_link" />
<body name="l_foot_link" quat="0.965926 0 0.258819 0">
<inertial pos="0.056607 -0.00200339 -0.0304112" quat="0.432893 0.587516 0.510475 0.454802" mass="1.1808" diaginertia="0.0088235 0.00838261 0.00210633" />
<joint name="l_ankle_pd" range="-1.5708 0.436332" actuatorfrcrange="-150 150" armature="0.205456" damping="21" frictionloss="1.87" axis="0 1 0" />
<geom name="l_foot_link_visual_0" class="visual_dark" pos="0 0 0" quat="1 0 0 0" mesh="l_foot_link" />
<geom name="collision_l_sole" class="collision" size="0.1 0.0425 0.009" pos="0.0646931 -0.00550529 -0.03843" quat="0.987443 -0.0797175 -0.132426 -0.0326442" type="box" contype="2" conaffinity="13" condim="3" priority="1" friction="0.8" margin="0" />
<frame pos="0.0614891 -0.00342931 -0.0265273" quat="0.987443 -0.0797175 -0.132426 -0.0326442">
<site name="l_foot_fr" pos="0.1 0.0425 -0.0215" />
<site name="l_foot_br" pos="-0.1 0.0425 -0.0215" />
<site name="l_foot_fl" pos="0.1 -0.0425 -0.0215" />
<site name="l_foot_bl" pos="-0.1 -0.0425 -0.0215" />
</frame>
<site name="l_foot" size="0.01" pos="0.067 -0.007 -0.047" />
</body>
</body>
</body>
</body>
</body>
</body>
<body name="r_hip_ie_link" pos="-0.02 -0.11 -0.16875" quat="0.957662 -0.126078 -0.256605 -0.0337826">
<inertial pos="-0.0275542 -0.000156081 0.0265869" quat="0.0347714 0.433956 0.0469856 0.899036" mass="1.49972" diaginertia="0.00446281 0.00343307 0.00204473" />
<joint name="r_hip_ie" range="-1.09083 0.567232" actuatorfrcrange="-120 120" armature="0.231424" damping="171" frictionloss="7.95" />
<geom name="r_hip_ie_link_visual_0" class="visual_dark" quat="1 0 0 0" mesh="r_hip_ie_link" />
<body name="r_hip_aa_link">
<inertial pos="0.000193891 -0.000687548 0.00240456" quat="0.469753 0.583843 -0.513458 0.418115" mass="0.429858" diaginertia="0.000369398 0.000323696 0.000129031" />
<joint name="r_hip_aa" range="-0.741765 0.218166" actuatorfrcrange="-494 494" armature="0.399396" damping="153" frictionloss="7" axis="1 0 0" />
<geom name="r_hip_aa_link_visual_0" class="visual_dark" quat="1 0 0 0" mesh="r_hip_aa_link" />
<body name="r_hip_fe_link" quat="0.957662 0.126079 0.256605 0.0337828">
<inertial pos="0.00662296 -0.00380822 -0.207131" quat="0.493002 0.049361 0.00782621 0.868591" mass="10.7924" diaginertia="0.124314 0.120603 0.0499318" />
<joint name="r_hip_fe" range="-1.85005 0.476475" actuatorfrcrange="-342 342" armature="0.187096" damping="92" frictionloss="21.06" axis="0 1 0" />
<geom name="r_hip_fe_link_visual_0" class="visual_light" pos="0 0 0" quat="1 0 0 0" mesh="r_hip_fe_link" />
<geom name="collision_capsule_body_r_thigh" class="collision" size="0.105 0.1" pos="0 -0.005 -0.215" type="capsule" contype="4" conaffinity="11" friction="0.8" margin="0" />
<body name="r_knee_fe_link" pos="-0.05 0 -0.425">
<inertial pos="0.0432854 -0.00516245 -0.152442" quat="0.399848 0.0111994 0.00899692 0.916469" mass="4.57434" diaginertia="0.063915 0.0630953 0.0107961" />
<joint name="r_knee_fe" range="0 2.61799" actuatorfrcrange="-336 336" armature="0.292264" damping="46" frictionloss="7.7" axis="0 1 0" />
<geom name="r_knee_fe_link_visual_0" class="visual_light" pos="0 0 0" quat="1 0 0 0" mesh="r_knee_fe_link" />
<geom name="collision_capsule_body_r_shin" class="collision" size="0.071 0.15" pos="0.0476 0.0061 -0.19" quat="0.998048 0 0.0624593 0" type="capsule" contype="4" conaffinity="11" friction="0.8" margin="0" />
<body name="r_ankle_ie_link" pos="0.05 0 -0.425" quat="0.987672 -0.0864102 -0.130029 -0.0113761">
<inertial pos="0.00995037 -9.921e-05 0.000832071" quat="0.69181 0.692928 0.142207 -0.145006" mass="0.12478" diaginertia="4.80035e-05 4.18021e-05 1.66065e-05" />
<joint name="r_ankle_ie" range="-0.305433 0.654498" actuatorfrcrange="-120 120" armature="0.121874" damping="11" frictionloss="0.89" axis="1 0 0" />
<geom name="r_ankle_ie_link_visual_0" class="visual_dark" quat="1 0 0 0" mesh="r_ankle_ie_link" />
<body name="r_foot_link" quat="0.965926 0 0.258819 0">
<inertial pos="0.056607 0.00200339 -0.0304112" quat="0.414902 0.606299 0.487904 0.471389" mass="1.1808" diaginertia="0.00883289 0.00837251 0.00210704" />
<joint name="r_ankle_pd" range="-1.5708 0.436332" actuatorfrcrange="-150 150" armature="0.205456" damping="21" frictionloss="1.87" axis="0 1 0" />
<geom name="r_foot_link_visual_0" class="visual_dark" pos="0 0 0" quat="1 0 0 0" mesh="r_foot_link" />
<geom name="collision_r_sole" class="collision" size="0.1 0.0425 0.009" pos="0.0646931 0.00550529 -0.03843" quat="0.987443 0.0797175 -0.132426 0.0326442" type="box" contype="4" conaffinity="11" condim="3" priority="1" friction="0.8" margin="0" />
<frame pos="0.0614891 0.00342931 -0.0265273" quat="0.987443 0.0797175 -0.132426 0.0326442">
<site name="r_foot_fr" pos="0.1 0.0425 -0.0215" />
<site name="r_foot_br" pos="-0.1 0.0425 -0.0215" />
<site name="r_foot_fl" pos="0.1 -0.0425 -0.0215" />
<site name="r_foot_bl" pos="-0.1 -0.0425 -0.0215" />
</frame>
<site name="r_foot" size="0.01" pos="0.067 0.007 -0.047" />
</body>
</body>
</body>
</body>
</body>
</body>
</body>
<body name="world_link" />
</worldbody>
<actuator>
<general name="torso_yaw" joint="torso_yaw" ctrlrange="-0.829031 0.829031" forcerange="-120 120" biastype="affine" gainprm="500" biasprm="0 -500" />
<general name="torso_roll" joint="torso_roll" ctrlrange="-0.20944 0.20944" forcerange="-414 414" biastype="affine" gainprm="2052" biasprm="0 -2052" />
<general name="torso_pitch" joint="torso_pitch" ctrlrange="-0.305433 1.35263" forcerange="-315 315" biastype="affine" gainprm="1525" biasprm="0 -1525" />
<general name="neck_yaw" joint="neck_yaw" ctrlrange="-1.65806 1.65806" forcerange="-10.6 10.6" biastype="affine" gainprm="28" biasprm="0 -28" />
<general name="neck_roll" joint="neck_roll" ctrlrange="-0.785398 0.785398" forcerange="-34.2 34.2" biastype="affine" gainprm="9" biasprm="0 -9" />
<general name="neck_pitch" joint="neck_pitch" ctrlrange="-0.261799 0.523599" forcerange="-34.2 34.2" biastype="affine" gainprm="8" biasprm="0 -8" />
<general name="l_shoulder_aa" joint="l_shoulder_aa" ctrlrange="-0.122173 1.6057" forcerange="-78 78" biastype="affine" gainprm="395" biasprm="0 -395" />
<general name="l_shoulder_ie" joint="l_shoulder_ie" ctrlrange="-0.471239 0.471239" forcerange="-67 67" biastype="affine" gainprm="436" biasprm="0 -436" />
<general name="l_shoulder_fe" joint="l_shoulder_fe" ctrlrange="-2.18166 0.610865" forcerange="-114 114" biastype="affine" gainprm="214" biasprm="0 -214" />
<general name="l_elbow_fe" joint="l_elbow_fe" ctrlrange="-2.61799 0.174533" forcerange="-114 114" biastype="affine" gainprm="200" biasprm="0 -200" />
<general name="l_wrist_roll" joint="l_wrist_roll" ctrlrange="-1.65806 1.65806" forcerange="-10.6 10.6" biastype="affine" gainprm="47" biasprm="0 -47" />
<general name="l_wrist_yaw" joint="l_wrist_yaw" ctrlrange="-0.785398 0.785398" forcerange="-34.2 34.2" biastype="affine" gainprm="20" biasprm="0 -20" />
<general name="l_wrist_pitch" joint="l_wrist_pitch" ctrlrange="-0.837758 1.67552" forcerange="-34.2 34.2" biastype="affine" gainprm="18" biasprm="0 -18" />
<general name="r_shoulder_aa" joint="r_shoulder_aa" ctrlrange="-1.6057 0.122173" forcerange="-78 78" biastype="affine" gainprm="395" biasprm="0 -395" />
<general name="r_shoulder_ie" joint="r_shoulder_ie" ctrlrange="-0.471239 0.471239" forcerange="-67 67" biastype="affine" gainprm="436" biasprm="0 -436" />
<general name="r_shoulder_fe" joint="r_shoulder_fe" ctrlrange="-2.18166 0.610865" forcerange="-114 114" biastype="affine" gainprm="214" biasprm="0 -214" />
<general name="r_elbow_fe" joint="r_elbow_fe" ctrlrange="-2.61799 0.174533" forcerange="-114 114" biastype="affine" gainprm="200" biasprm="0 -200" />
<general name="r_wrist_roll" joint="r_wrist_roll" ctrlrange="-1.65806 1.65806" forcerange="-10.6 10.6" biastype="affine" gainprm="47" biasprm="0 -47" />
<general name="r_wrist_yaw" joint="r_wrist_yaw" ctrlrange="-0.785398 0.785398" forcerange="-34.2 34.2" biastype="affine" gainprm="20" biasprm="0 -20" />
<general name="r_wrist_pitch" joint="r_wrist_pitch" ctrlrange="-1.67552 0.837758" forcerange="-34.2 34.2" biastype="affine" gainprm="18" biasprm="0 -18" />
<general name="l_hip_ie" joint="l_hip_ie" ctrlrange="-0.567232 1.09083" forcerange="-120 120" biastype="affine" gainprm="595" biasprm="0 -595" />
<general name="l_hip_aa" joint="l_hip_aa" ctrlrange="-0.218166 0.741765" forcerange="-494 494" biastype="affine" gainprm="1880" biasprm="0 -1880" />
<general name="l_hip_fe" joint="l_hip_fe" ctrlrange="-1.85005 0.476475" forcerange="-342 342" biastype="affine" gainprm="1047" biasprm="0 -1047" />
<general name="l_knee_fe" joint="l_knee_fe" ctrlrange="0 2.61799" forcerange="-336 336" biastype="affine" gainprm="606" biasprm="0 -606" />
<general name="l_ankle_ie" joint="l_ankle_ie" ctrlrange="-0.654498 0.305433" forcerange="-120 120" biastype="affine" gainprm="420" biasprm="0 -420" />
<general name="l_ankle_pd" joint="l_ankle_pd" ctrlrange="-1.5708 0.436332" forcerange="-150 150" biastype="affine" gainprm="882" biasprm="0 -882" />
<general name="r_hip_ie" joint="r_hip_ie" ctrlrange="-1.09083 0.567232" forcerange="-120 120" biastype="affine" gainprm="595" biasprm="0 -595" />
<general name="r_hip_aa" joint="r_hip_aa" ctrlrange="-0.741765 0.218166" forcerange="-494 494" biastype="affine" gainprm="1880" biasprm="0 -1880" />
<general name="r_hip_fe" joint="r_hip_fe" ctrlrange="-1.85005 0.476475" forcerange="-342 342" biastype="affine" gainprm="1047" biasprm="0 -1047" />
<general name="r_knee_fe" joint="r_knee_fe" ctrlrange="0 2.61799" forcerange="-336 336" biastype="affine" gainprm="606" biasprm="0 -606" />
<general name="r_ankle_ie" joint="r_ankle_ie" ctrlrange="-0.305433 0.654498" forcerange="-120 120" biastype="affine" gainprm="420" biasprm="0 -420" />
<general name="r_ankle_pd" joint="r_ankle_pd" ctrlrange="-1.5708 0.436332" forcerange="-150 150" biastype="affine" gainprm="882" biasprm="0 -882" />
</actuator>
<sensor>
<velocimeter site="base_imu" name="imu_lin_vel" />
<gyro site="base_imu" name="imu_ang_vel" />
<subtreeangmom body="base_link" name="root_angmom" />
</sensor>
<option timestep="0.005" integrator="implicitfast" solver="Newton" cone="pyramidal" iterations="20" ls_iterations="20" ccd_iterations="50" />
</mujoco>
Binary file not shown.
+699
View File
@@ -0,0 +1,699 @@
{
"schema": 1,
"id": "apollo",
"name": "Apptronik Apollo",
"family": "apollo",
"adapter": "generic",
"dimensions": {
"height": 1.73
},
"model": "model.xml",
"assets": [
{
"path": "assets/neck_mount_fix_link.stl",
"vfs": "assets/neck_mount_fix_link.stl"
},
{
"path": "assets/neck_yaw_link.stl",
"vfs": "assets/neck_yaw_link.stl"
},
{
"path": "assets/afh_2_1_link.stl",
"vfs": "assets/afh_2_1_link.stl"
},
{
"path": "assets/torso_link.stl",
"vfs": "assets/torso_link.stl"
},
{
"path": "assets/battery_mount_fix.stl",
"vfs": "assets/battery_mount_fix.stl"
},
{
"path": "assets/torso_pitch_link.stl",
"vfs": "assets/torso_pitch_link.stl"
},
{
"path": "assets/torso_roll_link.stl",
"vfs": "assets/torso_roll_link.stl"
},
{
"path": "assets/pelvis_link.stl",
"vfs": "assets/pelvis_link.stl"
},
{
"path": "assets/l_shoulder_aa_link.stl",
"vfs": "assets/l_shoulder_aa_link.stl"
},
{
"path": "assets/l_shoulder_ie_link.stl",
"vfs": "assets/l_shoulder_ie_link.stl"
},
{
"path": "assets/l_shoulder_fe_link.stl",
"vfs": "assets/l_shoulder_fe_link.stl"
},
{
"path": "assets/l_elbow_fe_link.stl",
"vfs": "assets/l_elbow_fe_link.stl"
},
{
"path": "assets/l_wrist_roll_link.stl",
"vfs": "assets/l_wrist_roll_link.stl"
},
{
"path": "assets/l_wrist_yaw_link.stl",
"vfs": "assets/l_wrist_yaw_link.stl"
},
{
"path": "assets/l_wrist_pitch_link.stl",
"vfs": "assets/l_wrist_pitch_link.stl"
},
{
"path": "assets/ability_hand/wrist_adapter.STL",
"vfs": "assets/ability_hand/wrist_adapter.STL"
},
{
"path": "assets/ability_hand/wristmesh.STL",
"vfs": "assets/ability_hand/wristmesh.STL"
},
{
"path": "assets/ability_hand/FB_palm_ref.STL",
"vfs": "assets/ability_hand/FB_palm_ref.STL"
},
{
"path": "assets/ability_hand/idx-F1.STL",
"vfs": "assets/ability_hand/idx-F1.STL"
},
{
"path": "assets/ability_hand/idx-F2.STL",
"vfs": "assets/ability_hand/idx-F2.STL"
},
{
"path": "assets/ability_hand/thumb-F1.STL",
"vfs": "assets/ability_hand/thumb-F1.STL"
},
{
"path": "assets/ability_hand/thumb-F2-left.STL",
"vfs": "assets/ability_hand/thumb-F2-left.STL"
},
{
"path": "assets/r_shoulder_aa_link.stl",
"vfs": "assets/r_shoulder_aa_link.stl"
},
{
"path": "assets/r_shoulder_ie_link.stl",
"vfs": "assets/r_shoulder_ie_link.stl"
},
{
"path": "assets/r_shoulder_fe_link.stl",
"vfs": "assets/r_shoulder_fe_link.stl"
},
{
"path": "assets/r_elbow_fe_link.stl",
"vfs": "assets/r_elbow_fe_link.stl"
},
{
"path": "assets/r_wrist_roll_link.stl",
"vfs": "assets/r_wrist_roll_link.stl"
},
{
"path": "assets/r_wrist_yaw_link.stl",
"vfs": "assets/r_wrist_yaw_link.stl"
},
{
"path": "assets/r_wrist_pitch_link.stl",
"vfs": "assets/r_wrist_pitch_link.stl"
},
{
"path": "assets/ability_hand/FB_palm_ref_MIR.STL",
"vfs": "assets/ability_hand/FB_palm_ref_MIR.STL"
},
{
"path": "assets/ability_hand/thumb-F1-MIR.STL",
"vfs": "assets/ability_hand/thumb-F1-MIR.STL"
},
{
"path": "assets/ability_hand/thumb-F2-right.STL",
"vfs": "assets/ability_hand/thumb-F2-right.STL"
},
{
"path": "assets/l_hip_ie_link.stl",
"vfs": "assets/l_hip_ie_link.stl"
},
{
"path": "assets/l_hip_aa_link.stl",
"vfs": "assets/l_hip_aa_link.stl"
},
{
"path": "assets/l_hip_fe_link.stl",
"vfs": "assets/l_hip_fe_link.stl"
},
{
"path": "assets/l_knee_fe_link.stl",
"vfs": "assets/l_knee_fe_link.stl"
},
{
"path": "assets/l_ankle_ie_link.stl",
"vfs": "assets/l_ankle_ie_link.stl"
},
{
"path": "assets/l_foot_link.stl",
"vfs": "assets/l_foot_link.stl"
},
{
"path": "assets/r_hip_ie_link.stl",
"vfs": "assets/r_hip_ie_link.stl"
},
{
"path": "assets/r_hip_aa_link.stl",
"vfs": "assets/r_hip_aa_link.stl"
},
{
"path": "assets/r_hip_fe_link.stl",
"vfs": "assets/r_hip_fe_link.stl"
},
{
"path": "assets/r_knee_fe_link.stl",
"vfs": "assets/r_knee_fe_link.stl"
},
{
"path": "assets/r_ankle_ie_link.stl",
"vfs": "assets/r_ankle_ie_link.stl"
},
{
"path": "assets/r_foot_link.stl",
"vfs": "assets/r_foot_link.stl"
}
],
"base": {
"type": "floating",
"root_body": "base_link",
"freejoint": "floating_base",
"spawn": {
"pos": [
0,
0,
1.1008384999999998
],
"yaw": 0
}
},
"sim": {
"timestep": 0.005,
"decimation": 4
},
"joints": {
"groups": {
"policy": [
"torso_yaw",
"torso_roll",
"torso_pitch",
"neck_yaw",
"neck_roll",
"neck_pitch",
"l_shoulder_aa",
"l_shoulder_ie",
"l_shoulder_fe",
"l_elbow_fe",
"l_wrist_roll",
"l_wrist_yaw",
"l_wrist_pitch",
"r_shoulder_aa",
"r_shoulder_ie",
"r_shoulder_fe",
"r_elbow_fe",
"r_wrist_roll",
"r_wrist_yaw",
"r_wrist_pitch",
"l_hip_ie",
"l_hip_aa",
"l_hip_fe",
"l_knee_fe",
"l_ankle_ie",
"l_ankle_pd",
"r_hip_ie",
"r_hip_aa",
"r_hip_fe",
"r_knee_fe",
"r_ankle_ie",
"r_ankle_pd"
],
"torso_yaw": [
"torso_yaw"
],
"torso_roll": [
"torso_roll"
],
"torso_pitch": [
"torso_pitch"
],
"neck_yaw": [
"neck_yaw"
],
"neck_roll": [
"neck_roll"
],
"neck_pitch": [
"neck_pitch"
],
"l_shoulder_aa": [
"l_shoulder_aa"
],
"l_shoulder_ie": [
"l_shoulder_ie"
],
"l_shoulder_fe": [
"l_shoulder_fe"
],
"l_elbow_fe": [
"l_elbow_fe"
],
"l_wrist_roll": [
"l_wrist_roll"
],
"l_wrist_yaw": [
"l_wrist_yaw"
],
"l_wrist_pitch": [
"l_wrist_pitch"
],
"r_shoulder_aa": [
"r_shoulder_aa"
],
"r_shoulder_ie": [
"r_shoulder_ie"
],
"r_shoulder_fe": [
"r_shoulder_fe"
],
"r_elbow_fe": [
"r_elbow_fe"
],
"r_wrist_roll": [
"r_wrist_roll"
],
"r_wrist_yaw": [
"r_wrist_yaw"
],
"r_wrist_pitch": [
"r_wrist_pitch"
],
"l_hip_ie": [
"l_hip_ie"
],
"l_hip_aa": [
"l_hip_aa"
],
"l_hip_fe": [
"l_hip_fe"
],
"l_knee_fe": [
"l_knee_fe"
],
"l_ankle_ie": [
"l_ankle_ie"
],
"l_ankle_pd": [
"l_ankle_pd"
],
"r_hip_ie": [
"r_hip_ie"
],
"r_hip_aa": [
"r_hip_aa"
],
"r_hip_fe": [
"r_hip_fe"
],
"r_knee_fe": [
"r_knee_fe"
],
"r_ankle_ie": [
"r_ankle_ie"
],
"r_ankle_pd": [
"r_ankle_pd"
]
},
"defaults": {
"torso_yaw": 0.0,
"torso_roll": 0.0,
"torso_pitch": 0.0,
"neck_yaw": 0.0,
"neck_roll": 0.0,
"neck_pitch": 0.0,
"l_shoulder_aa": 0.15,
"l_shoulder_ie": 0.0,
"l_shoulder_fe": 0.05,
"l_elbow_fe": -0.25,
"l_wrist_roll": 0.0,
"l_wrist_yaw": 0.0,
"l_wrist_pitch": 0.0,
"r_shoulder_aa": -0.15,
"r_shoulder_ie": 0.0,
"r_shoulder_fe": 0.05,
"r_elbow_fe": -0.25,
"r_wrist_roll": 0.0,
"r_wrist_yaw": 0.0,
"r_wrist_pitch": 0.0,
"l_hip_ie": 0.01516856,
"l_hip_aa": 0.04179601,
"l_hip_fe": -0.15749093,
"l_knee_fe": 0.35,
"l_ankle_ie": -0.03606629,
"l_ankle_pd": -0.18624892,
"r_hip_ie": -0.01516856,
"r_hip_aa": -0.04179601,
"r_hip_fe": -0.15749093,
"r_knee_fe": 0.35,
"r_ankle_ie": 0.03606629,
"r_ankle_pd": -0.18624892
}
},
"observations": {
"obs": {
"terms": [
{
"type": "linear_velocity",
"sensor": "imu_lin_vel"
},
{
"type": "gyro",
"sensor": "imu_ang_vel"
},
{
"type": "projected_gravity",
"body": "base_link"
},
{
"type": "joint_pos_rel",
"group": "policy"
},
{
"type": "joint_vel",
"group": "policy"
},
{
"type": "last_action",
"group": "policy"
},
{
"type": "command",
"size": 3
}
]
}
},
"actions": [
{
"type": "joint_position",
"group": "torso_yaw",
"scale": 0.08,
"offset": "default"
},
{
"type": "joint_position",
"group": "torso_roll",
"scale": 0.08,
"offset": "default"
},
{
"type": "joint_position",
"group": "torso_pitch",
"scale": 0.08,
"offset": "default"
},
{
"type": "joint_position",
"group": "neck_yaw",
"scale": 0.0,
"offset": "default"
},
{
"type": "joint_position",
"group": "neck_roll",
"scale": 0.0,
"offset": "default"
},
{
"type": "joint_position",
"group": "neck_pitch",
"scale": 0.0,
"offset": "default"
},
{
"type": "joint_position",
"group": "l_shoulder_aa",
"scale": 0.0,
"offset": "default"
},
{
"type": "joint_position",
"group": "l_shoulder_ie",
"scale": 0.0,
"offset": "default"
},
{
"type": "joint_position",
"group": "l_shoulder_fe",
"scale": 0.0,
"offset": "default"
},
{
"type": "joint_position",
"group": "l_elbow_fe",
"scale": 0.0,
"offset": "default"
},
{
"type": "joint_position",
"group": "l_wrist_roll",
"scale": 0.0,
"offset": "default"
},
{
"type": "joint_position",
"group": "l_wrist_yaw",
"scale": 0.0,
"offset": "default"
},
{
"type": "joint_position",
"group": "l_wrist_pitch",
"scale": 0.0,
"offset": "default"
},
{
"type": "joint_position",
"group": "r_shoulder_aa",
"scale": 0.0,
"offset": "default"
},
{
"type": "joint_position",
"group": "r_shoulder_ie",
"scale": 0.0,
"offset": "default"
},
{
"type": "joint_position",
"group": "r_shoulder_fe",
"scale": 0.0,
"offset": "default"
},
{
"type": "joint_position",
"group": "r_elbow_fe",
"scale": 0.0,
"offset": "default"
},
{
"type": "joint_position",
"group": "r_wrist_roll",
"scale": 0.0,
"offset": "default"
},
{
"type": "joint_position",
"group": "r_wrist_yaw",
"scale": 0.0,
"offset": "default"
},
{
"type": "joint_position",
"group": "r_wrist_pitch",
"scale": 0.0,
"offset": "default"
},
{
"type": "joint_position",
"group": "l_hip_ie",
"scale": 0.2,
"offset": "default"
},
{
"type": "joint_position",
"group": "l_hip_aa",
"scale": 0.2,
"offset": "default"
},
{
"type": "joint_position",
"group": "l_hip_fe",
"scale": 0.3,
"offset": "default"
},
{
"type": "joint_position",
"group": "l_knee_fe",
"scale": 0.4,
"offset": "default"
},
{
"type": "joint_position",
"group": "l_ankle_ie",
"scale": 0.15,
"offset": "default"
},
{
"type": "joint_position",
"group": "l_ankle_pd",
"scale": 0.2,
"offset": "default"
},
{
"type": "joint_position",
"group": "r_hip_ie",
"scale": 0.2,
"offset": "default"
},
{
"type": "joint_position",
"group": "r_hip_aa",
"scale": 0.2,
"offset": "default"
},
{
"type": "joint_position",
"group": "r_hip_fe",
"scale": 0.3,
"offset": "default"
},
{
"type": "joint_position",
"group": "r_knee_fe",
"scale": 0.4,
"offset": "default"
},
{
"type": "joint_position",
"group": "r_ankle_ie",
"scale": 0.15,
"offset": "default"
},
{
"type": "joint_position",
"group": "r_ankle_pd",
"scale": 0.2,
"offset": "default"
}
],
"actuation": {
"type": "native_position"
},
"commands": {
"size": 3,
"limits": [
0.5,
-0.3,
0.4
]
},
"skills": [
{
"id": "walk",
"kind": "primary",
"owns": "policy",
"controller": {
"type": "onnx",
"policy": "policies/walk.onnx"
}
}
],
"input": {
"bindings": [
{
"action": "move_forward",
"codes": [
"KeyW",
"ArrowUp"
],
"label": "Forward"
},
{
"action": "move_backward",
"codes": [
"KeyS",
"ArrowDown"
],
"label": "Backward"
},
{
"action": "turn_left",
"codes": [
"KeyA",
"ArrowLeft"
],
"label": "Turn left"
},
{
"action": "turn_right",
"codes": [
"KeyD",
"ArrowRight"
],
"label": "Turn right"
},
{
"action": "reset",
"codes": [
"Space"
],
"label": "Reset"
},
{
"action": "pause",
"codes": [
"Escape"
],
"label": "Pause / continue"
},
{
"action": "chaseToggle",
"codes": [
"KeyC"
],
"label": "Restore chase camera"
}
]
},
"render": {
"source": "mjcf_visual",
"groups": [
1
],
"camera": {
"distance": 3.5
}
},
"presentation": {
"theme": "comic"
},
"termination": {
"tilt_z": -0.5,
"min_height": 0.98,
"on_fall": "hold"
},
"license": "See LICENSES/apollo.txt and LICENSES/mjlab.txt; policy is locally trained."
}
@@ -0,0 +1,303 @@
{
"checkpoint": "/home/cen/Embodied_Workspace/Mujoco_Projects/mjlab/logs/rsl_rl/apollo_flat_upright_v3/2026-11-02_17-39-21_apollo_upright_v3/model_9999.pt",
"checkpoint_sha256": "608895433d8ff3edfcd06d1ff583d7de1346415897182ec673a32af8af38dcba",
"model_source": "/home/cen/Embodied_Workspace/PlatformProject/apptronik_apollo/apptronik_apollo.xml",
"files": [
{
"source": "/home/cen/Embodied_Workspace/PlatformProject/apptronik_apollo/assets/neck_mount_fix_link.stl",
"file": "assets/neck_mount_fix_link.stl",
"bytes": 2267984,
"sha256": "d6f64c42a5bfed7abb0dc48414e58164ad0042a71f5f5fe27382049e48608d7a"
},
{
"source": "/home/cen/Embodied_Workspace/PlatformProject/apptronik_apollo/assets/neck_yaw_link.stl",
"file": "assets/neck_yaw_link.stl",
"bytes": 3512084,
"sha256": "86160c43db315955e9e24c323602a74bad026bdd83df55fecb3413c333b2e453"
},
{
"source": "/home/cen/Embodied_Workspace/PlatformProject/apptronik_apollo/assets/afh_2_1_link.stl",
"file": "assets/afh_2_1_link.stl",
"bytes": 2711384,
"sha256": "76604d5c5db8472bf24322c33fe03b32c0f2dff45985edefb96c11e1c9b0e801"
},
{
"source": "/home/cen/Embodied_Workspace/PlatformProject/apptronik_apollo/assets/torso_link.stl",
"file": "assets/torso_link.stl",
"bytes": 7500084,
"sha256": "0301f1d9c925df169ba12a79cb2dab532156aae3821750cb5820349ac60a92f6"
},
{
"source": "/home/cen/Embodied_Workspace/PlatformProject/apptronik_apollo/assets/battery_mount_fix.stl",
"file": "assets/battery_mount_fix.stl",
"bytes": 300084,
"sha256": "9785105c0b606b5f229b70ba95bd64b239ddb90578f1dc9ccd439f4c182b5c33"
},
{
"source": "/home/cen/Embodied_Workspace/PlatformProject/apptronik_apollo/assets/torso_pitch_link.stl",
"file": "assets/torso_pitch_link.stl",
"bytes": 705334,
"sha256": "dc5174d8a748a0c372641dca94172646693edd1d6ead8fb1259a26cf53a08f93"
},
{
"source": "/home/cen/Embodied_Workspace/PlatformProject/apptronik_apollo/assets/torso_roll_link.stl",
"file": "assets/torso_roll_link.stl",
"bytes": 1819384,
"sha256": "db0bfaecf809e3f877cbdfadcf7e3ec162b4f5f362f61d3ea62d26ab3e1983a6"
},
{
"source": "/home/cen/Embodied_Workspace/PlatformProject/apptronik_apollo/assets/pelvis_link.stl",
"file": "assets/pelvis_link.stl",
"bytes": 3500084,
"sha256": "f344c0928d39626394edff076e9eca00274cbf69830f6ff1b3ff6d019baa1a65"
},
{
"source": "/home/cen/Embodied_Workspace/PlatformProject/apptronik_apollo/assets/l_shoulder_aa_link.stl",
"file": "assets/l_shoulder_aa_link.stl",
"bytes": 276084,
"sha256": "96dea690d6288d5e81b651d4da7214ccf860aed74329d261aaef760533c12742"
},
{
"source": "/home/cen/Embodied_Workspace/PlatformProject/apptronik_apollo/assets/l_shoulder_ie_link.stl",
"file": "assets/l_shoulder_ie_link.stl",
"bytes": 3142984,
"sha256": "1932049068158367e09a6ee70a0a6d908c6e2adc6dcf471cb451205c320c4a2d"
},
{
"source": "/home/cen/Embodied_Workspace/PlatformProject/apptronik_apollo/assets/l_shoulder_fe_link.stl",
"file": "assets/l_shoulder_fe_link.stl",
"bytes": 2053634,
"sha256": "cd6b89ece6320388b792e910ddac222d6e4244d310efe4aa642788dc00232fb4"
},
{
"source": "/home/cen/Embodied_Workspace/PlatformProject/apptronik_apollo/assets/l_elbow_fe_link.stl",
"file": "assets/l_elbow_fe_link.stl",
"bytes": 1290984,
"sha256": "ac17ac98b6564a75886311fe88482b9cdbcb632556c80bdc3fd338dca26d43dd"
},
{
"source": "/home/cen/Embodied_Workspace/PlatformProject/apptronik_apollo/assets/l_wrist_roll_link.stl",
"file": "assets/l_wrist_roll_link.stl",
"bytes": 2500084,
"sha256": "e752b5aca75ebd5430e45ee37c9eb57f4cf9d6def6370c29234c811d40a028c0"
},
{
"source": "/home/cen/Embodied_Workspace/PlatformProject/apptronik_apollo/assets/l_wrist_yaw_link.stl",
"file": "assets/l_wrist_yaw_link.stl",
"bytes": 370384,
"sha256": "06b118e59ec0e6308a842f7fd3c572ed657c7d3cb93b17fb1de72ea56b3f27eb"
},
{
"source": "/home/cen/Embodied_Workspace/PlatformProject/apptronik_apollo/assets/l_wrist_pitch_link.stl",
"file": "assets/l_wrist_pitch_link.stl",
"bytes": 1587934,
"sha256": "6ae61d22369ae6ed0f7122df902c9ebabc482891739460cbb953317de24b90ab"
},
{
"source": "/home/cen/Embodied_Workspace/PlatformProject/apptronik_apollo/assets/ability_hand/wrist_adapter.STL",
"file": "assets/ability_hand/wrist_adapter.STL",
"bytes": 3853984,
"sha256": "b1d1c627d239acf058bb5df4b270e8f6ab7728c5b01f5fb7f0d269994f55b1a8"
},
{
"source": "/home/cen/Embodied_Workspace/PlatformProject/apptronik_apollo/assets/ability_hand/wristmesh.STL",
"file": "assets/ability_hand/wristmesh.STL",
"bytes": 2143184,
"sha256": "4d67306f775a5da7d6e8c9c2faddb12f1497ea289a6c7dc1584c213b42da43ee"
},
{
"source": "/home/cen/Embodied_Workspace/PlatformProject/apptronik_apollo/assets/ability_hand/FB_palm_ref.STL",
"file": "assets/ability_hand/FB_palm_ref.STL",
"bytes": 581984,
"sha256": "d9917a43ca1ce6fb2cbf7b651e0ed6b0c322acf75b8485806cc5d5a365b06db0"
},
{
"source": "/home/cen/Embodied_Workspace/PlatformProject/apptronik_apollo/assets/ability_hand/idx-F1.STL",
"file": "assets/ability_hand/idx-F1.STL",
"bytes": 44284,
"sha256": "f983d18177fecfdcf0141bd8e94e351fad67f62a74fb70819cdc017ae5af6d0e"
},
{
"source": "/home/cen/Embodied_Workspace/PlatformProject/apptronik_apollo/assets/ability_hand/idx-F2.STL",
"file": "assets/ability_hand/idx-F2.STL",
"bytes": 24384,
"sha256": "37d5e938e9e924fbf0262bd044ee04eec28c73cd73ea91ad97b37bf037c12198"
},
{
"source": "/home/cen/Embodied_Workspace/PlatformProject/apptronik_apollo/assets/ability_hand/thumb-F1.STL",
"file": "assets/ability_hand/thumb-F1.STL",
"bytes": 6084,
"sha256": "b20ec97a4682e85abe977fbe0d0108362719573830556f3b75350fe8a084af21"
},
{
"source": "/home/cen/Embodied_Workspace/PlatformProject/apptronik_apollo/assets/ability_hand/thumb-F2-left.STL",
"file": "assets/ability_hand/thumb-F2-left.STL",
"bytes": 13484,
"sha256": "9dcd48a151fe1de3f3eee0615ac679f0eb4182ae0ebee398d372ea102e718c89"
},
{
"source": "/home/cen/Embodied_Workspace/PlatformProject/apptronik_apollo/assets/r_shoulder_aa_link.stl",
"file": "assets/r_shoulder_aa_link.stl",
"bytes": 290084,
"sha256": "d41fd7d6d599cb28cdd8fb135bc1f905f89acf0f768d771d1c0d24fa54921247"
},
{
"source": "/home/cen/Embodied_Workspace/PlatformProject/apptronik_apollo/assets/r_shoulder_ie_link.stl",
"file": "assets/r_shoulder_ie_link.stl",
"bytes": 2896884,
"sha256": "0ccc0413f98ed9a9549a6a6cbc0395c69fa4516381956d37b7717c0cb8e89dbd"
},
{
"source": "/home/cen/Embodied_Workspace/PlatformProject/apptronik_apollo/assets/r_shoulder_fe_link.stl",
"file": "assets/r_shoulder_fe_link.stl",
"bytes": 2053084,
"sha256": "9d702645d36fc09958b9c61cb92cab48c4902b2049f0d7839cea61761c521fd0"
},
{
"source": "/home/cen/Embodied_Workspace/PlatformProject/apptronik_apollo/assets/r_elbow_fe_link.stl",
"file": "assets/r_elbow_fe_link.stl",
"bytes": 1290684,
"sha256": "9564899c9031c4d68942c5d6e065d3e8b39634bcaaf0a23b91f0241420d8530f"
},
{
"source": "/home/cen/Embodied_Workspace/PlatformProject/apptronik_apollo/assets/r_wrist_roll_link.stl",
"file": "assets/r_wrist_roll_link.stl",
"bytes": 2500084,
"sha256": "97a2459f5cba8c03a59c9f8a7e9795a91c1b46a5d81d054bbb6cbb629e8cdafd"
},
{
"source": "/home/cen/Embodied_Workspace/PlatformProject/apptronik_apollo/assets/r_wrist_yaw_link.stl",
"file": "assets/r_wrist_yaw_link.stl",
"bytes": 369534,
"sha256": "84ac6e91f41cd993ceba238969e7b09facc1733df603ac7dc439dfb327c60c12"
},
{
"source": "/home/cen/Embodied_Workspace/PlatformProject/apptronik_apollo/assets/r_wrist_pitch_link.stl",
"file": "assets/r_wrist_pitch_link.stl",
"bytes": 1589534,
"sha256": "61a85a23dc80ebbe954fcc669655d36159d4a7cb91a7a815f244f5ff94455f30"
},
{
"source": "/home/cen/Embodied_Workspace/PlatformProject/apptronik_apollo/assets/ability_hand/FB_palm_ref_MIR.STL",
"file": "assets/ability_hand/FB_palm_ref_MIR.STL",
"bytes": 581984,
"sha256": "3d0e9d29efbd8deb94bfab1c84464a2c46a33e839cb5f553115b27d2610193c2"
},
{
"source": "/home/cen/Embodied_Workspace/PlatformProject/apptronik_apollo/assets/ability_hand/thumb-F1-MIR.STL",
"file": "assets/ability_hand/thumb-F1-MIR.STL",
"bytes": 6084,
"sha256": "0ff962d2c6f8b0af39607439f4d48a247e4eb925f7a0c20ca00b0d27d8e7c2e0"
},
{
"source": "/home/cen/Embodied_Workspace/PlatformProject/apptronik_apollo/assets/ability_hand/thumb-F2-right.STL",
"file": "assets/ability_hand/thumb-F2-right.STL",
"bytes": 13484,
"sha256": "09623bce14d85db7b035ab4333f702505fa13f834845adda1179544d3c456a77"
},
{
"source": "/home/cen/Embodied_Workspace/PlatformProject/apptronik_apollo/assets/l_hip_ie_link.stl",
"file": "assets/l_hip_ie_link.stl",
"bytes": 2961884,
"sha256": "db5e43d77b0308b0aaed54b54f022c90d8d8efc9f809de14d8eee8ddda2d7430"
},
{
"source": "/home/cen/Embodied_Workspace/PlatformProject/apptronik_apollo/assets/l_hip_aa_link.stl",
"file": "assets/l_hip_aa_link.stl",
"bytes": 538784,
"sha256": "9192292ec7093bde26d5ead25685d69e1a75d079f5cd800ae1817e789977c515"
},
{
"source": "/home/cen/Embodied_Workspace/PlatformProject/apptronik_apollo/assets/l_hip_fe_link.stl",
"file": "assets/l_hip_fe_link.stl",
"bytes": 2499784,
"sha256": "3c0729f7988d19a99caecda8afedd690dfc939cf2017680b15af7906186aecf2"
},
{
"source": "/home/cen/Embodied_Workspace/PlatformProject/apptronik_apollo/assets/l_knee_fe_link.stl",
"file": "assets/l_knee_fe_link.stl",
"bytes": 1403484,
"sha256": "818d5660fb778eae7d28c05be806f76cbc298bd4fe8a8db7417a93b8ee9b48ed"
},
{
"source": "/home/cen/Embodied_Workspace/PlatformProject/apptronik_apollo/assets/l_ankle_ie_link.stl",
"file": "assets/l_ankle_ie_link.stl",
"bytes": 679134,
"sha256": "1e5526d09d5f418c4c428e5e58497c2807df06742e0e24874ccbcadd11e9bea1"
},
{
"source": "/home/cen/Embodied_Workspace/PlatformProject/apptronik_apollo/assets/l_foot_link.stl",
"file": "assets/l_foot_link.stl",
"bytes": 1841984,
"sha256": "6506c651d16a4fcf6f8ce37a056b349bd1abea1c4526866383249addd3609659"
},
{
"source": "/home/cen/Embodied_Workspace/PlatformProject/apptronik_apollo/assets/r_hip_ie_link.stl",
"file": "assets/r_hip_ie_link.stl",
"bytes": 2961634,
"sha256": "3b9cee0dab3e564b12fd6293fe21da1724261dd7347e184d84239d60cfe6551c"
},
{
"source": "/home/cen/Embodied_Workspace/PlatformProject/apptronik_apollo/assets/r_hip_aa_link.stl",
"file": "assets/r_hip_aa_link.stl",
"bytes": 539884,
"sha256": "5cca53bcd1cedff71106dd0d276d6767e1bd7a7ceb6a6a0601b28c7d4361f79c"
},
{
"source": "/home/cen/Embodied_Workspace/PlatformProject/apptronik_apollo/assets/r_hip_fe_link.stl",
"file": "assets/r_hip_fe_link.stl",
"bytes": 2103184,
"sha256": "53bbb28f4688e4a9562d6d54a51c1d7784774117ecb2034f10f6f9ee470e0fa7"
},
{
"source": "/home/cen/Embodied_Workspace/PlatformProject/apptronik_apollo/assets/r_knee_fe_link.stl",
"file": "assets/r_knee_fe_link.stl",
"bytes": 1386984,
"sha256": "19a73643d4dd8ff90894b0a712cf828c7f79a8091218ad2dee8fec419f5f0666"
},
{
"source": "/home/cen/Embodied_Workspace/PlatformProject/apptronik_apollo/assets/r_ankle_ie_link.stl",
"file": "assets/r_ankle_ie_link.stl",
"bytes": 652134,
"sha256": "cb33be19fe5202b6ed638c1816e3ba8e14d8407be9e0045c6e644f1fd54a5056"
},
{
"source": "/home/cen/Embodied_Workspace/PlatformProject/apptronik_apollo/assets/r_foot_link.stl",
"file": "assets/r_foot_link.stl",
"bytes": 1845934,
"sha256": "0ea4a2c27a89f944cd838c6c6c471c1b3a859bc4174c86c41773e6a20079b903"
},
{
"source": "/home/cen/Embodied_Workspace/Mujoco_Projects/mjlab/logs/rsl_rl/apollo_flat_upright_v3/2026-11-02_17-39-21_apollo_upright_v3/2026-11-02_17-39-21_apollo_upright_v3.onnx",
"file": "policies/walk.onnx",
"bytes": 900731,
"sha256": "0dc72b1965b9a37d43c4224dc5dd81d8df8520b40ed02d6ef7ff44e0e90987b5"
},
{
"source": "/home/cen/Embodied_Workspace/PlatformProject/apptronik_apollo/LICENSE",
"file": "LICENSES/apollo.txt",
"bytes": 11344,
"sha256": "10b53325854d8aa8172b846f96eb47d5fbba6366161c480292cf4627606ef430"
},
{
"source": "/home/cen/Embodied_Workspace/Mujoco_Projects/mjlab/LICENSE",
"file": "LICENSES/mjlab.txt",
"bytes": 11352,
"sha256": "8ca04d9dbbccd20f0f1842fb33e13949f9e57064b150dad36af8e7a751ca3321"
}
],
"behaviorReferences": [
{
"file": "src/mjlab/asset_zoo/robots/apptronik_apollo/apollo_constants.py",
"sha256": "29b341ce6603f77d15a608b9b2a4a3005c0db5b569006a154f894e8b18b1d8b1"
},
{
"file": "src/mjlab/tasks/velocity/config/apptronik_apollo/env_cfgs.py",
"sha256": "5170f829d2753eed8859e7a6c29b615c30da27a33d3c428a9836518b6f1a87a8"
},
{
"file": "src/mjlab/tasks/velocity/velocity_env_cfg.py",
"sha256": "8a272a8b3c81c4a0321027e404645c99a4143076522ed3521636bf293dd3137b"
}
]
}
+12
View File
@@ -22,5 +22,17 @@
"family": "rp1", "family": "rp1",
"name": "RP1", "name": "RP1",
"manifest": "rp1/robot.json" "manifest": "rp1/robot.json"
},
{
"id": "t1",
"family": "t1",
"name": "Booster T1",
"manifest": "t1/robot.json"
},
{
"id": "apollo",
"family": "apollo",
"name": "Apptronik Apollo",
"manifest": "apollo/robot.json"
} }
] ]
@@ -0,0 +1,28 @@
BSD 3-Clause License
Copyright (c) 2025, BoosterRobotics
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
1. Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.
2. Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.
3. Neither the name of the copyright holder nor the names of its
contributors may be used to endorse or promote products derived from
this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+202
View File
@@ -0,0 +1,202 @@
Apache License
Version 2.0, January 2004
http://www.apache.org/licenses/
TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
1. Definitions.
"License" shall mean the terms and conditions for use, reproduction,
and distribution as defined by Sections 1 through 9 of this document.
"Licensor" shall mean the copyright owner or entity authorized by
the copyright owner that is granting the License.
"Legal Entity" shall mean the union of the acting entity and all
other entities that control, are controlled by, or are under common
control with that entity. For the purposes of this definition,
"control" means (i) the power, direct or indirect, to cause the
direction or management of such entity, whether by contract or
otherwise, or (ii) ownership of fifty percent (50%) or more of the
outstanding shares, or (iii) beneficial ownership of such entity.
"You" (or "Your") shall mean an individual or Legal Entity
exercising permissions granted by this License.
"Source" form shall mean the preferred form for making modifications,
including but not limited to software source code, documentation
source, and configuration files.
"Object" form shall mean any form resulting from mechanical
transformation or translation of a Source form, including but
not limited to compiled object code, generated documentation,
and conversions to other media types.
"Work" shall mean the work of authorship, whether in Source or
Object form, made available under the License, as indicated by a
copyright notice that is included in or attached to the work
(an example is provided in the Appendix below).
"Derivative Works" shall mean any work, whether in Source or Object
form, that is based on (or derived from) the Work and for which the
editorial revisions, annotations, elaborations, or other modifications
represent, as a whole, an original work of authorship. For the purposes
of this License, Derivative Works shall not include works that remain
separable from, or merely link (or bind by name) to the interfaces of,
the Work and Derivative Works thereof.
"Contribution" shall mean any work of authorship, including
the original version of the Work and any modifications or additions
to that Work or Derivative Works thereof, that is intentionally
submitted to Licensor for inclusion in the Work by the copyright owner
or by an individual or Legal Entity authorized to submit on behalf of
the copyright owner. For the purposes of this definition, "submitted"
means any form of electronic, verbal, or written communication sent
to the Licensor or its representatives, including but not limited to
communication on electronic mailing lists, source code control systems,
and issue tracking systems that are managed by, or on behalf of, the
Licensor for the purpose of discussing and improving the Work, but
excluding communication that is conspicuously marked or otherwise
designated in writing by the copyright owner as "Not a Contribution."
"Contributor" shall mean Licensor and any individual or Legal Entity
on behalf of whom a Contribution has been received by Licensor and
subsequently incorporated within the Work.
2. Grant of Copyright License. Subject to the terms and conditions of
this License, each Contributor hereby grants to You a perpetual,
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
copyright license to reproduce, prepare Derivative Works of,
publicly display, publicly perform, sublicense, and distribute the
Work and such Derivative Works in Source or Object form.
3. Grant of Patent License. Subject to the terms and conditions of
this License, each Contributor hereby grants to You a perpetual,
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
(except as stated in this section) patent license to make, have made,
use, offer to sell, sell, import, and otherwise transfer the Work,
where such license applies only to those patent claims licensable
by such Contributor that are necessarily infringed by their
Contribution(s) alone or by combination of their Contribution(s)
with the Work to which such Contribution(s) was submitted. If You
institute patent litigation against any entity (including a
cross-claim or counterclaim in a lawsuit) alleging that the Work
or a Contribution incorporated within the Work constitutes direct
or contributory patent infringement, then any patent licenses
granted to You under this License for that Work shall terminate
as of the date such litigation is filed.
4. Redistribution. You may reproduce and distribute copies of the
Work or Derivative Works thereof in any medium, with or without
modifications, and in Source or Object form, provided that You
meet the following conditions:
(a) You must give any other recipients of the Work or
Derivative Works a copy of this License; and
(b) You must cause any modified files to carry prominent notices
stating that You changed the files; and
(c) You must retain, in the Source form of any Derivative Works
that You distribute, all copyright, patent, trademark, and
attribution notices from the Source form of the Work,
excluding those notices that do not pertain to any part of
the Derivative Works; and
(d) If the Work includes a "NOTICE" text file as part of its
distribution, then any Derivative Works that You distribute must
include a readable copy of the attribution notices contained
within such NOTICE file, excluding those notices that do not
pertain to any part of the Derivative Works, in at least one
of the following places: within a NOTICE text file distributed
as part of the Derivative Works; within the Source form or
documentation, if provided along with the Derivative Works; or,
within a display generated by the Derivative Works, if and
wherever such third-party notices normally appear. The contents
of the NOTICE file are for informational purposes only and
do not modify the License. You may add Your own attribution
notices within Derivative Works that You distribute, alongside
or as an addendum to the NOTICE text from the Work, provided
that such additional attribution notices cannot be construed
as modifying the License.
You may add Your own copyright statement to Your modifications and
may provide additional or different license terms and conditions
for use, reproduction, or distribution of Your modifications, or
for any such Derivative Works as a whole, provided Your use,
reproduction, and distribution of the Work otherwise complies with
the conditions stated in this License.
5. Submission of Contributions. Unless You explicitly state otherwise,
any Contribution intentionally submitted for inclusion in the Work
by You to the Licensor shall be under the terms and conditions of
this License, without any additional terms or conditions.
Notwithstanding the above, nothing herein shall supersede or modify
the terms of any separate license agreement you may have executed
with Licensor regarding such Contributions.
6. Trademarks. This License does not grant permission to use the trade
names, trademarks, service marks, or product names of the Licensor,
except as required for reasonable and customary use in describing the
origin of the Work and reproducing the content of the NOTICE file.
7. Disclaimer of Warranty. Unless required by applicable law or
agreed to in writing, Licensor provides the Work (and each
Contributor provides its Contributions) on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
implied, including, without limitation, any warranties or conditions
of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
PARTICULAR PURPOSE. You are solely responsible for determining the
appropriateness of using or redistributing the Work and assume any
risks associated with Your exercise of permissions under this License.
8. Limitation of Liability. In no event and under no legal theory,
whether in tort (including negligence), contract, or otherwise,
unless required by applicable law (such as deliberate and grossly
negligent acts) or agreed to in writing, shall any Contributor be
liable to You for damages, including any direct, indirect, special,
incidental, or consequential damages of any character arising as a
result of this License or out of the use or inability to use the
Work (including but not limited to damages for loss of goodwill,
work stoppage, computer failure or malfunction, or any and all
other commercial damages or losses), even if such Contributor
has been advised of the possibility of such damages.
9. Accepting Warranty or Additional Liability. While redistributing
the Work or Derivative Works thereof, You may choose to offer,
and charge a fee for, acceptance of support, warranty, indemnity,
or other liability obligations and/or rights consistent with this
License. However, in accepting such obligations, You may act only
on Your own behalf and on Your sole responsibility, not on behalf
of any other Contributor, and only if You agree to indemnify,
defend, and hold each Contributor harmless for any liability
incurred by, or claims asserted against, such Contributor by reason
of your accepting any such warranty or additional liability.
END OF TERMS AND CONDITIONS
APPENDIX: How to apply the Apache License to your work.
To apply the Apache License to your work, attach the following
boilerplate notice, with the fields enclosed by brackets "[]"
replaced with your own identifying information. (Don't include
the brackets!) The text should be enclosed in the appropriate
comment syntax for the file format. We also recommend that a
file or class name and description of purpose be included on the
same "printed page" as the copyright notice for easier
identification within third-party archives.
Copyright 2025, The mjlab Developers
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at
http://www.apache.org/licenses/LICENSE-2.0
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
+35
View File
@@ -0,0 +1,35 @@
# Booster T1 package
Imported from the local `mjlab` T1 flat-velocity task and `booster_assets/robots/T1/T1_23dof.xml`.
## Policy snapshot
- Training run: `2026-11-02_15-03-39_t1_flat`.
- Latest checkpoint at import: `model_6700.pt`; corresponding run-name ONNX export is bundled as `policies/walk.onnx`.
- Training may continue independently. This package intentionally pins the tested snapshot; it does not hot-reload weights.
- File hashes and source paths: `source-manifest.json`.
## Fidelity
23 controlled joints; 81 observations in exported policy order: body-frame linear velocity, angular velocity, projected gravity, relative joint positions, joint velocities, previous raw actions, and body-frame velocity command. The ONNX contains the exported observation normalization.
The MJCF is generated through mjlab's `Entity(get_t1_robot_cfg())`, preserving inertias, armatures, collision masks and native position actuators with per-joint gains and torque limits. Actions use the full-precision training scales (not the rounded ONNX metadata). Simulation timestep is 5 ms with four substeps (50 Hz policy), implicitfast/Newton. Training randomization is disabled; the platform supplies its shared floor, walls and props. Meshes are the source STL visuals; no texture replacement is included.
W/S command +0.6/-0.4 m/s; A/D command +0.6/-0.6 rad/s. Space resets, Escape pauses, C restores the chase camera. A fall holds targets pending manual reset; no recovery policy is supplied.
Browser verification: five separate 5-second standing/forward/backward/left/right trials remained upright with finite states. Forward/backward displacement was +2.84/-1.68 m; left/right yaw was +2.97/-2.82 rad. Keyboard press/release, reset and robot switching also passed. This is simulation validation, not hardware qualification.
## Re-import
From repository root:
```sh
/path/to/mjlab/.venv/bin/python app/scripts/import-t1.py /path/to/mjlab
cd app
npm run validate:packages
npx playwright test t1.spec.js
```
The importer selects the newest run and its highest numbered checkpoint, and requires a non-stale ONNX export. `BOOSTER_ASSETS_ROOT` can override the asset checkout. Re-run the browser tests after updating weights.
Licenses from both source repositories are preserved in `LICENSES/`; the policy was trained locally. Confirm asset/model redistribution rights before public deployment.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.

Some files were not shown because too many files have changed in this diff Show More