Compare commits
3 Commits
| Author | SHA1 | Date | |
|---|---|---|---|
| 164a397726 | |||
| 2b0c395508 | |||
| a8be5e0134 |
@@ -11,22 +11,45 @@ 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
|
||||||
|
|
||||||
|
- Brighter shared lighting and the original four-swatch **Line color** control for every robot.
|
||||||
|
- One session-wide layout of 5–7 physical, draggable geometries replaces the football and scales consistently across robots.
|
||||||
|
- RP1 uses the supplied textured GLBs, including fixed-hand detail, without changing its 24-DOF physics or trained skills.
|
||||||
|
- Verified with 66 unit tests, 55 browser tests and production-build checks. See [shared environment](docs/superpowers/verification/shared-robot-environment.md) and [RP1 materials](docs/superpowers/verification/rp1-glb-materials.md) verification records.
|
||||||
|
|
||||||
## Robots
|
## Robots
|
||||||
|
|
||||||
| Robot | Default scene / presentation | Controls |
|
| Robot | Default scene / presentation | Controls |
|
||||||
|---|---|---|
|
|---|---|---|
|
||||||
| Microduck | Shared 4.5 × 4.5 m bounded arena, ball, original GLB/palette/comic effects | Existing walking, skating, skills and recovery |
|
| Microduck | Shared 4.5 × 4.5 m bounded arena, 5–7 random interactive geometries, original GLB/comic effects | Existing walking, skating, skills and recovery |
|
||||||
| Microban | Robot-scaled 5.5 × 5.5 m arena, dark/orange grid and comic UI, 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 | Robot-scaled 22 × 22 m arena/comic UI, original 24-DOF MJCF visuals | 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) |
|
||||||
|
|
||||||
Switch robots using **Robot** in the toolbar. Returning to Microduck restores the session's colour and feet / rollers choice, but initializes a fresh physical state. Failed loads keep the previous robot available and show an inline retry action.
|
**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.
|
||||||
|
|
||||||
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).
|
||||||
|
|
||||||
|
### Shared scene controls (all robots)
|
||||||
|
|
||||||
|
The scene has brighter ambient / environment illumination plus a hemisphere fill light. The lower-left **Line color** palette retains the original Cream / Graphite / Lavender / Sky swatches and square comic buttons, but now changes **floor and wall grid lines**, not robot materials. The same palette is available for every robot, and its selection is global across robot switches, reset and feet / rollers changes.
|
||||||
|
|
||||||
|
The football is removed. One browser session randomly generates **5–7 lightweight boxes, spheres, cylinders and capsules** shared by **all robots**, clear of both bundled spawn anchors and each other. Their types, colours and relative layout stay identical when switching robots; geometry and positions scale with the existing arena-to-robot ratio. They have real MuJoCo free joints: every robot can push them, they collide with one another, the floor and walls, and the mouse can grab / lift them using spring forces. Reset, locomotion changes and robot switches restore the same session layout; refreshing generates a new layout. Escaped toys are returned individually. This shares environment configuration/layout, not live object momentum between different-scale robot simulations.
|
||||||
|
|
||||||
## Keyboard and mouse
|
## Keyboard and mouse
|
||||||
|
|
||||||
| Action | Binding |
|
| Action | Binding |
|
||||||
@@ -47,7 +70,7 @@ Microduck policies / assets originate from [pollen-robotics/microduck](https://g
|
|||||||
|
|
||||||
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). Microduck mouse dragging still applies physical forces to the robot / ball. Microban / RP1 mouse dragging orbits the camera. 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
|
||||||
|
|
||||||
@@ -62,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.
|
||||||
@@ -123,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.
|
||||||
|
|||||||
@@ -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([]);
|
||||||
|
});
|
||||||
@@ -59,7 +59,7 @@ test("arena appearance matches Microduck while Microban physics stays flat and W
|
|||||||
expect(state).toEqual({
|
expect(state).toEqual({
|
||||||
cell: 0.1 * (0.33 / 0.27),
|
cell: 0.1 * (0.33 / 0.27),
|
||||||
section: 0.6 * (0.33 / 0.27),
|
section: 0.6 * (0.33 / 0.27),
|
||||||
color: 0xffb366,
|
color: 0xf7e6cb,
|
||||||
reveal: 1,
|
reveal: 1,
|
||||||
topo: 0,
|
topo: 0,
|
||||||
hiddenFloor: true,
|
hiddenFloor: true,
|
||||||
|
|||||||
@@ -60,13 +60,15 @@ test("both robots default to the same rear camera and comic UI; Microban keeps f
|
|||||||
await expect(
|
await expect(
|
||||||
page.getByRole("navigation", { name: "Simulation controls" }),
|
page.getByRole("navigation", { name: "Simulation controls" }),
|
||||||
).toHaveCSS("border-top-width", "2px");
|
).toHaveCSS("border-top-width", "2px");
|
||||||
await expect(page.getByRole("group", { name: "Duck colours" })).toHaveCount(
|
await expect(
|
||||||
0,
|
page.getByRole("group", { name: "Scene line colours" }),
|
||||||
);
|
).toBeVisible();
|
||||||
await expect(
|
await expect(
|
||||||
page.getByRole("button", { name: "Rollers", exact: true }),
|
page.getByRole("button", { name: "Rollers", exact: true }),
|
||||||
).toHaveCount(0);
|
).toHaveCount(0);
|
||||||
await expect(page.getByText("Drag robot / ball: apply force")).toHaveCount(0);
|
await expect(
|
||||||
|
page.getByText("Drag robot / geometry: apply force"),
|
||||||
|
).toBeVisible();
|
||||||
await expect(page.getByRole("status")).toContainText("Simplified actuation");
|
await expect(page.getByRole("status")).toContainText("Simplified actuation");
|
||||||
expect(
|
expect(
|
||||||
await page.evaluate(() => window.rl.model.geom("platform_floor").id),
|
await page.evaluate(() => window.rl.model.geom("platform_floor").id),
|
||||||
@@ -74,7 +76,9 @@ test("both robots default to the same rear camera and comic UI; Microban keeps f
|
|||||||
await page.screenshot({ path: info.outputPath("microban-chase.png") });
|
await page.screenshot({ path: info.outputPath("microban-chase.png") });
|
||||||
await page.evaluate(() => window.__gameApi.switchRobot("microduck-legs"));
|
await page.evaluate(() => window.__gameApi.switchRobot("microduck-legs"));
|
||||||
await rear(page);
|
await rear(page);
|
||||||
await expect(page.getByRole("group", { name: "Duck colours" })).toBeVisible();
|
await expect(
|
||||||
|
page.getByRole("group", { name: "Scene line colours" }),
|
||||||
|
).toBeVisible();
|
||||||
expect(errors).toEqual([]);
|
expect(errors).toEqual([]);
|
||||||
});
|
});
|
||||||
for (const id of ["microban", "microduck-legs"])
|
for (const id of ["microban", "microduck-legs"])
|
||||||
|
|||||||
@@ -20,7 +20,24 @@ test("deployed assets respect the base path and all three Robot Packages boot",
|
|||||||
await expect(
|
await expect(
|
||||||
page.getByRole("button", { name: "Pause", exact: true }),
|
page.getByRole("button", { name: "Pause", exact: true }),
|
||||||
).toBeEnabled();
|
).toBeEnabled();
|
||||||
await page.waitForTimeout(500); // capture after the ball's existing appear FX
|
await page.waitForTimeout(500); // let the physical toys settle
|
||||||
|
const toys = await page.evaluate(() => {
|
||||||
|
const r = window.rl;
|
||||||
|
return {
|
||||||
|
count: r.interactiveProps.length,
|
||||||
|
visible: r.scene.getObjectByName("interactive-props").visible,
|
||||||
|
hasBall:
|
||||||
|
r.mujoco.mj_name2id(
|
||||||
|
r.model,
|
||||||
|
r.mujoco.mjtObj.mjOBJ_BODY.value,
|
||||||
|
"ball",
|
||||||
|
) >= 0,
|
||||||
|
};
|
||||||
|
});
|
||||||
|
expect(toys.count).toBeGreaterThanOrEqual(5);
|
||||||
|
expect(toys.count).toBeLessThanOrEqual(7);
|
||||||
|
expect(toys.visible).toBe(true);
|
||||||
|
expect(toys.hasBall).toBe(false);
|
||||||
await page.screenshot({ path: info.outputPath("microduck.png") });
|
await page.screenshot({ path: info.outputPath("microduck.png") });
|
||||||
await page.getByLabel("Robot", { exact: true }).selectOption("microban");
|
await page.getByLabel("Robot", { exact: true }).selectOption("microban");
|
||||||
await expect
|
await expect
|
||||||
|
|||||||
@@ -26,7 +26,7 @@ for (const scenario of [
|
|||||||
world.boundary.half_extent = 1.5;
|
world.boundary.half_extent = 1.5;
|
||||||
await route.fulfill({ response, json: world });
|
await route.fulfill({ response, json: world });
|
||||||
});
|
});
|
||||||
const errors = await bootDuck(page);
|
const errors = await bootDuck(page, { legacyPhysics: true });
|
||||||
await page.evaluate(async (scenario) => {
|
await page.evaluate(async (scenario) => {
|
||||||
const r = window.rl;
|
const r = window.rl;
|
||||||
if (scenario === "rollers" || scenario === "crouch") {
|
if (scenario === "rollers" || scenario === "crouch") {
|
||||||
|
|||||||
@@ -11,7 +11,7 @@ test("Microduck → Microban → Microduck restores theme, colours and locomotio
|
|||||||
const errors = [];
|
const errors = [];
|
||||||
page.on("pageerror", (e) => errors.push(e.message));
|
page.on("pageerror", (e) => errors.push(e.message));
|
||||||
await ready(page);
|
await ready(page);
|
||||||
await page.getByRole("button", { name: "Lavender colours" }).click();
|
await page.getByRole("button", { name: "Lavender scene lines" }).click();
|
||||||
await page.getByRole("button", { name: "Rollers", exact: true }).click();
|
await page.getByRole("button", { name: "Rollers", exact: true }).click();
|
||||||
await page.waitForFunction(() => window.rl?.loco === "rollers");
|
await page.waitForFunction(() => window.rl?.loco === "rollers");
|
||||||
await page.getByLabel("Robot", { exact: true }).selectOption("microban");
|
await page.getByLabel("Robot", { exact: true }).selectOption("microban");
|
||||||
@@ -26,9 +26,9 @@ test("Microduck → Microban → Microduck restores theme, colours and locomotio
|
|||||||
await expect(
|
await expect(
|
||||||
page.getByText("Toggle head rotation", { exact: false }),
|
page.getByText("Toggle head rotation", { exact: false }),
|
||||||
).toBeVisible();
|
).toBeVisible();
|
||||||
await expect(page.getByRole("group", { name: "Duck colours" })).toHaveCount(
|
await expect(
|
||||||
0,
|
page.getByRole("group", { name: "Scene line colours" }),
|
||||||
);
|
).toBeVisible();
|
||||||
expect(await page.evaluate(() => window.rl.buildObs().length)).toBe(63);
|
expect(await page.evaluate(() => window.rl.buildObs().length)).toBe(63);
|
||||||
await page.getByLabel("Robot", { exact: true }).selectOption("microduck");
|
await page.getByLabel("Robot", { exact: true }).selectOption("microduck");
|
||||||
await page.waitForFunction(
|
await page.waitForFunction(
|
||||||
@@ -37,7 +37,7 @@ test("Microduck → Microban → Microduck restores theme, colours and locomotio
|
|||||||
window.rl?.loco === "rollers",
|
window.rl?.loco === "rollers",
|
||||||
);
|
);
|
||||||
await expect(
|
await expect(
|
||||||
page.getByRole("button", { name: "Lavender colours" }),
|
page.getByRole("button", { name: "Lavender scene lines" }),
|
||||||
).toHaveAttribute("aria-pressed", "true");
|
).toHaveAttribute("aria-pressed", "true");
|
||||||
expect(await page.evaluate(() => window.rl.data.time)).toBeLessThan(3);
|
expect(await page.evaluate(() => window.rl.data.time)).toBeLessThan(3);
|
||||||
expect(errors).toEqual([]);
|
expect(errors).toEqual([]);
|
||||||
|
|||||||
@@ -0,0 +1,274 @@
|
|||||||
|
import { test, expect } from "@playwright/test";
|
||||||
|
import { bootDuck } from "./support/simulation.js";
|
||||||
|
|
||||||
|
async function boot(page) {
|
||||||
|
await bootDuck(page);
|
||||||
|
await page.evaluate(() => window.__gameApi.pause());
|
||||||
|
}
|
||||||
|
|
||||||
|
test("all 63 RP1 material parts follow 25 physical bodies, including fixed fingers, with original shapes aligned", async ({
|
||||||
|
page,
|
||||||
|
}, info) => {
|
||||||
|
const errors = [];
|
||||||
|
page.on("pageerror", (e) => errors.push(e.message));
|
||||||
|
await boot(page);
|
||||||
|
expect(await page.evaluate(() => window.__gameApi.switchRobot("rp1"))).toBe(
|
||||||
|
true,
|
||||||
|
);
|
||||||
|
const inspect = () =>
|
||||||
|
page.evaluate(async () => {
|
||||||
|
const r = window.rl,
|
||||||
|
layer = r.scene.getObjectByName("body-visuals"),
|
||||||
|
rig = layer.parent;
|
||||||
|
rig.updateMatrixWorld(true);
|
||||||
|
const xml = new DOMParser().parseFromString(
|
||||||
|
await (await fetch("robots/rp1/visuals/rp1_hand_fixed.urdf")).text(),
|
||||||
|
"text/xml",
|
||||||
|
);
|
||||||
|
const joints = new Map(
|
||||||
|
[...xml.querySelectorAll("joint")].map((j) => [
|
||||||
|
j.querySelector("child").getAttribute("link"),
|
||||||
|
j,
|
||||||
|
]),
|
||||||
|
);
|
||||||
|
const Matrix = rig.matrix.constructor,
|
||||||
|
Euler = r.controls.object.rotation.constructor;
|
||||||
|
const correction = new Matrix().makeRotationX(Math.PI / 2);
|
||||||
|
const urdfRelative = (name, anchor) => {
|
||||||
|
const result = new Matrix();
|
||||||
|
while (name !== anchor) {
|
||||||
|
const joint = joints.get(name),
|
||||||
|
origin = joint.querySelector("origin");
|
||||||
|
const xyz = (origin?.getAttribute("xyz") ?? "0 0 0")
|
||||||
|
.split(/\s+/)
|
||||||
|
.map(Number);
|
||||||
|
const angles = (origin?.getAttribute("rpy") ?? "0 0 0")
|
||||||
|
.split(/\s+/)
|
||||||
|
.map(Number);
|
||||||
|
const matrix = new Matrix().makeRotationFromEuler(
|
||||||
|
new Euler(...angles, "ZYX"),
|
||||||
|
);
|
||||||
|
matrix.setPosition(...xyz);
|
||||||
|
result.premultiply(matrix);
|
||||||
|
name = joint.querySelector("parent").getAttribute("link");
|
||||||
|
}
|
||||||
|
return result.multiply(correction);
|
||||||
|
};
|
||||||
|
const bounds = (objects, frame) => {
|
||||||
|
const lo = [Infinity, Infinity, Infinity],
|
||||||
|
hi = [-Infinity, -Infinity, -Infinity];
|
||||||
|
const inverse = frame.matrixWorld.clone().invert(),
|
||||||
|
vector = frame.position.clone();
|
||||||
|
for (const mesh of objects) {
|
||||||
|
const transform = inverse.clone().multiply(mesh.matrixWorld),
|
||||||
|
attr = mesh.geometry.attributes.position;
|
||||||
|
for (let i = 0; i < attr.count; i++) {
|
||||||
|
vector.fromBufferAttribute(attr, i).applyMatrix4(transform);
|
||||||
|
vector.toArray().forEach((v, a) => {
|
||||||
|
lo[a] = Math.min(lo[a], v);
|
||||||
|
hi[a] = Math.max(hi[a], v);
|
||||||
|
});
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return [...lo, ...hi];
|
||||||
|
};
|
||||||
|
let frameError = 0,
|
||||||
|
boundsError = 0,
|
||||||
|
handError = 0,
|
||||||
|
handParts = 0,
|
||||||
|
normals = 0,
|
||||||
|
screens = 0,
|
||||||
|
emissive = 0,
|
||||||
|
hidden = 0,
|
||||||
|
meshes = 0;
|
||||||
|
for (const frame of layer.children) {
|
||||||
|
const body = frame.name.slice("body-visual:".length),
|
||||||
|
id = r.mujoco.mj_name2id(
|
||||||
|
r.model,
|
||||||
|
r.mujoco.mjtObj.mjOBJ_BODY.value,
|
||||||
|
body,
|
||||||
|
);
|
||||||
|
const p = Array.from(r.data.xpos.slice(id * 3, id * 3 + 3)),
|
||||||
|
q = Array.from(r.data.xquat.slice(id * 4, id * 4 + 4));
|
||||||
|
frameError = Math.max(
|
||||||
|
frameError,
|
||||||
|
...p.map((v, i) => Math.abs(v - frame.position.toArray()[i])),
|
||||||
|
...[q[1], q[2], q[3], q[0]].map((v, i) =>
|
||||||
|
Math.abs(v - frame.quaternion.toArray()[i]),
|
||||||
|
),
|
||||||
|
);
|
||||||
|
const originals = rig.children.filter(
|
||||||
|
(m) =>
|
||||||
|
m.userData.geomId !== undefined &&
|
||||||
|
r.model.geom_bodyid[m.userData.geomId] === id,
|
||||||
|
);
|
||||||
|
hidden += originals.filter((m) => !m.visible).length;
|
||||||
|
const replacements = [];
|
||||||
|
frame.traverse((node) => {
|
||||||
|
if (node.userData.sourcePart) {
|
||||||
|
handParts++;
|
||||||
|
const actual = frame.matrixWorld
|
||||||
|
.clone()
|
||||||
|
.invert()
|
||||||
|
.multiply(node.matrixWorld);
|
||||||
|
const expected = urdfRelative(
|
||||||
|
node.userData.sourcePart,
|
||||||
|
`${body.split("_")[0]}_wrist_roll_link`,
|
||||||
|
);
|
||||||
|
handError = Math.max(
|
||||||
|
handError,
|
||||||
|
...actual.elements.map((v, i) =>
|
||||||
|
Math.abs(v - expected.elements[i]),
|
||||||
|
),
|
||||||
|
);
|
||||||
|
}
|
||||||
|
if (!node.isMesh) return;
|
||||||
|
meshes++;
|
||||||
|
replacements.push(node);
|
||||||
|
for (const m of [node.material].flat()) {
|
||||||
|
if (m.normalMap?.image?.width > 0) normals++;
|
||||||
|
if (m.map?.image?.width > 0) screens++;
|
||||||
|
if (m.emissiveIntensity > 1) emissive++;
|
||||||
|
}
|
||||||
|
});
|
||||||
|
if (!body.endsWith("_wrist_link")) {
|
||||||
|
const before = bounds(originals, frame),
|
||||||
|
after = bounds(replacements, frame);
|
||||||
|
boundsError = Math.max(
|
||||||
|
boundsError,
|
||||||
|
...before.map((v, i) => Math.abs(v - after[i])),
|
||||||
|
);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return {
|
||||||
|
bodies: layer.children.length,
|
||||||
|
frameError,
|
||||||
|
boundsError,
|
||||||
|
handError,
|
||||||
|
handParts,
|
||||||
|
normals,
|
||||||
|
screens,
|
||||||
|
emissive,
|
||||||
|
hidden,
|
||||||
|
meshes,
|
||||||
|
obs: r.buildObs().length,
|
||||||
|
actions: r.lastAction.length,
|
||||||
|
actuators: r.model.nu,
|
||||||
|
};
|
||||||
|
});
|
||||||
|
const initial = await inspect();
|
||||||
|
console.log("RP1 GLB alignment", initial);
|
||||||
|
expect(initial.bodies).toBe(25);
|
||||||
|
expect(initial.handParts).toBe(40);
|
||||||
|
expect(initial.frameError).toBeLessThan(1e-12);
|
||||||
|
expect(initial.boundsError).toBeLessThan(1e-7);
|
||||||
|
expect(initial.handError).toBeLessThan(1e-10);
|
||||||
|
expect(initial.normals).toBeGreaterThanOrEqual(63);
|
||||||
|
expect(initial.screens).toBeGreaterThanOrEqual(1);
|
||||||
|
expect(initial.emissive).toBeGreaterThanOrEqual(1);
|
||||||
|
expect(initial.hidden).toBe(25);
|
||||||
|
expect(initial.obs).toBe(243);
|
||||||
|
expect(initial.actions).toBe(24);
|
||||||
|
expect(initial.actuators).toBe(24);
|
||||||
|
await page.evaluate(() => {
|
||||||
|
const r = window.rl;
|
||||||
|
r.data.qpos[3] = Math.cos(0.4);
|
||||||
|
r.data.qpos[6] = Math.sin(0.4);
|
||||||
|
for (let j = 0; j < r.model.njnt; j++)
|
||||||
|
if (r.model.jnt_type[j] === 3)
|
||||||
|
r.data.qpos[r.model.jnt_qposadr[j]] += 0.12;
|
||||||
|
r.mujoco.mj_forward(r.model, r.data);
|
||||||
|
});
|
||||||
|
await page.waitForTimeout(200);
|
||||||
|
const posed = await inspect();
|
||||||
|
expect(posed.frameError).toBeLessThan(1e-12);
|
||||||
|
expect(posed.boundsError).toBeLessThan(1e-7);
|
||||||
|
expect(posed.handError).toBeLessThan(1e-10);
|
||||||
|
await page.evaluate(() => window.rl.resetSim());
|
||||||
|
await page.getByRole("button", { name: /Keyboard controls/ }).click();
|
||||||
|
await page.mouse.click(950, 750, { button: "right" });
|
||||||
|
await page.evaluate(() => {
|
||||||
|
const r = window.rl;
|
||||||
|
r.setPropsParked(true);
|
||||||
|
r.camera.position.set(2.4, 1.35, 1.3);
|
||||||
|
r.controls.target.set(0, 0.7, 0);
|
||||||
|
r.controls.update();
|
||||||
|
});
|
||||||
|
await page.waitForTimeout(300);
|
||||||
|
await page.screenshot({ path: info.outputPath("rp1-material-front.png") });
|
||||||
|
expect(errors).toEqual([]);
|
||||||
|
});
|
||||||
|
|
||||||
|
test("RP1 GLBs change no physics, observations or ONNX actions for walking, wave and dance", async ({
|
||||||
|
page,
|
||||||
|
}) => {
|
||||||
|
await boot(page);
|
||||||
|
let textured = false;
|
||||||
|
await page.route("**/robots/rp1/robot.json*", async (route) => {
|
||||||
|
const response = await route.fetch(),
|
||||||
|
config = await response.json();
|
||||||
|
if (!textured) delete config.render.body_visuals;
|
||||||
|
await route.fulfill({ response, json: config });
|
||||||
|
});
|
||||||
|
async function run() {
|
||||||
|
expect(await page.evaluate(() => window.__gameApi.switchRobot("rp1"))).toBe(
|
||||||
|
true,
|
||||||
|
);
|
||||||
|
return page.evaluate(async () => {
|
||||||
|
const r = window.rl,
|
||||||
|
result = [];
|
||||||
|
for (const scenario of ["walk", "wave", "dance"]) {
|
||||||
|
await r.resetSim();
|
||||||
|
await r.step(100);
|
||||||
|
if (scenario === "walk") r.command.set([0.4, 0, 0.15]);
|
||||||
|
else r.policyState.request(scenario, r.data.time);
|
||||||
|
for (let i = 0; i < 5; i++) {
|
||||||
|
await r.step(50);
|
||||||
|
result.push({
|
||||||
|
scenario,
|
||||||
|
time: r.data.time,
|
||||||
|
qpos: Array.from(r.data.qpos),
|
||||||
|
qvel: Array.from(r.data.qvel),
|
||||||
|
ctrl: Array.from(r.data.ctrl),
|
||||||
|
action: Array.from(r.lastAction),
|
||||||
|
obs: Array.from(r.buildObs()),
|
||||||
|
state: r.policyState.state,
|
||||||
|
reference: r.referenceFrame,
|
||||||
|
});
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return result;
|
||||||
|
});
|
||||||
|
}
|
||||||
|
const old = await run();
|
||||||
|
await page.evaluate(() => window.__gameApi.switchRobot("microduck-legs"));
|
||||||
|
textured = true;
|
||||||
|
expect(await run()).toEqual(old);
|
||||||
|
});
|
||||||
|
|
||||||
|
test("a partial RP1 material load failure releases the candidate and retry succeeds", async ({
|
||||||
|
page,
|
||||||
|
}) => {
|
||||||
|
await boot(page);
|
||||||
|
let fail = true;
|
||||||
|
await page.route(
|
||||||
|
"**/robots/rp1/visuals/left_hip_roll_link__visual_0__left_hip_roll_link.glb",
|
||||||
|
(route) =>
|
||||||
|
fail ? route.fulfill({ status: 404, body: "missing" }) : route.continue(),
|
||||||
|
);
|
||||||
|
expect(await page.evaluate(() => window.__gameApi.switchRobot("rp1"))).toBe(
|
||||||
|
false,
|
||||||
|
);
|
||||||
|
await expect(page.getByRole("alert")).toContainText("404");
|
||||||
|
expect(await page.evaluate(() => window.__store.getState().robotId)).toBe(
|
||||||
|
"microduck-legs",
|
||||||
|
);
|
||||||
|
expect(await page.evaluate(() => window.rl.model.isDeleted())).toBe(false);
|
||||||
|
fail = false;
|
||||||
|
expect(await page.evaluate(() => window.__gameApi.retry())).toBe(true);
|
||||||
|
expect(
|
||||||
|
await page.evaluate(
|
||||||
|
() => window.rl.scene.getObjectByName("body-visuals").children.length,
|
||||||
|
),
|
||||||
|
).toBe(25);
|
||||||
|
});
|
||||||
+1
-1
@@ -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
|
||||||
|
|||||||
@@ -3,7 +3,9 @@ import { test, expect } from "@playwright/test";
|
|||||||
test("20 robot transitions release old WASM objects and plateau scene/GPU/listeners", async ({
|
test("20 robot transitions release old WASM objects and plateau scene/GPU/listeners", async ({
|
||||||
page,
|
page,
|
||||||
}, info) => {
|
}, info) => {
|
||||||
test.setTimeout(180_000);
|
// Twenty cold transitions now also decode RP1's ~93 MB textured GLBs.
|
||||||
|
// Keep allocation/listener assertions unchanged; allow load-time headroom.
|
||||||
|
test.setTimeout(240_000);
|
||||||
await page.addInitScript(() => {
|
await page.addInitScript(() => {
|
||||||
const add = EventTarget.prototype.addEventListener,
|
const add = EventTarget.prototype.addEventListener,
|
||||||
remove = EventTarget.prototype.removeEventListener,
|
remove = EventTarget.prototype.removeEventListener,
|
||||||
@@ -57,6 +59,9 @@ test("20 robot transitions release old WASM objects and plateau scene/GPU/listen
|
|||||||
dataDeleted: data.isDeleted(),
|
dataDeleted: data.isDeleted(),
|
||||||
...window.__gameApi.diagnostics(),
|
...window.__gameApi.diagnostics(),
|
||||||
listeners: window.listenerCount(),
|
listeners: window.listenerCount(),
|
||||||
|
toyGeometries:
|
||||||
|
window.rl.scene.getObjectByName("interactive-props")?.children
|
||||||
|
.length ?? 0,
|
||||||
};
|
};
|
||||||
}, id);
|
}, id);
|
||||||
expect(sample.success).toBe(true);
|
expect(sample.success).toBe(true);
|
||||||
@@ -70,8 +75,12 @@ test("20 robot transitions release old WASM objects and plateau scene/GPU/listen
|
|||||||
for (const samples of [duck, ban, rp1]) {
|
for (const samples of [duck, ban, rp1]) {
|
||||||
expect(samples.at(-1).listeners).toBe(samples[0].listeners);
|
expect(samples.at(-1).listeners).toBe(samples[0].listeners);
|
||||||
expect(samples.at(-1).sceneChildren).toBe(samples[0].sceneChildren);
|
expect(samples.at(-1).sceneChildren).toBe(samples[0].sceneChildren);
|
||||||
expect(samples.at(-1).memory.geometries).toBeLessThanOrEqual(
|
// Each runtime owns 5–7 independent toy geometries. Compare the fixed
|
||||||
samples[1].memory.geometries + 1,
|
// scene allocation after removing this intentionally variable count.
|
||||||
|
expect(
|
||||||
|
samples.at(-1).memory.geometries - samples.at(-1).toyGeometries,
|
||||||
|
).toBeLessThanOrEqual(
|
||||||
|
samples[1].memory.geometries - samples[1].toyGeometries + 1,
|
||||||
);
|
);
|
||||||
expect(samples.at(-1).memory.textures).toBeLessThanOrEqual(
|
expect(samples.at(-1).memory.textures).toBeLessThanOrEqual(
|
||||||
samples[1].memory.textures + 1,
|
samples[1].memory.textures + 1,
|
||||||
|
|||||||
@@ -0,0 +1,255 @@
|
|||||||
|
import { test, expect } from "@playwright/test";
|
||||||
|
import { bootDuck } from "./support/simulation.js";
|
||||||
|
|
||||||
|
test("bright scene has 5–7 free-body toys, no football, and four original-style line colour buttons", async ({
|
||||||
|
page,
|
||||||
|
}) => {
|
||||||
|
const errors = await bootDuck(page);
|
||||||
|
await page.evaluate(async () => {
|
||||||
|
const r = window.rl;
|
||||||
|
r.setPropsParked(false);
|
||||||
|
await r.step(60);
|
||||||
|
r.render();
|
||||||
|
});
|
||||||
|
const initial = await page.evaluate(() => {
|
||||||
|
const r = window.rl;
|
||||||
|
return {
|
||||||
|
toys: r.interactiveProps.map((p) => ({
|
||||||
|
type: p.type,
|
||||||
|
dof: p.dofAdr,
|
||||||
|
mass: r.model.body_mass[p.bodyId],
|
||||||
|
})),
|
||||||
|
meshes: r.scene.getObjectByName("interactive-props").children.length,
|
||||||
|
ballId: r.mujoco.mj_name2id(
|
||||||
|
r.model,
|
||||||
|
r.mujoco.mjtObj.mjOBJ_BODY.value,
|
||||||
|
"ball",
|
||||||
|
),
|
||||||
|
hemisphere: r.scene.parent.children.some(
|
||||||
|
(o) => o.isHemisphereLight && o.intensity >= 0.7,
|
||||||
|
),
|
||||||
|
environment: r.scene.parent.environmentIntensity,
|
||||||
|
robotMaterials: [...r.scene.children]
|
||||||
|
.filter((o) => o.name !== "interactive-props")
|
||||||
|
.flatMap((o) => {
|
||||||
|
const result = [];
|
||||||
|
o.traverse((m) => {
|
||||||
|
if (m.isMesh && m.material?.color)
|
||||||
|
result.push([m.uuid, m.material.color.toArray()]);
|
||||||
|
});
|
||||||
|
return result;
|
||||||
|
}),
|
||||||
|
};
|
||||||
|
});
|
||||||
|
expect(initial.toys.length).toBeGreaterThanOrEqual(5);
|
||||||
|
expect(initial.toys.length).toBeLessThanOrEqual(7);
|
||||||
|
expect(initial.meshes).toBe(initial.toys.length);
|
||||||
|
expect(new Set(initial.toys.map((p) => p.type)).size).toBe(4);
|
||||||
|
expect(initial.toys.every((p) => p.mass > 0 && p.dof >= 0)).toBe(true);
|
||||||
|
expect(initial.ballId).toBe(-1);
|
||||||
|
expect(initial.hemisphere).toBe(true);
|
||||||
|
expect(initial.environment).toBeGreaterThanOrEqual(0.7);
|
||||||
|
const group = page.getByRole("group", { name: "Scene line colours" });
|
||||||
|
await expect(group.getByRole("button")).toHaveCount(4);
|
||||||
|
for (const [label, hex] of [
|
||||||
|
["Cream", "f7e6cb"],
|
||||||
|
["Graphite", "6c6a68"],
|
||||||
|
["Lavender", "bfa9cf"],
|
||||||
|
["Sky", "a9dbe8"],
|
||||||
|
]) {
|
||||||
|
const button = group.getByRole("button", { name: `${label} scene lines` });
|
||||||
|
await button.click();
|
||||||
|
await expect(button).toHaveAttribute("aria-pressed", "true");
|
||||||
|
const values = await page.evaluate(() => {
|
||||||
|
const r = window.rl,
|
||||||
|
lines = [],
|
||||||
|
robotMaterials = [];
|
||||||
|
r.scene.traverse((o) => {
|
||||||
|
if (o.material?.uniforms?.uSectionColor)
|
||||||
|
lines.push(o.material.uniforms.uSectionColor.value.getHexString());
|
||||||
|
});
|
||||||
|
for (const o of r.scene.children.filter(
|
||||||
|
(o) => o.name !== "interactive-props",
|
||||||
|
))
|
||||||
|
o.traverse((m) => {
|
||||||
|
if (m.isMesh && m.material?.color)
|
||||||
|
robotMaterials.push([m.uuid, m.material.color.toArray()]);
|
||||||
|
});
|
||||||
|
const style = getComputedStyle(
|
||||||
|
document.querySelector('[aria-label="Scene line colours"] button'),
|
||||||
|
);
|
||||||
|
return {
|
||||||
|
lines,
|
||||||
|
robotMaterials,
|
||||||
|
radius: style.borderRadius,
|
||||||
|
width: style.width,
|
||||||
|
height: style.height,
|
||||||
|
};
|
||||||
|
});
|
||||||
|
expect(values.lines).toEqual(Array(5).fill(hex));
|
||||||
|
expect(values.robotMaterials).toEqual(initial.robotMaterials);
|
||||||
|
expect(values.radius).toBe("0px");
|
||||||
|
expect(values.width).toBe(values.height);
|
||||||
|
}
|
||||||
|
await page.evaluate(() => {
|
||||||
|
const r = window.rl;
|
||||||
|
r.chaseCam = false;
|
||||||
|
r.camera.position.set(-0.2, 1.9, 2.7);
|
||||||
|
r.controls.target.set(0.1, 0.05, 0);
|
||||||
|
r.controls.update();
|
||||||
|
});
|
||||||
|
await page.screenshot({
|
||||||
|
path: "test-results/scene-interaction-overview.png",
|
||||||
|
});
|
||||||
|
expect(errors).toEqual([]);
|
||||||
|
});
|
||||||
|
|
||||||
|
test("geometry can be dragged with physical force; pause/reset clear grab and restore layout", async ({
|
||||||
|
page,
|
||||||
|
}) => {
|
||||||
|
const errors = await bootDuck(page);
|
||||||
|
await page.evaluate(() => {
|
||||||
|
const r = window.rl;
|
||||||
|
r.setPropsParked(false);
|
||||||
|
r.chaseCam = false;
|
||||||
|
const spec = r.interactiveProps[0],
|
||||||
|
q = r.data.qpos;
|
||||||
|
q.set([-0.25, 0, spec.pos[2]], spec.qAdr);
|
||||||
|
r.mujoco.mj_forward(r.model, r.data);
|
||||||
|
r.camera.position.set(-0.6, 0.65, 1.2);
|
||||||
|
r.controls.target.set(q[0], q[2], -q[1]);
|
||||||
|
r.controls.update();
|
||||||
|
r.camera.updateMatrixWorld();
|
||||||
|
r.render();
|
||||||
|
});
|
||||||
|
await page.waitForTimeout(250);
|
||||||
|
const point = await page.evaluate(() => {
|
||||||
|
const r = window.rl,
|
||||||
|
spec = r.interactiveProps[0],
|
||||||
|
q = r.data.qpos;
|
||||||
|
const p = r.controls.target
|
||||||
|
.clone()
|
||||||
|
.set(q[spec.qAdr], q[spec.qAdr + 2], -q[spec.qAdr + 1])
|
||||||
|
.project(r.camera);
|
||||||
|
const rect = document.querySelector("canvas").getBoundingClientRect();
|
||||||
|
return {
|
||||||
|
x: ((p.x + 1) * rect.width) / 2,
|
||||||
|
y: ((1 - p.y) * rect.height) / 2,
|
||||||
|
};
|
||||||
|
});
|
||||||
|
await page.mouse.move(point.x, point.y);
|
||||||
|
await page.mouse.down();
|
||||||
|
expect(await page.evaluate(() => window.rl.controls.enabled)).toBe(false);
|
||||||
|
await page.mouse.move(point.x + 90, point.y - 40, { steps: 6 });
|
||||||
|
const result = await page.evaluate(async () => {
|
||||||
|
const r = window.rl,
|
||||||
|
spec = r.interactiveProps[0];
|
||||||
|
await r.step(15);
|
||||||
|
return {
|
||||||
|
x: r.data.qpos[spec.qAdr],
|
||||||
|
force: Array.from(
|
||||||
|
r.data.xfrc_applied.slice(spec.bodyId * 6, spec.bodyId * 6 + 3),
|
||||||
|
),
|
||||||
|
};
|
||||||
|
});
|
||||||
|
expect(Math.abs(result.x + 0.25)).toBeGreaterThan(0.01);
|
||||||
|
expect(result.force.some((v) => Math.abs(v) > 0.001)).toBe(true);
|
||||||
|
await page.evaluate(() => window.__gameApi.pause());
|
||||||
|
expect(await page.evaluate(() => window.rl.controls.enabled)).toBe(true);
|
||||||
|
await page.mouse.up();
|
||||||
|
expect(
|
||||||
|
await page.evaluate(() =>
|
||||||
|
Array.from(window.rl.data.xfrc_applied).every((v) => v === 0),
|
||||||
|
),
|
||||||
|
).toBe(true);
|
||||||
|
await page.evaluate(() => window.__gameApi.reset());
|
||||||
|
expect(
|
||||||
|
await page.evaluate(() => {
|
||||||
|
const r = window.rl;
|
||||||
|
return (
|
||||||
|
r.interactiveProps.every((p) =>
|
||||||
|
p.pos.every((v, i) => r.data.qpos[p.qAdr + i] === v),
|
||||||
|
) && r.controls.enabled
|
||||||
|
);
|
||||||
|
}),
|
||||||
|
).toBe(true);
|
||||||
|
expect(errors).toEqual([]);
|
||||||
|
});
|
||||||
|
|
||||||
|
test("toys transfer collision momentum, collide with the robot and floor, and survive a mode switch", async ({
|
||||||
|
page,
|
||||||
|
}) => {
|
||||||
|
const errors = await bootDuck(page);
|
||||||
|
const collision = await page.evaluate(async () => {
|
||||||
|
const r = window.rl;
|
||||||
|
r.setPropsParked(false);
|
||||||
|
const specs = r.interactiveProps;
|
||||||
|
// Keep unrelated toys clear of a two-body collision lane.
|
||||||
|
specs.forEach((p, i) => {
|
||||||
|
r.data.qpos.set([-1 + i * 0.3, -1.1, p.pos[2]], p.qAdr);
|
||||||
|
});
|
||||||
|
const a = specs[0],
|
||||||
|
b = specs[1];
|
||||||
|
r.data.qpos.set([0.1, 0.7, a.pos[2]], a.qAdr);
|
||||||
|
r.data.qpos.set([0.35, 0.7, b.pos[2]], b.qAdr);
|
||||||
|
r.data.qvel[a.dofAdr] = 1.8;
|
||||||
|
r.mujoco.mj_forward(r.model, r.data);
|
||||||
|
await r.step(15);
|
||||||
|
const moved = r.data.qpos[b.qAdr] - 0.35;
|
||||||
|
// Launch a toy at the duck. A non-colliding body would cross x=-0.6.
|
||||||
|
r.resetSim();
|
||||||
|
const p = r.interactiveProps.find((s) => s.type === "sphere");
|
||||||
|
r.data.qpos.set([-0.35, 0, 0.09], p.qAdr);
|
||||||
|
r.data.qvel[p.dofAdr] = -1.5;
|
||||||
|
r.mujoco.mj_forward(r.model, r.data);
|
||||||
|
let contacts = 0,
|
||||||
|
minX = 10;
|
||||||
|
for (let i = 0; i < 15; i++) {
|
||||||
|
await r.step(1);
|
||||||
|
contacts = Math.max(contacts, r.data.ncon);
|
||||||
|
minX = Math.min(minX, r.data.qpos[p.qAdr]);
|
||||||
|
}
|
||||||
|
await r.step(100);
|
||||||
|
const half = r.world.boundary.half_extent;
|
||||||
|
r.data.qpos.set([half - 0.2, 0.9, p.pos[2]], p.qAdr);
|
||||||
|
r.data.qvel.fill(0, p.dofAdr, p.dofAdr + 6);
|
||||||
|
r.data.qvel[p.dofAdr] = 1.5;
|
||||||
|
r.mujoco.mj_forward(r.model, r.data);
|
||||||
|
let wallMax = -Infinity;
|
||||||
|
for (let i = 0; i < 15; i++) {
|
||||||
|
await r.step(1);
|
||||||
|
wallMax = Math.max(wallMax, r.data.qpos[p.qAdr]);
|
||||||
|
}
|
||||||
|
return {
|
||||||
|
moved,
|
||||||
|
contacts,
|
||||||
|
minX,
|
||||||
|
wallMax,
|
||||||
|
half,
|
||||||
|
finite: Array.from(r.data.qpos).every(Number.isFinite),
|
||||||
|
aboveFloor: r.interactiveProps.every(
|
||||||
|
(s) => r.data.qpos[s.qAdr + 2] > 0.025,
|
||||||
|
),
|
||||||
|
};
|
||||||
|
});
|
||||||
|
expect(collision.moved).toBeGreaterThan(0.025);
|
||||||
|
expect(collision.contacts).toBeGreaterThan(0);
|
||||||
|
expect(collision.minX).toBeGreaterThan(-0.58);
|
||||||
|
expect(collision.wallMax).toBeLessThan(collision.half - 0.03);
|
||||||
|
expect(collision.aboveFloor).toBe(true);
|
||||||
|
expect(collision.finite).toBe(true);
|
||||||
|
await page.evaluate(() => window.rl.setLoco("rollers", { force: true }));
|
||||||
|
const switched = await page.evaluate(async () => {
|
||||||
|
const r = window.rl;
|
||||||
|
await r.step(100);
|
||||||
|
r.render();
|
||||||
|
return {
|
||||||
|
count: r.interactiveProps.length,
|
||||||
|
meshes: r.scene.getObjectByName("interactive-props").children.length,
|
||||||
|
finite: Array.from(r.data.qpos).every(Number.isFinite),
|
||||||
|
};
|
||||||
|
});
|
||||||
|
expect(switched.count).toBe(switched.meshes);
|
||||||
|
expect(switched.finite).toBe(true);
|
||||||
|
expect(errors).toEqual([]);
|
||||||
|
});
|
||||||
@@ -0,0 +1,68 @@
|
|||||||
|
import { test, expect } from "@playwright/test";
|
||||||
|
import { bootDuck } from "./support/simulation.js";
|
||||||
|
|
||||||
|
test("two simulated minutes of movement with physical toys stay finite in feet and rollers; escape recovery is local", async ({
|
||||||
|
page,
|
||||||
|
}) => {
|
||||||
|
const errors = await bootDuck(page);
|
||||||
|
await page.evaluate(() => {
|
||||||
|
const r = window.rl;
|
||||||
|
r.setPropsParked(false);
|
||||||
|
// A deterministic controller source exercises actual commanded motion,
|
||||||
|
// independent of frame-rate keyboard polling during manual stepping.
|
||||||
|
r.controller.addSource({
|
||||||
|
id: "stability-drive",
|
||||||
|
isActive: () => true,
|
||||||
|
command: new Float32Array([0.25, 0, 0.4]),
|
||||||
|
});
|
||||||
|
});
|
||||||
|
for (const mode of ["legs", "rollers"]) {
|
||||||
|
await page.evaluate(async (mode) => {
|
||||||
|
const r = window.rl;
|
||||||
|
await r.setLoco(mode, { force: true });
|
||||||
|
r.controller.sources
|
||||||
|
.find((s) => s.id === "stability-drive")
|
||||||
|
.command.set(mode === "legs" ? [0.25, 0, 0.4] : [0.4, 0, 0.15]);
|
||||||
|
}, mode);
|
||||||
|
let moved = false;
|
||||||
|
for (let chunk = 0; chunk < 30; chunk++) {
|
||||||
|
const sample = await page.evaluate(async () => {
|
||||||
|
const r = window.rl;
|
||||||
|
await r.step(100);
|
||||||
|
return {
|
||||||
|
finite: [r.data.qpos, r.data.qvel, r.data.ctrl].every((array) =>
|
||||||
|
Array.from(array).every(Number.isFinite),
|
||||||
|
),
|
||||||
|
moved: Math.hypot(r.data.qpos[0] + 0.6, r.data.qpos[1]) > 0.04,
|
||||||
|
inside: r.interactiveProps.every(
|
||||||
|
(p) =>
|
||||||
|
Math.abs(r.data.qpos[p.qAdr]) <
|
||||||
|
r.world.boundary.half_extent + 0.11 &&
|
||||||
|
Math.abs(r.data.qpos[p.qAdr + 1]) <
|
||||||
|
r.world.boundary.half_extent + 0.11,
|
||||||
|
),
|
||||||
|
};
|
||||||
|
});
|
||||||
|
expect(sample.finite).toBe(true);
|
||||||
|
expect(sample.inside).toBe(true);
|
||||||
|
moved ||= sample.moved;
|
||||||
|
}
|
||||||
|
expect(moved).toBe(true);
|
||||||
|
}
|
||||||
|
const recovery = await page.evaluate(async () => {
|
||||||
|
const r = window.rl,
|
||||||
|
p = r.interactiveProps[0];
|
||||||
|
r.data.qpos[p.qAdr] = r.world.boundary.half_extent + 1;
|
||||||
|
r.mujoco.mj_forward(r.model, r.data);
|
||||||
|
await r.step(1);
|
||||||
|
return {
|
||||||
|
actual: Array.from(r.data.qpos.slice(p.qAdr, p.qAdr + 3)),
|
||||||
|
expected: p.pos,
|
||||||
|
count: r.interactiveProps.length,
|
||||||
|
};
|
||||||
|
});
|
||||||
|
expect(recovery.actual).toEqual(recovery.expected);
|
||||||
|
expect(recovery.count).toBeGreaterThanOrEqual(5);
|
||||||
|
expect(recovery.count).toBeLessThanOrEqual(7);
|
||||||
|
expect(errors).toEqual([]);
|
||||||
|
});
|
||||||
@@ -0,0 +1,157 @@
|
|||||||
|
import { test, expect } from "@playwright/test";
|
||||||
|
import { bootDuck } from "./support/simulation.js";
|
||||||
|
|
||||||
|
test("all robots share the same scaled random layout, lighting, line palette and global colour selection", async ({
|
||||||
|
page,
|
||||||
|
}) => {
|
||||||
|
const errors = await bootDuck(page);
|
||||||
|
let reference;
|
||||||
|
let expected = { name: "Sky", key: "blue", hex: "a9dbe8" };
|
||||||
|
for (const id of ["microduck-legs", "microban", "rp1", "microduck-legs"]) {
|
||||||
|
if ((await page.evaluate(() => window.__store.getState().robotId)) !== id)
|
||||||
|
await page.evaluate((id) => window.__gameApi.switchRobot(id), id);
|
||||||
|
await expect(
|
||||||
|
page.getByRole("group", { name: "Scene line colours" }),
|
||||||
|
).toBeVisible();
|
||||||
|
if (!reference)
|
||||||
|
await page.getByRole("button", { name: "Sky scene lines" }).click();
|
||||||
|
const state = await page.evaluate(() => {
|
||||||
|
const r = window.rl,
|
||||||
|
scale = r.world.boundary.half_extent / 2.25;
|
||||||
|
const lines = [];
|
||||||
|
r.scene.traverse((o) => {
|
||||||
|
if (o.material?.uniforms?.uSectionColor)
|
||||||
|
lines.push(o.material.uniforms.uSectionColor.value.getHexString());
|
||||||
|
});
|
||||||
|
return {
|
||||||
|
specs: r.interactiveProps.map((p) => ({
|
||||||
|
name: p.name,
|
||||||
|
type: p.type,
|
||||||
|
color: p.color,
|
||||||
|
pos: p.pos.map((v) => v / scale),
|
||||||
|
size: p.size.map((v) => v / scale),
|
||||||
|
})),
|
||||||
|
count: r.scene.getObjectByName("interactive-props").children.length,
|
||||||
|
lines,
|
||||||
|
lineColor: window.__store.getState().lineColor,
|
||||||
|
lights: r.scene.parent.children
|
||||||
|
.filter((o) => o.isLight)
|
||||||
|
.map((o) => [o.type, o.intensity, o.color.getHex()]),
|
||||||
|
environment: r.scene.parent.environmentIntensity,
|
||||||
|
drag: window.__store.getState().runtimeCapabilities.physicalDrag,
|
||||||
|
};
|
||||||
|
});
|
||||||
|
expect(state.count).toBeGreaterThanOrEqual(5);
|
||||||
|
expect(state.count).toBeLessThanOrEqual(7);
|
||||||
|
expect(state.lines).toEqual(Array(5).fill(expected.hex));
|
||||||
|
expect(state.lineColor).toBe(expected.key);
|
||||||
|
expect(state.drag).toBe(true);
|
||||||
|
if (!reference) reference = state;
|
||||||
|
state.specs.forEach((p, i) => {
|
||||||
|
expect(p.type).toBe(reference.specs[i].type);
|
||||||
|
expect(p.color).toBe(reference.specs[i].color);
|
||||||
|
p.pos.forEach((v, j) =>
|
||||||
|
expect(v).toBeCloseTo(reference.specs[i].pos[j], 12),
|
||||||
|
);
|
||||||
|
p.size.forEach((v, j) =>
|
||||||
|
expect(v).toBeCloseTo(reference.specs[i].size[j], 12),
|
||||||
|
);
|
||||||
|
});
|
||||||
|
expect(state.lights).toEqual(reference.lights);
|
||||||
|
expect(state.environment).toBe(reference.environment);
|
||||||
|
await expect(
|
||||||
|
page.getByRole("button", { name: `${expected.name} scene lines` }),
|
||||||
|
).toHaveAttribute("aria-pressed", "true");
|
||||||
|
await expect(
|
||||||
|
page.getByRole("button", { name: "Rollers", exact: true }),
|
||||||
|
).toHaveCount(id === "microduck-legs" ? 1 : 0);
|
||||||
|
// Also carry a choice made in another robot back to Microduck.
|
||||||
|
if (id === "rp1") {
|
||||||
|
await page.getByRole("button", { name: "Lavender scene lines" }).click();
|
||||||
|
expected = { name: "Lavender", key: "purple", hex: "bfa9cf" };
|
||||||
|
}
|
||||||
|
}
|
||||||
|
expect(errors).toEqual([]);
|
||||||
|
});
|
||||||
|
|
||||||
|
for (const id of ["microban", "rp1"]) {
|
||||||
|
test(`${id}: shared toys collide with walls, drag with real force and release on pause`, async ({
|
||||||
|
page,
|
||||||
|
}) => {
|
||||||
|
const errors = await bootDuck(page);
|
||||||
|
await page.evaluate((id) => window.__gameApi.switchRobot(id), id);
|
||||||
|
const wall = await page.evaluate(async () => {
|
||||||
|
const r = window.rl,
|
||||||
|
scale = r.world.boundary.half_extent / 2.25,
|
||||||
|
p = r.interactiveProps.find((s) => s.type === "sphere"),
|
||||||
|
half = r.world.boundary.half_extent;
|
||||||
|
r.data.qpos.set([half - 0.2 * scale, 0.9 * scale, p.pos[2]], p.qAdr);
|
||||||
|
r.data.qvel[p.dofAdr] = 1.5 * scale;
|
||||||
|
r.mujoco.mj_forward(r.model, r.data);
|
||||||
|
let max = -Infinity;
|
||||||
|
for (let i = 0; i < 15; i++) {
|
||||||
|
await r.step(1);
|
||||||
|
max = Math.max(max, r.data.qpos[p.qAdr]);
|
||||||
|
}
|
||||||
|
await r.resetSim();
|
||||||
|
return { max, limit: half - 0.025 * scale };
|
||||||
|
});
|
||||||
|
expect(wall.max).toBeLessThan(wall.limit);
|
||||||
|
await page.getByRole("button", { name: /Keyboard controls/ }).click();
|
||||||
|
await page.evaluate(() => {
|
||||||
|
const r = window.rl,
|
||||||
|
scale = r.world.scale ?? 1,
|
||||||
|
p = r.interactiveProps[0],
|
||||||
|
q = r.data.qpos;
|
||||||
|
q.set([0.25 * scale, -0.3 * scale, p.pos[2]], p.qAdr);
|
||||||
|
r.mujoco.mj_forward(r.model, r.data);
|
||||||
|
r.camera.position.set(q[0], q[2] + 0.6 * scale, 1.5 * scale);
|
||||||
|
r.controls.target.set(q[0], q[2], -q[1]);
|
||||||
|
r.controls.update();
|
||||||
|
window.__gameApi.resume();
|
||||||
|
});
|
||||||
|
await page.waitForTimeout(200);
|
||||||
|
const point = await page.evaluate(() => {
|
||||||
|
const r = window.rl,
|
||||||
|
p = r.interactiveProps[0],
|
||||||
|
q = r.data.qpos,
|
||||||
|
rect = document.querySelector("canvas").getBoundingClientRect();
|
||||||
|
const v = r.controls.target
|
||||||
|
.clone()
|
||||||
|
.set(q[p.qAdr], q[p.qAdr + 2], -q[p.qAdr + 1])
|
||||||
|
.project(r.camera);
|
||||||
|
return {
|
||||||
|
x: ((v.x + 1) * rect.width) / 2,
|
||||||
|
y: ((1 - v.y) * rect.height) / 2,
|
||||||
|
};
|
||||||
|
});
|
||||||
|
await page.mouse.move(point.x, point.y);
|
||||||
|
await page.mouse.down();
|
||||||
|
expect(await page.evaluate(() => window.rl.controls.enabled)).toBe(false);
|
||||||
|
await page.mouse.move(point.x + 70, point.y - 35, { steps: 5 });
|
||||||
|
await page.waitForTimeout(200);
|
||||||
|
expect(
|
||||||
|
await page.evaluate(() =>
|
||||||
|
Array.from(window.rl.data.xfrc_applied).some((v) => Math.abs(v) > 1e-5),
|
||||||
|
),
|
||||||
|
).toBe(true);
|
||||||
|
await page.evaluate(() => window.__gameApi.pause());
|
||||||
|
expect(await page.evaluate(() => window.rl.controls.enabled)).toBe(true);
|
||||||
|
expect(
|
||||||
|
await page.evaluate(() =>
|
||||||
|
Array.from(window.rl.data.xfrc_applied).every((v) => v === 0),
|
||||||
|
),
|
||||||
|
).toBe(true);
|
||||||
|
await page.mouse.up();
|
||||||
|
await page.evaluate(() => window.__gameApi.reset());
|
||||||
|
expect(
|
||||||
|
await page.evaluate(() => {
|
||||||
|
const r = window.rl;
|
||||||
|
return r.interactiveProps.every((p) =>
|
||||||
|
p.pos.every((v, i) => r.data.qpos[p.qAdr + i] === v),
|
||||||
|
);
|
||||||
|
}),
|
||||||
|
).toBe(true);
|
||||||
|
expect(errors).toEqual([]);
|
||||||
|
});
|
||||||
|
}
|
||||||
@@ -1,5 +1,5 @@
|
|||||||
import { expect } from "@playwright/test";
|
import { expect } from "@playwright/test";
|
||||||
export async function bootDuck(page) {
|
export async function bootDuck(page, { legacyPhysics = false } = {}) {
|
||||||
const errors = [];
|
const errors = [];
|
||||||
page.on("pageerror", (e) => errors.push(e.message));
|
page.on("pageerror", (e) => errors.push(e.message));
|
||||||
await page.addInitScript(() => {
|
await page.addInitScript(() => {
|
||||||
@@ -7,7 +7,7 @@ export async function bootDuck(page) {
|
|||||||
Math.random = () =>
|
Math.random = () =>
|
||||||
(seed = (1664525 * seed + 1013904223) >>> 0) / 4294967296;
|
(seed = (1664525 * seed + 1013904223) >>> 0) / 4294967296;
|
||||||
});
|
});
|
||||||
await page.goto("/?boot=1&test=1");
|
await page.goto(`/?boot=1&test=1${legacyPhysics ? "&oracle=legacy" : ""}`);
|
||||||
await page.waitForFunction(
|
await page.waitForFunction(
|
||||||
() => window.rl?.prepareTest || window.__store?.getState().bootFailed,
|
() => window.rl?.prepareTest || window.__store?.getState().bootFailed,
|
||||||
);
|
);
|
||||||
|
|||||||
@@ -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([]);
|
||||||
|
});
|
||||||
Generated
+2
-2
@@ -1,12 +1,12 @@
|
|||||||
{
|
{
|
||||||
"name": "microduck-sandbox",
|
"name": "microduck-sandbox",
|
||||||
"version": "0.4.0",
|
"version": "0.6.0",
|
||||||
"lockfileVersion": 3,
|
"lockfileVersion": 3,
|
||||||
"requires": true,
|
"requires": true,
|
||||||
"packages": {
|
"packages": {
|
||||||
"": {
|
"": {
|
||||||
"name": "microduck-sandbox",
|
"name": "microduck-sandbox",
|
||||||
"version": "0.4.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
@@ -1,18 +1,20 @@
|
|||||||
{
|
{
|
||||||
"name": "microduck-sandbox",
|
"name": "microduck-sandbox",
|
||||||
"private": true,
|
"private": true,
|
||||||
"version": "0.4.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",
|
||||||
|
|||||||
@@ -0,0 +1,54 @@
|
|||||||
|
# Microduck — Arduino UNO Q 实机控制与无硬件联调框架
|
||||||
|
|
||||||
|
该目录为 **Microduck** 在 **Arduino UNO Q**(QRB2210 Debian Linux MPU + STM32U585 Zephyr/Arduino MCU)上的完整板端控制架构,兼容 **Arduino App Lab** 目录规范。
|
||||||
|
|
||||||
|
## 目录结构
|
||||||
|
|
||||||
|
```text
|
||||||
|
hardware/unoq/
|
||||||
|
├── app.yaml # Arduino App Lab 清单
|
||||||
|
├── config/
|
||||||
|
│ ├── robot_manifest.json # 与网页仿真对齐的策略/关节/观测/状态机配置(可由网站导入覆盖)
|
||||||
|
│ └── hardware_profile.json # 14 路飞特 HD-1910-C001 与 QMI8658 标定参数(在线修改持久化)
|
||||||
|
├── policies/ # ONNX 策略存放目录(由网站导出包或 HTTP 推送填充)
|
||||||
|
├── python/
|
||||||
|
│ ├── main.py # 服务入口(支持 --hal mock 与 --hal router)
|
||||||
|
│ ├── core/ # 配置、61D 观测、50Hz 状态机、ONNX 推理、部署解包
|
||||||
|
│ ├── hal/ # RouterBridgeHal (实机) & MockHal (PC 无硬件联调)
|
||||||
|
│ └── web/ # FastAPI / Dev Fallback HTTP+WebSocket 服务与离线手机控制页
|
||||||
|
├── sketch/ # STM32U585 Arduino 固件 (QMI8658 + 飞特 HD-1910-C001 + Bridge)
|
||||||
|
└── scripts/
|
||||||
|
├── install_deps.sh # 板端 Python 依赖安装
|
||||||
|
└── setup_ap.sh # 一键开启 UNO Q Wi-Fi 热点 (Microduck-AP -> 192.168.4.1)
|
||||||
|
```
|
||||||
|
|
||||||
|
## 1. 无硬件 PC 端预联调(立即运行)
|
||||||
|
|
||||||
|
即使暂时未连接 Arduino UNO Q 硬件,也可以在电脑本地启动完整服务测试:
|
||||||
|
|
||||||
|
```bash
|
||||||
|
cd hardware/unoq
|
||||||
|
python3 python/main.py --hal mock --policy-backend dummy --port 8000
|
||||||
|
```
|
||||||
|
|
||||||
|
> 若环境已安装 `onnxruntime`、`fastapi`、`uvicorn`,可直接省略 `--policy-backend dummy`(默认 `auto` 会优先使用真实 `onnxruntime` 与 `FastAPI`)。
|
||||||
|
|
||||||
|
随后:
|
||||||
|
1. 在浏览器启动 Microduck 仿真网站(`cd app && npm run dev`),点击顶部工具栏 **Hardware (UNO Q)**,输入 `http://127.0.0.1:8000` 并点击 **推送到 UNO Q**,即可测试在线导入控制文件与热重载。
|
||||||
|
2. 用浏览器打开 `http://127.0.0.1:8000`(或模拟手机视图),可测试虚拟摇杆、技能按钮、E-STOP 急停、准备站立软启动与舵机/IMU 标定页。
|
||||||
|
|
||||||
|
## 2. 上电连接 Arduino UNO Q 调试流程
|
||||||
|
|
||||||
|
1. **导出并拷入项目**:在仿真网站点击 **Hardware (UNO Q) -> 下载完整部署包 (.zip)**,解压后得到完整 `microduck_unoq/` 目录(已包含你选中的 `.onnx` 策略与配置),放入 UNO Q `/home/arduino/ArduinoApps/microduck_unoq/`(或通过 Arduino App Lab 打开)。
|
||||||
|
2. **确认 STM32 引脚与总线宏**([sketch/config.h](sketch/config.h)):
|
||||||
|
- `SERVO_SERIAL`(默认 `Serial1`)、`SERVO_BAUD`(默认 `1000000`)、`SERVO_DIR_PIN`(`-1` 为硬件自动流向/单线,使用 74HC126 等方向引脚时填对应 GPIO)。
|
||||||
|
- `IMU_WIRE`(`Wire` 或 UNO Q Qwiic 接口对应的 `Wire1`)、`IMU_I2C_ADDR`(`0x6A` 或 `0x6B`)。
|
||||||
|
3. **烧录 STM32 并启动 App Lab 应用**:
|
||||||
|
- 使用 Arduino App Lab 或 `arduino-app-cli` 编译上传 `sketch/` 并启动 `python/main.py`(默认 `--hal router`)。
|
||||||
|
- 如需开启 Wi-Fi 热点:在 UNO Q Linux 终端执行 `sudo bash scripts/setup_ap.sh`,手机连接 `Microduck-AP` 后访问 `http://192.168.4.1:8000`。
|
||||||
|
4. **首次上机标定(务必先悬空/架起机身!)**:
|
||||||
|
- 打开手机页面 **标定调试** 页签:
|
||||||
|
- 点击 **扫描舵机 ID** 确认 14 个飞特 HD-1910-C001 全部在线;
|
||||||
|
- 点击 **IMU 静止零偏校准**,观察站立放置时 `projected_gravity` 接近 `[0, 0, -1]`;若不一致调整 `imu.axis_map`;
|
||||||
|
- 逐关节小幅测试并校准 `sign`(`+1`/`-1`)与 `zero_offset_ticks`(一键设为当前物理中位);
|
||||||
|
- 确认无误后回到 **控制** 页签,拉低 **速度比例滑条(如 30%)**,点击 **准备站立** 完成 1.5s 软插值,再开始步态测试。
|
||||||
@@ -0,0 +1,6 @@
|
|||||||
|
name: microduck-unoq
|
||||||
|
description: Microduck RL policy runtime, mobile Wi-Fi AP controller, and STM32 HAL for Arduino UNO Q (QMI8658 + Feetech HD-1910-C001)
|
||||||
|
version: 0.1.0
|
||||||
|
ports:
|
||||||
|
- 8000
|
||||||
|
bricks: []
|
||||||
@@ -0,0 +1,147 @@
|
|||||||
|
{
|
||||||
|
"schema": 1,
|
||||||
|
"board": "arduino_uno_q",
|
||||||
|
"servo": {
|
||||||
|
"model": "FEETECH_HD-1910-C001",
|
||||||
|
"protocol": "hls_sts_half_duplex",
|
||||||
|
"baudrate": 1000000,
|
||||||
|
"ticks_per_rev": 4096,
|
||||||
|
"neutral_tick": 2048,
|
||||||
|
"speed_limit": 0,
|
||||||
|
"acc": 50,
|
||||||
|
"torque_limit": 1000,
|
||||||
|
"vel_source": "fd",
|
||||||
|
"vel_lpf_alpha": 0.4,
|
||||||
|
"vel_scale_rad_s_per_unit": 0.0015339807878856412
|
||||||
|
},
|
||||||
|
"imu": {
|
||||||
|
"model": "QMI8658",
|
||||||
|
"i2c_addr": "0x6A",
|
||||||
|
"axis_map": [
|
||||||
|
[
|
||||||
|
1,
|
||||||
|
0,
|
||||||
|
0
|
||||||
|
],
|
||||||
|
[
|
||||||
|
0,
|
||||||
|
1,
|
||||||
|
0
|
||||||
|
],
|
||||||
|
[
|
||||||
|
0,
|
||||||
|
0,
|
||||||
|
1
|
||||||
|
]
|
||||||
|
],
|
||||||
|
"gyro_bias_rad_s": [0.0, 0.0, 0.0],
|
||||||
|
"accel_bias_g": [0.0, 0.0, 0.0]
|
||||||
|
},
|
||||||
|
"joints": {
|
||||||
|
"left_hip_yaw": {
|
||||||
|
"id": 1,
|
||||||
|
"sign": 1,
|
||||||
|
"zero_offset_ticks": 0,
|
||||||
|
"min_rad": -0.4363323129985824,
|
||||||
|
"max_rad": 0.5235987755982988
|
||||||
|
},
|
||||||
|
"left_hip_roll": {
|
||||||
|
"id": 2,
|
||||||
|
"sign": 1,
|
||||||
|
"zero_offset_ticks": 0,
|
||||||
|
"min_rad": -0.3839724354387525,
|
||||||
|
"max_rad": 0.3839724354387525
|
||||||
|
},
|
||||||
|
"left_hip_pitch": {
|
||||||
|
"id": 3,
|
||||||
|
"sign": 1,
|
||||||
|
"zero_offset_ticks": 0,
|
||||||
|
"min_rad": -1.570796326795005,
|
||||||
|
"max_rad": 1.5707963267947882
|
||||||
|
},
|
||||||
|
"left_knee": {
|
||||||
|
"id": 4,
|
||||||
|
"sign": 1,
|
||||||
|
"zero_offset_ticks": 0,
|
||||||
|
"min_rad": -1.570796326795012,
|
||||||
|
"max_rad": 1.570796326794781
|
||||||
|
},
|
||||||
|
"left_ankle": {
|
||||||
|
"id": 5,
|
||||||
|
"sign": 1,
|
||||||
|
"zero_offset_ticks": 0,
|
||||||
|
"min_rad": -1.5707963267949019,
|
||||||
|
"max_rad": 1.5707963267948912
|
||||||
|
},
|
||||||
|
"neck_pitch": {
|
||||||
|
"id": 6,
|
||||||
|
"sign": 1,
|
||||||
|
"zero_offset_ticks": 0,
|
||||||
|
"min_rad": -1.5707963267948966,
|
||||||
|
"max_rad": 1.0471975511965976
|
||||||
|
},
|
||||||
|
"head_pitch": {
|
||||||
|
"id": 7,
|
||||||
|
"sign": 1,
|
||||||
|
"zero_offset_ticks": 0,
|
||||||
|
"min_rad": -1.5707963267948974,
|
||||||
|
"max_rad": 1.5707963267948957
|
||||||
|
},
|
||||||
|
"head_yaw": {
|
||||||
|
"id": 8,
|
||||||
|
"sign": 1,
|
||||||
|
"zero_offset_ticks": 0,
|
||||||
|
"min_rad": -2.967059728390373,
|
||||||
|
"max_rad": 2.967059728390348
|
||||||
|
},
|
||||||
|
"head_roll": {
|
||||||
|
"id": 9,
|
||||||
|
"sign": 1,
|
||||||
|
"zero_offset_ticks": 0,
|
||||||
|
"min_rad": -0.43633231299859127,
|
||||||
|
"max_rad": 0.4363323129985735
|
||||||
|
},
|
||||||
|
"right_hip_yaw": {
|
||||||
|
"id": 10,
|
||||||
|
"sign": 1,
|
||||||
|
"zero_offset_ticks": 0,
|
||||||
|
"min_rad": -0.5235987755982988,
|
||||||
|
"max_rad": 0.4363323129985824
|
||||||
|
},
|
||||||
|
"right_hip_roll": {
|
||||||
|
"id": 11,
|
||||||
|
"sign": 1,
|
||||||
|
"zero_offset_ticks": 0,
|
||||||
|
"min_rad": -0.3839724354387525,
|
||||||
|
"max_rad": 0.3839724354387525
|
||||||
|
},
|
||||||
|
"right_hip_pitch": {
|
||||||
|
"id": 12,
|
||||||
|
"sign": 1,
|
||||||
|
"zero_offset_ticks": 0,
|
||||||
|
"min_rad": -1.5707963267949268,
|
||||||
|
"max_rad": 1.5707963267948664
|
||||||
|
},
|
||||||
|
"right_knee": {
|
||||||
|
"id": 13,
|
||||||
|
"sign": 1,
|
||||||
|
"zero_offset_ticks": 0,
|
||||||
|
"min_rad": -1.570796326794932,
|
||||||
|
"max_rad": 1.570796326794861
|
||||||
|
},
|
||||||
|
"right_ankle": {
|
||||||
|
"id": 14,
|
||||||
|
"sign": 1,
|
||||||
|
"zero_offset_ticks": 0,
|
||||||
|
"min_rad": -1.5707963267949054,
|
||||||
|
"max_rad": 1.5707963267948877
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"safety": {
|
||||||
|
"soft_start_s": 1.5,
|
||||||
|
"watchdog_ms": 150,
|
||||||
|
"ws_timeout_ms": 500,
|
||||||
|
"auto_stand_on_fall": false,
|
||||||
|
"cmd_vel_scale": 0.5
|
||||||
|
}
|
||||||
|
}
|
||||||
@@ -0,0 +1,127 @@
|
|||||||
|
{
|
||||||
|
"schema": 1,
|
||||||
|
"robot_id": "microduck-legs",
|
||||||
|
"family": "microduck",
|
||||||
|
"sim": {
|
||||||
|
"timestep": 0.005,
|
||||||
|
"decimation": 4,
|
||||||
|
"ctrl_dt": 0.02,
|
||||||
|
"hz": 50
|
||||||
|
},
|
||||||
|
"obs_size": 61,
|
||||||
|
"action_size": 14,
|
||||||
|
"command_size": 13,
|
||||||
|
"joint_names": [
|
||||||
|
"left_hip_yaw",
|
||||||
|
"left_hip_roll",
|
||||||
|
"left_hip_pitch",
|
||||||
|
"left_knee",
|
||||||
|
"left_ankle",
|
||||||
|
"neck_pitch",
|
||||||
|
"head_pitch",
|
||||||
|
"head_yaw",
|
||||||
|
"head_roll",
|
||||||
|
"right_hip_yaw",
|
||||||
|
"right_hip_roll",
|
||||||
|
"right_hip_pitch",
|
||||||
|
"right_knee",
|
||||||
|
"right_ankle"
|
||||||
|
],
|
||||||
|
"default_pose": [
|
||||||
|
0.0,
|
||||||
|
-0.0872664600610733,
|
||||||
|
-0.4579240083694458,
|
||||||
|
-0.004939999897032976,
|
||||||
|
0.4529840052127838,
|
||||||
|
0.3490658402442932,
|
||||||
|
0.3490658402442932,
|
||||||
|
0.0,
|
||||||
|
0.0,
|
||||||
|
0.0,
|
||||||
|
0.0872664600610733,
|
||||||
|
0.4579240083694458,
|
||||||
|
0.004939999897032976,
|
||||||
|
-0.4529840052127838
|
||||||
|
],
|
||||||
|
"action_scale": 1.0,
|
||||||
|
"variants": {
|
||||||
|
"legs": {
|
||||||
|
"limits": [0.25, -0.2, 1.0],
|
||||||
|
"enabled": true
|
||||||
|
},
|
||||||
|
"rollers": {
|
||||||
|
"limits": [0.6, -0.5, 0.3],
|
||||||
|
"enabled": false
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"legacy": {
|
||||||
|
"crouch_period": 5.0,
|
||||||
|
"crouch_end": 0.7,
|
||||||
|
"groundpick_period": 4.0,
|
||||||
|
"groundpick_end": 0.7
|
||||||
|
},
|
||||||
|
"head": {
|
||||||
|
"max_rad": 2.5,
|
||||||
|
"alpha": 0.2,
|
||||||
|
"signs": [1, -1, 1, -1]
|
||||||
|
},
|
||||||
|
"safety": {
|
||||||
|
"tilt_z": -0.5,
|
||||||
|
"upright_z": -0.85,
|
||||||
|
"fall_debounce_steps": 10,
|
||||||
|
"fall_settle_steps": 15,
|
||||||
|
"recover_upright_steps": 50,
|
||||||
|
"recover_giveup_steps": 300,
|
||||||
|
"kick_steps": 25,
|
||||||
|
"post_kick_lock_steps": 20,
|
||||||
|
"roll_max_steps": 150,
|
||||||
|
"sit_delay_s": 0.8,
|
||||||
|
"stand_delay_s": 2.0
|
||||||
|
},
|
||||||
|
"skills": {
|
||||||
|
"walk": {
|
||||||
|
"path": "policies/walk.onnx",
|
||||||
|
"kind": "perpetual",
|
||||||
|
"slot": "walk",
|
||||||
|
"action_scale": 1.0
|
||||||
|
},
|
||||||
|
"stand": {
|
||||||
|
"path": "policies/stand.onnx",
|
||||||
|
"kind": "recovery",
|
||||||
|
"slot": "stand",
|
||||||
|
"action_scale": 1.0
|
||||||
|
},
|
||||||
|
"sitstand": {
|
||||||
|
"path": "policies/sitstand.onnx",
|
||||||
|
"kind": "perpetual",
|
||||||
|
"slot": "sitstand",
|
||||||
|
"action_scale": 1.0
|
||||||
|
},
|
||||||
|
"kickL": {
|
||||||
|
"path": "policies/kickL.onnx",
|
||||||
|
"kind": "episodic",
|
||||||
|
"slot": "kickL",
|
||||||
|
"action_scale": 1.0
|
||||||
|
},
|
||||||
|
"kickR": {
|
||||||
|
"path": "policies/kickR.onnx",
|
||||||
|
"kind": "episodic",
|
||||||
|
"slot": "kickR",
|
||||||
|
"action_scale": 1.0
|
||||||
|
},
|
||||||
|
"groundpick": {
|
||||||
|
"path": "policies/groundpick.onnx",
|
||||||
|
"kind": "episodic",
|
||||||
|
"slot": "groundpick",
|
||||||
|
"action_scale": 1.0
|
||||||
|
},
|
||||||
|
"roll": {
|
||||||
|
"path": "policies/roll.onnx",
|
||||||
|
"kind": "episodic",
|
||||||
|
"slot": "roll",
|
||||||
|
"action_scale": 1.0,
|
||||||
|
"duration_s": 3.0
|
||||||
|
}
|
||||||
|
},
|
||||||
|
"custom_move": null
|
||||||
|
}
|
||||||
@@ -0,0 +1,133 @@
|
|||||||
|
{
|
||||||
|
"schema": 1,
|
||||||
|
"files": [
|
||||||
|
{
|
||||||
|
"path": "app.yaml",
|
||||||
|
"executable": false
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"path": "config/hardware_profile.json",
|
||||||
|
"executable": false
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"path": "config/robot_manifest.json",
|
||||||
|
"executable": false
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"path": "python/__init__.py",
|
||||||
|
"executable": false
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"path": "python/core/__init__.py",
|
||||||
|
"executable": false
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"path": "python/core/config.py",
|
||||||
|
"executable": false
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"path": "python/core/controller.py",
|
||||||
|
"executable": false
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"path": "python/core/deploy_service.py",
|
||||||
|
"executable": false
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"path": "python/core/observations.py",
|
||||||
|
"executable": false
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"path": "python/core/policy_runner.py",
|
||||||
|
"executable": false
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"path": "python/core/state_machine.py",
|
||||||
|
"executable": false
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"path": "python/hal/__init__.py",
|
||||||
|
"executable": false
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"path": "python/hal/base.py",
|
||||||
|
"executable": false
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"path": "python/hal/mock_hal.py",
|
||||||
|
"executable": false
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"path": "python/hal/router_hal.py",
|
||||||
|
"executable": false
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"path": "python/main.py",
|
||||||
|
"executable": false
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"path": "python/requirements.txt",
|
||||||
|
"executable": false
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"path": "python/web/__init__.py",
|
||||||
|
"executable": false
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"path": "python/web/api.py",
|
||||||
|
"executable": false
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"path": "python/web/fallback_server.py",
|
||||||
|
"executable": false
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"path": "python/web/server.py",
|
||||||
|
"executable": false
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"path": "python/web/static/index.html",
|
||||||
|
"executable": false
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"path": "README.md",
|
||||||
|
"executable": false
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"path": "scripts/install_deps.sh",
|
||||||
|
"executable": true
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"path": "scripts/setup_ap.sh",
|
||||||
|
"executable": true
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"path": "sketch/config.h",
|
||||||
|
"executable": false
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"path": "sketch/feetech_hls.cpp",
|
||||||
|
"executable": false
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"path": "sketch/feetech_hls.h",
|
||||||
|
"executable": false
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"path": "sketch/qmi8658.cpp",
|
||||||
|
"executable": false
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"path": "sketch/qmi8658.h",
|
||||||
|
"executable": false
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"path": "sketch/sketch.ino",
|
||||||
|
"executable": false
|
||||||
|
},
|
||||||
|
{
|
||||||
|
"path": "sketch/sketch.yaml",
|
||||||
|
"executable": false
|
||||||
|
}
|
||||||
|
]
|
||||||
|
}
|
||||||
@@ -0,0 +1 @@
|
|||||||
|
"""Microduck Arduino UNO Q Python runtime package."""
|
||||||
@@ -0,0 +1 @@
|
|||||||
|
"""Core configuration, observation pipeline, ONNX runner, and control loop."""
|
||||||
@@ -0,0 +1,176 @@
|
|||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import json
|
||||||
|
import math
|
||||||
|
from pathlib import Path
|
||||||
|
from typing import Any
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
|
||||||
|
TWO_PI = 2.0 * math.pi
|
||||||
|
|
||||||
|
|
||||||
|
def validate_robot_manifest(manifest: dict[str, Any]) -> dict[str, Any]:
|
||||||
|
if not isinstance(manifest, dict):
|
||||||
|
raise ValueError("robot_manifest must be a JSON object")
|
||||||
|
if manifest.get("obs_size") != 61:
|
||||||
|
raise ValueError(f"Unsupported obs_size {manifest.get('obs_size')}; expected 61")
|
||||||
|
if manifest.get("action_size") != 14:
|
||||||
|
raise ValueError(f"Unsupported action_size {manifest.get('action_size')}; expected 14")
|
||||||
|
if manifest.get("command_size") != 13:
|
||||||
|
raise ValueError(f"Unsupported command_size {manifest.get('command_size')}; expected 13")
|
||||||
|
|
||||||
|
joint_names = manifest.get("joint_names")
|
||||||
|
default_pose = manifest.get("default_pose")
|
||||||
|
if not isinstance(joint_names, list) or len(joint_names) != 14:
|
||||||
|
raise ValueError("robot_manifest.joint_names must contain 14 joints")
|
||||||
|
if not isinstance(default_pose, list) or len(default_pose) != 14:
|
||||||
|
raise ValueError("robot_manifest.default_pose must contain 14 floats")
|
||||||
|
if not all(isinstance(v, (int, float)) and math.isfinite(float(v)) for v in default_pose):
|
||||||
|
raise ValueError("robot_manifest.default_pose contains non-finite value")
|
||||||
|
|
||||||
|
skills = manifest.get("skills")
|
||||||
|
if not isinstance(skills, dict):
|
||||||
|
raise ValueError("robot_manifest.skills must be an object")
|
||||||
|
for required in ("walk", "stand"):
|
||||||
|
if required not in skills or not isinstance(skills[required], dict) or not skills[required].get("path"):
|
||||||
|
raise ValueError(f"robot_manifest.skills missing required '{required}' policy")
|
||||||
|
return manifest
|
||||||
|
|
||||||
|
|
||||||
|
def load_robot_manifest(path: str | Path) -> dict[str, Any]:
|
||||||
|
with Path(path).open("r", encoding="utf-8") as f:
|
||||||
|
data = json.load(f)
|
||||||
|
return validate_robot_manifest(data)
|
||||||
|
|
||||||
|
|
||||||
|
def validate_hardware_profile(profile: dict[str, Any], joint_names: list[str] | None = None) -> dict[str, Any]:
|
||||||
|
if not isinstance(profile, dict):
|
||||||
|
raise ValueError("hardware_profile must be a JSON object")
|
||||||
|
servo = profile.get("servo")
|
||||||
|
if not isinstance(servo, dict):
|
||||||
|
raise ValueError("hardware_profile.servo missing")
|
||||||
|
ticks_per_rev = int(servo.get("ticks_per_rev", 0))
|
||||||
|
if ticks_per_rev <= 0:
|
||||||
|
raise ValueError("hardware_profile.servo.ticks_per_rev must be > 0")
|
||||||
|
|
||||||
|
imu = profile.get("imu")
|
||||||
|
if not isinstance(imu, dict):
|
||||||
|
raise ValueError("hardware_profile.imu missing")
|
||||||
|
axis_map = np.asarray(imu.get("axis_map", []), dtype=np.float64)
|
||||||
|
if axis_map.shape != (3, 3) or not np.all(np.isfinite(axis_map)):
|
||||||
|
raise ValueError("hardware_profile.imu.axis_map must be a 3x3 finite matrix")
|
||||||
|
|
||||||
|
joints = profile.get("joints")
|
||||||
|
if not isinstance(joints, dict):
|
||||||
|
raise ValueError("hardware_profile.joints missing")
|
||||||
|
expected = joint_names or list(joints.keys())
|
||||||
|
if len(expected) != 14:
|
||||||
|
raise ValueError("hardware_profile.joints must configure all 14 joints")
|
||||||
|
seen_ids: set[int] = set()
|
||||||
|
for name in expected:
|
||||||
|
if name not in joints:
|
||||||
|
raise ValueError(f"hardware_profile.joints missing '{name}'")
|
||||||
|
cfg = joints[name]
|
||||||
|
jid = int(cfg["id"])
|
||||||
|
if not (0 <= jid <= 253) or jid in seen_ids:
|
||||||
|
raise ValueError(f"Invalid or duplicate servo id {jid} for joint {name}")
|
||||||
|
seen_ids.add(jid)
|
||||||
|
if int(cfg["sign"]) not in (-1, 1):
|
||||||
|
raise ValueError(f"Joint {name} sign must be -1 or 1")
|
||||||
|
if float(cfg["min_rad"]) > float(cfg["max_rad"]):
|
||||||
|
raise ValueError(f"Joint {name} min_rad > max_rad")
|
||||||
|
return profile
|
||||||
|
|
||||||
|
|
||||||
|
def load_hardware_profile(path: str | Path, joint_names: list[str] | None = None) -> dict[str, Any]:
|
||||||
|
with Path(path).open("r", encoding="utf-8") as f:
|
||||||
|
data = json.load(f)
|
||||||
|
return validate_hardware_profile(data, joint_names=joint_names)
|
||||||
|
|
||||||
|
|
||||||
|
def save_hardware_profile(path: str | Path, profile: dict[str, Any], joint_names: list[str] | None = None) -> dict[str, Any]:
|
||||||
|
validated = validate_hardware_profile(profile, joint_names=joint_names)
|
||||||
|
target = Path(path)
|
||||||
|
target.parent.mkdir(parents=True, exist_ok=True)
|
||||||
|
tmp = target.with_suffix(".json.tmp")
|
||||||
|
with tmp.open("w", encoding="utf-8") as f:
|
||||||
|
json.dump(validated, f, indent=2, ensure_ascii=False)
|
||||||
|
f.write("\n")
|
||||||
|
tmp.replace(target)
|
||||||
|
return validated
|
||||||
|
|
||||||
|
|
||||||
|
def rad_to_ticks(q_rad: np.ndarray | list[float], joint_names: list[str], profile: dict[str, Any]) -> list[int]:
|
||||||
|
servo = profile["servo"]
|
||||||
|
ticks_per_rev = int(servo["ticks_per_rev"])
|
||||||
|
neutral = int(servo["neutral_tick"])
|
||||||
|
rad_per_tick = TWO_PI / ticks_per_rev
|
||||||
|
joints_cfg = profile["joints"]
|
||||||
|
max_tick = ticks_per_rev - 1
|
||||||
|
|
||||||
|
out: list[int] = []
|
||||||
|
for idx, name in enumerate(joint_names):
|
||||||
|
cfg = joints_cfg[name]
|
||||||
|
angle = float(q_rad[idx])
|
||||||
|
if not math.isfinite(angle):
|
||||||
|
raise ValueError(f"Non-finite angle for joint {name}")
|
||||||
|
angle = max(float(cfg["min_rad"]), min(float(cfg["max_rad"]), angle))
|
||||||
|
tick = round(neutral + int(cfg["zero_offset_ticks"]) + int(cfg["sign"]) * (angle / rad_per_tick))
|
||||||
|
tick = max(0, min(max_tick, tick))
|
||||||
|
out.append(int(tick))
|
||||||
|
return out
|
||||||
|
|
||||||
|
|
||||||
|
def ticks_to_rad(ticks: np.ndarray | list[int], joint_names: list[str], profile: dict[str, Any]) -> np.ndarray:
|
||||||
|
servo = profile["servo"]
|
||||||
|
ticks_per_rev = int(servo["ticks_per_rev"])
|
||||||
|
neutral = int(servo["neutral_tick"])
|
||||||
|
rad_per_tick = TWO_PI / ticks_per_rev
|
||||||
|
joints_cfg = profile["joints"]
|
||||||
|
|
||||||
|
out = np.zeros(len(joint_names), dtype=np.float64)
|
||||||
|
for idx, name in enumerate(joint_names):
|
||||||
|
cfg = joints_cfg[name]
|
||||||
|
zero = neutral + int(cfg["zero_offset_ticks"])
|
||||||
|
out[idx] = int(cfg["sign"]) * (float(ticks[idx]) - zero) * rad_per_tick
|
||||||
|
return out
|
||||||
|
|
||||||
|
|
||||||
|
def raw_vel_to_rad_s(raw_vel: list[int], joint_names: list[str], profile: dict[str, Any]) -> np.ndarray:
|
||||||
|
"""Decode Feetech signed-magnitude speed ticks (bit 15 = negative) to rad/s."""
|
||||||
|
servo = profile["servo"]
|
||||||
|
scale = float(servo.get("vel_scale_rad_s_per_unit", TWO_PI / int(servo["ticks_per_rev"])))
|
||||||
|
joints_cfg = profile["joints"]
|
||||||
|
out = np.zeros(len(joint_names), dtype=np.float64)
|
||||||
|
for idx, name in enumerate(joint_names):
|
||||||
|
raw = int(raw_vel[idx])
|
||||||
|
mag = raw & 0x7FFF
|
||||||
|
signed = -mag if (raw & 0x8000) else mag
|
||||||
|
out[idx] = int(joints_cfg[name]["sign"]) * signed * scale
|
||||||
|
return out
|
||||||
|
|
||||||
|
|
||||||
|
def apply_imu_calibration(
|
||||||
|
gyro_raw: np.ndarray | list[float],
|
||||||
|
grav_raw: np.ndarray | list[float],
|
||||||
|
profile: dict[str, Any],
|
||||||
|
) -> tuple[np.ndarray, np.ndarray]:
|
||||||
|
imu = profile["imu"]
|
||||||
|
axis_map = np.asarray(imu["axis_map"], dtype=np.float64)
|
||||||
|
gyro_bias = np.asarray(imu.get("gyro_bias_rad_s", [0.0, 0.0, 0.0]), dtype=np.float64)
|
||||||
|
|
||||||
|
g_in = np.asarray(gyro_raw, dtype=np.float64)
|
||||||
|
p_in = np.asarray(grav_raw, dtype=np.float64)
|
||||||
|
if g_in.shape != (3,) or p_in.shape != (3,):
|
||||||
|
raise ValueError("IMU vectors must be 3D")
|
||||||
|
if not (np.all(np.isfinite(g_in)) and np.all(np.isfinite(p_in))):
|
||||||
|
raise ValueError("IMU readings contain non-finite values")
|
||||||
|
|
||||||
|
gyro = axis_map @ (g_in - gyro_bias)
|
||||||
|
grav = axis_map @ p_in
|
||||||
|
norm = float(np.linalg.norm(grav))
|
||||||
|
if norm > 1e-6:
|
||||||
|
grav = grav / norm
|
||||||
|
return gyro, grav
|
||||||
@@ -0,0 +1,380 @@
|
|||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import threading
|
||||||
|
import time
|
||||||
|
from pathlib import Path
|
||||||
|
from typing import Any
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
from hal.base import HardwareBridge, HardwareFrame
|
||||||
|
|
||||||
|
from .config import (
|
||||||
|
apply_imu_calibration,
|
||||||
|
rad_to_ticks,
|
||||||
|
raw_vel_to_rad_s,
|
||||||
|
save_hardware_profile,
|
||||||
|
ticks_to_rad,
|
||||||
|
)
|
||||||
|
from .observations import build_observation, decode_action
|
||||||
|
from .policy_runner import PolicySet
|
||||||
|
from .state_machine import LifecycleState, MicroduckStateMachine
|
||||||
|
|
||||||
|
|
||||||
|
class MicroduckController:
|
||||||
|
"""50Hz real-time control loop coordinating HAL, 61D observations, ONNX policies, and safety."""
|
||||||
|
|
||||||
|
def __init__(
|
||||||
|
self,
|
||||||
|
base_dir: str | Path,
|
||||||
|
manifest: dict[str, Any],
|
||||||
|
profile: dict[str, Any],
|
||||||
|
hal: HardwareBridge,
|
||||||
|
policies: PolicySet,
|
||||||
|
custom_script: dict[str, Any] | None = None,
|
||||||
|
profile_path: str | Path | None = None,
|
||||||
|
) -> None:
|
||||||
|
self.base_dir = Path(base_dir)
|
||||||
|
self.profile_path = Path(profile_path) if profile_path else self.base_dir / "config" / "hardware_profile.json"
|
||||||
|
self.manifest = manifest
|
||||||
|
self.profile = profile
|
||||||
|
self.hal = hal
|
||||||
|
self.policies = policies
|
||||||
|
|
||||||
|
self.joint_names: list[str] = list(manifest["joint_names"])
|
||||||
|
self.default_pose = np.asarray(manifest["default_pose"], dtype=np.float32)
|
||||||
|
self.ctrl_dt = float(manifest.get("sim", {}).get("ctrl_dt", 0.02))
|
||||||
|
|
||||||
|
self.state_machine = MicroduckStateMachine(manifest, custom_script=custom_script)
|
||||||
|
self._lock = threading.RLock()
|
||||||
|
self._running = False
|
||||||
|
self._thread: threading.Thread | None = None
|
||||||
|
|
||||||
|
self.last_action = np.zeros(14, dtype=np.float32)
|
||||||
|
self._last_q_rad: np.ndarray | None = None
|
||||||
|
self._vel_lpf = np.zeros(14, dtype=np.float64)
|
||||||
|
self._last_remote_input_s: float = 0.0
|
||||||
|
self._last_frame: HardwareFrame | None = None
|
||||||
|
self._last_gyro = np.zeros(3, dtype=np.float64)
|
||||||
|
self._last_grav = np.array([0.0, 0.0, -1.0], dtype=np.float64)
|
||||||
|
self._last_q = self.default_pose.astype(np.float64).copy()
|
||||||
|
self._last_targets = self.default_pose.astype(np.float64).copy()
|
||||||
|
self._ctrl_hz = 0.0
|
||||||
|
self._last_error: str | None = None
|
||||||
|
self._calib_target_ticks: list[int] | None = None
|
||||||
|
self._calib_torque_on = False
|
||||||
|
|
||||||
|
def connect(self) -> None:
|
||||||
|
with self._lock:
|
||||||
|
self.hal.connect()
|
||||||
|
self.hal.configure(self.joint_names, self.profile)
|
||||||
|
self._last_frame = self.hal.sync_step(
|
||||||
|
enable_torque=False,
|
||||||
|
target_ticks=rad_to_ticks(self.default_pose, self.joint_names, self.profile),
|
||||||
|
)
|
||||||
|
self._last_gyro, self._last_grav = apply_imu_calibration(
|
||||||
|
self._last_frame.gyro_raw, self._last_frame.proj_gravity_raw, self.profile
|
||||||
|
)
|
||||||
|
self._last_q = ticks_to_rad(self._last_frame.ticks, self.joint_names, self.profile)
|
||||||
|
|
||||||
|
def start(self) -> None:
|
||||||
|
if self._running:
|
||||||
|
return
|
||||||
|
self.connect()
|
||||||
|
self._running = True
|
||||||
|
self._thread = threading.Thread(target=self._loop, name="microduck-50hz", daemon=True)
|
||||||
|
self._thread.start()
|
||||||
|
|
||||||
|
def stop(self) -> None:
|
||||||
|
self._running = False
|
||||||
|
if self._thread is not None:
|
||||||
|
self._thread.join(timeout=2.0)
|
||||||
|
self._thread = None
|
||||||
|
with self._lock:
|
||||||
|
try:
|
||||||
|
self.state_machine.emergency_stop()
|
||||||
|
self.hal.set_torque(False)
|
||||||
|
finally:
|
||||||
|
self.hal.close()
|
||||||
|
|
||||||
|
def _loop(self) -> None:
|
||||||
|
next_t = time.monotonic()
|
||||||
|
hz_t0 = next_t
|
||||||
|
ticks = 0
|
||||||
|
while self._running:
|
||||||
|
now = time.monotonic()
|
||||||
|
try:
|
||||||
|
self.step_once(dt=self.ctrl_dt, now_s=now)
|
||||||
|
self._last_error = None
|
||||||
|
except Exception as exc:
|
||||||
|
self._last_error = str(exc)
|
||||||
|
with self._lock:
|
||||||
|
self.state_machine.emergency_stop()
|
||||||
|
try:
|
||||||
|
self.hal.set_torque(False)
|
||||||
|
except Exception:
|
||||||
|
pass
|
||||||
|
ticks += 1
|
||||||
|
if now - hz_t0 >= 0.5:
|
||||||
|
self._ctrl_hz = ticks / max(now - hz_t0, 1e-6)
|
||||||
|
ticks = 0
|
||||||
|
hz_t0 = now
|
||||||
|
next_t += self.ctrl_dt
|
||||||
|
sleep_s = next_t - time.monotonic()
|
||||||
|
if sleep_s > 0:
|
||||||
|
time.sleep(sleep_s)
|
||||||
|
else:
|
||||||
|
next_t = time.monotonic()
|
||||||
|
|
||||||
|
def set_remote_input(
|
||||||
|
self,
|
||||||
|
norm_x: float,
|
||||||
|
norm_wz: float,
|
||||||
|
speed_scale: float | None = None,
|
||||||
|
now_s: float | None = None,
|
||||||
|
) -> None:
|
||||||
|
with self._lock:
|
||||||
|
scale = (
|
||||||
|
float(speed_scale)
|
||||||
|
if speed_scale is not None
|
||||||
|
else float(self.profile.get("safety", {}).get("cmd_vel_scale", 0.5))
|
||||||
|
)
|
||||||
|
self.state_machine.set_velocity_cmd(norm_x, norm_wz, speed_scale=scale)
|
||||||
|
self._last_remote_input_s = time.monotonic() if now_s is None else float(now_s)
|
||||||
|
|
||||||
|
def set_head_target(self, neck_pitch: float, head_pitch: float, head_yaw: float, head_roll: float) -> None:
|
||||||
|
with self._lock:
|
||||||
|
self.state_machine.set_head_target(neck_pitch, head_pitch, head_yaw, head_roll)
|
||||||
|
|
||||||
|
def set_auto_stand_on_fall(self, enabled: bool) -> None:
|
||||||
|
with self._lock:
|
||||||
|
self.profile.setdefault("safety", {})["auto_stand_on_fall"] = bool(enabled)
|
||||||
|
save_hardware_profile(self.profile_path, self.profile, self.joint_names)
|
||||||
|
|
||||||
|
def set_speed_scale(self, scale: float) -> None:
|
||||||
|
with self._lock:
|
||||||
|
clamped = max(0.1, min(1.0, float(scale)))
|
||||||
|
self.profile.setdefault("safety", {})["cmd_vel_scale"] = clamped
|
||||||
|
save_hardware_profile(self.profile_path, self.profile, self.joint_names)
|
||||||
|
|
||||||
|
def request_stand(self) -> None:
|
||||||
|
with self._lock:
|
||||||
|
if "walk" not in self.policies.policies:
|
||||||
|
raise RuntimeError("No control policies loaded yet — deploy a bundle from the website first")
|
||||||
|
self._calib_torque_on = False
|
||||||
|
self._calib_target_ticks = None
|
||||||
|
soft_start_s = float(self.profile.get("safety", {}).get("soft_start_s", 1.5))
|
||||||
|
self.state_machine.request_stand(self._last_q, soft_start_s=soft_start_s)
|
||||||
|
|
||||||
|
def emergency_stop(self) -> None:
|
||||||
|
with self._lock:
|
||||||
|
self._calib_torque_on = False
|
||||||
|
self._calib_target_ticks = None
|
||||||
|
self.state_machine.emergency_stop()
|
||||||
|
self.last_action.fill(0)
|
||||||
|
self.hal.set_torque(False)
|
||||||
|
|
||||||
|
def trigger_action(self, action: str) -> bool:
|
||||||
|
with self._lock:
|
||||||
|
return self.state_machine.trigger_action(action)
|
||||||
|
|
||||||
|
def reload_runtime(
|
||||||
|
self,
|
||||||
|
manifest: dict[str, Any],
|
||||||
|
policies: PolicySet,
|
||||||
|
profile: dict[str, Any] | None = None,
|
||||||
|
custom_script: dict[str, Any] | None = None,
|
||||||
|
) -> None:
|
||||||
|
with self._lock:
|
||||||
|
self.state_machine.emergency_stop()
|
||||||
|
self.hal.set_torque(False)
|
||||||
|
self.manifest = manifest
|
||||||
|
self.policies = policies
|
||||||
|
self.joint_names = list(manifest["joint_names"])
|
||||||
|
self.default_pose = np.asarray(manifest["default_pose"], dtype=np.float32)
|
||||||
|
self.ctrl_dt = float(manifest.get("sim", {}).get("ctrl_dt", 0.02))
|
||||||
|
self.state_machine.update_manifest(manifest, custom_script=custom_script)
|
||||||
|
if profile is not None:
|
||||||
|
self.profile = profile
|
||||||
|
self.hal.configure(self.joint_names, self.profile)
|
||||||
|
self.last_action.fill(0)
|
||||||
|
self._last_error = None
|
||||||
|
|
||||||
|
def update_profile(self, profile: dict[str, Any]) -> dict[str, Any]:
|
||||||
|
with self._lock:
|
||||||
|
self.profile = save_hardware_profile(self.profile_path, profile, self.joint_names)
|
||||||
|
self.hal.configure(self.joint_names, self.profile)
|
||||||
|
return self.profile
|
||||||
|
|
||||||
|
def capture_joint_zero(self, joint_name: str) -> dict[str, Any]:
|
||||||
|
with self._lock:
|
||||||
|
if joint_name not in self.joint_names:
|
||||||
|
raise ValueError(f"Unknown joint '{joint_name}'")
|
||||||
|
idx = self.joint_names.index(joint_name)
|
||||||
|
if self._last_frame is None:
|
||||||
|
raise RuntimeError("No hardware frame captured yet")
|
||||||
|
neutral = int(self.profile["servo"]["neutral_tick"])
|
||||||
|
current_tick = int(self._last_frame.ticks[idx])
|
||||||
|
self.profile["joints"][joint_name]["zero_offset_ticks"] = current_tick - neutral
|
||||||
|
return self.update_profile(self.profile)
|
||||||
|
|
||||||
|
def test_single_joint(self, joint_name: str, target_rad: float) -> list[int]:
|
||||||
|
with self._lock:
|
||||||
|
if joint_name not in self.joint_names:
|
||||||
|
raise ValueError(f"Unknown joint '{joint_name}'")
|
||||||
|
if self.state_machine.lifecycle not in (LifecycleState.IDLE, LifecycleState.CALIBRATION):
|
||||||
|
raise RuntimeError("Emergency stop first before testing individual joints")
|
||||||
|
self.state_machine.enter_calibration()
|
||||||
|
idx = self.joint_names.index(joint_name)
|
||||||
|
pose = self._last_q.copy()
|
||||||
|
pose[idx] = float(target_rad)
|
||||||
|
ticks = rad_to_ticks(pose, self.joint_names, self.profile)
|
||||||
|
self._calib_target_ticks = ticks
|
||||||
|
self._calib_torque_on = True
|
||||||
|
return ticks
|
||||||
|
|
||||||
|
def exit_calibration(self) -> None:
|
||||||
|
with self._lock:
|
||||||
|
self._calib_torque_on = False
|
||||||
|
self._calib_target_ticks = None
|
||||||
|
self.state_machine.exit_calibration()
|
||||||
|
self.hal.set_torque(False)
|
||||||
|
|
||||||
|
def calibrate_imu_zero(self, samples: int = 200) -> dict[str, Any]:
|
||||||
|
with self._lock:
|
||||||
|
bias = self.hal.calibrate_imu_raw(samples=samples)
|
||||||
|
self.profile["imu"]["gyro_bias_rad_s"] = [float(b) for b in bias]
|
||||||
|
self.update_profile(self.profile)
|
||||||
|
return self.profile["imu"]
|
||||||
|
|
||||||
|
def ping_servos(self, max_id: int = 20) -> list[int]:
|
||||||
|
with self._lock:
|
||||||
|
return self.hal.ping_servos(max_id=max_id)
|
||||||
|
|
||||||
|
def _compute_joint_vel(self, q_rad: np.ndarray, raw_speeds: list[int], dt: float) -> np.ndarray:
|
||||||
|
servo_cfg = self.profile["servo"]
|
||||||
|
if servo_cfg.get("vel_source") == "servo":
|
||||||
|
return raw_vel_to_rad_s(raw_speeds, self.joint_names, self.profile)
|
||||||
|
if self._last_q_rad is None:
|
||||||
|
self._last_q_rad = q_rad.copy()
|
||||||
|
self._vel_lpf.fill(0.0)
|
||||||
|
return self._vel_lpf.copy()
|
||||||
|
fd = (q_rad - self._last_q_rad) / max(dt, 1e-4)
|
||||||
|
self._last_q_rad = q_rad.copy()
|
||||||
|
alpha = float(servo_cfg.get("vel_lpf_alpha", 0.4))
|
||||||
|
self._vel_lpf = (1.0 - alpha) * self._vel_lpf + alpha * fd
|
||||||
|
return self._vel_lpf.copy()
|
||||||
|
|
||||||
|
def step_once(self, dt: float | None = None, now_s: float | None = None) -> dict[str, Any]:
|
||||||
|
with self._lock:
|
||||||
|
dt_s = self.ctrl_dt if dt is None else float(dt)
|
||||||
|
now = time.monotonic() if now_s is None else float(now_s)
|
||||||
|
|
||||||
|
# Enforce mobile WS heartbeat timeout while walking
|
||||||
|
ws_timeout_s = float(self.profile.get("safety", {}).get("ws_timeout_ms", 500)) / 1000.0
|
||||||
|
if self._last_remote_input_s > 0.0 and (now - self._last_remote_input_s) > ws_timeout_s:
|
||||||
|
self.state_machine.set_velocity_cmd(0.0, 0.0)
|
||||||
|
|
||||||
|
servo_cfg = self.profile["servo"]
|
||||||
|
speed_limit = int(servo_cfg.get("speed_limit", 0))
|
||||||
|
acc = int(servo_cfg.get("acc", 50))
|
||||||
|
|
||||||
|
# Determine command to send for THIS cycle from current state + previous sensor state
|
||||||
|
auto_stand = bool(self.profile.get("safety", {}).get("auto_stand_on_fall", False))
|
||||||
|
if self.state_machine.lifecycle == LifecycleState.CALIBRATION:
|
||||||
|
enable_torque = self._calib_torque_on
|
||||||
|
cmd_ticks = (
|
||||||
|
list(self._calib_target_ticks)
|
||||||
|
if self._calib_target_ticks is not None
|
||||||
|
else rad_to_ticks(self._last_q, self.joint_names, self.profile)
|
||||||
|
)
|
||||||
|
decision = self.state_machine.step(dt_s, float(self._last_grav[2]), auto_stand)
|
||||||
|
else:
|
||||||
|
decision = self.state_machine.step(dt_s, float(self._last_grav[2]), auto_stand)
|
||||||
|
if decision.zero_last_action:
|
||||||
|
self.last_action.fill(0)
|
||||||
|
|
||||||
|
if decision.lifecycle == LifecycleState.SOFT_START:
|
||||||
|
q_interp = (
|
||||||
|
1.0 - decision.soft_start_alpha
|
||||||
|
) * self.state_machine.soft_start_q0 + decision.soft_start_alpha * self.default_pose
|
||||||
|
self._last_targets = q_interp
|
||||||
|
cmd_ticks = rad_to_ticks(q_interp, self.joint_names, self.profile)
|
||||||
|
enable_torque = True
|
||||||
|
elif decision.lifecycle == LifecycleState.ACTIVE and decision.policy_key is not None:
|
||||||
|
policy = self.policies.policies.get(decision.policy_key) or self.policies.policies["walk"]
|
||||||
|
q_vel = self._vel_lpf
|
||||||
|
obs = build_observation(
|
||||||
|
self._last_gyro,
|
||||||
|
self._last_grav,
|
||||||
|
self._last_q,
|
||||||
|
q_vel,
|
||||||
|
self.last_action,
|
||||||
|
decision.command_13,
|
||||||
|
self.default_pose,
|
||||||
|
)
|
||||||
|
raw_act = policy.infer(obs)
|
||||||
|
self.last_action = raw_act
|
||||||
|
targets = decode_action(raw_act, self.default_pose, policy.action_scale)
|
||||||
|
self._last_targets = targets.astype(np.float64)
|
||||||
|
cmd_ticks = rad_to_ticks(targets, self.joint_names, self.profile)
|
||||||
|
enable_torque = True
|
||||||
|
elif decision.lifecycle == LifecycleState.ACTIVE and decision.recovery_state == "fallen":
|
||||||
|
cmd_ticks = rad_to_ticks(self._last_targets, self.joint_names, self.profile)
|
||||||
|
enable_torque = True
|
||||||
|
else:
|
||||||
|
cmd_ticks = rad_to_ticks(self._last_q, self.joint_names, self.profile)
|
||||||
|
enable_torque = False
|
||||||
|
|
||||||
|
frame = self.hal.sync_step(
|
||||||
|
enable_torque=enable_torque,
|
||||||
|
target_ticks=cmd_ticks,
|
||||||
|
speed_limit=speed_limit,
|
||||||
|
acc=acc,
|
||||||
|
)
|
||||||
|
self._last_frame = frame
|
||||||
|
self._last_gyro, self._last_grav = apply_imu_calibration(
|
||||||
|
frame.gyro_raw, frame.proj_gravity_raw, self.profile
|
||||||
|
)
|
||||||
|
self._last_q = ticks_to_rad(frame.ticks, self.joint_names, self.profile)
|
||||||
|
self._compute_joint_vel(self._last_q, frame.raw_speeds, dt_s)
|
||||||
|
|
||||||
|
return self._build_status_unlocked(decision, enable_torque)
|
||||||
|
|
||||||
|
def _build_status_unlocked(self, decision: Any | None = None, torque_enabled: bool | None = None) -> dict[str, Any]:
|
||||||
|
frame = self._last_frame
|
||||||
|
life = self.state_machine.lifecycle.value
|
||||||
|
active_policy = decision.policy_key if decision else None
|
||||||
|
if torque_enabled is None:
|
||||||
|
torque_enabled = life in (LifecycleState.SOFT_START.value, LifecycleState.ACTIVE.value) or self._calib_torque_on
|
||||||
|
return {
|
||||||
|
"hal": self.hal.name,
|
||||||
|
"policy_backend": self.policies.backend,
|
||||||
|
"lifecycle": life,
|
||||||
|
"mode_label": decision.mode_label if decision else self.state_machine._mode_label(),
|
||||||
|
"skill_mode": self.state_machine.skill_mode,
|
||||||
|
"active_policy": active_policy,
|
||||||
|
"recovery_state": self.state_machine.recovery_state,
|
||||||
|
"loco": self.state_machine.loco,
|
||||||
|
"rollers_enabled": self.state_machine.rollers_enabled,
|
||||||
|
"head_mode": self.state_machine.head_mode,
|
||||||
|
"torque_enabled": bool(torque_enabled),
|
||||||
|
"auto_stand_on_fall": bool(self.profile.get("safety", {}).get("auto_stand_on_fall", False)),
|
||||||
|
"speed_scale": float(self.profile.get("safety", {}).get("cmd_vel_scale", 0.5)),
|
||||||
|
"ctrl_hz": round(self._ctrl_hz, 1),
|
||||||
|
"voltage_v": round(frame.voltage_v, 2) if frame else 0.0,
|
||||||
|
"max_temp_c": round(frame.max_temp_c, 1) if frame else 0.0,
|
||||||
|
"imu_ok": frame.imu_ok if frame else False,
|
||||||
|
"servo_err_mask": frame.servo_err_mask if frame else 0,
|
||||||
|
"gyro": [round(float(x), 4) for x in self._last_gyro],
|
||||||
|
"projected_gravity": [round(float(x), 4) for x in self._last_grav],
|
||||||
|
"ticks": list(frame.ticks) if frame else [2048] * 14,
|
||||||
|
"joint_pos_rad": [round(float(x), 4) for x in self._last_q],
|
||||||
|
"loaded_skills": sorted(self.policies.policies.keys()),
|
||||||
|
"custom_move": self.manifest.get("custom_move"),
|
||||||
|
"error": self._last_error,
|
||||||
|
}
|
||||||
|
|
||||||
|
def get_status(self) -> dict[str, Any]:
|
||||||
|
with self._lock:
|
||||||
|
return self._build_status_unlocked()
|
||||||
@@ -0,0 +1,252 @@
|
|||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import io
|
||||||
|
import json
|
||||||
|
import shutil
|
||||||
|
import stat
|
||||||
|
import tempfile
|
||||||
|
import threading
|
||||||
|
import time
|
||||||
|
import zipfile
|
||||||
|
from pathlib import Path, PurePosixPath
|
||||||
|
from typing import Any
|
||||||
|
|
||||||
|
from .config import save_hardware_profile, validate_hardware_profile, validate_robot_manifest
|
||||||
|
from .controller import MicroduckController
|
||||||
|
from .policy_runner import PolicySet
|
||||||
|
|
||||||
|
|
||||||
|
MAX_ZIP_FILES = 512
|
||||||
|
MAX_ZIP_UNCOMPRESSED_BYTES = 128 * 1024 * 1024
|
||||||
|
|
||||||
|
|
||||||
|
def _safe_rel_path(member_name: str) -> PurePosixPath:
|
||||||
|
if not member_name or "\\" in member_name:
|
||||||
|
raise ValueError(f"Unsafe zip member path: {member_name!r}")
|
||||||
|
if member_name.startswith("/") or ":" in member_name:
|
||||||
|
raise ValueError(f"Absolute or drive path forbidden in zip: {member_name!r}")
|
||||||
|
parts = PurePosixPath(member_name).parts
|
||||||
|
if not parts or any(part in ("..", ".", "") for part in parts):
|
||||||
|
raise ValueError(f"Path traversal forbidden in zip: {member_name!r}")
|
||||||
|
return PurePosixPath(*parts)
|
||||||
|
|
||||||
|
|
||||||
|
class DeployService:
|
||||||
|
"""Atomic, validated hot-reload deployment of Microduck manifest + ONNX policies."""
|
||||||
|
|
||||||
|
def __init__(
|
||||||
|
self,
|
||||||
|
base_dir: str | Path,
|
||||||
|
controller: MicroduckController,
|
||||||
|
policy_backend: str = "auto",
|
||||||
|
) -> None:
|
||||||
|
self.base_dir = Path(base_dir)
|
||||||
|
self.controller = controller
|
||||||
|
self.policy_backend = policy_backend
|
||||||
|
self._lock = threading.Lock()
|
||||||
|
self.last_deploy: dict[str, Any] | None = self._read_last_deploy()
|
||||||
|
|
||||||
|
def _read_last_deploy(self) -> dict[str, Any] | None:
|
||||||
|
meta_path = self.base_dir / "config" / ".last_deploy.json"
|
||||||
|
if not meta_path.is_file():
|
||||||
|
return None
|
||||||
|
try:
|
||||||
|
with meta_path.open("r", encoding="utf-8") as f:
|
||||||
|
return json.load(f)
|
||||||
|
except Exception:
|
||||||
|
return None
|
||||||
|
|
||||||
|
def _extract_bundle_to_stage(self, raw_bytes: bytes, stage_dir: Path) -> None:
|
||||||
|
if not raw_bytes:
|
||||||
|
raise ValueError("Empty deployment bundle")
|
||||||
|
try:
|
||||||
|
zf = zipfile.ZipFile(io.BytesIO(raw_bytes), "r")
|
||||||
|
except zipfile.BadZipFile as exc:
|
||||||
|
raise ValueError("Deployment payload is not a valid ZIP archive") from exc
|
||||||
|
|
||||||
|
with zf:
|
||||||
|
infos = zf.infolist()
|
||||||
|
if len(infos) > MAX_ZIP_FILES:
|
||||||
|
raise ValueError(f"Too many files in deployment zip ({len(infos)} > {MAX_ZIP_FILES})")
|
||||||
|
|
||||||
|
total_size = 0
|
||||||
|
entries: list[tuple[zipfile.ZipInfo, PurePosixPath]] = []
|
||||||
|
for info in infos:
|
||||||
|
if info.is_dir():
|
||||||
|
if info.filename.rstrip("/"):
|
||||||
|
_safe_rel_path(info.filename.rstrip("/"))
|
||||||
|
continue
|
||||||
|
mode = (info.external_attr >> 16) & 0o170000
|
||||||
|
if mode == stat.S_IFLNK:
|
||||||
|
raise ValueError(f"Symlinks are forbidden in deployment zip: {info.filename!r}")
|
||||||
|
total_size += max(0, int(info.file_size))
|
||||||
|
if total_size > MAX_ZIP_UNCOMPRESSED_BYTES:
|
||||||
|
raise ValueError("Deployment zip exceeds maximum uncompressed size (128 MB)")
|
||||||
|
entries.append((info, _safe_rel_path(info.filename)))
|
||||||
|
|
||||||
|
if not entries:
|
||||||
|
raise ValueError("Deployment zip contains no files")
|
||||||
|
|
||||||
|
top_dirs = {p.parts[0] for _, p in entries if len(p.parts) > 1}
|
||||||
|
has_root_manifest = any(
|
||||||
|
str(p) in ("config/robot_manifest.json", "robot_manifest.json") for _, p in entries
|
||||||
|
)
|
||||||
|
strip_prefix: str | None = None
|
||||||
|
if not has_root_manifest and len(top_dirs) == 1:
|
||||||
|
only_top = next(iter(top_dirs))
|
||||||
|
if any(
|
||||||
|
str(p) in (f"{only_top}/config/robot_manifest.json", f"{only_top}/robot_manifest.json")
|
||||||
|
for _, p in entries
|
||||||
|
):
|
||||||
|
strip_prefix = only_top
|
||||||
|
|
||||||
|
for info, rel in entries:
|
||||||
|
if strip_prefix and rel.parts[0] == strip_prefix:
|
||||||
|
if len(rel.parts) == 1:
|
||||||
|
continue
|
||||||
|
rel = PurePosixPath(*rel.parts[1:])
|
||||||
|
|
||||||
|
dest = (stage_dir / Path(*rel.parts)).resolve()
|
||||||
|
if not dest.is_relative_to(stage_dir.resolve()):
|
||||||
|
raise ValueError(f"Escaping path rejected: {info.filename!r}")
|
||||||
|
dest.parent.mkdir(parents=True, exist_ok=True)
|
||||||
|
with zf.open(info, "r") as src, dest.open("wb") as dst:
|
||||||
|
shutil.copyfileobj(src, dst)
|
||||||
|
|
||||||
|
def apply_zip_bytes(self, raw_bytes: bytes, overwrite_profile: bool = False) -> dict[str, Any]:
|
||||||
|
with self._lock, tempfile.TemporaryDirectory(prefix="microduck_deploy_") as tmp:
|
||||||
|
stage = Path(tmp) / "stage"
|
||||||
|
backup = Path(tmp) / "backup"
|
||||||
|
stage.mkdir(parents=True, exist_ok=True)
|
||||||
|
backup.mkdir(parents=True, exist_ok=True)
|
||||||
|
|
||||||
|
self._extract_bundle_to_stage(raw_bytes, stage)
|
||||||
|
|
||||||
|
manifest_src = stage / "config" / "robot_manifest.json"
|
||||||
|
if not manifest_src.is_file():
|
||||||
|
alt = stage / "robot_manifest.json"
|
||||||
|
if alt.is_file():
|
||||||
|
manifest_src = alt
|
||||||
|
else:
|
||||||
|
raise ValueError("Deployment zip missing config/robot_manifest.json")
|
||||||
|
|
||||||
|
try:
|
||||||
|
manifest = validate_robot_manifest(json.loads(manifest_src.read_text(encoding="utf-8")))
|
||||||
|
except json.JSONDecodeError as exc:
|
||||||
|
raise ValueError(f"Invalid robot_manifest.json: {exc}") from exc
|
||||||
|
|
||||||
|
# Normalize into stage/config & verify every referenced ONNX exists & is non-empty
|
||||||
|
(stage / "config").mkdir(parents=True, exist_ok=True)
|
||||||
|
(stage / "config" / "robot_manifest.json").write_text(
|
||||||
|
json.dumps(manifest, indent=2) + "\n", encoding="utf-8"
|
||||||
|
)
|
||||||
|
|
||||||
|
stage_policies: dict[str, Path] = {}
|
||||||
|
for key, spec in manifest["skills"].items():
|
||||||
|
rel = spec.get("path")
|
||||||
|
if not rel:
|
||||||
|
continue
|
||||||
|
rel_posix = _safe_rel_path(str(rel))
|
||||||
|
pol_file = (stage / Path(*rel_posix.parts)).resolve()
|
||||||
|
if not pol_file.is_relative_to(stage.resolve()) or not pol_file.is_file():
|
||||||
|
raise ValueError(f"Deployment zip missing policy '{key}' at '{rel}'")
|
||||||
|
if pol_file.stat().st_size == 0:
|
||||||
|
raise ValueError(f"Policy '{key}' at '{rel}' is empty")
|
||||||
|
stage_policies[key] = pol_file
|
||||||
|
|
||||||
|
# Optional custom move script
|
||||||
|
custom_script: dict[str, Any] | None = None
|
||||||
|
script_src = stage / "config" / "custom_move_script.json"
|
||||||
|
if not script_src.is_file():
|
||||||
|
alt_script = stage / "custom_move_script.json"
|
||||||
|
if alt_script.is_file():
|
||||||
|
script_src = alt_script
|
||||||
|
if script_src.is_file():
|
||||||
|
try:
|
||||||
|
custom_script = json.loads(script_src.read_text(encoding="utf-8"))
|
||||||
|
except json.JSONDecodeError as exc:
|
||||||
|
raise ValueError(f"Invalid custom_move_script.json: {exc}") from exc
|
||||||
|
if not isinstance(custom_script, dict) or not isinstance(custom_script.get("keyframes"), list):
|
||||||
|
raise ValueError("custom_move_script.json must contain a 'keyframes' list")
|
||||||
|
elif isinstance(manifest.get("custom_move"), dict) and manifest["custom_move"].get("slot") == "script":
|
||||||
|
raise ValueError("Manifest references custom_move script, but config/custom_move_script.json is missing")
|
||||||
|
|
||||||
|
# Optional hardware profile
|
||||||
|
target_profile_path = self.base_dir / "config" / "hardware_profile.json"
|
||||||
|
incoming_profile: dict[str, Any] | None = None
|
||||||
|
profile_src = stage / "config" / "hardware_profile.json"
|
||||||
|
if not profile_src.is_file():
|
||||||
|
alt_prof = stage / "hardware_profile.json"
|
||||||
|
if alt_prof.is_file():
|
||||||
|
profile_src = alt_prof
|
||||||
|
if (overwrite_profile or not target_profile_path.is_file()) and profile_src.is_file():
|
||||||
|
try:
|
||||||
|
incoming_profile = validate_hardware_profile(
|
||||||
|
json.loads(profile_src.read_text(encoding="utf-8")),
|
||||||
|
joint_names=manifest["joint_names"],
|
||||||
|
)
|
||||||
|
except json.JSONDecodeError as exc:
|
||||||
|
raise ValueError(f"Invalid hardware_profile.json: {exc}") from exc
|
||||||
|
|
||||||
|
# Pre-validate ONNX graph & metadata inside stage before touching live files
|
||||||
|
PolicySet.load_from_manifest(stage, manifest, backend=self.policy_backend)
|
||||||
|
|
||||||
|
# Backup existing config + policies for atomic rollback
|
||||||
|
live_config = self.base_dir / "config"
|
||||||
|
live_policies = self.base_dir / "policies"
|
||||||
|
live_config.mkdir(parents=True, exist_ok=True)
|
||||||
|
live_policies.mkdir(parents=True, exist_ok=True)
|
||||||
|
for sub in ("config", "policies"):
|
||||||
|
src_dir = self.base_dir / sub
|
||||||
|
if src_dir.is_dir():
|
||||||
|
shutil.copytree(src_dir, backup / sub, dirs_exist_ok=True)
|
||||||
|
|
||||||
|
try:
|
||||||
|
shutil.copy2(stage / "config" / "robot_manifest.json", live_config / "robot_manifest.json")
|
||||||
|
|
||||||
|
target_script = live_config / "custom_move_script.json"
|
||||||
|
if custom_script is not None:
|
||||||
|
target_script.write_text(json.dumps(custom_script, indent=2) + "\n", encoding="utf-8")
|
||||||
|
elif target_script.is_file():
|
||||||
|
target_script.unlink()
|
||||||
|
|
||||||
|
for _, src_pol in stage_policies.items():
|
||||||
|
rel_to_stage = src_pol.relative_to(stage)
|
||||||
|
dst_pol = self.base_dir / rel_to_stage
|
||||||
|
dst_pol.parent.mkdir(parents=True, exist_ok=True)
|
||||||
|
shutil.copy2(src_pol, dst_pol)
|
||||||
|
|
||||||
|
if incoming_profile is not None:
|
||||||
|
save_hardware_profile(target_profile_path, incoming_profile, joint_names=manifest["joint_names"])
|
||||||
|
|
||||||
|
live_policies_set = PolicySet.load_from_manifest(
|
||||||
|
self.base_dir, manifest, backend=self.policy_backend
|
||||||
|
)
|
||||||
|
self.controller.reload_runtime(
|
||||||
|
manifest=manifest,
|
||||||
|
policies=live_policies_set,
|
||||||
|
profile=incoming_profile,
|
||||||
|
custom_script=custom_script,
|
||||||
|
)
|
||||||
|
except Exception:
|
||||||
|
# Roll back live config + policies on reload failure
|
||||||
|
for sub in ("config", "policies"):
|
||||||
|
bak_dir = backup / sub
|
||||||
|
live_dir = self.base_dir / sub
|
||||||
|
if bak_dir.is_dir():
|
||||||
|
shutil.copytree(bak_dir, live_dir, dirs_exist_ok=True)
|
||||||
|
raise
|
||||||
|
|
||||||
|
deploy_meta = {
|
||||||
|
"ok": True,
|
||||||
|
"deployed_at_s": round(time.time(), 3),
|
||||||
|
"robot_id": manifest.get("robot_id", "microduck"),
|
||||||
|
"loaded_skills": sorted(live_policies_set.policies.keys()),
|
||||||
|
"profile_overwritten": incoming_profile is not None,
|
||||||
|
"custom_move": manifest.get("custom_move"),
|
||||||
|
}
|
||||||
|
(live_config / ".last_deploy.json").write_text(
|
||||||
|
json.dumps(deploy_meta, indent=2) + "\n", encoding="utf-8"
|
||||||
|
)
|
||||||
|
self.last_deploy = deploy_meta
|
||||||
|
return deploy_meta
|
||||||
@@ -0,0 +1,85 @@
|
|||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
from typing import Any
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
|
||||||
|
def build_observation(
|
||||||
|
gyro: np.ndarray,
|
||||||
|
projected_gravity: np.ndarray,
|
||||||
|
joint_pos_rad: np.ndarray,
|
||||||
|
joint_vel_rad_s: np.ndarray,
|
||||||
|
last_action: np.ndarray,
|
||||||
|
command_13: np.ndarray,
|
||||||
|
default_pose: np.ndarray,
|
||||||
|
) -> np.ndarray:
|
||||||
|
"""Assemble the exact 61D float32 observation expected by Microduck ONNX policies.
|
||||||
|
|
||||||
|
Layout:
|
||||||
|
[0:3] base_ang_vel (gyro, rad/s)
|
||||||
|
[3:6] projected_gravity (trunk_base frame, unit vector pointing down: [0,0,-1] upright)
|
||||||
|
[6:20] joint_pos_rel (joint_pos_rad - default_pose)
|
||||||
|
[20:34] joint_vel (rad/s)
|
||||||
|
[34:48] last_action (previous raw actor output)
|
||||||
|
[48:61] command (13D: [vx, vy, wz, head(4), body(6)])
|
||||||
|
"""
|
||||||
|
obs = np.empty(61, dtype=np.float32)
|
||||||
|
obs[0:3] = np.asarray(gyro, dtype=np.float32)
|
||||||
|
obs[3:6] = np.asarray(projected_gravity, dtype=np.float32)
|
||||||
|
obs[6:20] = np.asarray(joint_pos_rad, dtype=np.float32) - np.asarray(default_pose, dtype=np.float32)
|
||||||
|
obs[20:34] = np.asarray(joint_vel_rad_s, dtype=np.float32)
|
||||||
|
obs[34:48] = np.asarray(last_action, dtype=np.float32)
|
||||||
|
obs[48:61] = np.asarray(command_13, dtype=np.float32)
|
||||||
|
|
||||||
|
if not np.all(np.isfinite(obs)):
|
||||||
|
raise ValueError("Observation vector contains non-finite values")
|
||||||
|
return obs
|
||||||
|
|
||||||
|
|
||||||
|
def decode_action(
|
||||||
|
raw_action: np.ndarray,
|
||||||
|
default_pose: np.ndarray,
|
||||||
|
action_scale: float = 1.0,
|
||||||
|
) -> np.ndarray:
|
||||||
|
act = np.asarray(raw_action, dtype=np.float32)
|
||||||
|
if act.shape != (14,) or not np.all(np.isfinite(act)):
|
||||||
|
raise ValueError("raw_action must be a finite 14D vector")
|
||||||
|
return np.asarray(default_pose, dtype=np.float32) + np.float32(action_scale) * act
|
||||||
|
|
||||||
|
|
||||||
|
def script_command_at(script: dict[str, Any], t_s: float, cmd_size: int = 13) -> np.ndarray:
|
||||||
|
"""Linearly interpolate a custom move_script.json timeline at time t_s."""
|
||||||
|
keyframes = script.get("keyframes", [])
|
||||||
|
out = np.zeros(cmd_size, dtype=np.float32)
|
||||||
|
if not keyframes:
|
||||||
|
return out
|
||||||
|
|
||||||
|
duration_s = float(script.get("durationS") or script.get("duration_s") or 0.0)
|
||||||
|
loop = script.get("loop", True) is not False
|
||||||
|
tt = float(t_s)
|
||||||
|
if loop and duration_s > 0.0:
|
||||||
|
tt = ((tt % duration_s) + duration_s) % duration_s
|
||||||
|
|
||||||
|
def _pad(raw_cmd: list[float]) -> np.ndarray:
|
||||||
|
arr = np.zeros(cmd_size, dtype=np.float32)
|
||||||
|
n = min(cmd_size, len(raw_cmd))
|
||||||
|
if n > 0:
|
||||||
|
arr[:n] = np.asarray(raw_cmd[:n], dtype=np.float32)
|
||||||
|
return arr
|
||||||
|
|
||||||
|
if tt <= float(keyframes[0]["t"]):
|
||||||
|
return _pad(keyframes[0]["cmd"])
|
||||||
|
|
||||||
|
for i in range(len(keyframes) - 1):
|
||||||
|
a = keyframes[i]
|
||||||
|
b = keyframes[i + 1]
|
||||||
|
ta = float(a["t"])
|
||||||
|
tb = float(b["t"])
|
||||||
|
if ta <= tt <= tb:
|
||||||
|
u = (tt - ta) / max(tb - ta, 1e-6)
|
||||||
|
ca = _pad(a["cmd"])
|
||||||
|
cb = _pad(b["cmd"])
|
||||||
|
return (1.0 - u) * ca + u * cb
|
||||||
|
|
||||||
|
return _pad(keyframes[-1]["cmd"])
|
||||||
@@ -0,0 +1,123 @@
|
|||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
from dataclasses import dataclass
|
||||||
|
from pathlib import Path
|
||||||
|
from typing import Any
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass
|
||||||
|
class LoadedPolicy:
|
||||||
|
key: str
|
||||||
|
action_scale: float
|
||||||
|
backend: str
|
||||||
|
_session: Any | None = None
|
||||||
|
_input_name: str = "obs"
|
||||||
|
_output_name: str = "actions"
|
||||||
|
|
||||||
|
def infer(self, obs_61: np.ndarray) -> np.ndarray:
|
||||||
|
if obs_61.shape != (61,):
|
||||||
|
raise ValueError(f"Expected 61D observation, got {obs_61.shape}")
|
||||||
|
if self.backend == "dummy":
|
||||||
|
# Deterministic zero-action dummy policy for PC dry-run without onnxruntime.
|
||||||
|
return np.zeros(14, dtype=np.float32)
|
||||||
|
|
||||||
|
inp = np.asarray(obs_61, dtype=np.float32).reshape(1, 61)
|
||||||
|
out = self._session.run([self._output_name], {self._input_name: inp})[0]
|
||||||
|
act = np.asarray(out, dtype=np.float32).reshape(14)
|
||||||
|
if not np.all(np.isfinite(act)):
|
||||||
|
raise ValueError(f"Policy '{self.key}' returned non-finite action")
|
||||||
|
return act
|
||||||
|
|
||||||
|
|
||||||
|
class PolicySet:
|
||||||
|
def __init__(self, policies: dict[str, LoadedPolicy], backend: str) -> None:
|
||||||
|
self.policies = policies
|
||||||
|
self.backend = backend
|
||||||
|
|
||||||
|
@classmethod
|
||||||
|
def resolve_backend(cls, requested: str) -> str:
|
||||||
|
if requested in ("onnx", "dummy"):
|
||||||
|
return requested
|
||||||
|
if requested != "auto":
|
||||||
|
raise ValueError(f"Unknown policy backend: {requested}")
|
||||||
|
try:
|
||||||
|
import onnxruntime # noqa: F401
|
||||||
|
|
||||||
|
return "onnx"
|
||||||
|
except ImportError as exc:
|
||||||
|
raise RuntimeError(
|
||||||
|
"onnxruntime is not installed. Run 'bash scripts/install_deps.sh' on UNO Q, "
|
||||||
|
"or pass '--policy-backend dummy' for pre-hardware UI/protocol dry-run."
|
||||||
|
) from exc
|
||||||
|
|
||||||
|
@classmethod
|
||||||
|
def load_from_manifest(
|
||||||
|
cls,
|
||||||
|
base_dir: str | Path,
|
||||||
|
manifest: dict[str, Any],
|
||||||
|
backend: str = "auto",
|
||||||
|
) -> "PolicySet":
|
||||||
|
resolved_backend = cls.resolve_backend(backend)
|
||||||
|
ort = None
|
||||||
|
if resolved_backend == "onnx":
|
||||||
|
try:
|
||||||
|
import onnxruntime as ort_mod
|
||||||
|
|
||||||
|
ort = ort_mod
|
||||||
|
except ImportError as exc:
|
||||||
|
raise RuntimeError("onnxruntime is required for backend='onnx'") from exc
|
||||||
|
|
||||||
|
root = Path(base_dir)
|
||||||
|
default_scale = float(manifest.get("action_scale", 1.0))
|
||||||
|
skills = manifest.get("skills", {})
|
||||||
|
loaded: dict[str, LoadedPolicy] = {}
|
||||||
|
|
||||||
|
for key, spec in skills.items():
|
||||||
|
rel_path = spec.get("path")
|
||||||
|
if not rel_path:
|
||||||
|
continue
|
||||||
|
scale = float(spec.get("action_scale", default_scale))
|
||||||
|
if resolved_backend == "dummy":
|
||||||
|
loaded[key] = LoadedPolicy(key=key, action_scale=scale, backend="dummy")
|
||||||
|
continue
|
||||||
|
|
||||||
|
model_path = (root / rel_path).resolve()
|
||||||
|
if not model_path.is_file():
|
||||||
|
raise FileNotFoundError(f"Missing ONNX policy for '{key}': {model_path}")
|
||||||
|
|
||||||
|
sess_opts = ort.SessionOptions()
|
||||||
|
sess_opts.intra_op_num_threads = 2
|
||||||
|
sess = ort.InferenceSession(
|
||||||
|
str(model_path),
|
||||||
|
sess_options=sess_opts,
|
||||||
|
providers=["CPUExecutionProvider"],
|
||||||
|
)
|
||||||
|
inps = sess.get_inputs()
|
||||||
|
outs = sess.get_outputs()
|
||||||
|
if len(inps) != 1 or len(outs) != 1:
|
||||||
|
raise ValueError(f"Policy '{key}' must have 1 input / 1 output")
|
||||||
|
|
||||||
|
# Smoke validation mirroring custom-policy.js
|
||||||
|
smoke_obs = np.zeros((1, 61), dtype=np.float32)
|
||||||
|
smoke_out = sess.run([outs[0].name], {inps[0].name: smoke_obs})[0]
|
||||||
|
if tuple(np.asarray(smoke_out).shape) != (1, 14):
|
||||||
|
raise ValueError(
|
||||||
|
f"Policy '{key}' output shape {np.asarray(smoke_out).shape} != (1, 14)"
|
||||||
|
)
|
||||||
|
if not np.all(np.isfinite(smoke_out)):
|
||||||
|
raise ValueError(f"Policy '{key}' smoke output is non-finite")
|
||||||
|
|
||||||
|
loaded[key] = LoadedPolicy(
|
||||||
|
key=key,
|
||||||
|
action_scale=scale,
|
||||||
|
backend="onnx",
|
||||||
|
_session=sess,
|
||||||
|
_input_name=inps[0].name,
|
||||||
|
_output_name=outs[0].name,
|
||||||
|
)
|
||||||
|
|
||||||
|
if "walk" not in loaded or "stand" not in loaded:
|
||||||
|
raise ValueError("PolicySet must load at least 'walk' and 'stand'")
|
||||||
|
return cls(policies=loaded, backend=resolved_backend)
|
||||||
@@ -0,0 +1,561 @@
|
|||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import math
|
||||||
|
from dataclasses import dataclass
|
||||||
|
from enum import Enum
|
||||||
|
from typing import Any
|
||||||
|
|
||||||
|
import numpy as np
|
||||||
|
|
||||||
|
from .observations import script_command_at
|
||||||
|
|
||||||
|
|
||||||
|
class LifecycleState(str, Enum):
|
||||||
|
IDLE = "IDLE"
|
||||||
|
SOFT_START = "SOFT_START"
|
||||||
|
ACTIVE = "ACTIVE"
|
||||||
|
CALIBRATION = "CALIBRATION"
|
||||||
|
FALL_SAFE = "FALL_SAFE"
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass
|
||||||
|
class StepDecision:
|
||||||
|
lifecycle: LifecycleState
|
||||||
|
skill_mode: str
|
||||||
|
policy_key: str | None
|
||||||
|
torque_enabled: bool
|
||||||
|
soft_start_alpha: float
|
||||||
|
command_13: np.ndarray
|
||||||
|
zero_last_action: bool
|
||||||
|
mode_label: str
|
||||||
|
recovery_state: str | None
|
||||||
|
loco: str
|
||||||
|
head_mode: bool
|
||||||
|
|
||||||
|
|
||||||
|
class MicroduckStateMachine:
|
||||||
|
"""Hardware-safe 50Hz state machine mirroring app/src/robots/microduck/runtime.js."""
|
||||||
|
|
||||||
|
def __init__(self, manifest: dict[str, Any], custom_script: dict[str, Any] | None = None) -> None:
|
||||||
|
self.manifest = manifest
|
||||||
|
self.custom_script = custom_script
|
||||||
|
self._load_params(manifest)
|
||||||
|
|
||||||
|
self.lifecycle: LifecycleState = LifecycleState.IDLE
|
||||||
|
self.loco: str = "legs" # "legs" | "rollers"
|
||||||
|
self.skill_mode: str = "walk" # "walk"|"sitstand"|"roll"|"kickL"|"kickR"|"crouch"|"groundpick"
|
||||||
|
self.sit_flag: int = 0
|
||||||
|
self.head_mode: bool = False
|
||||||
|
|
||||||
|
self.vel_cmd = np.zeros(3, dtype=np.float32) # [vx, 0, wz]
|
||||||
|
self.head_target = np.zeros(4, dtype=np.float32)
|
||||||
|
self.head_smooth = np.zeros(4, dtype=np.float32)
|
||||||
|
|
||||||
|
self.soft_start_q0 = np.zeros(14, dtype=np.float64)
|
||||||
|
self.soft_start_elapsed = 0.0
|
||||||
|
self.soft_start_duration = 1.5
|
||||||
|
|
||||||
|
self.sit_delay_rem: float | None = None
|
||||||
|
self.stand_delay_rem: float | None = None
|
||||||
|
self.kick_steps: int | None = None
|
||||||
|
self.post_kick_lock: int = 0
|
||||||
|
self.crouch_phase: float | None = None
|
||||||
|
self.pick_phase: float | None = None
|
||||||
|
self.roll_steps: int | None = None
|
||||||
|
self.roll_tipped: bool = False
|
||||||
|
self.script_t: float | None = None
|
||||||
|
|
||||||
|
self.fall_debounce: int = 0
|
||||||
|
self.recovery_state: str | None = None # None | "fallen" | "recovering"
|
||||||
|
self.recovery_steps: int = 0
|
||||||
|
self.recovery_upright_steps: int = 0
|
||||||
|
|
||||||
|
def _load_params(self, manifest: dict[str, Any]) -> None:
|
||||||
|
safety = manifest.get("safety", {})
|
||||||
|
head = manifest.get("head", {})
|
||||||
|
legacy = manifest.get("legacy", {})
|
||||||
|
variants = manifest.get("variants", {})
|
||||||
|
|
||||||
|
self.tilt_z = float(safety.get("tilt_z", -0.5))
|
||||||
|
self.upright_z = float(safety.get("upright_z", -0.85))
|
||||||
|
self.fall_debounce_steps = int(safety.get("fall_debounce_steps", 10))
|
||||||
|
self.fall_settle_steps = int(safety.get("fall_settle_steps", 15))
|
||||||
|
self.recover_upright_steps_req = int(safety.get("recover_upright_steps", 50))
|
||||||
|
self.recover_giveup_steps = int(safety.get("recover_giveup_steps", 300))
|
||||||
|
self.kick_total_steps = int(safety.get("kick_steps", 25))
|
||||||
|
self.post_kick_lock_total = int(safety.get("post_kick_lock_steps", 20))
|
||||||
|
self.roll_max_steps = int(safety.get("roll_max_steps", 150))
|
||||||
|
self.sit_delay_s = float(safety.get("sit_delay_s", 0.8))
|
||||||
|
self.stand_delay_s = float(safety.get("stand_delay_s", 2.0))
|
||||||
|
|
||||||
|
self.head_max = float(head.get("max_rad", 2.5))
|
||||||
|
self.head_alpha = float(head.get("alpha", 0.2))
|
||||||
|
self.head_signs = np.asarray(head.get("signs", [1, -1, 1, -1]), dtype=np.float32)
|
||||||
|
|
||||||
|
self.crouch_period_s = float(legacy.get("crouch_period", 5.0))
|
||||||
|
self.crouch_end_phase = float(legacy.get("crouch_end", 0.7))
|
||||||
|
self.groundpick_period_s = float(legacy.get("groundpick_period", 4.0))
|
||||||
|
self.groundpick_end_phase = float(legacy.get("groundpick_end", 0.7))
|
||||||
|
|
||||||
|
self.legs_limits = list(variants.get("legs", {}).get("limits", [0.25, -0.2, 1.0]))
|
||||||
|
self.rollers_limits = list(variants.get("rollers", {}).get("limits", [0.6, -0.5, 0.3]))
|
||||||
|
self.rollers_enabled = bool(variants.get("rollers", {}).get("enabled", False))
|
||||||
|
|
||||||
|
custom_move = manifest.get("custom_move")
|
||||||
|
self.custom_slot = custom_move.get("slot") if isinstance(custom_move, dict) else None
|
||||||
|
roll_skill = manifest.get("skills", {}).get("roll", {})
|
||||||
|
ctrl_dt = float(manifest.get("sim", {}).get("ctrl_dt", 0.02))
|
||||||
|
if self.custom_slot == "trick" and roll_skill.get("duration_s"):
|
||||||
|
self.roll_max_steps = max(10, round(float(roll_skill["duration_s"]) / ctrl_dt))
|
||||||
|
|
||||||
|
def update_manifest(self, manifest: dict[str, Any], custom_script: dict[str, Any] | None = None) -> None:
|
||||||
|
self.manifest = manifest
|
||||||
|
self.custom_script = custom_script
|
||||||
|
self._load_params(manifest)
|
||||||
|
if not self.rollers_enabled and self.loco == "rollers":
|
||||||
|
self.loco = "legs"
|
||||||
|
|
||||||
|
def _clear_skill_runs(self) -> None:
|
||||||
|
self.sit_delay_rem = None
|
||||||
|
self.stand_delay_rem = None
|
||||||
|
self.kick_steps = None
|
||||||
|
self.post_kick_lock = 0
|
||||||
|
self.crouch_phase = None
|
||||||
|
self.pick_phase = None
|
||||||
|
self.roll_steps = None
|
||||||
|
self.roll_tipped = False
|
||||||
|
self.script_t = None
|
||||||
|
self.fall_debounce = 0
|
||||||
|
self.recovery_state = None
|
||||||
|
self.recovery_steps = 0
|
||||||
|
self.recovery_upright_steps = 0
|
||||||
|
|
||||||
|
def emergency_stop(self) -> None:
|
||||||
|
self._clear_skill_runs()
|
||||||
|
self.skill_mode = "walk"
|
||||||
|
self.sit_flag = 0
|
||||||
|
self.head_mode = False
|
||||||
|
self.vel_cmd.fill(0)
|
||||||
|
self.head_target.fill(0)
|
||||||
|
self.head_smooth.fill(0)
|
||||||
|
self.lifecycle = LifecycleState.IDLE
|
||||||
|
|
||||||
|
def enter_calibration(self) -> None:
|
||||||
|
self.emergency_stop()
|
||||||
|
self.lifecycle = LifecycleState.CALIBRATION
|
||||||
|
|
||||||
|
def exit_calibration(self) -> None:
|
||||||
|
if self.lifecycle == LifecycleState.CALIBRATION:
|
||||||
|
self.lifecycle = LifecycleState.IDLE
|
||||||
|
|
||||||
|
def request_stand(self, current_q_rad: np.ndarray, soft_start_s: float = 1.5) -> None:
|
||||||
|
self._clear_skill_runs()
|
||||||
|
self.skill_mode = "walk"
|
||||||
|
self.sit_flag = 0
|
||||||
|
self.head_mode = False
|
||||||
|
self.vel_cmd.fill(0)
|
||||||
|
self.head_target.fill(0)
|
||||||
|
self.head_smooth.fill(0)
|
||||||
|
self.soft_start_q0 = np.asarray(current_q_rad, dtype=np.float64).copy()
|
||||||
|
self.soft_start_elapsed = 0.0
|
||||||
|
self.soft_start_duration = max(0.02, float(soft_start_s))
|
||||||
|
self.lifecycle = LifecycleState.SOFT_START
|
||||||
|
|
||||||
|
def set_velocity_cmd(self, norm_x: float, norm_wz: float, speed_scale: float = 1.0) -> None:
|
||||||
|
lim_f, lim_b, lim_a = self.rollers_limits if self.loco == "rollers" else self.legs_limits
|
||||||
|
nx = max(-1.0, min(1.0, float(norm_x)))
|
||||||
|
nwz = max(-1.0, min(1.0, float(norm_wz)))
|
||||||
|
scale = max(0.05, min(1.0, float(speed_scale)))
|
||||||
|
vx = (nx * lim_f if nx >= 0.0 else nx * (-lim_b)) * scale
|
||||||
|
wz = nwz * lim_a * scale
|
||||||
|
self.vel_cmd[0] = vx
|
||||||
|
self.vel_cmd[1] = 0.0
|
||||||
|
self.vel_cmd[2] = wz
|
||||||
|
|
||||||
|
def set_head_target(self, neck_pitch: float, head_pitch: float, head_yaw: float, head_roll: float) -> None:
|
||||||
|
raw = np.array([neck_pitch, head_pitch, head_yaw, head_roll], dtype=np.float32)
|
||||||
|
raw = np.clip(raw, -1.0, 1.0)
|
||||||
|
self.head_target = self.head_signs * raw * self.head_max
|
||||||
|
|
||||||
|
def _is_one_shot_busy(self) -> bool:
|
||||||
|
return (
|
||||||
|
(self.skill_mode == "roll" and self.roll_steps is not None)
|
||||||
|
or (self.skill_mode in ("kickL", "kickR") and self.kick_steps is not None)
|
||||||
|
or (self.skill_mode == "crouch" and self.crouch_phase is not None)
|
||||||
|
or (self.skill_mode == "groundpick" and self.pick_phase is not None)
|
||||||
|
)
|
||||||
|
|
||||||
|
def trigger_action(self, action: str) -> bool:
|
||||||
|
if self.lifecycle != LifecycleState.ACTIVE or self.recovery_state is not None:
|
||||||
|
return False
|
||||||
|
|
||||||
|
if action == "headToggle":
|
||||||
|
if self.head_mode:
|
||||||
|
self.head_mode = False
|
||||||
|
return True
|
||||||
|
if (
|
||||||
|
self.skill_mode in ("walk", "sitstand")
|
||||||
|
and self.post_kick_lock == 0
|
||||||
|
and self.stand_delay_rem is None
|
||||||
|
):
|
||||||
|
self.head_mode = True
|
||||||
|
return True
|
||||||
|
return False
|
||||||
|
|
||||||
|
if action == "locoToggle":
|
||||||
|
if not self.rollers_enabled:
|
||||||
|
return False
|
||||||
|
if self._is_one_shot_busy() or self.stand_delay_rem is not None:
|
||||||
|
return False
|
||||||
|
self._clear_skill_runs()
|
||||||
|
self.skill_mode = "walk"
|
||||||
|
self.sit_flag = 0
|
||||||
|
self.head_mode = False
|
||||||
|
self.loco = "rollers" if self.loco == "legs" else "legs"
|
||||||
|
return True
|
||||||
|
|
||||||
|
if action == "sitToggle":
|
||||||
|
if self.loco != "legs":
|
||||||
|
return self.trigger_action("crouch")
|
||||||
|
if self.custom_slot == "trick":
|
||||||
|
return self.trigger_action("roll")
|
||||||
|
if self.custom_slot == "script" and self.custom_script is not None:
|
||||||
|
return self.trigger_action("scriptToggle")
|
||||||
|
if self._is_one_shot_busy():
|
||||||
|
return False
|
||||||
|
self.head_mode = False
|
||||||
|
if self.skill_mode == "sitstand" and self.sit_flag == 1:
|
||||||
|
self.sit_flag = 0
|
||||||
|
self.sit_delay_rem = None
|
||||||
|
self.stand_delay_rem = self.stand_delay_s
|
||||||
|
return True
|
||||||
|
if self.skill_mode == "walk" and self.stand_delay_rem is None:
|
||||||
|
self.skill_mode = "sitstand"
|
||||||
|
self.sit_flag = 0
|
||||||
|
self.sit_delay_rem = self.sit_delay_s
|
||||||
|
return True
|
||||||
|
return False
|
||||||
|
|
||||||
|
if action == "scriptToggle":
|
||||||
|
if self.custom_script is None or self.loco != "legs":
|
||||||
|
return False
|
||||||
|
if self.script_t is not None:
|
||||||
|
self.script_t = None
|
||||||
|
return True
|
||||||
|
if self.skill_mode == "walk" and self.stand_delay_rem is None:
|
||||||
|
self.head_mode = False
|
||||||
|
self.script_t = 0.0
|
||||||
|
return True
|
||||||
|
return False
|
||||||
|
|
||||||
|
if action in ("kickL", "kickR"):
|
||||||
|
if self.loco != "legs" or self.skill_mode != "walk" or self.stand_delay_rem is not None:
|
||||||
|
return False
|
||||||
|
self.head_mode = False
|
||||||
|
self.sit_delay_rem = None
|
||||||
|
self.skill_mode = action
|
||||||
|
self.sit_flag = 0
|
||||||
|
self.kick_steps = 0
|
||||||
|
return True
|
||||||
|
|
||||||
|
if action == "groundPick":
|
||||||
|
if self.loco != "legs" or self.skill_mode != "walk" or self.stand_delay_rem is not None:
|
||||||
|
return False
|
||||||
|
self.head_mode = False
|
||||||
|
self.sit_delay_rem = None
|
||||||
|
self.skill_mode = "groundpick"
|
||||||
|
self.sit_flag = 0
|
||||||
|
self.pick_phase = 0.0
|
||||||
|
return True
|
||||||
|
|
||||||
|
if action == "roll":
|
||||||
|
if self.loco == "rollers":
|
||||||
|
return self.trigger_action("crouch")
|
||||||
|
if self.skill_mode != "walk" or self.stand_delay_rem is not None:
|
||||||
|
return False
|
||||||
|
self.head_mode = False
|
||||||
|
self.sit_delay_rem = None
|
||||||
|
self.skill_mode = "roll"
|
||||||
|
self.sit_flag = 0
|
||||||
|
self.roll_steps = 0
|
||||||
|
self.roll_tipped = False
|
||||||
|
return True
|
||||||
|
|
||||||
|
if action == "crouch":
|
||||||
|
if self.loco != "rollers" or self.skill_mode != "walk":
|
||||||
|
return False
|
||||||
|
self.head_mode = False
|
||||||
|
self.skill_mode = "crouch"
|
||||||
|
self.crouch_phase = 0.0
|
||||||
|
return True
|
||||||
|
|
||||||
|
return False
|
||||||
|
|
||||||
|
def _active_policy_key(self) -> str | None:
|
||||||
|
if self.recovery_state == "fallen":
|
||||||
|
return None
|
||||||
|
if self.recovery_state == "recovering":
|
||||||
|
return "stand"
|
||||||
|
if self.loco == "rollers" and self.skill_mode == "walk":
|
||||||
|
return "drive"
|
||||||
|
return self.skill_mode
|
||||||
|
|
||||||
|
def _mode_label(self) -> str:
|
||||||
|
if self.lifecycle == LifecycleState.IDLE:
|
||||||
|
return "Idle (Torque Off)"
|
||||||
|
if self.lifecycle == LifecycleState.CALIBRATION:
|
||||||
|
return "Calibration"
|
||||||
|
if self.lifecycle == LifecycleState.FALL_SAFE:
|
||||||
|
return "Fall Safe"
|
||||||
|
if self.lifecycle == LifecycleState.SOFT_START:
|
||||||
|
return "Soft Start"
|
||||||
|
if self.recovery_state:
|
||||||
|
return "Recovery"
|
||||||
|
if self.skill_mode == "roll":
|
||||||
|
return "Trick" if self.custom_slot == "trick" else "Roll"
|
||||||
|
if self.script_t is not None:
|
||||||
|
return "Script"
|
||||||
|
if self.skill_mode == "crouch":
|
||||||
|
return "Crouch"
|
||||||
|
if self.skill_mode == "groundpick":
|
||||||
|
return "Pick"
|
||||||
|
if self.skill_mode in ("kickL", "kickR"):
|
||||||
|
return "Kick"
|
||||||
|
if self.head_mode:
|
||||||
|
return "Head"
|
||||||
|
if self.skill_mode == "sitstand" and self.sit_flag == 1:
|
||||||
|
return "Sit"
|
||||||
|
return "Drive" if self.loco == "rollers" else "Run"
|
||||||
|
|
||||||
|
def _build_command_13(self) -> np.ndarray:
|
||||||
|
cmd = np.zeros(13, dtype=np.float32)
|
||||||
|
if self.skill_mode == "sitstand":
|
||||||
|
cmd[0] = float(self.sit_flag)
|
||||||
|
elif self.skill_mode == "crouch" and self.crouch_phase is not None:
|
||||||
|
a = 2.0 * math.pi * self.crouch_phase
|
||||||
|
cmd[0] = math.cos(a)
|
||||||
|
cmd[1] = math.sin(a)
|
||||||
|
elif self.skill_mode == "groundpick" and self.pick_phase is not None:
|
||||||
|
a = 2.0 * math.pi * self.pick_phase
|
||||||
|
cmd[0] = math.cos(a)
|
||||||
|
cmd[1] = math.sin(a)
|
||||||
|
elif not (
|
||||||
|
self.head_mode
|
||||||
|
or self.skill_mode == "roll"
|
||||||
|
or self.skill_mode in ("kickL", "kickR")
|
||||||
|
or self.post_kick_lock > 0
|
||||||
|
or self.recovery_state is not None
|
||||||
|
):
|
||||||
|
cmd[0:3] = self.vel_cmd
|
||||||
|
|
||||||
|
for h in range(4):
|
||||||
|
self.head_smooth[h] += self.head_alpha * (self.head_target[h] - self.head_smooth[h])
|
||||||
|
|
||||||
|
gp_zero = self.skill_mode == "groundpick" or self.recovery_state is not None
|
||||||
|
if not gp_zero:
|
||||||
|
cmd[3:7] = self.head_smooth
|
||||||
|
|
||||||
|
if (
|
||||||
|
self.script_t is not None
|
||||||
|
and self.skill_mode == "walk"
|
||||||
|
and self.recovery_state is None
|
||||||
|
and self.custom_script is not None
|
||||||
|
):
|
||||||
|
cmd = script_command_at(self.custom_script, self.script_t, 13)
|
||||||
|
return cmd
|
||||||
|
|
||||||
|
def step(
|
||||||
|
self,
|
||||||
|
dt: float,
|
||||||
|
proj_grav_z: float,
|
||||||
|
auto_stand_on_fall: bool = False,
|
||||||
|
) -> StepDecision:
|
||||||
|
zero_last_action = False
|
||||||
|
|
||||||
|
if self.lifecycle in (LifecycleState.IDLE, LifecycleState.CALIBRATION, LifecycleState.FALL_SAFE):
|
||||||
|
return StepDecision(
|
||||||
|
lifecycle=self.lifecycle,
|
||||||
|
skill_mode=self.skill_mode,
|
||||||
|
policy_key=None,
|
||||||
|
torque_enabled=False,
|
||||||
|
soft_start_alpha=0.0,
|
||||||
|
command_13=np.zeros(13, dtype=np.float32),
|
||||||
|
zero_last_action=False,
|
||||||
|
mode_label=self._mode_label(),
|
||||||
|
recovery_state=self.recovery_state,
|
||||||
|
loco=self.loco,
|
||||||
|
head_mode=self.head_mode,
|
||||||
|
)
|
||||||
|
|
||||||
|
if self.lifecycle == LifecycleState.SOFT_START:
|
||||||
|
self.soft_start_elapsed += dt
|
||||||
|
u = min(1.0, self.soft_start_elapsed / self.soft_start_duration)
|
||||||
|
alpha = 0.5 * (1.0 - math.cos(math.pi * u))
|
||||||
|
if u < 1.0 - 1e-6:
|
||||||
|
return StepDecision(
|
||||||
|
lifecycle=self.lifecycle,
|
||||||
|
skill_mode="soft_start",
|
||||||
|
policy_key=None,
|
||||||
|
torque_enabled=True,
|
||||||
|
soft_start_alpha=alpha,
|
||||||
|
command_13=np.zeros(13, dtype=np.float32),
|
||||||
|
zero_last_action=True,
|
||||||
|
mode_label=self._mode_label(),
|
||||||
|
recovery_state=None,
|
||||||
|
loco=self.loco,
|
||||||
|
head_mode=False,
|
||||||
|
)
|
||||||
|
self.lifecycle = LifecycleState.ACTIVE
|
||||||
|
self.skill_mode = "walk"
|
||||||
|
zero_last_action = True
|
||||||
|
|
||||||
|
# Active lifecycle: advance timers before fall/recovery and command build
|
||||||
|
if self.sit_delay_rem is not None:
|
||||||
|
self.sit_delay_rem -= dt
|
||||||
|
if self.sit_delay_rem <= 0.0:
|
||||||
|
self.sit_delay_rem = None
|
||||||
|
if self.skill_mode == "sitstand":
|
||||||
|
self.sit_flag = 1
|
||||||
|
|
||||||
|
if self.stand_delay_rem is not None:
|
||||||
|
self.stand_delay_rem -= dt
|
||||||
|
if self.stand_delay_rem <= 0.0:
|
||||||
|
self.stand_delay_rem = None
|
||||||
|
self.skill_mode = "walk"
|
||||||
|
zero_last_action = True
|
||||||
|
|
||||||
|
# Fall detection & recovery state machine
|
||||||
|
fallen_now = (not math.isfinite(proj_grav_z)) or (proj_grav_z > self.tilt_z)
|
||||||
|
if self.recovery_state == "fallen":
|
||||||
|
self.recovery_steps += 1
|
||||||
|
if self.recovery_steps >= self.fall_settle_steps:
|
||||||
|
self.recovery_state = "recovering"
|
||||||
|
self.recovery_steps = 0
|
||||||
|
self.recovery_upright_steps = 0
|
||||||
|
zero_last_action = True
|
||||||
|
elif self.recovery_state == "recovering":
|
||||||
|
self.recovery_steps += 1
|
||||||
|
if proj_grav_z < self.upright_z:
|
||||||
|
self.recovery_upright_steps += 1
|
||||||
|
else:
|
||||||
|
self.recovery_upright_steps = 0
|
||||||
|
if self.recovery_upright_steps >= self.recover_upright_steps_req:
|
||||||
|
self.recovery_state = None
|
||||||
|
self.recovery_steps = 0
|
||||||
|
self.recovery_upright_steps = 0
|
||||||
|
self.skill_mode = "walk"
|
||||||
|
zero_last_action = True
|
||||||
|
elif self.recovery_steps >= self.recover_giveup_steps:
|
||||||
|
self._clear_skill_runs()
|
||||||
|
self.lifecycle = LifecycleState.FALL_SAFE
|
||||||
|
return StepDecision(
|
||||||
|
lifecycle=self.lifecycle,
|
||||||
|
skill_mode="walk",
|
||||||
|
policy_key=None,
|
||||||
|
torque_enabled=False,
|
||||||
|
soft_start_alpha=0.0,
|
||||||
|
command_13=np.zeros(13, dtype=np.float32),
|
||||||
|
zero_last_action=True,
|
||||||
|
mode_label=self._mode_label(),
|
||||||
|
recovery_state=None,
|
||||||
|
loco=self.loco,
|
||||||
|
head_mode=False,
|
||||||
|
)
|
||||||
|
elif fallen_now:
|
||||||
|
recoverable = (
|
||||||
|
auto_stand_on_fall
|
||||||
|
and self.loco == "legs"
|
||||||
|
and self.skill_mode == "walk"
|
||||||
|
and self.post_kick_lock == 0
|
||||||
|
and self.stand_delay_rem is None
|
||||||
|
)
|
||||||
|
self.fall_debounce += 1
|
||||||
|
if self.fall_debounce >= self.fall_debounce_steps:
|
||||||
|
self.fall_debounce = 0
|
||||||
|
self.head_mode = False
|
||||||
|
if recoverable:
|
||||||
|
self.recovery_state = "fallen"
|
||||||
|
self.recovery_steps = 0
|
||||||
|
else:
|
||||||
|
self._clear_skill_runs()
|
||||||
|
self.lifecycle = LifecycleState.FALL_SAFE
|
||||||
|
return StepDecision(
|
||||||
|
lifecycle=self.lifecycle,
|
||||||
|
skill_mode="walk",
|
||||||
|
policy_key=None,
|
||||||
|
torque_enabled=False,
|
||||||
|
soft_start_alpha=0.0,
|
||||||
|
command_13=np.zeros(13, dtype=np.float32),
|
||||||
|
zero_last_action=True,
|
||||||
|
mode_label=self._mode_label(),
|
||||||
|
recovery_state=None,
|
||||||
|
loco=self.loco,
|
||||||
|
head_mode=False,
|
||||||
|
)
|
||||||
|
else:
|
||||||
|
self.fall_debounce = 0
|
||||||
|
|
||||||
|
policy_key = self._active_policy_key()
|
||||||
|
command_13 = self._build_command_13()
|
||||||
|
|
||||||
|
# Advance step-counted skills AFTER choosing this tick's policy/command
|
||||||
|
if self.post_kick_lock > 0 and self.skill_mode == "walk":
|
||||||
|
self.post_kick_lock -= 1
|
||||||
|
|
||||||
|
if self.skill_mode in ("kickL", "kickR") and self.kick_steps is not None:
|
||||||
|
self.kick_steps += 1
|
||||||
|
if self.kick_steps >= self.kick_total_steps:
|
||||||
|
self.kick_steps = None
|
||||||
|
self.skill_mode = "walk"
|
||||||
|
self.post_kick_lock = self.post_kick_lock_total
|
||||||
|
|
||||||
|
if self.skill_mode == "crouch" and self.crouch_phase is not None:
|
||||||
|
self.crouch_phase += dt / self.crouch_period_s
|
||||||
|
if self.crouch_phase >= self.crouch_end_phase:
|
||||||
|
self.crouch_phase = None
|
||||||
|
self.skill_mode = "walk"
|
||||||
|
|
||||||
|
if self.skill_mode == "groundpick" and self.pick_phase is not None:
|
||||||
|
self.pick_phase += dt / self.groundpick_period_s
|
||||||
|
if self.pick_phase >= self.groundpick_end_phase:
|
||||||
|
self.pick_phase = None
|
||||||
|
self.skill_mode = "walk"
|
||||||
|
|
||||||
|
if self.skill_mode == "roll" and self.roll_steps is not None:
|
||||||
|
self.roll_steps += 1
|
||||||
|
if proj_grav_z > -0.3:
|
||||||
|
self.roll_tipped = True
|
||||||
|
upright = proj_grav_z < self.upright_z
|
||||||
|
done = self.roll_tipped and upright and self.roll_steps >= 40
|
||||||
|
expired = self.roll_steps >= self.roll_max_steps
|
||||||
|
if done or expired:
|
||||||
|
self.roll_steps = None
|
||||||
|
self.roll_tipped = False
|
||||||
|
self.skill_mode = "walk"
|
||||||
|
zero_last_action = True
|
||||||
|
if not upright:
|
||||||
|
self.lifecycle = LifecycleState.FALL_SAFE
|
||||||
|
|
||||||
|
if self.script_t is not None:
|
||||||
|
self.script_t += dt
|
||||||
|
if self.custom_script is None:
|
||||||
|
self.script_t = None
|
||||||
|
else:
|
||||||
|
dur = float(self.custom_script.get("durationS") or self.custom_script.get("duration_s") or 0.0)
|
||||||
|
loop = self.custom_script.get("loop", True) is not False
|
||||||
|
if not loop and self.script_t >= dur:
|
||||||
|
self.script_t = None
|
||||||
|
|
||||||
|
return StepDecision(
|
||||||
|
lifecycle=self.lifecycle,
|
||||||
|
skill_mode=self.skill_mode,
|
||||||
|
policy_key=policy_key,
|
||||||
|
torque_enabled=self.lifecycle == LifecycleState.ACTIVE,
|
||||||
|
soft_start_alpha=1.0,
|
||||||
|
command_13=command_13,
|
||||||
|
zero_last_action=zero_last_action,
|
||||||
|
mode_label=self._mode_label(),
|
||||||
|
recovery_state=self.recovery_state,
|
||||||
|
loco=self.loco,
|
||||||
|
head_mode=self.head_mode,
|
||||||
|
)
|
||||||
@@ -0,0 +1 @@
|
|||||||
|
"""Hardware abstraction layers for Microduck on Arduino UNO Q."""
|
||||||
@@ -0,0 +1,57 @@
|
|||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
from abc import ABC, abstractmethod
|
||||||
|
from dataclasses import dataclass, field
|
||||||
|
from typing import Any
|
||||||
|
|
||||||
|
|
||||||
|
@dataclass
|
||||||
|
class HardwareFrame:
|
||||||
|
gyro_raw: list[float]
|
||||||
|
proj_gravity_raw: list[float]
|
||||||
|
ticks: list[int]
|
||||||
|
raw_speeds: list[int]
|
||||||
|
voltage_v: float = 7.4
|
||||||
|
max_temp_c: float = 35.0
|
||||||
|
imu_ok: bool = True
|
||||||
|
servo_err_mask: int = 0
|
||||||
|
timestamp_s: float = 0.0
|
||||||
|
extra: dict[str, Any] = field(default_factory=dict)
|
||||||
|
|
||||||
|
|
||||||
|
class HardwareBridge(ABC):
|
||||||
|
name: str = "abstract"
|
||||||
|
|
||||||
|
@abstractmethod
|
||||||
|
def connect(self) -> None:
|
||||||
|
"""Initialize connection to the MCU / servo bus."""
|
||||||
|
|
||||||
|
@abstractmethod
|
||||||
|
def close(self) -> None:
|
||||||
|
"""Release hardware resources safely."""
|
||||||
|
|
||||||
|
@abstractmethod
|
||||||
|
def configure(self, joint_names: list[str], profile: dict[str, Any]) -> None:
|
||||||
|
"""Push servo IDs, I2C address, watchdog, and IMU bias config to HAL."""
|
||||||
|
|
||||||
|
@abstractmethod
|
||||||
|
def sync_step(
|
||||||
|
self,
|
||||||
|
enable_torque: bool,
|
||||||
|
target_ticks: list[int],
|
||||||
|
speed_limit: int = 0,
|
||||||
|
acc: int = 50,
|
||||||
|
) -> HardwareFrame:
|
||||||
|
"""Send target joint ticks and read fresh IMU + servo feedback."""
|
||||||
|
|
||||||
|
@abstractmethod
|
||||||
|
def set_torque(self, enable: bool) -> None:
|
||||||
|
"""Immediately enable or disable torque on all configured servos."""
|
||||||
|
|
||||||
|
@abstractmethod
|
||||||
|
def ping_servos(self, max_id: int = 20) -> list[int]:
|
||||||
|
"""Scan and return responding servo IDs."""
|
||||||
|
|
||||||
|
@abstractmethod
|
||||||
|
def calibrate_imu_raw(self, samples: int = 200) -> list[float]:
|
||||||
|
"""Collect stationary gyro readings (rad/s in sensor frame) and return mean bias."""
|
||||||
@@ -0,0 +1,82 @@
|
|||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import math
|
||||||
|
import time
|
||||||
|
from typing import Any
|
||||||
|
|
||||||
|
from .base import HardwareBridge, HardwareFrame
|
||||||
|
|
||||||
|
|
||||||
|
class MockHal(HardwareBridge):
|
||||||
|
"""PC-runnable hardware simulator for pre-hardware verification."""
|
||||||
|
|
||||||
|
name = "mock"
|
||||||
|
|
||||||
|
def __init__(self, initial_tick: int = 2048) -> None:
|
||||||
|
self.connected = False
|
||||||
|
self.torque_enabled = False
|
||||||
|
self.servo_ids = list(range(1, 15))
|
||||||
|
self.ticks = [int(initial_tick)] * 14
|
||||||
|
self.raw_speeds = [0] * 14
|
||||||
|
self._gyro = [0.0, 0.0, 0.0]
|
||||||
|
self._gravity = [0.0, 0.0, -1.0]
|
||||||
|
self._alpha = 0.45
|
||||||
|
|
||||||
|
def connect(self) -> None:
|
||||||
|
self.connected = True
|
||||||
|
|
||||||
|
def close(self) -> None:
|
||||||
|
self.torque_enabled = False
|
||||||
|
self.connected = False
|
||||||
|
|
||||||
|
def configure(self, joint_names: list[str], profile: dict[str, Any]) -> None:
|
||||||
|
joints = profile.get("joints", {})
|
||||||
|
self.servo_ids = [int(joints[n]["id"]) for n in joint_names]
|
||||||
|
|
||||||
|
def set_mock_gravity(self, grav: list[float]) -> None:
|
||||||
|
gx, gy, gz = float(grav[0]), float(grav[1]), float(grav[2])
|
||||||
|
norm = math.sqrt(gx * gx + gy * gy + gz * gz) or 1.0
|
||||||
|
self._gravity = [gx / norm, gy / norm, gz / norm]
|
||||||
|
|
||||||
|
def set_mock_gyro(self, gyro: list[float]) -> None:
|
||||||
|
self._gyro = [float(gyro[0]), float(gyro[1]), float(gyro[2])]
|
||||||
|
|
||||||
|
def sync_step(
|
||||||
|
self,
|
||||||
|
enable_torque: bool,
|
||||||
|
target_ticks: list[int],
|
||||||
|
speed_limit: int = 0,
|
||||||
|
acc: int = 50,
|
||||||
|
) -> HardwareFrame:
|
||||||
|
self.torque_enabled = bool(enable_torque)
|
||||||
|
if self.torque_enabled:
|
||||||
|
for i in range(14):
|
||||||
|
prev = self.ticks[i]
|
||||||
|
nxt = int(round(prev + self._alpha * (int(target_ticks[i]) - prev)))
|
||||||
|
delta = nxt - prev
|
||||||
|
self.ticks[i] = max(0, min(4095, nxt))
|
||||||
|
mag = min(0x7FFF, abs(delta) * 50)
|
||||||
|
self.raw_speeds[i] = (mag | 0x8000) if delta < 0 else mag
|
||||||
|
else:
|
||||||
|
self.raw_speeds = [0] * 14
|
||||||
|
|
||||||
|
return HardwareFrame(
|
||||||
|
gyro_raw=list(self._gyro),
|
||||||
|
proj_gravity_raw=list(self._gravity),
|
||||||
|
ticks=list(self.ticks),
|
||||||
|
raw_speeds=list(self.raw_speeds),
|
||||||
|
voltage_v=7.8,
|
||||||
|
max_temp_c=34.0,
|
||||||
|
imu_ok=True,
|
||||||
|
servo_err_mask=0,
|
||||||
|
timestamp_s=time.monotonic(),
|
||||||
|
)
|
||||||
|
|
||||||
|
def set_torque(self, enable: bool) -> None:
|
||||||
|
self.torque_enabled = bool(enable)
|
||||||
|
|
||||||
|
def ping_servos(self, max_id: int = 20) -> list[int]:
|
||||||
|
return [sid for sid in self.servo_ids if 0 <= sid <= max_id]
|
||||||
|
|
||||||
|
def calibrate_imu_raw(self, samples: int = 200) -> list[float]:
|
||||||
|
return list(self._gyro)
|
||||||
@@ -0,0 +1,109 @@
|
|||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import time
|
||||||
|
from typing import Any
|
||||||
|
|
||||||
|
from .base import HardwareBridge, HardwareFrame
|
||||||
|
|
||||||
|
|
||||||
|
class RouterBridgeHal(HardwareBridge):
|
||||||
|
"""Arduino UNO Q HAL communicating with STM32U585 over Arduino_RouterBridge."""
|
||||||
|
|
||||||
|
name = "router"
|
||||||
|
|
||||||
|
def __init__(self) -> None:
|
||||||
|
self._bridge = None
|
||||||
|
self._servo_ids = list(range(1, 15))
|
||||||
|
|
||||||
|
def connect(self) -> None:
|
||||||
|
try:
|
||||||
|
from arduino.app_utils import Bridge # type: ignore
|
||||||
|
except ImportError as exc:
|
||||||
|
raise RuntimeError(
|
||||||
|
"arduino.app_utils.Bridge is not available. Run on Arduino UNO Q via App Lab / "
|
||||||
|
"arduino-app-cli, or pass '--hal mock' for PC pre-hardware debugging."
|
||||||
|
) from exc
|
||||||
|
self._bridge = Bridge
|
||||||
|
|
||||||
|
def close(self) -> None:
|
||||||
|
if self._bridge is not None:
|
||||||
|
try:
|
||||||
|
self.set_torque(False)
|
||||||
|
except Exception:
|
||||||
|
pass
|
||||||
|
self._bridge = None
|
||||||
|
|
||||||
|
def _require_bridge(self) -> Any:
|
||||||
|
if self._bridge is None:
|
||||||
|
raise RuntimeError("RouterBridgeHal is not connected")
|
||||||
|
return self._bridge
|
||||||
|
|
||||||
|
def configure(self, joint_names: list[str], profile: dict[str, Any]) -> None:
|
||||||
|
bridge = self._require_bridge()
|
||||||
|
joints = profile["joints"]
|
||||||
|
servo = profile["servo"]
|
||||||
|
imu = profile["imu"]
|
||||||
|
safety = profile.get("safety", {})
|
||||||
|
self._servo_ids = [int(joints[name]["id"]) for name in joint_names]
|
||||||
|
|
||||||
|
addr_raw = imu.get("i2c_addr", "0x6A")
|
||||||
|
i2c_addr = int(addr_raw, 16) if isinstance(addr_raw, str) else int(addr_raw)
|
||||||
|
bias = [float(x) for x in imu.get("gyro_bias_rad_s", [0.0, 0.0, 0.0])]
|
||||||
|
payload: list[float] = [
|
||||||
|
float(servo.get("baudrate", 1000000)),
|
||||||
|
float(i2c_addr),
|
||||||
|
float(safety.get("watchdog_ms", 150)),
|
||||||
|
bias[0],
|
||||||
|
bias[1],
|
||||||
|
bias[2],
|
||||||
|
*[float(sid) for sid in self._servo_ids],
|
||||||
|
]
|
||||||
|
bridge.call("hw_configure", payload)
|
||||||
|
|
||||||
|
def sync_step(
|
||||||
|
self,
|
||||||
|
enable_torque: bool,
|
||||||
|
target_ticks: list[int],
|
||||||
|
speed_limit: int = 0,
|
||||||
|
acc: int = 50,
|
||||||
|
) -> HardwareFrame:
|
||||||
|
bridge = self._require_bridge()
|
||||||
|
if len(target_ticks) != 14:
|
||||||
|
raise ValueError("Expected 14 target_ticks")
|
||||||
|
cmd_frame = [
|
||||||
|
1 if enable_torque else 0,
|
||||||
|
int(speed_limit),
|
||||||
|
int(acc),
|
||||||
|
*[int(t) for t in target_ticks],
|
||||||
|
]
|
||||||
|
res = bridge.call("hw_sync_step", cmd_frame)
|
||||||
|
if not isinstance(res, (list, tuple)) or len(res) < 38:
|
||||||
|
raise RuntimeError(f"Invalid hw_sync_step response from MCU: {res!r}")
|
||||||
|
|
||||||
|
return HardwareFrame(
|
||||||
|
gyro_raw=[float(res[0]), float(res[1]), float(res[2])],
|
||||||
|
proj_gravity_raw=[float(res[3]), float(res[4]), float(res[5])],
|
||||||
|
ticks=[int(round(res[6 + i])) for i in range(14)],
|
||||||
|
raw_speeds=[int(round(res[20 + i])) for i in range(14)],
|
||||||
|
voltage_v=float(res[34]),
|
||||||
|
max_temp_c=float(res[35]),
|
||||||
|
imu_ok=bool(int(round(res[36]))),
|
||||||
|
servo_err_mask=int(round(res[37])),
|
||||||
|
timestamp_s=time.monotonic(),
|
||||||
|
)
|
||||||
|
|
||||||
|
def set_torque(self, enable: bool) -> None:
|
||||||
|
bridge = self._require_bridge()
|
||||||
|
bridge.call("hw_set_torque", 1 if enable else 0)
|
||||||
|
|
||||||
|
def ping_servos(self, max_id: int = 20) -> list[int]:
|
||||||
|
bridge = self._require_bridge()
|
||||||
|
res = bridge.call("hw_ping_servos", int(max_id))
|
||||||
|
return [int(x) for x in res] if isinstance(res, (list, tuple)) else []
|
||||||
|
|
||||||
|
def calibrate_imu_raw(self, samples: int = 200) -> list[float]:
|
||||||
|
bridge = self._require_bridge()
|
||||||
|
res = bridge.call("hw_calibrate_imu", int(samples))
|
||||||
|
if not isinstance(res, (list, tuple)) or len(res) != 3:
|
||||||
|
raise RuntimeError(f"Invalid hw_calibrate_imu response: {res!r}")
|
||||||
|
return [float(res[0]), float(res[1]), float(res[2])]
|
||||||
@@ -0,0 +1,125 @@
|
|||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import argparse
|
||||||
|
import os
|
||||||
|
import signal
|
||||||
|
import sys
|
||||||
|
from pathlib import Path
|
||||||
|
|
||||||
|
PYTHON_ROOT = Path(__file__).resolve().parent
|
||||||
|
UNOQ_ROOT = PYTHON_ROOT.parent
|
||||||
|
if str(PYTHON_ROOT) not in sys.path:
|
||||||
|
sys.path.insert(0, str(PYTHON_ROOT))
|
||||||
|
|
||||||
|
from core.config import load_hardware_profile, load_robot_manifest # noqa: E402
|
||||||
|
from core.controller import MicroduckController # noqa: E402
|
||||||
|
from core.deploy_service import DeployService # noqa: E402
|
||||||
|
from core.policy_runner import PolicySet # noqa: E402
|
||||||
|
from hal.base import HardwareBridge # noqa: E402
|
||||||
|
from hal.mock_hal import MockHal # noqa: E402
|
||||||
|
from hal.router_hal import RouterBridgeHal # noqa: E402
|
||||||
|
from web.fallback_server import FallbackWebServer # noqa: E402
|
||||||
|
|
||||||
|
|
||||||
|
def _select_hal(hal_mode: str) -> HardwareBridge:
|
||||||
|
if hal_mode == "mock":
|
||||||
|
return MockHal()
|
||||||
|
if hal_mode == "router":
|
||||||
|
return RouterBridgeHal()
|
||||||
|
# auto: prefer RouterBridge when running on UNO Q App Lab, otherwise ask explicitly for --hal mock
|
||||||
|
try:
|
||||||
|
from arduino.app_utils import Bridge # type: ignore # noqa: F401
|
||||||
|
|
||||||
|
return RouterBridgeHal()
|
||||||
|
except ImportError as exc:
|
||||||
|
raise RuntimeError(
|
||||||
|
"arduino.app_utils is not available in this Python environment. "
|
||||||
|
"Launch inside UNO Q App Lab, or pass '--hal mock' for PC pre-hardware testing."
|
||||||
|
) from exc
|
||||||
|
|
||||||
|
|
||||||
|
def main(argv: list[str] | None = None) -> int:
|
||||||
|
parser = argparse.ArgumentParser(description="Microduck UNO Q 50Hz Controller & Deploy Server")
|
||||||
|
parser.add_argument("--base-dir", type=Path, default=UNOQ_ROOT, help="UNO Q config/policies directory")
|
||||||
|
parser.add_argument(
|
||||||
|
"--hal",
|
||||||
|
choices=("auto", "router", "mock"),
|
||||||
|
default=os.environ.get("MICRODUCK_HAL", "auto"),
|
||||||
|
help="Hardware backend: 'router' (UNO Q STM32), 'mock' (PC simulator), or 'auto'",
|
||||||
|
)
|
||||||
|
parser.add_argument(
|
||||||
|
"--policy-backend",
|
||||||
|
choices=("auto", "onnx", "dummy"),
|
||||||
|
default=os.environ.get("MICRODUCK_POLICY_BACKEND", "auto"),
|
||||||
|
help="Inference backend: 'onnx' / 'auto', or explicit 'dummy' dry-run",
|
||||||
|
)
|
||||||
|
parser.add_argument("--host", default="0.0.0.0")
|
||||||
|
parser.add_argument("--port", type=int, default=8000)
|
||||||
|
args = parser.parse_args(argv)
|
||||||
|
|
||||||
|
base_dir = Path(args.base_dir).resolve()
|
||||||
|
manifest = load_robot_manifest(base_dir / "config" / "robot_manifest.json")
|
||||||
|
profile = load_hardware_profile(base_dir / "config" / "hardware_profile.json", joint_names=manifest["joint_names"])
|
||||||
|
|
||||||
|
custom_script = None
|
||||||
|
script_path = base_dir / "config" / "custom_move_script.json"
|
||||||
|
if script_path.is_file():
|
||||||
|
import json
|
||||||
|
|
||||||
|
custom_script = json.loads(script_path.read_text(encoding="utf-8"))
|
||||||
|
|
||||||
|
hal = _select_hal(args.hal)
|
||||||
|
|
||||||
|
boot_error: str | None = None
|
||||||
|
try:
|
||||||
|
policies = PolicySet.load_from_manifest(base_dir, manifest, backend=args.policy_backend)
|
||||||
|
except FileNotFoundError as exc:
|
||||||
|
# Allow web server & /api/deploy to boot on a fresh board before first push from website
|
||||||
|
boot_error = f"Awaiting first policy deploy: {exc}"
|
||||||
|
policies = PolicySet({}, backend="awaiting_deploy")
|
||||||
|
|
||||||
|
controller = MicroduckController(
|
||||||
|
base_dir=base_dir,
|
||||||
|
manifest=manifest,
|
||||||
|
profile=profile,
|
||||||
|
hal=hal,
|
||||||
|
policies=policies,
|
||||||
|
custom_script=custom_script,
|
||||||
|
)
|
||||||
|
if boot_error:
|
||||||
|
controller._last_error = boot_error
|
||||||
|
|
||||||
|
deploy_backend = args.policy_backend
|
||||||
|
deploy_service = DeployService(base_dir=base_dir, controller=controller, policy_backend=deploy_backend)
|
||||||
|
|
||||||
|
controller.start()
|
||||||
|
|
||||||
|
def _shutdown(*_args: object) -> None:
|
||||||
|
raise SystemExit(0)
|
||||||
|
|
||||||
|
signal.signal(signal.SIGTERM, _shutdown)
|
||||||
|
|
||||||
|
srv: FallbackWebServer | None = None
|
||||||
|
try:
|
||||||
|
try:
|
||||||
|
import uvicorn # type: ignore
|
||||||
|
|
||||||
|
from web.server import create_fastapi_app
|
||||||
|
|
||||||
|
app = create_fastapi_app(controller, deploy_service)
|
||||||
|
uvicorn.run(app, host=args.host, port=args.port, log_level="info")
|
||||||
|
except ImportError:
|
||||||
|
srv = FallbackWebServer(args.host, args.port, controller, deploy_service)
|
||||||
|
print(f"[microduck-unoq] FastAPI/uvicorn not installed; serving stdlib HTTP/WS on {args.host}:{srv.port}")
|
||||||
|
srv.serve_forever()
|
||||||
|
except (KeyboardInterrupt, SystemExit):
|
||||||
|
pass
|
||||||
|
finally:
|
||||||
|
if srv is not None:
|
||||||
|
srv.server_close()
|
||||||
|
controller.stop()
|
||||||
|
return 0
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == "__main__":
|
||||||
|
raise SystemExit(main())
|
||||||
@@ -0,0 +1,5 @@
|
|||||||
|
numpy>=1.26.0
|
||||||
|
onnxruntime>=1.18.0
|
||||||
|
fastapi>=0.115.0
|
||||||
|
uvicorn[standard]>=0.30.0
|
||||||
|
python-multipart>=0.0.9
|
||||||
@@ -0,0 +1 @@
|
|||||||
|
"""Web HTTP/WebSocket control and deployment package for Microduck UNO Q."""
|
||||||
@@ -0,0 +1,184 @@
|
|||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import copy
|
||||||
|
import json
|
||||||
|
from email.parser import BytesParser
|
||||||
|
from email.policy import default as email_default_policy
|
||||||
|
from pathlib import Path
|
||||||
|
from typing import Any
|
||||||
|
|
||||||
|
from core.controller import MicroduckController
|
||||||
|
from core.deploy_service import DeployService
|
||||||
|
|
||||||
|
|
||||||
|
STATIC_DIR = Path(__file__).resolve().parent / "static"
|
||||||
|
|
||||||
|
|
||||||
|
def load_index_html() -> bytes:
|
||||||
|
path = STATIC_DIR / "index.html"
|
||||||
|
if not path.is_file():
|
||||||
|
return b"<html><body><h1>Microduck controller UI missing</h1></body></html>"
|
||||||
|
return path.read_bytes()
|
||||||
|
|
||||||
|
|
||||||
|
def parse_bool_flag(raw: str | None) -> bool:
|
||||||
|
if raw is None:
|
||||||
|
return False
|
||||||
|
return raw.strip().lower() in ("1", "true", "yes", "on")
|
||||||
|
|
||||||
|
|
||||||
|
def extract_deploy_payload(
|
||||||
|
content_type: str,
|
||||||
|
body: bytes,
|
||||||
|
query_overwrite: str | None = None,
|
||||||
|
) -> tuple[bytes, bool]:
|
||||||
|
overwrite = parse_bool_flag(query_overwrite)
|
||||||
|
ctype = (content_type or "").lower()
|
||||||
|
if "multipart/form-data" in ctype:
|
||||||
|
raw_msg = f"Content-Type: {content_type}\r\nMIME-Version: 1.0\r\n\r\n".encode("latin1") + body
|
||||||
|
msg = BytesParser(policy=email_default_policy).parsebytes(raw_msg)
|
||||||
|
if not msg.is_multipart():
|
||||||
|
raise ValueError("Malformed multipart/form-data upload")
|
||||||
|
zip_bytes: bytes | None = None
|
||||||
|
for part in msg.iter_parts():
|
||||||
|
field = part.get_param("name", header="content-disposition")
|
||||||
|
payload = part.get_payload(decode=True) or b""
|
||||||
|
if field in ("bundle", "file", "zip") and payload:
|
||||||
|
zip_bytes = payload
|
||||||
|
elif field == "overwrite_profile":
|
||||||
|
overwrite = parse_bool_flag(payload.decode("utf-8", errors="ignore"))
|
||||||
|
if not zip_bytes:
|
||||||
|
raise ValueError("Multipart upload missing 'bundle' zip field")
|
||||||
|
return zip_bytes, overwrite
|
||||||
|
return body, overwrite
|
||||||
|
|
||||||
|
|
||||||
|
def build_status_payload(controller: MicroduckController, deploy_service: DeployService) -> dict[str, Any]:
|
||||||
|
st = controller.get_status()
|
||||||
|
st["last_deploy"] = copy.deepcopy(deploy_service.last_deploy) if deploy_service.last_deploy else None
|
||||||
|
return st
|
||||||
|
|
||||||
|
|
||||||
|
def handle_ws_message(controller: MicroduckController, msg: dict[str, Any]) -> dict[str, Any] | None:
|
||||||
|
kind = msg.get("type")
|
||||||
|
if kind == "input":
|
||||||
|
controller.set_remote_input(
|
||||||
|
norm_x=float(msg.get("vx", 0.0)),
|
||||||
|
norm_wz=float(msg.get("wz", 0.0)),
|
||||||
|
speed_scale=float(msg["speed_scale"]) if "speed_scale" in msg else None,
|
||||||
|
)
|
||||||
|
return None
|
||||||
|
if kind == "head":
|
||||||
|
controller.set_head_target(
|
||||||
|
float(msg.get("neck_pitch", 0.0)),
|
||||||
|
float(msg.get("head_pitch", 0.0)),
|
||||||
|
float(msg.get("head_yaw", 0.0)),
|
||||||
|
float(msg.get("head_roll", 0.0)),
|
||||||
|
)
|
||||||
|
return None
|
||||||
|
if kind == "stand":
|
||||||
|
try:
|
||||||
|
controller.request_stand()
|
||||||
|
return {"type": "ack", "event": "stand"}
|
||||||
|
except RuntimeError as exc:
|
||||||
|
return {"type": "error", "error": str(exc)}
|
||||||
|
if kind == "estop":
|
||||||
|
controller.emergency_stop()
|
||||||
|
return {"type": "ack", "event": "estop"}
|
||||||
|
if kind == "action":
|
||||||
|
action = str(msg.get("action", ""))
|
||||||
|
ok = controller.trigger_action(action)
|
||||||
|
return {"type": "ack", "event": "action", "action": action, "ok": ok}
|
||||||
|
if kind == "settings":
|
||||||
|
if "auto_stand_on_fall" in msg:
|
||||||
|
controller.set_auto_stand_on_fall(bool(msg["auto_stand_on_fall"]))
|
||||||
|
if "speed_scale" in msg:
|
||||||
|
controller.set_speed_scale(float(msg["speed_scale"]))
|
||||||
|
return {"type": "ack", "event": "settings"}
|
||||||
|
if kind == "ping":
|
||||||
|
return {"type": "pong", "t": msg.get("t")}
|
||||||
|
return None
|
||||||
|
|
||||||
|
|
||||||
|
def dispatch_api_get(
|
||||||
|
path: str,
|
||||||
|
controller: MicroduckController,
|
||||||
|
deploy_service: DeployService,
|
||||||
|
) -> tuple[int, dict[str, Any]]:
|
||||||
|
if path == "/api/status":
|
||||||
|
return 200, build_status_payload(controller, deploy_service)
|
||||||
|
if path == "/api/profile":
|
||||||
|
return 200, {"ok": True, "profile": copy.deepcopy(controller.profile)}
|
||||||
|
return 404, {"ok": False, "error": f"Unknown GET endpoint: {path}"}
|
||||||
|
|
||||||
|
|
||||||
|
def dispatch_api_post(
|
||||||
|
path: str,
|
||||||
|
body_bytes: bytes,
|
||||||
|
content_type: str,
|
||||||
|
query_params: dict[str, list[str]],
|
||||||
|
controller: MicroduckController,
|
||||||
|
deploy_service: DeployService,
|
||||||
|
) -> tuple[int, dict[str, Any]]:
|
||||||
|
if path == "/api/deploy":
|
||||||
|
try:
|
||||||
|
q_ow = query_params.get("overwrite_profile", [None])[0]
|
||||||
|
raw_zip, overwrite = extract_deploy_payload(content_type, body_bytes, q_ow)
|
||||||
|
res = dict(deploy_service.apply_zip_bytes(raw_zip, overwrite_profile=overwrite))
|
||||||
|
res["status"] = build_status_payload(controller, deploy_service)
|
||||||
|
return 200, res
|
||||||
|
except ValueError as exc:
|
||||||
|
return 400, {"ok": False, "error": str(exc)}
|
||||||
|
except Exception as exc:
|
||||||
|
return 500, {"ok": False, "error": str(exc)}
|
||||||
|
|
||||||
|
try:
|
||||||
|
payload = json.loads(body_bytes.decode("utf-8")) if body_bytes.strip() else {}
|
||||||
|
except Exception:
|
||||||
|
return 400, {"ok": False, "error": "Invalid JSON body"}
|
||||||
|
|
||||||
|
try:
|
||||||
|
if path == "/api/stand":
|
||||||
|
controller.request_stand()
|
||||||
|
return 200, build_status_payload(controller, deploy_service)
|
||||||
|
if path == "/api/estop":
|
||||||
|
controller.emergency_stop()
|
||||||
|
return 200, build_status_payload(controller, deploy_service)
|
||||||
|
if path == "/api/action":
|
||||||
|
action = str(payload.get("action", ""))
|
||||||
|
ok = controller.trigger_action(action)
|
||||||
|
return 200, {"ok": ok, "status": build_status_payload(controller, deploy_service)}
|
||||||
|
if path == "/api/settings":
|
||||||
|
if "auto_stand_on_fall" in payload:
|
||||||
|
controller.set_auto_stand_on_fall(bool(payload["auto_stand_on_fall"]))
|
||||||
|
if "speed_scale" in payload:
|
||||||
|
controller.set_speed_scale(float(payload["speed_scale"]))
|
||||||
|
return 200, build_status_payload(controller, deploy_service)
|
||||||
|
if path == "/api/profile":
|
||||||
|
profile = controller.update_profile(payload)
|
||||||
|
return 200, {"ok": True, "profile": profile}
|
||||||
|
if path == "/api/calibrate/ping":
|
||||||
|
max_id = int(payload.get("max_id", 20))
|
||||||
|
return 200, {"ok": True, "ids": controller.ping_servos(max_id=max_id)}
|
||||||
|
if path == "/api/calibrate/joint_zero":
|
||||||
|
joint = str(payload.get("joint", ""))
|
||||||
|
profile = controller.capture_joint_zero(joint)
|
||||||
|
return 200, {"ok": True, "profile": profile}
|
||||||
|
if path == "/api/calibrate/test_joint":
|
||||||
|
joint = str(payload.get("joint", ""))
|
||||||
|
rad = float(payload.get("rad", 0.0))
|
||||||
|
ticks = controller.test_single_joint(joint, rad)
|
||||||
|
return 200, {"ok": True, "ticks": ticks}
|
||||||
|
if path == "/api/calibrate/exit":
|
||||||
|
controller.exit_calibration()
|
||||||
|
return 200, build_status_payload(controller, deploy_service)
|
||||||
|
if path == "/api/calibrate/imu_zero":
|
||||||
|
samples = int(payload.get("samples", 200))
|
||||||
|
imu = controller.calibrate_imu_zero(samples=samples)
|
||||||
|
return 200, {"ok": True, "imu": imu}
|
||||||
|
except (ValueError, RuntimeError) as exc:
|
||||||
|
return 400, {"ok": False, "error": str(exc)}
|
||||||
|
except Exception as exc:
|
||||||
|
return 500, {"ok": False, "error": str(exc)}
|
||||||
|
|
||||||
|
return 404, {"ok": False, "error": f"Unknown POST endpoint: {path}"}
|
||||||
@@ -0,0 +1,227 @@
|
|||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import base64
|
||||||
|
import hashlib
|
||||||
|
import json
|
||||||
|
import select
|
||||||
|
import socket
|
||||||
|
import struct
|
||||||
|
import threading
|
||||||
|
import time
|
||||||
|
from http.server import BaseHTTPRequestHandler, ThreadingHTTPServer
|
||||||
|
from typing import Any
|
||||||
|
from urllib.parse import parse_qs, urlparse
|
||||||
|
|
||||||
|
from core.controller import MicroduckController
|
||||||
|
from core.deploy_service import DeployService
|
||||||
|
|
||||||
|
from .api import (
|
||||||
|
build_status_payload,
|
||||||
|
dispatch_api_get,
|
||||||
|
dispatch_api_post,
|
||||||
|
handle_ws_message,
|
||||||
|
load_index_html,
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
WS_GUID = "258EAFA5-E914-47DA-95CA-C5AB0DC85B11"
|
||||||
|
|
||||||
|
|
||||||
|
def _ws_encode_frame(opcode: int, payload: bytes) -> bytes:
|
||||||
|
header = bytearray([0x80 | (opcode & 0x0F)])
|
||||||
|
n = len(payload)
|
||||||
|
if n < 126:
|
||||||
|
header.append(n)
|
||||||
|
elif n < 65536:
|
||||||
|
header.append(126)
|
||||||
|
header.extend(struct.pack("!H", n))
|
||||||
|
else:
|
||||||
|
header.append(127)
|
||||||
|
header.extend(struct.pack("!Q", n))
|
||||||
|
return bytes(header) + payload
|
||||||
|
|
||||||
|
|
||||||
|
def _recv_exact(sock: socket.socket, n: int) -> bytes | None:
|
||||||
|
chunks = bytearray()
|
||||||
|
while len(chunks) < n:
|
||||||
|
chunk = sock.recv(n - len(chunks))
|
||||||
|
if not chunk:
|
||||||
|
return None
|
||||||
|
chunks.extend(chunk)
|
||||||
|
return bytes(chunks)
|
||||||
|
|
||||||
|
|
||||||
|
def _ws_read_frame(sock: socket.socket) -> tuple[int, bytes] | None:
|
||||||
|
head = _recv_exact(sock, 2)
|
||||||
|
if head is None:
|
||||||
|
return None
|
||||||
|
b1, b2 = head[0], head[1]
|
||||||
|
opcode = b1 & 0x0F
|
||||||
|
masked = bool(b2 & 0x80)
|
||||||
|
length = b2 & 0x7F
|
||||||
|
if length == 126:
|
||||||
|
ext = _recv_exact(sock, 2)
|
||||||
|
if ext is None:
|
||||||
|
return None
|
||||||
|
length = struct.unpack("!H", ext)[0]
|
||||||
|
elif length == 127:
|
||||||
|
ext = _recv_exact(sock, 8)
|
||||||
|
if ext is None:
|
||||||
|
return None
|
||||||
|
length = struct.unpack("!Q", ext)[0]
|
||||||
|
if length > 1024 * 1024:
|
||||||
|
return None
|
||||||
|
mask = _recv_exact(sock, 4) if masked else b""
|
||||||
|
if masked and mask is None:
|
||||||
|
return None
|
||||||
|
payload = _recv_exact(sock, length) if length > 0 else b""
|
||||||
|
if payload is None:
|
||||||
|
return None
|
||||||
|
if masked:
|
||||||
|
payload = bytes(b ^ mask[i % 4] for i, b in enumerate(payload))
|
||||||
|
return opcode, payload
|
||||||
|
|
||||||
|
|
||||||
|
class FallbackWebServer(ThreadingHTTPServer):
|
||||||
|
"""Zero-dependency HTTP + RFC6455 WebSocket server (runs on stock Python 3.12)."""
|
||||||
|
|
||||||
|
allow_reuse_address = True
|
||||||
|
daemon_threads = True
|
||||||
|
|
||||||
|
def __init__(
|
||||||
|
self,
|
||||||
|
host: str,
|
||||||
|
port: int,
|
||||||
|
controller: MicroduckController,
|
||||||
|
deploy_service: DeployService,
|
||||||
|
) -> None:
|
||||||
|
self.controller = controller
|
||||||
|
self.deploy_service = deploy_service
|
||||||
|
super().__init__((host, port), _RequestHandler)
|
||||||
|
self.port = int(self.server_address[1])
|
||||||
|
|
||||||
|
|
||||||
|
class _RequestHandler(BaseHTTPRequestHandler):
|
||||||
|
protocol_version = "HTTP/1.1"
|
||||||
|
|
||||||
|
def log_message(self, format: str, *args: Any) -> None: # noqa: A002
|
||||||
|
return
|
||||||
|
|
||||||
|
def _send_cors_headers(self) -> None:
|
||||||
|
self.send_header("Access-Control-Allow-Origin", "*")
|
||||||
|
self.send_header("Access-Control-Allow-Methods", "GET, POST, OPTIONS")
|
||||||
|
self.send_header("Access-Control-Allow-Headers", "Content-Type")
|
||||||
|
self.send_header("Access-Control-Allow-Private-Network", "true")
|
||||||
|
|
||||||
|
def _send_json(self, status_code: int, payload: dict[str, Any]) -> None:
|
||||||
|
raw = json.dumps(payload).encode("utf-8")
|
||||||
|
self.send_response(status_code)
|
||||||
|
self._send_cors_headers()
|
||||||
|
self.send_header("Content-Type", "application/json; charset=utf-8")
|
||||||
|
self.send_header("Content-Length", str(len(raw)))
|
||||||
|
self.end_headers()
|
||||||
|
self.wfile.write(raw)
|
||||||
|
|
||||||
|
def _send_bytes(self, status_code: int, content_type: str, data: bytes) -> None:
|
||||||
|
self.send_response(status_code)
|
||||||
|
self._send_cors_headers()
|
||||||
|
self.send_header("Content-Type", content_type)
|
||||||
|
self.send_header("Content-Length", str(len(data)))
|
||||||
|
self.end_headers()
|
||||||
|
self.wfile.write(data)
|
||||||
|
|
||||||
|
def do_OPTIONS(self) -> None: # noqa: N802
|
||||||
|
self.send_response(204)
|
||||||
|
self._send_cors_headers()
|
||||||
|
self.send_header("Content-Length", "0")
|
||||||
|
self.end_headers()
|
||||||
|
|
||||||
|
def do_GET(self) -> None: # noqa: N802
|
||||||
|
parsed = urlparse(self.path)
|
||||||
|
if parsed.path == "/ws/control" and self.headers.get("Upgrade", "").lower() == "websocket":
|
||||||
|
self._handle_websocket()
|
||||||
|
return
|
||||||
|
if parsed.path in ("/", "/index.html"):
|
||||||
|
self._send_bytes(200, "text/html; charset=utf-8", load_index_html())
|
||||||
|
return
|
||||||
|
|
||||||
|
srv: FallbackWebServer = self.server # type: ignore[assignment]
|
||||||
|
code, payload = dispatch_api_get(parsed.path, srv.controller, srv.deploy_service)
|
||||||
|
self._send_json(code, payload)
|
||||||
|
|
||||||
|
def do_POST(self) -> None: # noqa: N802
|
||||||
|
parsed = urlparse(self.path)
|
||||||
|
length = int(self.headers.get("Content-Length", "0") or "0")
|
||||||
|
if length < 0 or length > 160 * 1024 * 1024:
|
||||||
|
self._send_json(413, {"ok": False, "error": "Payload too large"})
|
||||||
|
return
|
||||||
|
body = self.rfile.read(length) if length > 0 else b""
|
||||||
|
content_type = self.headers.get("Content-Type", "")
|
||||||
|
query = parse_qs(parsed.query)
|
||||||
|
srv: FallbackWebServer = self.server # type: ignore[assignment]
|
||||||
|
code, payload = dispatch_api_post(
|
||||||
|
parsed.path,
|
||||||
|
body,
|
||||||
|
content_type,
|
||||||
|
query,
|
||||||
|
srv.controller,
|
||||||
|
srv.deploy_service,
|
||||||
|
)
|
||||||
|
self._send_json(code, payload)
|
||||||
|
|
||||||
|
def _handle_websocket(self) -> None:
|
||||||
|
key = self.headers.get("Sec-WebSocket-Key")
|
||||||
|
if not key:
|
||||||
|
self.send_error(400, "Missing Sec-WebSocket-Key")
|
||||||
|
return
|
||||||
|
accept = base64.b64encode(hashlib.sha1((key.strip() + WS_GUID).encode("ascii")).digest()).decode("ascii")
|
||||||
|
self.send_response(101, "Switching Protocols")
|
||||||
|
self.send_header("Upgrade", "websocket")
|
||||||
|
self.send_header("Connection", "Upgrade")
|
||||||
|
self.send_header("Sec-WebSocket-Accept", accept)
|
||||||
|
self.end_headers()
|
||||||
|
|
||||||
|
srv: FallbackWebServer = self.server # type: ignore[assignment]
|
||||||
|
sock: socket.socket = self.connection
|
||||||
|
sock.settimeout(5.0)
|
||||||
|
write_lock = threading.Lock()
|
||||||
|
last_telemetry = 0.0
|
||||||
|
|
||||||
|
try:
|
||||||
|
while True:
|
||||||
|
now = time.monotonic()
|
||||||
|
if now - last_telemetry >= 0.1:
|
||||||
|
last_telemetry = now
|
||||||
|
msg = {"type": "telemetry", **build_status_payload(srv.controller, srv.deploy_service)}
|
||||||
|
frame = _ws_encode_frame(0x1, json.dumps(msg).encode("utf-8"))
|
||||||
|
with write_lock:
|
||||||
|
sock.sendall(frame)
|
||||||
|
|
||||||
|
readable, _, _ = select.select([sock], [], [], 0.05)
|
||||||
|
if not readable:
|
||||||
|
continue
|
||||||
|
parsed = _ws_read_frame(sock)
|
||||||
|
if parsed is None:
|
||||||
|
break
|
||||||
|
opcode, payload = parsed
|
||||||
|
if opcode == 0x8: # close
|
||||||
|
with write_lock:
|
||||||
|
sock.sendall(_ws_encode_frame(0x8, b""))
|
||||||
|
break
|
||||||
|
if opcode == 0x9: # ping -> pong
|
||||||
|
with write_lock:
|
||||||
|
sock.sendall(_ws_encode_frame(0xA, payload))
|
||||||
|
continue
|
||||||
|
if opcode == 0x1: # text
|
||||||
|
try:
|
||||||
|
data = json.loads(payload.decode("utf-8"))
|
||||||
|
except Exception:
|
||||||
|
continue
|
||||||
|
ack = handle_ws_message(srv.controller, data)
|
||||||
|
if ack is not None:
|
||||||
|
with write_lock:
|
||||||
|
sock.sendall(_ws_encode_frame(0x1, json.dumps(ack).encode("utf-8")))
|
||||||
|
except (BrokenPipeError, ConnectionResetError, TimeoutError, OSError):
|
||||||
|
pass
|
||||||
|
finally:
|
||||||
|
self.close_connection = True
|
||||||
@@ -0,0 +1,92 @@
|
|||||||
|
from __future__ import annotations
|
||||||
|
|
||||||
|
import asyncio
|
||||||
|
import json
|
||||||
|
from typing import Any
|
||||||
|
from urllib.parse import parse_qs
|
||||||
|
|
||||||
|
from core.controller import MicroduckController
|
||||||
|
from core.deploy_service import DeployService
|
||||||
|
|
||||||
|
from .api import (
|
||||||
|
build_status_payload,
|
||||||
|
dispatch_api_get,
|
||||||
|
dispatch_api_post,
|
||||||
|
handle_ws_message,
|
||||||
|
load_index_html,
|
||||||
|
)
|
||||||
|
|
||||||
|
|
||||||
|
def create_fastapi_app(controller: MicroduckController, deploy_service: DeployService) -> Any:
|
||||||
|
"""Create FastAPI app backed by the shared web.api dispatcher."""
|
||||||
|
from fastapi import FastAPI, Request, WebSocket, WebSocketDisconnect
|
||||||
|
from fastapi.middleware.cors import CORSMiddleware
|
||||||
|
from fastapi.responses import HTMLResponse, JSONResponse
|
||||||
|
|
||||||
|
app = FastAPI(title="Microduck UNO Q Controller", version="0.1.0")
|
||||||
|
app.add_middleware(
|
||||||
|
CORSMiddleware,
|
||||||
|
allow_origins=["*"],
|
||||||
|
allow_methods=["*"],
|
||||||
|
allow_headers=["*"],
|
||||||
|
)
|
||||||
|
|
||||||
|
@app.middleware("http")
|
||||||
|
async def add_pna_header(request: Request, call_next: Any) -> Any:
|
||||||
|
resp = await call_next(request)
|
||||||
|
resp.headers["Access-Control-Allow-Private-Network"] = "true"
|
||||||
|
return resp
|
||||||
|
|
||||||
|
@app.get("/", response_class=HTMLResponse)
|
||||||
|
async def index() -> HTMLResponse:
|
||||||
|
return HTMLResponse(content=load_index_html().decode("utf-8"))
|
||||||
|
|
||||||
|
@app.get("/api/{subpath:path}")
|
||||||
|
async def api_get(subpath: str) -> JSONResponse:
|
||||||
|
code, payload = dispatch_api_get(f"/api/{subpath}", controller, deploy_service)
|
||||||
|
return JSONResponse(status_code=code, content=payload)
|
||||||
|
|
||||||
|
@app.post("/api/{subpath:path}")
|
||||||
|
async def api_post(subpath: str, request: Request) -> JSONResponse:
|
||||||
|
body = await request.body()
|
||||||
|
content_type = request.headers.get("content-type", "")
|
||||||
|
query = parse_qs(request.url.query)
|
||||||
|
code, payload = dispatch_api_post(
|
||||||
|
f"/api/{subpath}",
|
||||||
|
body,
|
||||||
|
content_type,
|
||||||
|
query,
|
||||||
|
controller,
|
||||||
|
deploy_service,
|
||||||
|
)
|
||||||
|
return JSONResponse(status_code=code, content=payload)
|
||||||
|
|
||||||
|
@app.websocket("/ws/control")
|
||||||
|
async def ws_control(websocket: WebSocket) -> None:
|
||||||
|
await websocket.accept()
|
||||||
|
stop_event = asyncio.Event()
|
||||||
|
|
||||||
|
async def telemetry_loop() -> None:
|
||||||
|
while not stop_event.is_set():
|
||||||
|
msg = {"type": "telemetry", **build_status_payload(controller, deploy_service)}
|
||||||
|
await websocket.send_text(json.dumps(msg))
|
||||||
|
await asyncio.sleep(0.1)
|
||||||
|
|
||||||
|
sender = asyncio.create_task(telemetry_loop())
|
||||||
|
try:
|
||||||
|
while True:
|
||||||
|
raw = await websocket.receive_text()
|
||||||
|
try:
|
||||||
|
data = json.loads(raw)
|
||||||
|
except Exception:
|
||||||
|
continue
|
||||||
|
ack = handle_ws_message(controller, data)
|
||||||
|
if ack is not None:
|
||||||
|
await websocket.send_text(json.dumps(ack))
|
||||||
|
except WebSocketDisconnect:
|
||||||
|
pass
|
||||||
|
finally:
|
||||||
|
stop_event.set()
|
||||||
|
sender.cancel()
|
||||||
|
|
||||||
|
return app
|
||||||
@@ -0,0 +1,619 @@
|
|||||||
|
<!doctype html>
|
||||||
|
<html lang="en">
|
||||||
|
<head>
|
||||||
|
<meta charset="utf-8" />
|
||||||
|
<meta name="viewport" content="width=device-width,initial-scale=1,maximum-scale=1,user-scalable=no" />
|
||||||
|
<title>Microduck UNO Q Controller</title>
|
||||||
|
<style>
|
||||||
|
:root {
|
||||||
|
--bg: #0f1117;
|
||||||
|
--panel: #191d27;
|
||||||
|
--ink: #f6f4ed;
|
||||||
|
--muted: #98a2b3;
|
||||||
|
--accent: #ff7a2f;
|
||||||
|
--accent-dark: #c95212;
|
||||||
|
--danger: #e53935;
|
||||||
|
--ok: #2ecc71;
|
||||||
|
--warn: #f39c12;
|
||||||
|
--border: #2a3142;
|
||||||
|
--mono: ui-monospace, SFMono-Regular, Menlo, Monaco, Consolas, monospace;
|
||||||
|
}
|
||||||
|
* { box-sizing: border-box; -webkit-tap-highlight-color: transparent; }
|
||||||
|
body {
|
||||||
|
margin: 0;
|
||||||
|
background: var(--bg);
|
||||||
|
color: var(--ink);
|
||||||
|
font-family: var(--mono);
|
||||||
|
overflow-x: hidden;
|
||||||
|
min-height: 100vh;
|
||||||
|
}
|
||||||
|
.topbar {
|
||||||
|
display: flex;
|
||||||
|
flex-wrap: wrap;
|
||||||
|
align-items: center;
|
||||||
|
justify-content: space-between;
|
||||||
|
gap: 8px;
|
||||||
|
padding: 10px 14px;
|
||||||
|
background: var(--panel);
|
||||||
|
border-bottom: 2px solid var(--border);
|
||||||
|
}
|
||||||
|
.brand { font-weight: 700; font-size: 15px; letter-spacing: 0.04em; color: var(--accent); }
|
||||||
|
.badges { display: flex; flex-wrap: wrap; gap: 6px; align-items: center; font-size: 12px; }
|
||||||
|
.badge {
|
||||||
|
padding: 3px 8px;
|
||||||
|
border-radius: 999px;
|
||||||
|
border: 1px solid var(--border);
|
||||||
|
background: #12151d;
|
||||||
|
}
|
||||||
|
.badge.ok { border-color: var(--ok); color: var(--ok); }
|
||||||
|
.badge.warn { border-color: var(--warn); color: var(--warn); }
|
||||||
|
.badge.danger { border-color: var(--danger); color: var(--danger); }
|
||||||
|
.tabs { display: flex; gap: 6px; }
|
||||||
|
.tab-btn {
|
||||||
|
padding: 6px 12px;
|
||||||
|
background: #12151d;
|
||||||
|
color: var(--muted);
|
||||||
|
border: 1px solid var(--border);
|
||||||
|
border-radius: 6px;
|
||||||
|
font-family: inherit;
|
||||||
|
font-size: 12px;
|
||||||
|
font-weight: 700;
|
||||||
|
cursor: pointer;
|
||||||
|
}
|
||||||
|
.tab-btn.active { background: var(--accent); color: #08080c; border-color: var(--accent); }
|
||||||
|
.safety-bar {
|
||||||
|
display: grid;
|
||||||
|
grid-template-columns: 1.3fr 1.2fr 1fr 1fr;
|
||||||
|
gap: 8px;
|
||||||
|
padding: 10px 14px;
|
||||||
|
background: #141821;
|
||||||
|
border-bottom: 1px solid var(--border);
|
||||||
|
align-items: center;
|
||||||
|
}
|
||||||
|
@media (max-width: 720px) {
|
||||||
|
.safety-bar { grid-template-columns: 1fr 1fr; }
|
||||||
|
}
|
||||||
|
button.btn {
|
||||||
|
font-family: inherit;
|
||||||
|
font-weight: 700;
|
||||||
|
font-size: 13px;
|
||||||
|
border-radius: 8px;
|
||||||
|
border: 1px solid var(--border);
|
||||||
|
padding: 10px 12px;
|
||||||
|
background: var(--panel);
|
||||||
|
color: var(--ink);
|
||||||
|
cursor: pointer;
|
||||||
|
}
|
||||||
|
button.btn:active { transform: translateY(1px); }
|
||||||
|
button.btn:disabled { opacity: 0.4; cursor: not-allowed; }
|
||||||
|
button.btn-estop {
|
||||||
|
background: var(--danger);
|
||||||
|
border-color: #ff6b6b;
|
||||||
|
color: #fff;
|
||||||
|
font-size: 15px;
|
||||||
|
letter-spacing: 0.06em;
|
||||||
|
}
|
||||||
|
button.btn-stand {
|
||||||
|
background: var(--accent);
|
||||||
|
border-color: #ffa573;
|
||||||
|
color: #08080c;
|
||||||
|
}
|
||||||
|
.toggle-box, .slider-box {
|
||||||
|
display: flex;
|
||||||
|
flex-direction: column;
|
||||||
|
gap: 4px;
|
||||||
|
font-size: 11px;
|
||||||
|
color: var(--muted);
|
||||||
|
background: var(--panel);
|
||||||
|
border: 1px solid var(--border);
|
||||||
|
border-radius: 8px;
|
||||||
|
padding: 6px 10px;
|
||||||
|
}
|
||||||
|
.toggle-row { display: flex; justify-content: space-between; align-items: center; gap: 8px; color: var(--ink); font-size: 12px; }
|
||||||
|
.main { padding: 12px 14px; }
|
||||||
|
.hidden { display: none !important; }
|
||||||
|
.control-grid {
|
||||||
|
display: grid;
|
||||||
|
grid-template-columns: minmax(240px, 1fr) minmax(240px, 1fr);
|
||||||
|
gap: 14px;
|
||||||
|
align-items: stretch;
|
||||||
|
}
|
||||||
|
@media (max-width: 700px) {
|
||||||
|
.control-grid { grid-template-columns: 1fr; }
|
||||||
|
}
|
||||||
|
.card {
|
||||||
|
background: var(--panel);
|
||||||
|
border: 1px solid var(--border);
|
||||||
|
border-radius: 10px;
|
||||||
|
padding: 12px;
|
||||||
|
}
|
||||||
|
.card-title { font-size: 12px; color: var(--muted); margin: 0 0 10px; text-transform: uppercase; letter-spacing: 0.06em; }
|
||||||
|
.stick-zone {
|
||||||
|
position: relative;
|
||||||
|
height: 250px;
|
||||||
|
border-radius: 10px;
|
||||||
|
background: radial-gradient(circle at center, #202636 0%, #11151f 75%);
|
||||||
|
border: 1px dashed #374151;
|
||||||
|
touch-action: none;
|
||||||
|
user-select: none;
|
||||||
|
overflow: hidden;
|
||||||
|
display: flex;
|
||||||
|
align-items: center;
|
||||||
|
justify-content: center;
|
||||||
|
}
|
||||||
|
.stick-hint { color: var(--muted); font-size: 12px; pointer-events: none; text-align: center; padding: 12px; }
|
||||||
|
.stick-base {
|
||||||
|
position: absolute;
|
||||||
|
width: 112px;
|
||||||
|
height: 112px;
|
||||||
|
border-radius: 50%;
|
||||||
|
border: 2px solid rgba(255, 122, 47, 0.75);
|
||||||
|
background: rgba(255, 122, 47, 0.08);
|
||||||
|
transform: translate(-50%, -50%);
|
||||||
|
pointer-events: none;
|
||||||
|
}
|
||||||
|
.stick-knob {
|
||||||
|
position: absolute;
|
||||||
|
width: 44px;
|
||||||
|
height: 44px;
|
||||||
|
border-radius: 50%;
|
||||||
|
background: var(--accent);
|
||||||
|
box-shadow: 0 0 16px rgba(255, 122, 47, 0.5);
|
||||||
|
transform: translate(-50%, -50%);
|
||||||
|
pointer-events: none;
|
||||||
|
}
|
||||||
|
.vel-readout { margin-top: 8px; font-size: 12px; color: var(--muted); display: flex; justify-content: space-between; }
|
||||||
|
.skills-grid {
|
||||||
|
display: grid;
|
||||||
|
grid-template-columns: repeat(2, minmax(0, 1fr));
|
||||||
|
gap: 8px;
|
||||||
|
}
|
||||||
|
.skill-btn.active { background: var(--accent); color: #08080c; border-color: #ffa573; }
|
||||||
|
.head-panel { margin-top: 10px; display: grid; grid-template-columns: 1fr 1fr; gap: 8px; }
|
||||||
|
.calib-actions { display: flex; flex-wrap: wrap; gap: 8px; margin-bottom: 10px; }
|
||||||
|
.joint-table { width: 100%; border-collapse: collapse; font-size: 12px; }
|
||||||
|
.joint-table th, .joint-table td { border-bottom: 1px solid var(--border); padding: 6px 4px; text-align: left; }
|
||||||
|
.joint-table input[type="number"] {
|
||||||
|
width: 64px;
|
||||||
|
background: #0f1117;
|
||||||
|
color: var(--ink);
|
||||||
|
border: 1px solid var(--border);
|
||||||
|
border-radius: 4px;
|
||||||
|
padding: 3px 5px;
|
||||||
|
font-family: inherit;
|
||||||
|
font-size: 12px;
|
||||||
|
}
|
||||||
|
.mini-btn {
|
||||||
|
padding: 4px 7px;
|
||||||
|
font-size: 11px;
|
||||||
|
border-radius: 5px;
|
||||||
|
border: 1px solid var(--border);
|
||||||
|
background: #12151d;
|
||||||
|
color: var(--ink);
|
||||||
|
font-family: inherit;
|
||||||
|
cursor: pointer;
|
||||||
|
}
|
||||||
|
.mini-btn:hover { border-color: var(--accent); }
|
||||||
|
.log-line { margin-top: 8px; font-size: 11px; color: var(--muted); word-break: break-word; }
|
||||||
|
</style>
|
||||||
|
</head>
|
||||||
|
<body>
|
||||||
|
<header class="topbar">
|
||||||
|
<div class="brand">MICRODUCK // UNO Q</div>
|
||||||
|
<div class="badges">
|
||||||
|
<span id="badge-life" class="badge warn">IDLE</span>
|
||||||
|
<span id="badge-mode" class="badge">Idle</span>
|
||||||
|
<span id="badge-ws" class="badge">WS ...</span>
|
||||||
|
<span id="badge-imu" class="badge">IMU --</span>
|
||||||
|
<span id="badge-batt" class="badge">--.-V</span>
|
||||||
|
<span id="badge-temp" class="badge">--C</span>
|
||||||
|
<span id="badge-hz" class="badge">--Hz</span>
|
||||||
|
</div>
|
||||||
|
<div class="tabs">
|
||||||
|
<button id="tab-ctrl" class="tab-btn active" type="button">Control</button>
|
||||||
|
<button id="tab-cal" class="tab-btn" type="button">Calibration</button>
|
||||||
|
</div>
|
||||||
|
</header>
|
||||||
|
|
||||||
|
<section class="safety-bar">
|
||||||
|
<button id="btn-estop" class="btn btn-estop" type="button">E-STOP (TORQUE OFF)</button>
|
||||||
|
<button id="btn-stand" class="btn btn-stand" type="button">Prepare Stand (1.5s)</button>
|
||||||
|
<label class="toggle-box">
|
||||||
|
<span>Recovery Policy</span>
|
||||||
|
<span class="toggle-row">
|
||||||
|
<span>Auto Stand on Fall</span>
|
||||||
|
<input id="chk-auto-stand" type="checkbox" />
|
||||||
|
</span>
|
||||||
|
</label>
|
||||||
|
<label class="slider-box">
|
||||||
|
<span>Speed Scale: <strong id="lbl-speed">50%</strong></span>
|
||||||
|
<input id="rng-speed" type="range" min="10" max="100" step="5" value="50" />
|
||||||
|
</label>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<main class="main">
|
||||||
|
<section id="view-control" class="control-grid">
|
||||||
|
<div class="card">
|
||||||
|
<h2 class="card-title">Virtual Joystick (Hold & Drag)</h2>
|
||||||
|
<div id="stick-zone" class="stick-zone">
|
||||||
|
<div id="stick-hint" class="stick-hint">Touch or drag anywhere here to walk / turn</div>
|
||||||
|
<div id="stick-base" class="stick-base hidden"></div>
|
||||||
|
<div id="stick-knob" class="stick-knob hidden"></div>
|
||||||
|
</div>
|
||||||
|
<div class="vel-readout">
|
||||||
|
<span id="lbl-vx">vx norm: 0.00</span>
|
||||||
|
<span id="lbl-wz">wz norm: 0.00</span>
|
||||||
|
</div>
|
||||||
|
|
||||||
|
<div id="head-panel" class="head-panel hidden">
|
||||||
|
<label class="slider-box">Neck Pitch <input id="hd-0" type="range" min="-1" max="1" step="0.05" value="0" /></label>
|
||||||
|
<label class="slider-box">Head Pitch <input id="hd-1" type="range" min="-1" max="1" step="0.05" value="0" /></label>
|
||||||
|
<label class="slider-box">Head Yaw <input id="hd-2" type="range" min="-1" max="1" step="0.05" value="0" /></label>
|
||||||
|
<label class="slider-box">Head Roll <input id="hd-3" type="range" min="-1" max="1" step="0.05" value="0" /></label>
|
||||||
|
</div>
|
||||||
|
</div>
|
||||||
|
|
||||||
|
<div class="card">
|
||||||
|
<h2 class="card-title">Skills & Modes</h2>
|
||||||
|
<div class="skills-grid">
|
||||||
|
<button class="btn skill-btn" data-action="sitStand" type="button">Sit / Stand</button>
|
||||||
|
<button class="btn skill-btn" data-action="head" id="btn-head" type="button">Head Mode</button>
|
||||||
|
<button class="btn skill-btn" data-action="kickL" type="button">Kick Left</button>
|
||||||
|
<button class="btn skill-btn" data-action="kickR" type="button">Kick Right</button>
|
||||||
|
<button class="btn skill-btn" data-action="groundPick" type="button">Ground Pick</button>
|
||||||
|
<button class="btn skill-btn" data-action="roll" id="btn-roll" type="button">Roll / Trick</button>
|
||||||
|
<button class="btn skill-btn" data-action="rollers" id="btn-rollers" type="button">Feet / Rollers</button>
|
||||||
|
<button class="btn skill-btn hidden" data-action="script" id="btn-script" type="button">Custom Script</button>
|
||||||
|
</div>
|
||||||
|
<div id="ctrl-log" class="log-line">Ready. Tap "Prepare Stand" when the robot is upright on the floor.</div>
|
||||||
|
</div>
|
||||||
|
</section>
|
||||||
|
|
||||||
|
<section id="view-calib" class="card hidden">
|
||||||
|
<h2 class="card-title">Hardware Profile & Servo Calibration (IDLE Required for Joint Test)</h2>
|
||||||
|
<div class="calib-actions">
|
||||||
|
<button id="btn-ping" class="btn" type="button">Ping Servos (1-20)</button>
|
||||||
|
<button id="btn-imu-zero" class="btn" type="button">IMU Bias Zero (Keep Still)</button>
|
||||||
|
<button id="btn-exit-cal" class="btn" type="button">Exit Calibration (Torque Off)</button>
|
||||||
|
<button id="btn-save-prof" class="btn btn-stand" type="button">Save Profile to Robot</button>
|
||||||
|
</div>
|
||||||
|
<div id="calib-status" class="log-line">No ping scan yet.</div>
|
||||||
|
<div style="overflow-x:auto; margin-top:10px;">
|
||||||
|
<table class="joint-table">
|
||||||
|
<thead>
|
||||||
|
<tr>
|
||||||
|
<th>Joint</th>
|
||||||
|
<th>ID</th>
|
||||||
|
<th>Sign</th>
|
||||||
|
<th>Offset</th>
|
||||||
|
<th>Live Tick</th>
|
||||||
|
<th>Live Rad</th>
|
||||||
|
<th>Actions</th>
|
||||||
|
</tr>
|
||||||
|
</thead>
|
||||||
|
<tbody id="joint-tbody"></tbody>
|
||||||
|
</table>
|
||||||
|
</div>
|
||||||
|
</section>
|
||||||
|
</main>
|
||||||
|
|
||||||
|
<script>
|
||||||
|
(() => {
|
||||||
|
const $ = (id) => document.getElementById(id);
|
||||||
|
let ws = null;
|
||||||
|
let reconnectTimer = null;
|
||||||
|
let profile = null;
|
||||||
|
let latestStatus = null;
|
||||||
|
const stickState = { active: false, pointerId: null, originX: 0, originY: 0, nx: 0, nwz: 0 };
|
||||||
|
const MAX_RADIUS = 56;
|
||||||
|
|
||||||
|
function log(msg) {
|
||||||
|
$("ctrl-log").textContent = msg;
|
||||||
|
}
|
||||||
|
|
||||||
|
async function apiPost(path, body) {
|
||||||
|
const res = await fetch(path, {
|
||||||
|
method: "POST",
|
||||||
|
headers: { "Content-Type": "application/json" },
|
||||||
|
body: JSON.stringify(body || {}),
|
||||||
|
});
|
||||||
|
const data = await res.json();
|
||||||
|
if (!res.ok || data.ok === false) {
|
||||||
|
throw new Error(data.error || `HTTP ${res.status}`);
|
||||||
|
}
|
||||||
|
return data;
|
||||||
|
}
|
||||||
|
|
||||||
|
function setTab(tab) {
|
||||||
|
const isCtrl = tab === "ctrl";
|
||||||
|
$("view-control").classList.toggle("hidden", !isCtrl);
|
||||||
|
$("view-calib").classList.toggle("hidden", isCtrl);
|
||||||
|
$("tab-ctrl").classList.toggle("active", isCtrl);
|
||||||
|
$("tab-cal").classList.toggle("active", !isCtrl);
|
||||||
|
if (!isCtrl) loadProfile();
|
||||||
|
}
|
||||||
|
|
||||||
|
$("tab-ctrl").onclick = () => setTab("ctrl");
|
||||||
|
$("tab-cal").onclick = () => setTab("cal");
|
||||||
|
|
||||||
|
function sendWs(obj) {
|
||||||
|
if (ws && ws.readyState === WebSocket.OPEN) {
|
||||||
|
ws.send(JSON.stringify(obj));
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
function updateStatusUI(st) {
|
||||||
|
latestStatus = st;
|
||||||
|
const lifeEl = $("badge-life");
|
||||||
|
lifeEl.textContent = st.lifecycle || "UNKNOWN";
|
||||||
|
lifeEl.className = "badge " + (st.lifecycle === "ACTIVE" ? "ok" : st.lifecycle === "FALL_SAFE" ? "danger" : "warn");
|
||||||
|
$("badge-mode").textContent = st.mode_label || st.skill_mode || "-";
|
||||||
|
$("badge-imu").textContent = st.imu_ok ? "IMU OK" : "IMU ERR";
|
||||||
|
$("badge-imu").className = "badge " + (st.imu_ok ? "ok" : "danger");
|
||||||
|
$("badge-batt").textContent = `${Number(st.voltage_v || 0).toFixed(2)}V`;
|
||||||
|
$("badge-temp").textContent = `${Number(st.max_temp_c || 0).toFixed(0)}C`;
|
||||||
|
$("badge-hz").textContent = `${Number(st.ctrl_hz || 0).toFixed(0)}Hz`;
|
||||||
|
|
||||||
|
$("chk-auto-stand").checked = !!st.auto_stand_on_fall;
|
||||||
|
const spdPct = Math.round((st.speed_scale || 0.5) * 100);
|
||||||
|
if (document.activeElement !== $("rng-speed")) {
|
||||||
|
$("rng-speed").value = String(spdPct);
|
||||||
|
$("lbl-speed").textContent = `${spdPct}%`;
|
||||||
|
}
|
||||||
|
|
||||||
|
$("btn-head").classList.toggle("active", !!st.head_mode);
|
||||||
|
$("head-panel").classList.toggle("hidden", !st.head_mode);
|
||||||
|
$("btn-rollers").disabled = !st.rollers_enabled;
|
||||||
|
$("btn-rollers").classList.toggle("active", st.loco === "rollers");
|
||||||
|
const hasScript = !!(st.custom_move && st.custom_move.slot === "script");
|
||||||
|
$("btn-script").classList.toggle("hidden", !hasScript);
|
||||||
|
if (st.error) log(`Error: ${st.error}`);
|
||||||
|
|
||||||
|
if (st.ticks && st.joint_pos_rad && profile) {
|
||||||
|
Object.keys(profile.joints).forEach((name, idx) => {
|
||||||
|
const tCell = document.getElementById(`live-tick-${name}`);
|
||||||
|
const rCell = document.getElementById(`live-rad-${name}`);
|
||||||
|
if (tCell) tCell.textContent = String(st.ticks[idx] ?? "-");
|
||||||
|
if (rCell) rCell.textContent = Number(st.joint_pos_rad[idx] ?? 0).toFixed(3);
|
||||||
|
});
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
function connectWs() {
|
||||||
|
if (reconnectTimer) clearTimeout(reconnectTimer);
|
||||||
|
const proto = location.protocol === "https:" ? "wss:" : "ws:";
|
||||||
|
ws = new WebSocket(`${proto}//${location.host}/ws/control`);
|
||||||
|
ws.onopen = () => {
|
||||||
|
$("badge-ws").textContent = "WS LIVE";
|
||||||
|
$("badge-ws").className = "badge ok";
|
||||||
|
};
|
||||||
|
ws.onclose = () => {
|
||||||
|
$("badge-ws").textContent = "WS OFF";
|
||||||
|
$("badge-ws").className = "badge danger";
|
||||||
|
reconnectTimer = setTimeout(connectWs, 1000);
|
||||||
|
};
|
||||||
|
ws.onmessage = (evt) => {
|
||||||
|
try {
|
||||||
|
const msg = JSON.parse(evt.data);
|
||||||
|
if (msg.type === "telemetry") updateStatusUI(msg);
|
||||||
|
} catch (_) {}
|
||||||
|
};
|
||||||
|
}
|
||||||
|
|
||||||
|
// Safety & Skill controls
|
||||||
|
$("btn-estop").onclick = async () => {
|
||||||
|
sendWs({ type: "estop" });
|
||||||
|
try { updateStatusUI(await apiPost("/api/estop")); log("E-STOP activated (torque off)"); } catch (e) { log(e.message); }
|
||||||
|
};
|
||||||
|
$("btn-stand").onclick = async () => {
|
||||||
|
try { updateStatusUI(await apiPost("/api/stand")); log("Soft-start stand engaged..."); } catch (e) { log(e.message); }
|
||||||
|
};
|
||||||
|
$("chk-auto-stand").onchange = async (e) => {
|
||||||
|
try { updateStatusUI(await apiPost("/api/settings", { auto_stand_on_fall: e.target.checked })); } catch (err) { log(err.message); }
|
||||||
|
};
|
||||||
|
$("rng-speed").oninput = (e) => {
|
||||||
|
$("lbl-speed").textContent = `${e.target.value}%`;
|
||||||
|
};
|
||||||
|
$("rng-speed").onchange = async (e) => {
|
||||||
|
try { updateStatusUI(await apiPost("/api/settings", { speed_scale: Number(e.target.value) / 100 })); } catch (err) { log(err.message); }
|
||||||
|
};
|
||||||
|
|
||||||
|
document.querySelectorAll(".skill-btn").forEach((btn) => {
|
||||||
|
btn.onclick = async () => {
|
||||||
|
const action = btn.dataset.action;
|
||||||
|
try {
|
||||||
|
const res = await apiPost("/api/action", { action });
|
||||||
|
if (res.status) updateStatusUI(res.status);
|
||||||
|
log(res.ok ? `Triggered ${action}` : `Action '${action}' rejected in current state`);
|
||||||
|
} catch (e) { log(e.message); }
|
||||||
|
};
|
||||||
|
});
|
||||||
|
|
||||||
|
const sendHead = () => {
|
||||||
|
sendWs({
|
||||||
|
type: "head",
|
||||||
|
neck_pitch: Number($("hd-0").value),
|
||||||
|
head_pitch: Number($("hd-1").value),
|
||||||
|
head_yaw: Number($("hd-2").value),
|
||||||
|
head_roll: Number($("hd-3").value),
|
||||||
|
});
|
||||||
|
};
|
||||||
|
["hd-0", "hd-1", "hd-2", "hd-3"].forEach((id) => { $(id).oninput = sendHead; });
|
||||||
|
|
||||||
|
// Floating virtual joystick
|
||||||
|
const zone = $("stick-zone");
|
||||||
|
const base = $("stick-base");
|
||||||
|
const knob = $("stick-knob");
|
||||||
|
const hint = $("stick-hint");
|
||||||
|
|
||||||
|
function updateStickVisual(x, y) {
|
||||||
|
if (!stickState.active) {
|
||||||
|
base.classList.add("hidden");
|
||||||
|
knob.classList.add("hidden");
|
||||||
|
hint.classList.remove("hidden");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
hint.classList.add("hidden");
|
||||||
|
base.classList.remove("hidden");
|
||||||
|
knob.classList.remove("hidden");
|
||||||
|
base.style.left = `${stickState.originX}px`;
|
||||||
|
base.style.top = `${stickState.originY}px`;
|
||||||
|
knob.style.left = `${x}px`;
|
||||||
|
knob.style.top = `${y}px`;
|
||||||
|
}
|
||||||
|
|
||||||
|
function applyStickCoords(clientX, clientY) {
|
||||||
|
const rect = zone.getBoundingClientRect();
|
||||||
|
const px = clientX - rect.left;
|
||||||
|
const py = clientY - rect.top;
|
||||||
|
const dx = px - stickState.originX;
|
||||||
|
const dy = py - stickState.originY;
|
||||||
|
const dist = Math.hypot(dx, dy);
|
||||||
|
const clamped = dist > MAX_RADIUS ? MAX_RADIUS / dist : 1.0;
|
||||||
|
const cx = stickState.originX + dx * clamped;
|
||||||
|
const cy = stickState.originY + dy * clamped;
|
||||||
|
stickState.nx = -((cy - stickState.originY) / MAX_RADIUS);
|
||||||
|
stickState.nwz = -((cx - stickState.originX) / MAX_RADIUS);
|
||||||
|
$("lbl-vx").textContent = `vx norm: ${stickState.nx.toFixed(2)}`;
|
||||||
|
$("lbl-wz").textContent = `wz norm: ${stickState.nwz.toFixed(2)}`;
|
||||||
|
updateStickVisual(cx, cy);
|
||||||
|
}
|
||||||
|
|
||||||
|
function releaseStick() {
|
||||||
|
stickState.active = false;
|
||||||
|
stickState.pointerId = null;
|
||||||
|
stickState.nx = 0;
|
||||||
|
stickState.nwz = 0;
|
||||||
|
$("lbl-vx").textContent = "vx norm: 0.00";
|
||||||
|
$("lbl-wz").textContent = "wz norm: 0.00";
|
||||||
|
updateStickVisual(0, 0);
|
||||||
|
sendWs({ type: "input", vx: 0, wz: 0, speed_scale: Number($("rng-speed").value) / 100 });
|
||||||
|
}
|
||||||
|
|
||||||
|
zone.addEventListener("pointerdown", (e) => {
|
||||||
|
const rect = zone.getBoundingClientRect();
|
||||||
|
stickState.active = true;
|
||||||
|
stickState.pointerId = e.pointerId;
|
||||||
|
stickState.originX = e.clientX - rect.left;
|
||||||
|
stickState.originY = e.clientY - rect.top;
|
||||||
|
zone.setPointerCapture(e.pointerId);
|
||||||
|
applyStickCoords(e.clientX, e.clientY);
|
||||||
|
});
|
||||||
|
zone.addEventListener("pointermove", (e) => {
|
||||||
|
if (!stickState.active || e.pointerId !== stickState.pointerId) return;
|
||||||
|
applyStickCoords(e.clientX, e.clientY);
|
||||||
|
});
|
||||||
|
zone.addEventListener("pointerup", (e) => {
|
||||||
|
if (e.pointerId === stickState.pointerId) releaseStick();
|
||||||
|
});
|
||||||
|
zone.addEventListener("pointercancel", releaseStick);
|
||||||
|
|
||||||
|
setInterval(() => {
|
||||||
|
if (stickState.active) {
|
||||||
|
sendWs({
|
||||||
|
type: "input",
|
||||||
|
vx: stickState.nx,
|
||||||
|
wz: stickState.nwz,
|
||||||
|
speed_scale: Number($("rng-speed").value) / 100,
|
||||||
|
});
|
||||||
|
}
|
||||||
|
}, 50);
|
||||||
|
|
||||||
|
// Calibration helpers
|
||||||
|
function renderJointTable() {
|
||||||
|
const tbody = $("joint-tbody");
|
||||||
|
tbody.innerHTML = "";
|
||||||
|
if (!profile || !profile.joints) return;
|
||||||
|
Object.entries(profile.joints).forEach(([name, cfg]) => {
|
||||||
|
const tr = document.createElement("tr");
|
||||||
|
tr.innerHTML = `
|
||||||
|
<td>${name}</td>
|
||||||
|
<td><input type="number" min="0" max="253" value="${cfg.id}" data-j="${name}" data-f="id" /></td>
|
||||||
|
<td><button class="mini-btn" type="button" data-flip="${name}">${cfg.sign > 0 ? "+1" : "-1"}</button></td>
|
||||||
|
<td><input type="number" min="-4096" max="4096" value="${cfg.zero_offset_ticks}" data-j="${name}" data-f="zero_offset_ticks" /></td>
|
||||||
|
<td id="live-tick-${name}">-</td>
|
||||||
|
<td id="live-rad-${name}">0.000</td>
|
||||||
|
<td>
|
||||||
|
<button class="mini-btn" type="button" data-zero="${name}">Zero</button>
|
||||||
|
<button class="mini-btn" type="button" data-test="${name}" data-rad="-0.2">-0.2</button>
|
||||||
|
<button class="mini-btn" type="button" data-test="${name}" data-rad="0">0</button>
|
||||||
|
<button class="mini-btn" type="button" data-test="${name}" data-rad="0.2">+0.2</button>
|
||||||
|
</td>
|
||||||
|
`;
|
||||||
|
tbody.appendChild(tr);
|
||||||
|
});
|
||||||
|
|
||||||
|
tbody.querySelectorAll("input[data-j]").forEach((inp) => {
|
||||||
|
inp.onchange = () => {
|
||||||
|
profile.joints[inp.dataset.j][inp.dataset.f] = Number(inp.value);
|
||||||
|
};
|
||||||
|
});
|
||||||
|
tbody.querySelectorAll("button[data-flip]").forEach((btn) => {
|
||||||
|
btn.onclick = () => {
|
||||||
|
const j = profile.joints[btn.dataset.flip];
|
||||||
|
j.sign = j.sign > 0 ? -1 : 1;
|
||||||
|
btn.textContent = j.sign > 0 ? "+1" : "-1";
|
||||||
|
};
|
||||||
|
});
|
||||||
|
tbody.querySelectorAll("button[data-zero]").forEach((btn) => {
|
||||||
|
btn.onclick = async () => {
|
||||||
|
try {
|
||||||
|
const res = await apiPost("/api/calibrate/joint_zero", { joint: btn.dataset.zero });
|
||||||
|
profile = res.profile;
|
||||||
|
renderJointTable();
|
||||||
|
$("calib-status").textContent = `Captured current tick as zero for ${btn.dataset.zero}`;
|
||||||
|
} catch (e) { $("calib-status").textContent = `Error: ${e.message}`; }
|
||||||
|
};
|
||||||
|
});
|
||||||
|
tbody.querySelectorAll("button[data-test]").forEach((btn) => {
|
||||||
|
btn.onclick = async () => {
|
||||||
|
try {
|
||||||
|
await apiPost("/api/calibrate/test_joint", { joint: btn.dataset.test, rad: Number(btn.dataset.rad) });
|
||||||
|
$("calib-status").textContent = `Commanded ${btn.dataset.test} -> ${btn.dataset.rad} rad`;
|
||||||
|
} catch (e) { $("calib-status").textContent = `Error: ${e.message}`; }
|
||||||
|
};
|
||||||
|
});
|
||||||
|
}
|
||||||
|
|
||||||
|
async function loadProfile() {
|
||||||
|
const res = await fetch("/api/profile");
|
||||||
|
const data = await res.json();
|
||||||
|
profile = data.profile;
|
||||||
|
renderJointTable();
|
||||||
|
}
|
||||||
|
|
||||||
|
$("btn-ping").onclick = async () => {
|
||||||
|
try {
|
||||||
|
const res = await apiPost("/api/calibrate/ping", { max_id: 20 });
|
||||||
|
$("calib-status").textContent = `Responding IDs (${res.ids.length}): [${res.ids.join(", ")}]`;
|
||||||
|
} catch (e) { $("calib-status").textContent = `Ping error: ${e.message}`; }
|
||||||
|
};
|
||||||
|
$("btn-imu-zero").onclick = async () => {
|
||||||
|
try {
|
||||||
|
$("calib-status").textContent = "Sampling IMU bias (keep robot stationary)...";
|
||||||
|
const res = await apiPost("/api/calibrate/imu_zero", { samples: 200 });
|
||||||
|
if (profile) profile.imu = res.imu;
|
||||||
|
$("calib-status").textContent = `IMU gyro bias updated: [${res.imu.gyro_bias_rad_s.map((x) => x.toFixed(4)).join(", ")}]`;
|
||||||
|
} catch (e) { $("calib-status").textContent = `IMU zero error: ${e.message}`; }
|
||||||
|
};
|
||||||
|
$("btn-exit-cal").onclick = async () => {
|
||||||
|
try { updateStatusUI(await apiPost("/api/calibrate/exit")); $("calib-status").textContent = "Exited calibration (torque off)"; } catch (e) { $("calib-status").textContent = e.message; }
|
||||||
|
};
|
||||||
|
$("btn-save-prof").onclick = async () => {
|
||||||
|
if (!profile) return;
|
||||||
|
try {
|
||||||
|
const res = await apiPost("/api/profile", profile);
|
||||||
|
profile = res.profile;
|
||||||
|
renderJointTable();
|
||||||
|
$("calib-status").textContent = "Saved hardware_profile.json to UNO Q.";
|
||||||
|
} catch (e) { $("calib-status").textContent = `Save error: ${e.message}`; }
|
||||||
|
};
|
||||||
|
|
||||||
|
fetch("/api/status").then((r) => r.json()).then(updateStatusUI).catch(() => {});
|
||||||
|
connectWs();
|
||||||
|
})();
|
||||||
|
</script>
|
||||||
|
</body>
|
||||||
|
</html>
|
||||||
@@ -0,0 +1,10 @@
|
|||||||
|
#!/usr/bin/env bash
|
||||||
|
set -euo pipefail
|
||||||
|
|
||||||
|
SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
|
||||||
|
REQ_FILE="${SCRIPT_DIR}/../python/requirements.txt"
|
||||||
|
|
||||||
|
echo "[microduck-unoq] Installing Python dependencies from ${REQ_FILE}..."
|
||||||
|
python3 -m pip install --upgrade pip
|
||||||
|
python3 -m pip install -r "${REQ_FILE}"
|
||||||
|
echo "[microduck-unoq] Done."
|
||||||
@@ -0,0 +1,44 @@
|
|||||||
|
#!/usr/bin/env bash
|
||||||
|
set -euo pipefail
|
||||||
|
|
||||||
|
# Configure an offline Wi-Fi Access Point on Arduino UNO Q (Debian + NetworkManager)
|
||||||
|
# Phone connects to this hotspot and opens http://192.168.4.1:8000
|
||||||
|
|
||||||
|
CON_NAME="${AP_CON_NAME:-microduck-ap}"
|
||||||
|
SSID="${AP_SSID:-Microduck-AP}"
|
||||||
|
PASS="${AP_PASS:-microduck123}"
|
||||||
|
IFACE="${AP_IFACE:-wlan0}"
|
||||||
|
IP_CIDR="${AP_IP_CIDR:-192.168.4.1/24}"
|
||||||
|
|
||||||
|
if ! command -v nmcli >/dev/null 2>&1; then
|
||||||
|
echo "Error: nmcli (NetworkManager) not found. Configure hostapd + dnsmasq manually for ${IFACE} -> ${IP_CIDR}." >&2
|
||||||
|
exit 1
|
||||||
|
fi
|
||||||
|
|
||||||
|
echo "[microduck-ap] Configuring Wi-Fi AP '${SSID}' on ${IFACE} (${IP_CIDR})..."
|
||||||
|
|
||||||
|
if nmcli connection show "${CON_NAME}" >/dev/null 2>&1; then
|
||||||
|
nmcli connection delete "${CON_NAME}"
|
||||||
|
fi
|
||||||
|
|
||||||
|
nmcli connection add \
|
||||||
|
type wifi \
|
||||||
|
ifname "${IFACE}" \
|
||||||
|
con-name "${CON_NAME}" \
|
||||||
|
autoconnect yes \
|
||||||
|
ssid "${SSID}"
|
||||||
|
|
||||||
|
nmcli connection modify "${CON_NAME}" \
|
||||||
|
802-11-wireless.mode ap \
|
||||||
|
802-11-wireless.band bg \
|
||||||
|
ipv4.method shared \
|
||||||
|
ipv4.addresses "${IP_CIDR}" \
|
||||||
|
wifi-sec.key-mgmt wpa-psk \
|
||||||
|
wifi-sec.psk "${PASS}"
|
||||||
|
|
||||||
|
nmcli connection up "${CON_NAME}"
|
||||||
|
|
||||||
|
echo "[microduck-ap] Hotspot active!"
|
||||||
|
echo " SSID: ${SSID}"
|
||||||
|
echo " Pass: ${PASS}"
|
||||||
|
echo " Control URL: http://${IP_CIDR%/*}:8000"
|
||||||
@@ -0,0 +1,41 @@
|
|||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <Arduino.h>
|
||||||
|
#include <Wire.h>
|
||||||
|
|
||||||
|
// =============================================================================
|
||||||
|
// Microduck UNO Q Hardware Pin & Bus Configuration
|
||||||
|
// Centralizes physical wiring constants so schematic changes require one edit.
|
||||||
|
// =============================================================================
|
||||||
|
|
||||||
|
// Feetech HD-1910-C001 serial bus
|
||||||
|
// On Arduino UNO Q, Serial1 maps to header D0 (RX / PB7) and D1 (TX / PB6).
|
||||||
|
#define SERVO_SERIAL Serial1
|
||||||
|
#define SERVO_BAUD 1000000UL
|
||||||
|
|
||||||
|
// Set to a GPIO pin if using a 74HC126 / RS485 direction-controlled half-duplex
|
||||||
|
// transceiver (HIGH = TX, LOW = RX). Leave -1 for resistor/diode auto-direction
|
||||||
|
// single-wire boards (TX tied cleanly through hardware buffer) or Waveshare board.
|
||||||
|
#define SERVO_DIR_PIN -1
|
||||||
|
#define SERVO_DIR_TX_LEVEL HIGH
|
||||||
|
#define SERVO_DIR_RX_LEVEL LOW
|
||||||
|
|
||||||
|
// QMI8658 6-axis IMU on I2C
|
||||||
|
// 0x6A when SA0/AD0 is LOW (default on many breakouts), 0x6B when HIGH.
|
||||||
|
#define IMU_WIRE Wire
|
||||||
|
#define IMU_I2C_ADDR 0x6A
|
||||||
|
#define IMU_I2C_CLOCK_HZ 400000UL
|
||||||
|
|
||||||
|
// IMU Mahony filter loop rate (runs non-blocking inside MCU loop())
|
||||||
|
#define IMU_LOOP_INTERVAL_US 4000UL // 250 Hz
|
||||||
|
|
||||||
|
// Safety watchdog: disable servo torque if Linux stops sending hw_sync_step
|
||||||
|
#define WATCHDOG_DEFAULT_MS 150UL
|
||||||
|
|
||||||
|
// Servo feedback strategy:
|
||||||
|
// 1 = Feetech SyncRead (0x82) followed by single-read fallback if bus drops packet
|
||||||
|
// 0 = Sequential single Read (0x02) per joint (slower, simpler fallback for noisy lines)
|
||||||
|
#define USE_FEETECH_SYNC_READ 1
|
||||||
|
|
||||||
|
// Total joints on Microduck
|
||||||
|
#define NUM_JOINTS 14
|
||||||
@@ -0,0 +1,349 @@
|
|||||||
|
#include "feetech_hls.h"
|
||||||
|
|
||||||
|
#include <string.h>
|
||||||
|
|
||||||
|
namespace {
|
||||||
|
constexpr uint8_t kHeader = 0xFF;
|
||||||
|
constexpr uint8_t kBroadcastId = 0xFE;
|
||||||
|
|
||||||
|
constexpr uint8_t kInstPing = 0x01;
|
||||||
|
constexpr uint8_t kInstRead = 0x02;
|
||||||
|
constexpr uint8_t kInstWrite = 0x03;
|
||||||
|
constexpr uint8_t kInstSyncRead = 0x82;
|
||||||
|
constexpr uint8_t kInstSyncWrite = 0x83;
|
||||||
|
|
||||||
|
constexpr uint8_t kRegTorqueEnable = 0x28;
|
||||||
|
constexpr uint8_t kRegAcc = 0x29;
|
||||||
|
constexpr uint8_t kRegPresentPos = 0x38;
|
||||||
|
constexpr uint8_t kFeedbackReadLen = 8; // pos(2), speed(2), load(2), volt(1), temp(1)
|
||||||
|
} // namespace
|
||||||
|
|
||||||
|
void FeetechHlsBus::begin(HardwareSerial& serial, uint32_t baud, int dir_pin) {
|
||||||
|
serial_ = &serial;
|
||||||
|
dir_pin_ = dir_pin;
|
||||||
|
if (dir_pin_ >= 0) {
|
||||||
|
pinMode(dir_pin_, OUTPUT);
|
||||||
|
setTxMode(false);
|
||||||
|
}
|
||||||
|
serial_->begin(baud);
|
||||||
|
delay(5);
|
||||||
|
flushInput();
|
||||||
|
}
|
||||||
|
|
||||||
|
void FeetechHlsBus::setTxMode(bool tx) {
|
||||||
|
if (dir_pin_ >= 0) {
|
||||||
|
digitalWrite(dir_pin_, tx ? SERVO_DIR_TX_LEVEL : SERVO_DIR_RX_LEVEL);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void FeetechHlsBus::flushInput() {
|
||||||
|
if (!serial_) return;
|
||||||
|
while (serial_->available() > 0) {
|
||||||
|
(void)serial_->read();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
uint8_t FeetechHlsBus::computeChecksum(const uint8_t* body_from_id, size_t body_len) {
|
||||||
|
uint32_t sum = 0;
|
||||||
|
for (size_t i = 0; i < body_len; ++i) {
|
||||||
|
sum += body_from_id[i];
|
||||||
|
}
|
||||||
|
return static_cast<uint8_t>((~sum) & 0xFFU);
|
||||||
|
}
|
||||||
|
|
||||||
|
void FeetechHlsBus::sendRawPacket(const uint8_t* pkt, size_t len) {
|
||||||
|
if (!serial_ || len == 0 || len > sizeof(last_tx_)) return;
|
||||||
|
flushInput();
|
||||||
|
memcpy(last_tx_, pkt, len);
|
||||||
|
last_tx_len_ = len;
|
||||||
|
setTxMode(true);
|
||||||
|
serial_->write(pkt, len);
|
||||||
|
serial_->flush();
|
||||||
|
delayMicroseconds(8);
|
||||||
|
setTxMode(false);
|
||||||
|
|
||||||
|
// Strip local half-duplex TX echo if TX/RX share a single physical trace without gating
|
||||||
|
if (dir_pin_ < 0) {
|
||||||
|
uint32_t t0 = micros();
|
||||||
|
size_t matched = 0;
|
||||||
|
while (matched < len && (micros() - t0) < 400) {
|
||||||
|
if (serial_->available() > 0) {
|
||||||
|
int b = serial_->peek();
|
||||||
|
if (b == last_tx_[matched]) {
|
||||||
|
(void)serial_->read();
|
||||||
|
matched++;
|
||||||
|
} else {
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void FeetechHlsBus::sendPacket(uint8_t id, uint8_t inst, const uint8_t* params, uint8_t param_len) {
|
||||||
|
uint8_t pkt[128];
|
||||||
|
uint8_t length_field = static_cast<uint8_t>(param_len + 2);
|
||||||
|
pkt[0] = kHeader;
|
||||||
|
pkt[1] = kHeader;
|
||||||
|
pkt[2] = id;
|
||||||
|
pkt[3] = length_field;
|
||||||
|
pkt[4] = inst;
|
||||||
|
for (uint8_t i = 0; i < param_len; ++i) {
|
||||||
|
pkt[5 + i] = params[i];
|
||||||
|
}
|
||||||
|
pkt[5 + param_len] = computeChecksum(&pkt[2], static_cast<size_t>(3 + param_len));
|
||||||
|
sendRawPacket(pkt, static_cast<size_t>(6 + param_len));
|
||||||
|
}
|
||||||
|
|
||||||
|
bool FeetechHlsBus::readPacketForId(
|
||||||
|
uint8_t expected_id,
|
||||||
|
uint8_t* out_err,
|
||||||
|
uint8_t* params,
|
||||||
|
uint8_t expected_len,
|
||||||
|
uint32_t timeout_us) {
|
||||||
|
if (!serial_) return false;
|
||||||
|
uint32_t t0 = micros();
|
||||||
|
uint8_t state = 0;
|
||||||
|
uint8_t id = 0, len_field = 0, err = 0;
|
||||||
|
uint8_t buf[64] = {};
|
||||||
|
uint8_t idx = 0;
|
||||||
|
uint8_t payload_len = 0;
|
||||||
|
|
||||||
|
while ((micros() - t0) < timeout_us) {
|
||||||
|
if (serial_->available() <= 0) {
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
uint8_t b = static_cast<uint8_t>(serial_->read());
|
||||||
|
switch (state) {
|
||||||
|
case 0:
|
||||||
|
if (b == kHeader) state = 1;
|
||||||
|
break;
|
||||||
|
case 1:
|
||||||
|
if (b == kHeader) {
|
||||||
|
state = 2;
|
||||||
|
} else {
|
||||||
|
state = 0;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
case 2:
|
||||||
|
if (b == kHeader) break; // extra 0xFF header alignment
|
||||||
|
id = b;
|
||||||
|
state = 3;
|
||||||
|
break;
|
||||||
|
case 3:
|
||||||
|
len_field = b;
|
||||||
|
if (len_field < 2 || len_field > 60) {
|
||||||
|
state = 0;
|
||||||
|
} else {
|
||||||
|
payload_len = static_cast<uint8_t>(len_field - 2);
|
||||||
|
state = 4;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
case 4:
|
||||||
|
err = b;
|
||||||
|
idx = 0;
|
||||||
|
state = (payload_len == 0) ? 6 : 5;
|
||||||
|
break;
|
||||||
|
case 5:
|
||||||
|
buf[idx++] = b;
|
||||||
|
if (idx >= payload_len) state = 6;
|
||||||
|
break;
|
||||||
|
case 6: {
|
||||||
|
uint8_t check_buf[64];
|
||||||
|
check_buf[0] = id;
|
||||||
|
check_buf[1] = len_field;
|
||||||
|
check_buf[2] = err;
|
||||||
|
for (uint8_t i = 0; i < payload_len; ++i) check_buf[3 + i] = buf[i];
|
||||||
|
uint8_t chk = computeChecksum(check_buf, static_cast<size_t>(3 + payload_len));
|
||||||
|
if (chk == b && id == expected_id && payload_len >= expected_len) {
|
||||||
|
if (out_err) *out_err = err;
|
||||||
|
if (params && expected_len > 0) memcpy(params, buf, expected_len);
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
state = 0;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
default:
|
||||||
|
state = 0;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool FeetechHlsBus::ping(uint8_t id, uint32_t timeout_us) {
|
||||||
|
sendPacket(id, kInstPing, nullptr, 0);
|
||||||
|
uint8_t err = 0;
|
||||||
|
return readPacketForId(id, &err, nullptr, 0, timeout_us);
|
||||||
|
}
|
||||||
|
|
||||||
|
int FeetechHlsBus::pingRange(uint8_t max_id, int32_t* out_ids, int max_out) {
|
||||||
|
int found = 0;
|
||||||
|
for (uint8_t id = 1; id <= max_id && found < max_out; ++id) {
|
||||||
|
if (ping(id, 2000)) {
|
||||||
|
out_ids[found++] = static_cast<int32_t>(id);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return found;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool FeetechHlsBus::setTorqueBroadcast(bool enable) {
|
||||||
|
uint8_t params[2] = {kRegTorqueEnable, static_cast<uint8_t>(enable ? 1 : 0)};
|
||||||
|
sendPacket(kBroadcastId, kInstWrite, params, 2);
|
||||||
|
delayMicroseconds(120);
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool FeetechHlsBus::setTorqueAll(const uint8_t* ids, int count, bool enable) {
|
||||||
|
setTorqueBroadcast(enable);
|
||||||
|
// Also sync-write individual entries cleanly in case broadcast is masked in servo EEPROM
|
||||||
|
uint8_t pkt[64];
|
||||||
|
if (count <= 0 || count > NUM_JOINTS) return false;
|
||||||
|
uint8_t param_len = static_cast<uint8_t>(2 + count * 2);
|
||||||
|
pkt[0] = kHeader;
|
||||||
|
pkt[1] = kHeader;
|
||||||
|
pkt[2] = kBroadcastId;
|
||||||
|
pkt[3] = static_cast<uint8_t>(param_len + 2);
|
||||||
|
pkt[4] = kInstSyncWrite;
|
||||||
|
pkt[5] = kRegTorqueEnable;
|
||||||
|
pkt[6] = 1;
|
||||||
|
uint8_t val = enable ? 1 : 0;
|
||||||
|
for (int i = 0; i < count; ++i) {
|
||||||
|
pkt[7 + i * 2] = ids[i];
|
||||||
|
pkt[8 + i * 2] = val;
|
||||||
|
}
|
||||||
|
pkt[5 + param_len] = computeChecksum(&pkt[2], static_cast<size_t>(3 + param_len));
|
||||||
|
sendRawPacket(pkt, static_cast<size_t>(6 + param_len));
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool FeetechHlsBus::syncWriteTargets(
|
||||||
|
const uint8_t* ids,
|
||||||
|
int count,
|
||||||
|
const int32_t* target_ticks,
|
||||||
|
uint8_t acc,
|
||||||
|
uint16_t speed_limit) {
|
||||||
|
if (!serial_ || count <= 0 || count > NUM_JOINTS) return false;
|
||||||
|
// 7 bytes starting at 0x29: [acc, pos_L, pos_H, time_L=0, time_H=0, spd_L, spd_H]
|
||||||
|
constexpr uint8_t kPerServo = 7;
|
||||||
|
uint8_t pkt[128];
|
||||||
|
uint8_t param_len = static_cast<uint8_t>(2 + count * (1 + kPerServo));
|
||||||
|
pkt[0] = kHeader;
|
||||||
|
pkt[1] = kHeader;
|
||||||
|
pkt[2] = kBroadcastId;
|
||||||
|
pkt[3] = static_cast<uint8_t>(param_len + 2);
|
||||||
|
pkt[4] = kInstSyncWrite;
|
||||||
|
pkt[5] = kRegAcc;
|
||||||
|
pkt[6] = kPerServo;
|
||||||
|
|
||||||
|
for (int i = 0; i < count; ++i) {
|
||||||
|
int32_t ticks = target_ticks[i];
|
||||||
|
if (ticks < 0) ticks = 0;
|
||||||
|
if (ticks > 4095) ticks = 4095;
|
||||||
|
uint16_t pos = static_cast<uint16_t>(ticks);
|
||||||
|
size_t off = 7 + static_cast<size_t>(i) * (1 + kPerServo);
|
||||||
|
pkt[off + 0] = ids[i];
|
||||||
|
pkt[off + 1] = acc;
|
||||||
|
pkt[off + 2] = static_cast<uint8_t>(pos & 0xFFU);
|
||||||
|
pkt[off + 3] = static_cast<uint8_t>((pos >> 8) & 0xFFU);
|
||||||
|
pkt[off + 4] = 0x00; // moving time low
|
||||||
|
pkt[off + 5] = 0x00; // moving time high
|
||||||
|
pkt[off + 6] = static_cast<uint8_t>(speed_limit & 0xFFU);
|
||||||
|
pkt[off + 7] = static_cast<uint8_t>((speed_limit >> 8) & 0xFFU);
|
||||||
|
}
|
||||||
|
pkt[5 + param_len] = computeChecksum(&pkt[2], static_cast<size_t>(3 + param_len));
|
||||||
|
sendRawPacket(pkt, static_cast<size_t>(6 + param_len));
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool FeetechHlsBus::readSingleJoint(
|
||||||
|
uint8_t id,
|
||||||
|
int32_t* out_tick,
|
||||||
|
int32_t* out_speed,
|
||||||
|
float* out_voltage_v,
|
||||||
|
float* out_temp_c) {
|
||||||
|
uint8_t params[2] = {kRegPresentPos, kFeedbackReadLen};
|
||||||
|
sendPacket(id, kInstRead, params, 2);
|
||||||
|
uint8_t err = 0;
|
||||||
|
uint8_t buf[kFeedbackReadLen] = {};
|
||||||
|
if (!readPacketForId(id, &err, buf, kFeedbackReadLen, 1600)) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
uint16_t pos_u16 = static_cast<uint16_t>(buf[0]) | (static_cast<uint16_t>(buf[1]) << 8);
|
||||||
|
uint16_t spd_u16 = static_cast<uint16_t>(buf[2]) | (static_cast<uint16_t>(buf[3]) << 8);
|
||||||
|
*out_tick = static_cast<int32_t>(pos_u16 & 0x7FFFU);
|
||||||
|
*out_speed = static_cast<int32_t>(spd_u16);
|
||||||
|
*out_voltage_v = static_cast<float>(buf[6]) * 0.1f;
|
||||||
|
*out_temp_c = static_cast<float>(buf[7]);
|
||||||
|
return err == 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
uint32_t FeetechHlsBus::readFeedback(
|
||||||
|
const uint8_t* ids,
|
||||||
|
int count,
|
||||||
|
int32_t* out_ticks,
|
||||||
|
int32_t* out_speeds,
|
||||||
|
float* out_min_voltage_v,
|
||||||
|
float* out_max_temp_c) {
|
||||||
|
uint32_t err_mask = 0;
|
||||||
|
float min_v = 99.0f;
|
||||||
|
float max_t = -40.0f;
|
||||||
|
bool any_ok = false;
|
||||||
|
|
||||||
|
#if USE_FEETECH_SYNC_READ
|
||||||
|
uint8_t sr_params[2 + NUM_JOINTS];
|
||||||
|
sr_params[0] = kRegPresentPos;
|
||||||
|
sr_params[1] = kFeedbackReadLen;
|
||||||
|
for (int i = 0; i < count; ++i) {
|
||||||
|
sr_params[2 + i] = ids[i];
|
||||||
|
}
|
||||||
|
sendPacket(kBroadcastId, kInstSyncRead, sr_params, static_cast<uint8_t>(2 + count));
|
||||||
|
|
||||||
|
bool got[NUM_JOINTS] = {};
|
||||||
|
for (int i = 0; i < count; ++i) {
|
||||||
|
uint8_t err = 0;
|
||||||
|
uint8_t buf[kFeedbackReadLen] = {};
|
||||||
|
if (readPacketForId(ids[i], &err, buf, kFeedbackReadLen, 1200)) {
|
||||||
|
got[i] = true;
|
||||||
|
uint16_t pos_u16 = static_cast<uint16_t>(buf[0]) | (static_cast<uint16_t>(buf[1]) << 8);
|
||||||
|
uint16_t spd_u16 = static_cast<uint16_t>(buf[2]) | (static_cast<uint16_t>(buf[3]) << 8);
|
||||||
|
out_ticks[i] = static_cast<int32_t>(pos_u16 & 0x7FFFU);
|
||||||
|
out_speeds[i] = static_cast<int32_t>(spd_u16);
|
||||||
|
float v = static_cast<float>(buf[6]) * 0.1f;
|
||||||
|
float t = static_cast<float>(buf[7]);
|
||||||
|
if (v > 0.1f && v < min_v) min_v = v;
|
||||||
|
if (t > max_t) max_t = t;
|
||||||
|
any_ok = true;
|
||||||
|
if (err != 0) err_mask |= (1UL << i);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
// Fallback single-read for any joints dropped during SyncRead
|
||||||
|
for (int i = 0; i < count; ++i) {
|
||||||
|
if (!got[i]) {
|
||||||
|
float v = 0.0f, t = 0.0f;
|
||||||
|
if (readSingleJoint(ids[i], &out_ticks[i], &out_speeds[i], &v, &t)) {
|
||||||
|
if (v > 0.1f && v < min_v) min_v = v;
|
||||||
|
if (t > max_t) max_t = t;
|
||||||
|
any_ok = true;
|
||||||
|
} else {
|
||||||
|
err_mask |= (1UL << i);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
#else
|
||||||
|
for (int i = 0; i < count; ++i) {
|
||||||
|
float v = 0.0f, t = 0.0f;
|
||||||
|
if (readSingleJoint(ids[i], &out_ticks[i], &out_speeds[i], &v, &t)) {
|
||||||
|
if (v > 0.1f && v < min_v) min_v = v;
|
||||||
|
if (t > max_t) max_t = t;
|
||||||
|
any_ok = true;
|
||||||
|
} else {
|
||||||
|
err_mask |= (1UL << i);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
*out_min_voltage_v = any_ok ? min_v : 0.0f;
|
||||||
|
*out_max_temp_c = any_ok ? max_t : 0.0f;
|
||||||
|
return err_mask;
|
||||||
|
}
|
||||||
@@ -0,0 +1,50 @@
|
|||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <Arduino.h>
|
||||||
|
#include "config.h"
|
||||||
|
|
||||||
|
class FeetechHlsBus {
|
||||||
|
public:
|
||||||
|
void begin(HardwareSerial& serial, uint32_t baud, int dir_pin = SERVO_DIR_PIN);
|
||||||
|
|
||||||
|
bool ping(uint8_t id, uint32_t timeout_us = 1800);
|
||||||
|
int pingRange(uint8_t max_id, int32_t* out_ids, int max_out);
|
||||||
|
|
||||||
|
bool setTorqueBroadcast(bool enable);
|
||||||
|
bool setTorqueAll(const uint8_t* ids, int count, bool enable);
|
||||||
|
|
||||||
|
bool syncWriteTargets(
|
||||||
|
const uint8_t* ids,
|
||||||
|
int count,
|
||||||
|
const int32_t* target_ticks,
|
||||||
|
uint8_t acc,
|
||||||
|
uint16_t speed_limit);
|
||||||
|
|
||||||
|
// Returns bitmask of joints with comm/servo error (bit i set -> joint i fault/timeout)
|
||||||
|
uint32_t readFeedback(
|
||||||
|
const uint8_t* ids,
|
||||||
|
int count,
|
||||||
|
int32_t* out_ticks,
|
||||||
|
int32_t* out_speeds,
|
||||||
|
float* out_min_voltage_v,
|
||||||
|
float* out_max_temp_c);
|
||||||
|
|
||||||
|
private:
|
||||||
|
HardwareSerial* serial_ = nullptr;
|
||||||
|
int dir_pin_ = -1;
|
||||||
|
uint8_t last_tx_[128] = {};
|
||||||
|
size_t last_tx_len_ = 0;
|
||||||
|
|
||||||
|
void setTxMode(bool tx);
|
||||||
|
void flushInput();
|
||||||
|
void sendRawPacket(const uint8_t* pkt, size_t len);
|
||||||
|
void sendPacket(uint8_t id, uint8_t inst, const uint8_t* params, uint8_t param_len);
|
||||||
|
bool readPacketForId(uint8_t expected_id, uint8_t* out_err, uint8_t* params, uint8_t expected_len, uint32_t timeout_us);
|
||||||
|
bool readSingleJoint(
|
||||||
|
uint8_t id,
|
||||||
|
int32_t* out_tick,
|
||||||
|
int32_t* out_speed,
|
||||||
|
float* out_voltage_v,
|
||||||
|
float* out_temp_c);
|
||||||
|
static uint8_t computeChecksum(const uint8_t* body_from_id, size_t body_len);
|
||||||
|
};
|
||||||
@@ -0,0 +1,216 @@
|
|||||||
|
#include "qmi8658.h"
|
||||||
|
|
||||||
|
#include <math.h>
|
||||||
|
|
||||||
|
namespace {
|
||||||
|
constexpr uint8_t kRegWhoAmI = 0x00;
|
||||||
|
constexpr uint8_t kRegCtrl1 = 0x02;
|
||||||
|
constexpr uint8_t kRegCtrl2 = 0x03;
|
||||||
|
constexpr uint8_t kRegCtrl3 = 0x04;
|
||||||
|
constexpr uint8_t kRegCtrl5 = 0x06;
|
||||||
|
constexpr uint8_t kRegCtrl7 = 0x08;
|
||||||
|
constexpr uint8_t kRegAxL = 0x35;
|
||||||
|
constexpr uint8_t kExpectedWhoAmI = 0x05;
|
||||||
|
|
||||||
|
// CTRL2: aFS=±4g (1 << 4), aODR=250Hz (5) -> 8192 LSB/g
|
||||||
|
constexpr uint8_t kCtrl2Val = 0x15;
|
||||||
|
constexpr float kAccelScaleG = 4.0f / 32768.0f;
|
||||||
|
|
||||||
|
// CTRL3: gFS=±1024 dps (6 << 4), gODR=448.4Hz (4) -> 32 LSB/dps
|
||||||
|
constexpr uint8_t kCtrl3Val = 0x64;
|
||||||
|
constexpr float kGyroScaleRadS = (1024.0f / 32768.0f) * (PI / 180.0f);
|
||||||
|
|
||||||
|
// CTRL5: enable LPF mode 2 on both accel and gyro
|
||||||
|
constexpr uint8_t kCtrl5Val = 0x55;
|
||||||
|
|
||||||
|
constexpr float kTwoKp = 2.0f * 1.2f; // Proportional gain
|
||||||
|
constexpr float kTwoKi = 2.0f * 0.02f; // Integral gain
|
||||||
|
} // namespace
|
||||||
|
|
||||||
|
bool Qmi8658Imu::writeReg(uint8_t reg, uint8_t val) {
|
||||||
|
wire_->beginTransmission(addr_);
|
||||||
|
wire_->write(reg);
|
||||||
|
wire_->write(val);
|
||||||
|
return wire_->endTransmission(true) == 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool Qmi8658Imu::readRegs(uint8_t reg, uint8_t* buf, size_t len) {
|
||||||
|
wire_->beginTransmission(addr_);
|
||||||
|
wire_->write(reg);
|
||||||
|
if (wire_->endTransmission(false) != 0) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
size_t got = wire_->requestFrom(static_cast<int>(addr_), static_cast<int>(len));
|
||||||
|
if (got != len) {
|
||||||
|
while (wire_->available()) {
|
||||||
|
(void)wire_->read();
|
||||||
|
}
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
for (size_t i = 0; i < len; ++i) {
|
||||||
|
buf[i] = static_cast<uint8_t>(wire_->read());
|
||||||
|
}
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool Qmi8658Imu::begin(TwoWire& wire, uint8_t addr) {
|
||||||
|
wire_ = &wire;
|
||||||
|
addr_ = addr;
|
||||||
|
healthy_ = false;
|
||||||
|
err_streak_ = 0;
|
||||||
|
|
||||||
|
uint8_t who = 0;
|
||||||
|
if (!readRegs(kRegWhoAmI, &who, 1) || who != kExpectedWhoAmI) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
// CTRL1 = 0x40 sets ADDR_AI (auto-increment for burst reads) and little-endian data
|
||||||
|
if (!writeReg(kRegCtrl7, 0x00)) return false;
|
||||||
|
delay(2);
|
||||||
|
if (!writeReg(kRegCtrl1, 0x40)) return false;
|
||||||
|
if (!writeReg(kRegCtrl2, kCtrl2Val)) return false;
|
||||||
|
if (!writeReg(kRegCtrl3, kCtrl3Val)) return false;
|
||||||
|
if (!writeReg(kRegCtrl5, kCtrl5Val)) return false;
|
||||||
|
if (!writeReg(kRegCtrl7, 0x03)) return false; // Enable accel + gyro
|
||||||
|
delay(10);
|
||||||
|
|
||||||
|
// Align initial Mahony orientation from static gravity vector if available
|
||||||
|
q0_ = 1.0f;
|
||||||
|
q1_ = 0.0f;
|
||||||
|
q2_ = 0.0f;
|
||||||
|
q3_ = 0.0f;
|
||||||
|
ix_ = 0.0f;
|
||||||
|
iy_ = 0.0f;
|
||||||
|
iz_ = 0.0f;
|
||||||
|
|
||||||
|
if (readSampleRaw()) {
|
||||||
|
for (int i = 0; i < 40; ++i) {
|
||||||
|
mahonyUpdate(0.0f, 0.0f, 0.0f, raw_accel_[0], raw_accel_[1], raw_accel_[2], 0.01f);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
healthy_ = true;
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
void Qmi8658Imu::setGyroBias(float bx, float by, float bz) {
|
||||||
|
gyro_bias_[0] = bx;
|
||||||
|
gyro_bias_[1] = by;
|
||||||
|
gyro_bias_[2] = bz;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool Qmi8658Imu::readSampleRaw() {
|
||||||
|
if (!wire_) return false;
|
||||||
|
uint8_t buf[12];
|
||||||
|
if (!readRegs(kRegAxL, buf, sizeof(buf))) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
auto to_i16 = [](uint8_t lo, uint8_t hi) -> int16_t {
|
||||||
|
return static_cast<int16_t>((static_cast<uint16_t>(hi) << 8) | lo);
|
||||||
|
};
|
||||||
|
raw_accel_[0] = static_cast<float>(to_i16(buf[0], buf[1])) * kAccelScaleG;
|
||||||
|
raw_accel_[1] = static_cast<float>(to_i16(buf[2], buf[3])) * kAccelScaleG;
|
||||||
|
raw_accel_[2] = static_cast<float>(to_i16(buf[4], buf[5])) * kAccelScaleG;
|
||||||
|
raw_gyro_[0] = static_cast<float>(to_i16(buf[6], buf[7])) * kGyroScaleRadS;
|
||||||
|
raw_gyro_[1] = static_cast<float>(to_i16(buf[8], buf[9])) * kGyroScaleRadS;
|
||||||
|
raw_gyro_[2] = static_cast<float>(to_i16(buf[10], buf[11])) * kGyroScaleRadS;
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool Qmi8658Imu::update(float dt_s) {
|
||||||
|
if (!readSampleRaw()) {
|
||||||
|
if (err_streak_ < 255) err_streak_++;
|
||||||
|
if (err_streak_ >= 10) healthy_ = false;
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
err_streak_ = 0;
|
||||||
|
healthy_ = true;
|
||||||
|
|
||||||
|
float gx = raw_gyro_[0] - gyro_bias_[0];
|
||||||
|
float gy = raw_gyro_[1] - gyro_bias_[1];
|
||||||
|
float gz = raw_gyro_[2] - gyro_bias_[2];
|
||||||
|
mahonyUpdate(gx, gy, gz, raw_accel_[0], raw_accel_[1], raw_accel_[2], dt_s);
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
void Qmi8658Imu::mahonyUpdate(float gx, float gy, float gz, float ax, float ay, float az, float dt_s) {
|
||||||
|
float norm = sqrtf(ax * ax + ay * ay + az * az);
|
||||||
|
if (norm > 0.25f) {
|
||||||
|
float inv = 1.0f / norm;
|
||||||
|
ax *= inv;
|
||||||
|
ay *= inv;
|
||||||
|
az *= inv;
|
||||||
|
|
||||||
|
// Estimated world-UP in body frame
|
||||||
|
float vx = 2.0f * (q1_ * q3_ - q0_ * q2_);
|
||||||
|
float vy = 2.0f * (q0_ * q1_ + q2_ * q3_);
|
||||||
|
float vz = 1.0f - 2.0f * (q1_ * q1_ + q2_ * q2_);
|
||||||
|
|
||||||
|
float ex = ay * vz - az * vy;
|
||||||
|
float ey = az * vx - ax * vz;
|
||||||
|
float ez = ax * vy - ay * vx;
|
||||||
|
|
||||||
|
if (kTwoKi > 0.0f) {
|
||||||
|
ix_ += kTwoKi * ex * dt_s;
|
||||||
|
iy_ += kTwoKi * ey * dt_s;
|
||||||
|
iz_ += kTwoKi * ez * dt_s;
|
||||||
|
gx += ix_;
|
||||||
|
gy += iy_;
|
||||||
|
gz += iz_;
|
||||||
|
}
|
||||||
|
gx += kTwoKp * ex;
|
||||||
|
gy += kTwoKp * ey;
|
||||||
|
gz += kTwoKp * ez;
|
||||||
|
}
|
||||||
|
|
||||||
|
float half_dt = 0.5f * dt_s;
|
||||||
|
float dq0 = (-q1_ * gx - q2_ * gy - q3_ * gz) * half_dt;
|
||||||
|
float dq1 = (q0_ * gx + q2_ * gz - q3_ * gy) * half_dt;
|
||||||
|
float dq2 = (q0_ * gy - q1_ * gz + q3_ * gx) * half_dt;
|
||||||
|
float dq3 = (q0_ * gz + q1_ * gy - q2_ * gx) * half_dt;
|
||||||
|
q0_ += dq0;
|
||||||
|
q1_ += dq1;
|
||||||
|
q2_ += dq2;
|
||||||
|
q3_ += dq3;
|
||||||
|
|
||||||
|
float qnorm = sqrtf(q0_ * q0_ + q1_ * q1_ + q2_ * q2_ + q3_ * q3_);
|
||||||
|
if (qnorm > 1e-6f) {
|
||||||
|
float inv_q = 1.0f / qnorm;
|
||||||
|
q0_ *= inv_q;
|
||||||
|
q1_ *= inv_q;
|
||||||
|
q2_ *= inv_q;
|
||||||
|
q3_ *= inv_q;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
bool Qmi8658Imu::sampleStationaryGyroBias(int samples, float out_bias[3]) {
|
||||||
|
if (samples <= 0) samples = 1;
|
||||||
|
double sx = 0.0, sy = 0.0, sz = 0.0;
|
||||||
|
int ok_cnt = 0;
|
||||||
|
for (int i = 0; i < samples; ++i) {
|
||||||
|
if (readSampleRaw()) {
|
||||||
|
sx += raw_gyro_[0];
|
||||||
|
sy += raw_gyro_[1];
|
||||||
|
sz += raw_gyro_[2];
|
||||||
|
ok_cnt++;
|
||||||
|
}
|
||||||
|
delay(4);
|
||||||
|
}
|
||||||
|
if (ok_cnt == 0) return false;
|
||||||
|
out_bias[0] = static_cast<float>(sx / ok_cnt);
|
||||||
|
out_bias[1] = static_cast<float>(sy / ok_cnt);
|
||||||
|
out_bias[2] = static_cast<float>(sz / ok_cnt);
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
void Qmi8658Imu::getRawGyro(float out[3]) const {
|
||||||
|
out[0] = raw_gyro_[0];
|
||||||
|
out[1] = raw_gyro_[1];
|
||||||
|
out[2] = raw_gyro_[2];
|
||||||
|
}
|
||||||
|
|
||||||
|
void Qmi8658Imu::getProjectedGravity(float out[3]) const {
|
||||||
|
// Gravity DOWN in world [0, 0, -1] rotated into body frame = -v_up
|
||||||
|
out[0] = -2.0f * (q1_ * q3_ - q0_ * q2_);
|
||||||
|
out[1] = -2.0f * (q0_ * q1_ + q2_ * q3_);
|
||||||
|
out[2] = -(1.0f - 2.0f * (q1_ * q1_ + q2_ * q2_));
|
||||||
|
}
|
||||||
@@ -0,0 +1,36 @@
|
|||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <Arduino.h>
|
||||||
|
#include <Wire.h>
|
||||||
|
|
||||||
|
class Qmi8658Imu {
|
||||||
|
public:
|
||||||
|
bool begin(TwoWire& wire, uint8_t addr);
|
||||||
|
void setGyroBias(float bx, float by, float bz);
|
||||||
|
bool update(float dt_s);
|
||||||
|
bool sampleStationaryGyroBias(int samples, float out_bias[3]);
|
||||||
|
|
||||||
|
bool isHealthy() const { return healthy_; }
|
||||||
|
void getRawGyro(float out[3]) const;
|
||||||
|
void getProjectedGravity(float out[3]) const;
|
||||||
|
|
||||||
|
private:
|
||||||
|
TwoWire* wire_ = nullptr;
|
||||||
|
uint8_t addr_ = 0x6A;
|
||||||
|
bool healthy_ = false;
|
||||||
|
uint8_t err_streak_ = 0;
|
||||||
|
|
||||||
|
// Bias subtracted strictly inside Mahony filter (raw gyro stays unbiased for Python HAL)
|
||||||
|
float gyro_bias_[3] = {0.0f, 0.0f, 0.0f};
|
||||||
|
float raw_gyro_[3] = {0.0f, 0.0f, 0.0f};
|
||||||
|
float raw_accel_[3] = {0.0f, 0.0f, 1.0f};
|
||||||
|
|
||||||
|
// Mahony quaternion (body -> world) and integral feedback
|
||||||
|
float q0_ = 1.0f, q1_ = 0.0f, q2_ = 0.0f, q3_ = 0.0f;
|
||||||
|
float ix_ = 0.0f, iy_ = 0.0f, iz_ = 0.0f;
|
||||||
|
|
||||||
|
bool writeReg(uint8_t reg, uint8_t val);
|
||||||
|
bool readRegs(uint8_t reg, uint8_t* buf, size_t len);
|
||||||
|
bool readSampleRaw();
|
||||||
|
void mahonyUpdate(float gx, float gy, float gz, float ax, float ay, float az, float dt_s);
|
||||||
|
};
|
||||||
@@ -0,0 +1,195 @@
|
|||||||
|
#include <Arduino_RouterBridge.h>
|
||||||
|
#include <Wire.h>
|
||||||
|
#include <vector>
|
||||||
|
|
||||||
|
#include "config.h"
|
||||||
|
#include "feetech_hls.h"
|
||||||
|
#include "qmi8658.h"
|
||||||
|
|
||||||
|
namespace {
|
||||||
|
Qmi8658Imu g_imu;
|
||||||
|
FeetechHlsBus g_servo;
|
||||||
|
|
||||||
|
uint8_t g_servo_ids[NUM_JOINTS] = {1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14};
|
||||||
|
uint32_t g_watchdog_ms = WATCHDOG_DEFAULT_MS;
|
||||||
|
uint32_t g_last_sync_ms = 0;
|
||||||
|
uint32_t g_last_imu_us = 0;
|
||||||
|
|
||||||
|
bool g_torque_enabled = false;
|
||||||
|
bool g_imu_ready = false;
|
||||||
|
|
||||||
|
int32_t g_last_ticks[NUM_JOINTS] = {2048, 2048, 2048, 2048, 2048, 2048, 2048, 2048, 2048, 2048, 2048, 2048, 2048, 2048};
|
||||||
|
int32_t g_last_speeds[NUM_JOINTS] = {};
|
||||||
|
} // namespace
|
||||||
|
|
||||||
|
int32_t hw_configure(std::vector<float> payload) {
|
||||||
|
g_torque_enabled = false;
|
||||||
|
g_servo.setTorqueAll(g_servo_ids, NUM_JOINTS, false);
|
||||||
|
|
||||||
|
uint32_t baud = SERVO_BAUD;
|
||||||
|
uint8_t i2c_addr = IMU_I2C_ADDR;
|
||||||
|
if (payload.size() >= 3) {
|
||||||
|
baud = static_cast<uint32_t>(payload[0] > 9600.0f ? payload[0] : SERVO_BAUD);
|
||||||
|
i2c_addr = static_cast<uint8_t>(static_cast<int>(payload[1] + 0.5f));
|
||||||
|
g_watchdog_ms = static_cast<uint32_t>(payload[2] >= 40.0f ? payload[2] : WATCHDOG_DEFAULT_MS);
|
||||||
|
}
|
||||||
|
if (payload.size() >= 6) {
|
||||||
|
g_imu.setGyroBias(payload[3], payload[4], payload[5]);
|
||||||
|
}
|
||||||
|
if (payload.size() >= 6 + NUM_JOINTS) {
|
||||||
|
for (int i = 0; i < NUM_JOINTS; ++i) {
|
||||||
|
int sid = static_cast<int>(payload[6 + i] + 0.5f);
|
||||||
|
if (sid >= 0 && sid <= 253) {
|
||||||
|
g_servo_ids[i] = static_cast<uint8_t>(sid);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
g_servo.begin(SERVO_SERIAL, baud, SERVO_DIR_PIN);
|
||||||
|
g_servo.setTorqueAll(g_servo_ids, NUM_JOINTS, false);
|
||||||
|
|
||||||
|
IMU_WIRE.begin();
|
||||||
|
IMU_WIRE.setClock(IMU_I2C_CLOCK_HZ);
|
||||||
|
g_imu_ready = g_imu.begin(IMU_WIRE, i2c_addr);
|
||||||
|
if (payload.size() >= 6) {
|
||||||
|
g_imu.setGyroBias(payload[3], payload[4], payload[5]);
|
||||||
|
}
|
||||||
|
|
||||||
|
g_last_sync_ms = millis();
|
||||||
|
g_last_imu_us = micros();
|
||||||
|
return g_imu_ready ? 1 : 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
std::vector<float> hw_sync_step(std::vector<int32_t> cmd_frame) {
|
||||||
|
std::vector<float> res(38, 0.0f);
|
||||||
|
if (cmd_frame.size() < 3 + NUM_JOINTS) {
|
||||||
|
res[36] = g_imu.isHealthy() ? 1.0f : 0.0f;
|
||||||
|
res[37] = static_cast<float>(0xFFFF);
|
||||||
|
return res;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool want_torque = cmd_frame[0] != 0;
|
||||||
|
uint16_t speed_limit = static_cast<uint16_t>(cmd_frame[1] > 0 ? cmd_frame[1] : 0);
|
||||||
|
uint8_t acc = static_cast<uint8_t>(cmd_frame[2] >= 0 ? (cmd_frame[2] <= 254 ? cmd_frame[2] : 254) : 0);
|
||||||
|
|
||||||
|
int32_t target_ticks[NUM_JOINTS];
|
||||||
|
for (int i = 0; i < NUM_JOINTS; ++i) {
|
||||||
|
target_ticks[i] = cmd_frame[3 + i];
|
||||||
|
}
|
||||||
|
|
||||||
|
// Write targets BEFORE enabling torque on 0->1 transition to prevent initial snap
|
||||||
|
if (want_torque) {
|
||||||
|
g_servo.syncWriteTargets(g_servo_ids, NUM_JOINTS, target_ticks, acc, speed_limit);
|
||||||
|
if (!g_torque_enabled) {
|
||||||
|
g_servo.setTorqueAll(g_servo_ids, NUM_JOINTS, true);
|
||||||
|
g_torque_enabled = true;
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
if (g_torque_enabled) {
|
||||||
|
g_servo.setTorqueAll(g_servo_ids, NUM_JOINTS, false);
|
||||||
|
g_torque_enabled = false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
g_last_sync_ms = millis();
|
||||||
|
|
||||||
|
// Sample fresh IMU reading if at least 2ms elapsed since last 250Hz integration tick
|
||||||
|
uint32_t now_us = micros();
|
||||||
|
uint32_t dt_us = now_us - g_last_imu_us;
|
||||||
|
if (dt_us >= 2000UL && dt_us < 100000UL) {
|
||||||
|
g_imu.update(static_cast<float>(dt_us) * 1e-6f);
|
||||||
|
g_last_imu_us = now_us;
|
||||||
|
}
|
||||||
|
|
||||||
|
float min_v = 0.0f;
|
||||||
|
float max_t = 0.0f;
|
||||||
|
uint32_t err_mask = g_servo.readFeedback(
|
||||||
|
g_servo_ids, NUM_JOINTS, g_last_ticks, g_last_speeds, &min_v, &max_t);
|
||||||
|
|
||||||
|
float gyro[3] = {0.0f, 0.0f, 0.0f};
|
||||||
|
float grav[3] = {0.0f, 0.0f, -1.0f};
|
||||||
|
g_imu.getRawGyro(gyro);
|
||||||
|
g_imu.getProjectedGravity(grav);
|
||||||
|
|
||||||
|
res[0] = gyro[0];
|
||||||
|
res[1] = gyro[1];
|
||||||
|
res[2] = gyro[2];
|
||||||
|
res[3] = grav[0];
|
||||||
|
res[4] = grav[1];
|
||||||
|
res[5] = grav[2];
|
||||||
|
for (int i = 0; i < NUM_JOINTS; ++i) {
|
||||||
|
res[6 + i] = static_cast<float>(g_last_ticks[i]);
|
||||||
|
res[20 + i] = static_cast<float>(g_last_speeds[i]);
|
||||||
|
}
|
||||||
|
res[34] = min_v;
|
||||||
|
res[35] = max_t;
|
||||||
|
res[36] = g_imu.isHealthy() ? 1.0f : 0.0f;
|
||||||
|
res[37] = static_cast<float>(err_mask);
|
||||||
|
return res;
|
||||||
|
}
|
||||||
|
|
||||||
|
int32_t hw_set_torque(int32_t enable) {
|
||||||
|
bool on = enable != 0;
|
||||||
|
g_torque_enabled = on;
|
||||||
|
g_servo.setTorqueAll(g_servo_ids, NUM_JOINTS, on);
|
||||||
|
g_last_sync_ms = millis();
|
||||||
|
return 1;
|
||||||
|
}
|
||||||
|
|
||||||
|
std::vector<int32_t> hw_ping_servos(int32_t max_id) {
|
||||||
|
if (max_id < 1) max_id = 1;
|
||||||
|
if (max_id > 253) max_id = 253;
|
||||||
|
int32_t found[64];
|
||||||
|
int n = g_servo.pingRange(static_cast<uint8_t>(max_id), found, 64);
|
||||||
|
return std::vector<int32_t>(found, found + n);
|
||||||
|
}
|
||||||
|
|
||||||
|
std::vector<float> hw_calibrate_imu(int32_t samples) {
|
||||||
|
if (samples < 10) samples = 10;
|
||||||
|
if (samples > 1000) samples = 1000;
|
||||||
|
float bias[3] = {0.0f, 0.0f, 0.0f};
|
||||||
|
g_imu.sampleStationaryGyroBias(static_cast<int>(samples), bias);
|
||||||
|
g_imu.setGyroBias(bias[0], bias[1], bias[2]);
|
||||||
|
return {bias[0], bias[1], bias[2]};
|
||||||
|
}
|
||||||
|
|
||||||
|
void setup() {
|
||||||
|
Bridge.begin();
|
||||||
|
Monitor.begin(115200);
|
||||||
|
|
||||||
|
g_servo.begin(SERVO_SERIAL, SERVO_BAUD, SERVO_DIR_PIN);
|
||||||
|
g_servo.setTorqueBroadcast(false);
|
||||||
|
|
||||||
|
IMU_WIRE.begin();
|
||||||
|
IMU_WIRE.setClock(IMU_I2C_CLOCK_HZ);
|
||||||
|
g_imu_ready = g_imu.begin(IMU_WIRE, IMU_I2C_ADDR);
|
||||||
|
|
||||||
|
Bridge.provide_safe("hw_configure", hw_configure);
|
||||||
|
Bridge.provide_safe("hw_sync_step", hw_sync_step);
|
||||||
|
Bridge.provide_safe("hw_set_torque", hw_set_torque);
|
||||||
|
Bridge.provide_safe("hw_ping_servos", hw_ping_servos);
|
||||||
|
Bridge.provide_safe("hw_calibrate_imu", hw_calibrate_imu);
|
||||||
|
|
||||||
|
g_last_sync_ms = millis();
|
||||||
|
g_last_imu_us = micros();
|
||||||
|
Monitor.println("Microduck UNO Q STM32 firmware initialized (torque off).");
|
||||||
|
}
|
||||||
|
|
||||||
|
void loop() {
|
||||||
|
uint32_t now_us = micros();
|
||||||
|
uint32_t dt_us = now_us - g_last_imu_us;
|
||||||
|
if (dt_us >= IMU_LOOP_INTERVAL_US) {
|
||||||
|
float dt_s = static_cast<float>(dt_us) * 1e-6f;
|
||||||
|
if (dt_s > 0.05f) dt_s = 0.004f;
|
||||||
|
g_imu.update(dt_s);
|
||||||
|
g_last_imu_us = now_us;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Hardware safety watchdog: disable servo torque if Linux 50Hz loop stalls
|
||||||
|
if (g_torque_enabled && (millis() - g_last_sync_ms) > g_watchdog_ms) {
|
||||||
|
g_torque_enabled = false;
|
||||||
|
g_servo.setTorqueBroadcast(false);
|
||||||
|
g_servo.setTorqueAll(g_servo_ids, NUM_JOINTS, false);
|
||||||
|
Monitor.println("WATCHDOG: Linux sync_step timeout -> servo torque disabled.");
|
||||||
|
}
|
||||||
|
}
|
||||||
@@ -0,0 +1,12 @@
|
|||||||
|
profiles:
|
||||||
|
default:
|
||||||
|
fqbn: arduino:zephyr:unoq
|
||||||
|
platforms:
|
||||||
|
- platform: arduino:zephyr
|
||||||
|
libraries:
|
||||||
|
- Arduino_RouterBridge
|
||||||
|
- Arduino_RPClite
|
||||||
|
- ArxContainer
|
||||||
|
- ArxTypeTraits
|
||||||
|
- DebugLog
|
||||||
|
- MsgPack
|
||||||
@@ -0,0 +1,8 @@
|
|||||||
|
<!doctype html><html lang="zh-CN"><meta charset="utf-8"><meta name="viewport" content="width=device-width, initial-scale=1"><title>Microduck 影子策略状态</title>
|
||||||
|
<style>body{margin:0;font:15px/1.5 system-ui;color:#16201f;background:#f4f8f6;padding:8px;border-radius:6px}p{margin:0;overflow-wrap:anywhere}</style>
|
||||||
|
<p id="summary">正在读取板端状态…</p><p id="details"></p><p id="skills"></p>
|
||||||
|
<script>
|
||||||
|
const names={walk:'行走',stand:'站立/起身',sitstand:'坐下/站起',kickL:'左脚踢球',kickR:'右脚踢球',groundpick:'拾取',roll:'翻滚',drive:'轮式行驶',crouch:'蹲伏'};
|
||||||
|
const text=(id,value)=>document.getElementById(id).textContent=value;
|
||||||
|
async function update(){try{if(document.hidden)return;const r=await fetch('http://127.0.0.1:18000/api/status',{cache:'no-store',signal:AbortSignal.timeout(1800)});if(!r.ok)throw Error();const s=await r.json();if(s.hal!=='xgoduck-shadow')throw Error();text('summary',s.version?'板端版本:'+s.version.slice(0,12):'尚未加载影子策略');text('details',s.shadow_error?'推理异常:'+s.shadow_error:s.native_mode==='shadow'?`当前影子策略:${names[s.shadow_skill]??s.shadow_skill??'无'} · 推理次数:${s.shadow_steps}`:'影子推理未运行 · 原机模式:'+({off:'扭矩已关闭',hold:'保持姿态',policy:'运动控制'}[s.native_mode]??s.native_mode));text('skills','已加载:'+(s.loaded_skills.map(k=>names[k]??k).join('、')||'无'));}catch{text('summary','影子服务未连接,请检查 USB 热更新转发(18000)。');text('skills','');text('details','');}finally{setTimeout(update,1200)}}update();
|
||||||
|
</script></html>
|
||||||
@@ -0,0 +1,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.
|
||||||
@@ -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.
|
||||||
@@ -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.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -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.
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user