feat(lekiwi): release V0.10.1 初步集成 LeKiwi,优化碰撞模型
web-platform-ci / TypeScript, lint, unit, build (push) Has been cancelled
web-platform-ci / Playwright E2E (push) Has been cancelled
lekiwi-compatibility / cpu-compatibility (push) Has been cancelled

集成通用机器人数值接口、本机控制桥、LeRobot 插件和统一键盘遥操作。采用离线 CoACD 全臂碰撞配方 revision 4、局部装配区切分与结构自接触,限制直接关节位姿写入并保留安全看门狗。同步版本号、变更记录、来源许可证和兼容性验证。
This commit is contained in:
2026-09-20 14:42:30 +08:00
parent da4d59c31a
commit 3ad29356c9
103 changed files with 23055 additions and 280 deletions
+11
View File
@@ -40,4 +40,15 @@ jobs:
- run: npm ci
- run: npx playwright install --with-deps chromium
- run: npm run build
- uses: actions/setup-python@v6
with:
python-version: '3.12'
- run: python -m pip install -e ./control_bridge
- run: npm run test:e2e
env:
BRIDGE_PYTHON: python3
- uses: actions/upload-artifact@v6
if: failure()
with:
name: ordinary-e2e-evidence
path: test-results/
+87
View File
@@ -0,0 +1,87 @@
name: lekiwi-compatibility
on:
workflow_dispatch:
push:
paths:
- 'contracts/**'
- 'control_bridge/**'
- 'integrations/lerobot/**'
- 'examples/lekiwi/**'
- 'robot_profiles/**'
- 'web_platform/**'
- 'package*.json'
- '.github/workflows/lekiwi-compat.yml'
pull_request:
paths:
- 'contracts/**'
- 'control_bridge/**'
- 'integrations/lerobot/**'
- 'examples/lekiwi/**'
- 'robot_profiles/**'
- 'web_platform/**'
- 'package*.json'
- '.github/workflows/lekiwi-compat.yml'
permissions:
contents: read
jobs:
cpu-compatibility:
runs-on: ubuntu-24.04
timeout-minutes: 25
steps:
- uses: actions/checkout@v6
- uses: actions/setup-node@v6
with:
node-version-file: .nvmrc
cache: npm
- uses: actions/setup-python@v6
with:
python-version: '3.12'
cache: pip
cache-dependency-path: |
integrations/lerobot/requirements-cpu.txt
examples/lekiwi/requirements-collision.txt
- run: npm install --global npm@11.17.0
- run: npm ci
- run: npx playwright install --with-deps chromium
- run: python -m pip install -e control_bridge
- uses: actions/cache@v5
with:
path: build/lekiwi
key: lekiwi-efa608d7-${{ hashFiles('robot_profiles/lekiwi-v1.json', 'examples/lekiwi/prepare_assets.py') }}
- name: Prepare pinned upstream assets (never skipped)
run: |
if [ -f build/lekiwi/URDF/LeKiwi.urdf ]; then
python examples/lekiwi/prepare_assets.py --source build/lekiwi --output build/lekiwi-verified
cp build/lekiwi-verified/lekiwi-v1.zip build/lekiwi/lekiwi-v1.zip
else
python examples/lekiwi/prepare_assets.py --download
fi
- name: Rebuild and verify collision recipe in its own environment
run: |
python -m venv build/venvs/collision
build/venvs/collision/bin/python -m pip install -r examples/lekiwi/requirements-collision.txt
build/venvs/collision/bin/python -m unittest discover -s examples/lekiwi/tests -v
build/venvs/collision/bin/python examples/lekiwi/generate_full_collisions.py --check
env:
PYTHONPATH: ''
PYTHONNOUSERSITE: '1'
- run: python examples/lekiwi/setup_lerobot.py
- run: build/venvs/lerobot/bin/python -m unittest discover -s integrations/lerobot/tests -v
env:
PYTHONPATH: ''
PYTHONNOUSERSITE: '1'
- run: npm run test:e2e:lekiwi
env:
BRIDGE_PYTHON: python3
LEROBOT_PYTHON: build/venvs/lerobot/bin/python
- uses: actions/upload-artifact@v6
if: always()
with:
name: lekiwi-compatibility-evidence
path: |
build/e2e/lekiwi/
build/lerobot-environment.json
build/lekiwi/source-manifest.json
+21
View File
@@ -4,6 +4,27 @@
## [未发布]
## [0.10.1] - 2026-09-20
### 初步集成 LeKiwi,优化碰撞模型
- LeKiwi 碰撞配方升级到 revision 4:新增隔离环境的离线 CoACD 全臂“从视觉生成碰撞体”管线,校验固定 URDF/STL 来源、版本/参数缓存和配方指纹;18 个视觉网格(含焊接附件)的 538 个分解凸包经配合区分割得到 1,220 个零附加质量碰撞体,替代旧胶囊和 revision 2/3 手工分段。旧 MJCF 必须从原始 URDF 重新转换,参考仓库及训练/LeRobot 环境不改。
- 移除整对相邻连杆排除,以关节局部装配几何分割、有限配合例外和保守整周包络筛选启用 5,581 对结构接触;修复 Mirror/Square 穿入,避免跨轴承区域凸包误锁肩旋。采用 1 ms 物理步长,保持协议、关节范围、单写入者与 500 ms 看门狗;新增 108 次原 CAD 覆盖探针、六关节双向扫掠、实际接触/反向脱离和性能回归。完整几何有额外开销,不承诺任意穿透初态、高速运动或硬实时;已知引擎诊断限制见碰撞数据说明。
- 机器人 profile 的关节角改为只读,底层拒绝直接 qpos 位姿写入、单独重置关节及忽略限位,禁用关节拖动入口,避免网页姿态编辑绕过物理;手动执行器目标及统一外控仍通过物理积分运动,普通模型的姿态编辑不变。夹爪间隙验收增加独立三角面距离 oracle,不依赖异常零距离查询或放宽断言。
- 修正夹爪开度方向与闭合限位,补齐指尖/指面接触并支持碰撞几何显示;保留早期 revision 2/3 手工分段的历史重建工具,当前运行时统一使用 revision 4,旧 MJCF 必须从原始 URDF 重新转换。
- LeKiwi 终端遥操作新增五个臂关节与夹爪正反点动、可配置速度和空格停止点动;同一30Hz循环合成底盘/臂动作,从实测初始姿态与限幅确认目标累加,保留超时撤权;补充按键表、leader接入边界、单测和真实PTY/浏览器物理回归。
- 修复 LeKiwi/通用桥接将暂停超过500ms误报为观测过期:优先提示播放并重新授权,底层禁止暂停时授权且不切换控制权;保留运行中的500ms安全看门狗,补充回归与键盘演示排障说明。
- 新增通用机器人 V1 契约、共享 TS/Python fixtures、模型地址绑定与单写入者仲裁;保留现有 Python、Go2 ONNX 和训练接口。
- 新增显式 LeKiwi v1:可重建固定来源资产、简化被动滚子/碰撞与九路伺服,真实 WASM 验证全向运动、臂/夹爪和障碍阻挡;不是实机标定。
- 新增独立回环控制桥、通用同步 Python SDK 和实际 LeRobot 0.6.1 插件;CPU 依赖隔离在 build/venvs/lerobot,不改训练环境或上游源码。
- 控制台集成 profile 校验、连接/token、明确授权、状态/新鲜度与安全停止;控制凭据仅在页面内存。覆盖 lease/epoch、确认目标与实测状态区分、重载/回滚、过期消息、隐藏/冻结和旧控制回调隔离。
- 外控使用独立固定步长调度,软件渲染时关闭阴影并限制3D绘制为5FPS,为30Hz通信保留主线程时间;非硬实时、非锁步。修复跨 reset 的观测序号判断及冻结后迟到动作续租问题。
- 增加普通桥接 CI 与独立固定 CPU LeKiwi 兼容任务、完整工作台60秒真实控制证据、部署/安全/能力矩阵和许可证记录;相机、LeRobot 数据集和实际 RL 训练仍不支持。
## [0.9.5] - 2026-09-09
- 修复外力拖动箭头起点停留在初始位置:保存拾取点的 body 局部坐标,每帧随部件平移和旋转更新,即使鼠标静止也持续跟随;保持现有外力方向/大小控制,补充位姿跟随与释放清理测试。
+19 -2
View File
@@ -13,6 +13,7 @@
- 统一地图资产库:工程地图、认证资产、内置地图及 9 类系统参数化地形共用点击/拖放、场景树、轻量预览、实例变换、放弃和一次编译事务
- 工程地图包:静态 MJCF/OBJ/STL/高度场碰撞层、GLB 视觉层和机器人出生点;同源实例拥有独立位姿,物理/视觉/出生点同步变换
- V3 地图创作层:认证资产可在程序地形上自动重力落位,共用右侧属性和视口操纵器,支持源内容编辑、空白取消选择及导出地图 ZIP
- 通用机器人数值接口、本机控制桥接与 LeKiwi / LeRobot 0.6.1 仿真插件(不接实体硬件)
- 浏览器内 Python 控制器(Pyodide)
- ONNX 强化学习策略推理(ONNX Runtime Web)
- 内置 Go2 PPO 任务的本机 mjlab 训练桥接服务,以及 DeepSeek 驱动的奖励函数自调参、固定评估和 TensorBoard 风格独立工作台
@@ -20,7 +21,7 @@
## 快速开始
环境要求:Node.js 24(版本见 `.nvmrc`)和 npm 11.17;仅使用训练桥接服务或执行 Python 检查时需要 Python 3.12。
环境要求:Node.js 24(版本见 `.nvmrc`)和 npm 11.17;仅使用本机桥接服务或执行 Python 检查时需要 Python 3.12。
```bash
nvm use
@@ -43,10 +44,12 @@ npm run check:format # Prettier 格式检查
npm test # Vitest 单元测试
npm run test:coverage # 核心模块覆盖率检查
npm run test:e2e # Playwright Chromium 浏览器测试
npm run test:control-bridge # 轻量机器人桥接测试,不依赖 LeRobot
npm run control-bridge # 本机数值桥,默认 8766
npm run test:training-server # Python 训练桥接服务测试
npm run check # 除 E2E 和 Ruff 外的完整检查
# 修改 training_server/ 时额外执行
# Python 服务/插件/示例检查(使用项目 .venv)
python3 -m pip install -r requirements-dev.txt
npm run lint:python
```
@@ -58,6 +61,11 @@ npm run lint:python
├── web_platform/ React、TypeScript、Three.js 前端与测试夹具
├── training_server/ 本机训练桥接服务及内置 Go2 PPO 训练代码/资产
├── controllers/ 可导入平台的 Python 控制器示例
├── contracts/ 跨语言机器人协议与 fixtures
├── robot_profiles/ 唯一模型参数/单位/来源清单
├── control_bridge/ 独立本机数值控制桥与通用 Python SDK
├── integrations/ 可选上游集成(LeRobot)
├── examples/lekiwi/ 资产准备、隔离环境安装及无硬件演示
├── package.json 根目录统一开发入口
└── LICENSE Apache License 2.0
```
@@ -70,6 +78,7 @@ npm run lint:python
- `web_platform/src/simulation/`:MuJoCo WASM 会话、仿真调度及执行器控制 UI
- `web_platform/src/viewer/`:Three.js 渲染、选择和物理交互
- `web_platform/src/map/`:地图加载、创作、校验及对应 UI
- `web_platform/src/robot/`:机器人契约、profile、实时运行时及外部控制客户端/面板
- `web_platform/src/controller/`:Pyodide 控制器运行时及控制面板
- `web_platform/src/rl/`:ONNX 策略运行时、任务绑定及策略面板
- `web_platform/src/training/`:本地训练服务客户端、类型及训练面板
@@ -80,10 +89,18 @@ npm run lint:python
详细使用说明见 [`web_platform/README.md`](web_platform/README.md),前端依赖边界见 [`web_platform/ARCHITECTURE.md`](web_platform/ARCHITECTURE.md),训练服务说明见 [`training_server/README.md`](training_server/README.md)。
## LeKiwi / LeRobot
按 [LeKiwi 示例](examples/lekiwi/README.md) 重建固定版本模型、安装 `build/venvs/lerobot/` CPU 环境,再从工作台文件导入、选择 profile、连接本机桥并明确授权。已有训练 `.venv` 不安装 LeRobot,也不修改上游代码。支持底盘、臂、夹爪与实测状态;没有相机、dataset、锁步或实际 RL 训练。
[通用接口/能力矩阵](docs/robot-interface.md) · [桥接协议与安全](control_bridge/README.md) · [LeRobot 插件](integrations/lerobot/README.md)。普通 CI 不安装重型上游依赖;`npm run test:e2e:lekiwi` 是独立的真实物理/60 秒兼容验收。
## 数据与安全边界
模型、资源、Python 控制器和 ONNX 策略默认只在当前浏览器会话中处理,不上传到服务器。训练桥接服务只监听本机回环地址,并仅执行服务端允许列表中的任务。启用自调参时,DeepSeek 只接收脱敏后的奖励参数、曲线摘要和评估数值;API key 仅存在训练服务环境中,不进入浏览器、URL、SQLite 或训练日志。
机器人控制桥与训练桥是两个服务(分别 8766 / 8765),不共享令牌或训练执行能力。控制令牌只保存在页面内存;外控超时、隐藏、暂停、重载均撤销授权,必须手动重新允许。
## 上游与许可证
本项目使用 Google DeepMind 的 MuJoCo WebAssembly npm 包,并保留 Apache License 2.0 许可证。MuJoCo 是 Google DeepMind 的开源项目;本仓库不是 Google 官方支持的产品。
+14
View File
@@ -0,0 +1,14 @@
{
"upstream": "lerobot 0.6.1",
"wheelOrder": ["base_left_wheel", "base_back_wheel", "base_right_wheel"],
"cases": [
{ "body": [0.1, 0, 0], "wheelRaw": [-1129, 0, 1129] },
{ "body": [-0.1, 0, 0], "wheelRaw": [1129, 0, -1129] },
{ "body": [0, 0.1, 0], "wheelRaw": [652, -1304, 652] },
{ "body": [0, -0.1, 0], "wheelRaw": [-652, 1304, -652] },
{ "body": [0, 0, 30], "wheelRaw": [853, 853, 853] },
{ "body": [0, 0, -30], "wheelRaw": [-853, -853, -853] },
{ "body": [0.2, 0.2, 60], "wheelRaw": [428, -513, 3000] },
{ "body": [-0.2, 0.2, -60], "wheelRaw": [1290, -3000, -1850] }
]
}
+62
View File
@@ -0,0 +1,62 @@
{
"descriptor": {
"protocolVersion": 1,
"profileId": "test-slider",
"profileVersion": 1,
"modelFingerprint": "aaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaaa",
"frame": "x-forward-y-left-z-up",
"actionChannels": [
{ "id": "slider.position", "unit": "m", "mode": "position", "min": -1, "max": 1 }
],
"observationChannels": [
{ "id": "slider.position", "unit": "m", "mode": "position", "min": -1, "max": 1 }
],
"capabilities": { "reset": true, "lockstep": false, "cameras": false, "training": false }
},
"identity": { "sessionId": "sim-test", "modelEpoch": 2, "leaseId": "lease-test" },
"validAction": {
"protocolVersion": 1,
"sessionId": "sim-test",
"modelEpoch": 2,
"leaseId": "lease-test",
"actionSeq": 1,
"values": { "slider.position": 0.3 }
},
"observation": {
"protocolVersion": 1,
"sessionId": "sim-test",
"modelEpoch": 2,
"sequence": 5,
"simTime": 0.1,
"appliedActionSeq": 1,
"paused": false,
"values": { "slider.position": 0.12 }
},
"invalidActions": [
{ "label": "old epoch", "patch": { "modelEpoch": 1 }, "code": "STALE" },
{ "label": "other lease", "patch": { "leaseId": "other" }, "code": "STALE" },
{ "label": "replayed sequence", "patch": { "actionSeq": 0 }, "code": "INVALID_MESSAGE" },
{ "label": "wrong version", "patch": { "protocolVersion": 2 }, "code": "UNSUPPORTED" },
{ "label": "missing channel", "patch": { "values": {} }, "code": "INVALID_MESSAGE" },
{
"label": "extra channel",
"patch": { "values": { "slider.position": 0, "unknown": 1 } },
"code": "INVALID_MESSAGE"
},
{
"label": "string number",
"patch": { "values": { "slider.position": "0.1" } },
"code": "INVALID_MESSAGE"
},
{
"label": "boolean",
"patch": { "values": { "slider.position": true } },
"code": "INVALID_MESSAGE"
},
{
"label": "unknown envelope field",
"patch": { "script": "print('no')" },
"code": "INVALID_MESSAGE"
}
]
}
+110
View File
@@ -0,0 +1,110 @@
{
"$schema": "https://json-schema.org/draft/2020-12/schema",
"$id": "urn:mujoco-web:robot:v1",
"description": "V1 numeric robot payloads. Descriptor semantics and action epoch/lease/sequence are additionally validated at runtime. JSON numbers must be finite. No camera, lockstep or training support.",
"oneOf": [
{ "$ref": "#/$defs/descriptor" },
{ "$ref": "#/$defs/action" },
{ "$ref": "#/$defs/observation" }
],
"$defs": {
"id": { "type": "string", "pattern": "^[a-zA-Z0-9][a-zA-Z0-9_.:-]{0,127}$" },
"counter": { "type": "integer", "minimum": 0, "maximum": 9007199254740991 },
"values": {
"type": "object",
"minProperties": 1,
"maxProperties": 256,
"propertyNames": { "$ref": "#/$defs/id" },
"additionalProperties": { "type": "number" }
},
"channel": {
"type": "object",
"additionalProperties": false,
"required": ["id", "unit", "mode", "min", "max"],
"properties": {
"id": { "$ref": "#/$defs/id" },
"unit": { "enum": ["rad", "rad/s", "m", "m/s", "ratio", "N", "N.m"] },
"mode": { "enum": ["position", "velocity", "effort", "opening"] },
"min": { "type": "number" },
"max": { "type": "number" }
}
},
"channels": {
"type": "array",
"minItems": 1,
"maxItems": 256,
"items": { "$ref": "#/$defs/channel" }
},
"descriptor": {
"type": "object",
"additionalProperties": false,
"required": [
"protocolVersion",
"profileId",
"profileVersion",
"modelFingerprint",
"frame",
"actionChannels",
"observationChannels",
"capabilities"
],
"properties": {
"protocolVersion": { "const": 1 },
"profileId": { "$ref": "#/$defs/id" },
"profileVersion": { "type": "integer", "minimum": 1, "maximum": 9007199254740991 },
"modelFingerprint": { "type": "string", "pattern": "^[a-f0-9]{64}$" },
"frame": { "const": "x-forward-y-left-z-up" },
"actionChannels": { "$ref": "#/$defs/channels" },
"observationChannels": { "$ref": "#/$defs/channels" },
"capabilities": {
"type": "object",
"additionalProperties": false,
"required": ["reset", "lockstep", "cameras", "training"],
"properties": {
"reset": { "type": "boolean" },
"lockstep": { "const": false },
"cameras": { "const": false },
"training": { "const": false }
}
}
}
},
"action": {
"type": "object",
"additionalProperties": false,
"required": ["protocolVersion", "sessionId", "modelEpoch", "leaseId", "actionSeq", "values"],
"properties": {
"protocolVersion": { "const": 1 },
"sessionId": { "$ref": "#/$defs/id" },
"modelEpoch": { "$ref": "#/$defs/counter" },
"leaseId": { "$ref": "#/$defs/id" },
"actionSeq": { "type": "integer", "minimum": 1, "maximum": 9007199254740991 },
"values": { "$ref": "#/$defs/values" }
}
},
"observation": {
"type": "object",
"additionalProperties": false,
"required": [
"protocolVersion",
"sessionId",
"modelEpoch",
"sequence",
"simTime",
"appliedActionSeq",
"paused",
"values"
],
"properties": {
"protocolVersion": { "const": 1 },
"sessionId": { "$ref": "#/$defs/id" },
"modelEpoch": { "$ref": "#/$defs/counter" },
"sequence": { "$ref": "#/$defs/counter" },
"simTime": { "type": "number", "minimum": 0 },
"appliedActionSeq": { "$ref": "#/$defs/counter" },
"paused": { "type": "boolean" },
"values": { "$ref": "#/$defs/values" }
}
}
}
}
+202
View File
@@ -0,0 +1,202 @@
Apache License
Version 2.0, January 2004
http://www.apache.org/licenses/
TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
1. Definitions.
"License" shall mean the terms and conditions for use, reproduction,
and distribution as defined by Sections 1 through 9 of this document.
"Licensor" shall mean the copyright owner or entity authorized by
the copyright owner that is granting the License.
"Legal Entity" shall mean the union of the acting entity and all
other entities that control, are controlled by, or are under common
control with that entity. For the purposes of this definition,
"control" means (i) the power, direct or indirect, to cause the
direction or management of such entity, whether by contract or
otherwise, or (ii) ownership of fifty percent (50%) or more of the
outstanding shares, or (iii) beneficial ownership of such entity.
"You" (or "Your") shall mean an individual or Legal Entity
exercising permissions granted by this License.
"Source" form shall mean the preferred form for making modifications,
including but not limited to software source code, documentation
source, and configuration files.
"Object" form shall mean any form resulting from mechanical
transformation or translation of a Source form, including but
not limited to compiled object code, generated documentation,
and conversions to other media types.
"Work" shall mean the work of authorship, whether in Source or
Object form, made available under the License, as indicated by a
copyright notice that is included in or attached to the work
(an example is provided in the Appendix below).
"Derivative Works" shall mean any work, whether in Source or Object
form, that is based on (or derived from) the Work and for which the
editorial revisions, annotations, elaborations, or other modifications
represent, as a whole, an original work of authorship. For the purposes
of this License, Derivative Works shall not include works that remain
separable from, or merely link (or bind by name) to the interfaces of,
the Work and Derivative Works thereof.
"Contribution" shall mean any work of authorship, including
the original version of the Work and any modifications or additions
to that Work or Derivative Works thereof, that is intentionally
submitted to Licensor for inclusion in the Work by the copyright owner
or by an individual or Legal Entity authorized to submit on behalf of
the copyright owner. For the purposes of this definition, "submitted"
means any form of electronic, verbal, or written communication sent
to the Licensor or its representatives, including but not limited to
communication on electronic mailing lists, source code control systems,
and issue tracking systems that are managed by, or on behalf of, the
Licensor for the purpose of discussing and improving the Work, but
excluding communication that is conspicuously marked or otherwise
designated in writing by the copyright owner as "Not a Contribution."
"Contributor" shall mean Licensor and any individual or Legal Entity
on behalf of whom a Contribution has been received by Licensor and
subsequently incorporated within the Work.
2. Grant of Copyright License. Subject to the terms and conditions of
this License, each Contributor hereby grants to You a perpetual,
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
copyright license to reproduce, prepare Derivative Works of,
publicly display, publicly perform, sublicense, and distribute the
Work and such Derivative Works in Source or Object form.
3. Grant of Patent License. Subject to the terms and conditions of
this License, each Contributor hereby grants to You a perpetual,
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
(except as stated in this section) patent license to make, have made,
use, offer to sell, sell, import, and otherwise transfer the Work,
where such license applies only to those patent claims licensable
by such Contributor that are necessarily infringed by their
Contribution(s) alone or by combination of their Contribution(s)
with the Work to which such Contribution(s) was submitted. If You
institute patent litigation against any entity (including a
cross-claim or counterclaim in a lawsuit) alleging that the Work
or a Contribution incorporated within the Work constitutes direct
or contributory patent infringement, then any patent licenses
granted to You under this License for that Work shall terminate
as of the date such litigation is filed.
4. Redistribution. You may reproduce and distribute copies of the
Work or Derivative Works thereof in any medium, with or without
modifications, and in Source or Object form, provided that You
meet the following conditions:
(a) You must give any other recipients of the Work or
Derivative Works a copy of this License; and
(b) You must cause any modified files to carry prominent notices
stating that You changed the files; and
(c) You must retain, in the Source form of any Derivative Works
that You distribute, all copyright, patent, trademark, and
attribution notices from the Source form of the Work,
excluding those notices that do not pertain to any part of
the Derivative Works; and
(d) If the Work includes a "NOTICE" text file as part of its
distribution, then any Derivative Works that You distribute must
include a readable copy of the attribution notices contained
within such NOTICE file, excluding those notices that do not
pertain to any part of the Derivative Works, in at least one
of the following places: within a NOTICE text file distributed
as part of the Derivative Works; within the Source form or
documentation, if provided along with the Derivative Works; or,
within a display generated by the Derivative Works, if and
wherever such third-party notices normally appear. The contents
of the NOTICE file are for informational purposes only and
do not modify the License. You may add Your own attribution
notices within Derivative Works that You distribute, alongside
or as an addendum to the NOTICE text from the Work, provided
that such additional attribution notices cannot be construed
as modifying the License.
You may add Your own copyright statement to Your modifications and
may provide additional or different license terms and conditions
for use, reproduction, or distribution of Your modifications, or
for any such Derivative Works as a whole, provided Your use,
reproduction, and distribution of the Work otherwise complies with
the conditions stated in this License.
5. Submission of Contributions. Unless You explicitly state otherwise,
any Contribution intentionally submitted for inclusion in the Work
by You to the Licensor shall be under the terms and conditions of
this License, without any additional terms or conditions.
Notwithstanding the above, nothing herein shall supersede or modify
the terms of any separate license agreement you may have executed
with Licensor regarding such Contributions.
6. Trademarks. This License does not grant permission to use the trade
names, trademarks, service marks, or product names of the Licensor,
except as required for reasonable and customary use in describing the
origin of the Work and reproducing the content of the NOTICE file.
7. Disclaimer of Warranty. Unless required by applicable law or
agreed to in writing, Licensor provides the Work (and each
Contributor provides its Contributions) on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
implied, including, without limitation, any warranties or conditions
of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
PARTICULAR PURPOSE. You are solely responsible for determining the
appropriateness of using or redistributing the Work and assume any
risks associated with Your exercise of permissions under this License.
8. Limitation of Liability. In no event and under no legal theory,
whether in tort (including negligence), contract, or otherwise,
unless required by applicable law (such as deliberate and grossly
negligent acts) or agreed to in writing, shall any Contributor be
liable to You for damages, including any direct, indirect, special,
incidental, or consequential damages of any character arising as a
result of this License or out of the use or inability to use the
Work (including but not limited to damages for loss of goodwill,
work stoppage, computer failure or malfunction, or any and all
other commercial damages or losses), even if such Contributor
has been advised of the possibility of such damages.
9. Accepting Warranty or Additional Liability. While redistributing
the Work or Derivative Works thereof, You may choose to offer,
and charge a fee for, acceptance of support, warranty, indemnity,
or other liability obligations and/or rights consistent with this
License. However, in accepting such obligations, You may act only
on Your own behalf and on Your sole responsibility, not on behalf
of any other Contributor, and only if You agree to indemnify,
defend, and hold each Contributor harmless for any liability
incurred by, or claims asserted against, such Contributor by reason
of your accepting any such warranty or additional liability.
END OF TERMS AND CONDITIONS
APPENDIX: How to apply the Apache License to your work.
To apply the Apache License to your work, attach the following
boilerplate notice, with the fields enclosed by brackets "[]"
replaced with your own identifying information. (Don't include
the brackets!) The text should be enclosed in the appropriate
comment syntax for the file format. We also recommend that a
file or class name and description of purpose be included on the
same "printed page" as the copyright notice for easier
identification within third-party archives.
Copyright [yyyy] [name of copyright owner]
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.
+64
View File
@@ -0,0 +1,64 @@
# 本机机器人控制桥接
独立于 `training_server` 的实时数值接口。默认监听 `127.0.0.1:8766`;不导入 LeRobot、PyTorch 或训练任务,不执行代码、Shell 或服务器文件路径。
## 启动
从仓库根目录执行(Python 3.12):
```bash
source .venv/bin/activate
python -m pip install -e ./control_bridge
python -m mujoco_control_bridge
# 或 npm run control-bridge
```
未设置 `MUJOCO_CONTROL_TOKEN` 时,服务生成至少 256 bit 随机 token,只在启动终端显示一次。也可通过该环境变量提供自己的 token(16–4096 个可打印 ASCII 字符,不含空白)。`--port 0` 自动分配端口,仅测试需要;不能配置非回环监听地址。
1. 在本机 HTTP 工作台加载受支持的机器人,显式选择 profile 并通过编译校验。
2. 在“控制台 → 开源项目 / 外部控制”填写地址和 token,连接桥接。
3. 播放,然后点击“允许外部控制”。接管会停止 Python/ONNX、锁定 1×并禁止冲突写入。
4. 在另一终端给 SDK 设置相同 token(不要把 token 放进 URL、源码或命令历史):
```bash
read -rsp '控制 token: ' MUJOCO_CONTROL_TOKEN; echo
export MUJOCO_CONTROL_TOKEN
export MUJOCO_CONTROL_ENDPOINT=http://127.0.0.1:8766
```
## 通用 Python SDK
客户端路径仅使用 Python 标准库;HTTP 不使用环境代理,也不跟随重定向。接口不依赖 LeKiwi 的通道名称或数量。
```python
import os
import time
from mujoco_control_bridge import SimRobotClient
with SimRobotClient(os.environ["MUJOCO_CONTROL_ENDPOINT"]) as robot:
descriptor = robot.describe()
values = {
channel["id"]: min(channel["max"], max(channel["min"], 0.0))
for channel in descriptor["actionChannels"]
}
start = time.monotonic()
for i in range(90):
measured = robot.get_observation() # 实际状态,不是目标回显
accepted = robot.send_action(values) # 物理步后确认,包含已接受的限幅目标
time.sleep(max(0, start + (i + 1) / 30 - time.monotonic()))
```
通用 SDK 要求提交完整通道集合;部分动作的保持/默认语义由具体插件处理。`robot.reset()` 返回新 `modelEpoch` 的暂停观测,并清除租约;必须在浏览器重新播放、授权,再连接。`is_connected` 不是永久健康承诺:距成功动作超过 500 ms 会在本地失效。
## 边界与故障处理
- HTTP Bearer token;WS 首帧认证。校验本机 Host/Origin,拒绝查询字符串 token、远程页面和未经认证的预检/连接。
- 一个浏览器后端、一个 Python 写入者;读取状态不取得写权限。不能自动抢占已有控制者。
- 64 KiB 帧/请求、每租约至多 100 动作/秒、8 个在途 RPC、一个最新待应用动作;旧目标被替换时返回 `SUPERSEDED`,不会堆积无限队列。
- 浏览器注册/认证有绝对超时;1 秒 RPC 确认期限、500 ms 动作/观测新鲜度边界。同序号心跳不刷新观测年龄。
- 暂停、断连、进程崩溃、重载、页面隐藏、冻结超时均撤销授权。轮目标归零、臂/夹爪保持实测姿态,并暂停外控仿真;**不是把所有位置伺服置零**。
- 回到页面或重新连接不会恢复旧命令。先检查错误、恢复关节限位,再播放/授权。
- 无 HTTPS/WSS、跨机器访问、锁步、图像流、LeRobot 数据集或训练 API。不要使用端口转发或反向代理扩大此 V1 的信任边界。
- 浏览器控制 token 仅在页面内存;不写入 `localStorage`/`sessionStorage`、工程导出或应用日志。拥有 token 的本机进程仍被视为可信控制者。
测试:`python -m unittest discover -s control_bridge/tests -v`。完整消息结构、错误码和扩展点见 [机器人接口](../docs/robot-interface.md);LeRobot 用法见 [LeKiwi 示例](../examples/lekiwi/README.md)。
+18
View File
@@ -0,0 +1,18 @@
[build-system]
requires = ["setuptools>=77,<82"]
build-backend = "setuptools.build_meta"
[project]
name = "mujoco-control-bridge"
version = "0.1.0"
description = "Loopback-only numeric robot control bridge for MuJoCo Web"
requires-python = ">=3.12"
license = "Apache-2.0"
license-files = ["LICENSE"]
dependencies = ["aiohttp>=3.12,<4"]
[project.scripts]
mujoco-control-bridge = "mujoco_control_bridge.__main__:main"
[tool.setuptools.packages.find]
where = ["src"]
@@ -0,0 +1,4 @@
from .client import SimRobotClient
from .protocol import RobotError
__all__ = ["RobotError", "SimRobotClient"]
@@ -0,0 +1,48 @@
"""Run an independent bridge on loopback only; token never goes in URLs."""
import argparse
import asyncio
import json
import os
import secrets
import signal
from aiohttp import web
from .server import create_app
async def serve(port, token):
runner = web.AppRunner(create_app(token), access_log=None, shutdown_timeout=2)
await runner.setup()
try:
await web.TCPSite(runner, "127.0.0.1", port).start()
address = runner.addresses[0]
print(
"CONTROL_BRIDGE_READY " + json.dumps({"endpoint": f"http://127.0.0.1:{address[1]}"}),
flush=True,
)
stopped = asyncio.Event()
loop = asyncio.get_running_loop()
for signum in (signal.SIGINT, signal.SIGTERM):
loop.add_signal_handler(signum, stopped.set)
await stopped.wait()
finally:
await runner.cleanup()
def main():
parser = argparse.ArgumentParser(description="MuJoCo 本机数值控制桥接(不是训练服务)")
parser.add_argument("--port", type=int, default=8766)
args = parser.parse_args()
if not 0 <= args.port <= 65535:
parser.error("端口必须为 0–65535")
token = os.environ.get("MUJOCO_CONTROL_TOKEN")
if not token:
token = secrets.token_urlsafe(32)
print(f"本次控制 token(仅显示一次;复制到浏览器/Python):{token}", flush=True)
asyncio.run(serve(args.port, token))
if __name__ == "__main__":
main()
@@ -0,0 +1,160 @@
"""Synchronous, generic robot SDK; stdlib only on the client path."""
import contextlib
import json
import os
import time
from urllib.error import HTTPError, URLError
from urllib.parse import urlparse
from urllib.request import HTTPRedirectHandler, ProxyHandler, Request, build_opener
from . import protocol as p
from .protocol import RobotError
class NoRedirect(HTTPRedirectHandler):
def redirect_request(self, req, fp, code, msg, headers, newurl):
return None # Never forward the bearer token to a redirected endpoint.
class SimRobotClient:
def __init__(self, endpoint="http://127.0.0.1:8766", token=None, timeout=2.0):
parsed = urlparse(endpoint)
if (
parsed.scheme != "http"
or parsed.hostname not in {"127.0.0.1", "localhost"}
or parsed.username
or parsed.password
or parsed.query
or parsed.fragment
or parsed.path not in {"", "/"}
):
raise ValueError("V1 仅支持本机 http://127.0.0.1:port 或 localhost")
self.endpoint = endpoint.rstrip("/")
self.token = token or os.environ.get("MUJOCO_CONTROL_TOKEN", "")
if not self.token:
raise ValueError("需要 token 或 MUJOCO_CONTROL_TOKEN")
if (
not isinstance(self.token, str)
or not 16 <= len(self.token) <= 4096
or not all(33 <= ord(c) <= 126 for c in self.token)
):
raise ValueError("控制 token 必须为16–4096字符的可打印 ASCII,且不含空白")
self.timeout = timeout
self._opener = build_opener(ProxyHandler({}), NoRedirect())
self._descriptor = None
self._identity = None
self._action_seq = 0
self._last_action_at = 0.0
def _request(self, method, path, payload=None, lease=None):
headers = {"Authorization": f"Bearer {self.token}"}
if lease:
headers["X-Control-Lease"] = lease["leaseId"]
raw = None
if payload is not None:
raw = json.dumps(payload, allow_nan=False).encode()
if len(raw) > 65536:
raise RobotError("INVALID_MESSAGE", "消息超过64KiB")
headers["Content-Type"] = "application/json"
request = Request(
f"{self.endpoint}/api/control/v1{path}", data=raw, headers=headers, method=method
)
try:
with self._opener.open(request, timeout=self.timeout) as response:
data = response.read(65537)
if len(data) > 65536:
raise RobotError("INVALID_MESSAGE", "响应超过64KiB")
return p.loads(data)
except HTTPError as exc:
try:
error = p.loads(exc.read(65537))["error"]
code, message = error["code"], error["message"]
except (RobotError, KeyError, TypeError):
code, message = "DISCONNECTED", f"桥接 HTTP {exc.code}"
raise RobotError(code, message) from None
except (URLError, TimeoutError, ConnectionError, OSError) as exc:
raise RobotError("DISCONNECTED", "无法访问本机控制桥接") from exc
@property
def is_connected(self):
return self._identity is not None and time.monotonic() - self._last_action_at <= 0.5
def describe(self):
self._descriptor = p.descriptor(self._request("GET", "/robot"))
return self._descriptor
def connect(self):
if self._identity:
raise RobotError("CONFLICT", "客户端已连接;请先 disconnect")
desc = self.describe()
obs = p.observation(self._request("GET", "/observation"), desc)
claim = {
"sessionId": obs["sessionId"],
"modelEpoch": obs["modelEpoch"],
"modelFingerprint": desc["modelFingerprint"],
}
lease = p.record(self._request("POST", "/lease", claim), p.IDENTITY)
p.identity(lease)
p.same_identity(lease, obs, ("sessionId", "modelEpoch"))
self._identity = lease
self._action_seq = 0
self._last_action_at = time.monotonic()
return desc
def _connected(self):
if not self.is_connected:
raise RobotError("DISCONNECTED", "没有有效租约;请重新在浏览器授权并连接")
return dict(self._identity)
def get_observation(self):
identity = self._connected()
obs = p.observation(self._request("GET", "/observation"), self._descriptor)
p.same_identity(obs, identity, ("sessionId", "modelEpoch"))
return obs
def send_action(self, values):
identity = self._connected()
values = p.values(values, self._descriptor["actionChannels"])
self._action_seq += 1
packet = {"protocolVersion": 1, **identity, "actionSeq": self._action_seq, "values": values}
try:
result = p.action_result(
self._request("POST", "/action", packet, lease=identity), self._descriptor
)
p.same_identity(result, packet, (*p.IDENTITY, "actionSeq"))
self._last_action_at = time.monotonic()
return result
except RobotError:
self.disconnect()
raise
def reset(self):
identity = self._connected()
try:
observed = p.observation(
self._request("POST", "/reset", {}, lease=identity), self._descriptor
)
if (
observed["sessionId"] != identity["sessionId"]
or observed["modelEpoch"] <= identity["modelEpoch"]
or not observed["paused"]
):
raise RobotError("STALE", "reset 没有返回新代次")
return observed
finally:
self.disconnect()
def disconnect(self):
identity, self._identity = self._identity, None
if identity:
# The server/browser watchdog remains the final safety barrier.
with contextlib.suppress(RobotError):
self._request("DELETE", "/lease", lease=identity)
def __enter__(self):
self.connect()
return self
def __exit__(self, *_):
self.disconnect()
@@ -0,0 +1,196 @@
"""Strict, framework-independent v1 wire validation (mirrors TS validation.ts)."""
import json
import math
import re
UNITS = {"rad", "rad/s", "m", "m/s", "ratio", "N", "N.m"}
MODES = {"position", "velocity", "effort", "opening"}
IDENTITY = {"sessionId", "modelEpoch", "leaseId"}
class RobotError(RuntimeError):
def __init__(self, code, message):
super().__init__(message)
self.code = code
def as_dict(self):
return {"code": self.code, "message": str(self)}
def invalid(message):
raise RobotError("INVALID_MESSAGE", message)
def record(value, keys, label="message"):
if not isinstance(value, dict) or set(value) != set(keys):
invalid(f"{label} 字段不完整或包含未知字段")
return value
def text(value):
if not isinstance(value, str) or not re.fullmatch(r"[a-zA-Z0-9][a-zA-Z0-9_.:-]{0,127}", value):
invalid("标识符无效")
return value
def finite(value):
try:
good = type(value) in (int, float) and math.isfinite(value)
except OverflowError:
good = False
if not good:
invalid("数值必须有限且不能为 bool")
return value
def integer(value, minimum=0):
finite(value)
if int(value) != value or not minimum <= value <= 2**53 - 1:
invalid("整数超出安全范围")
return value
def version(value):
if type(value) not in (int, float) or value != 1:
raise RobotError("UNSUPPORTED", "只支持协议 v1")
def identity(value):
text(value["sessionId"])
integer(value["modelEpoch"])
text(value["leaseId"])
def channels(value):
if not isinstance(value, list) or not 0 < len(value) <= 256:
invalid("通道列表无效")
seen = set()
for channel in value:
record(channel, {"id", "unit", "mode", "min", "max"}, "channel")
name = text(channel["id"])
if name in seen:
invalid("通道名称无效或重复")
seen.add(name)
if str(channel["unit"]) not in UNITS or str(channel["mode"]) not in MODES:
invalid("未知单位或通道模式")
for key in ("min", "max"):
finite(channel[key])
if channel["min"] >= channel["max"]:
invalid("通道范围无效")
return value
def descriptor(value):
record(
value,
{
"protocolVersion",
"profileId",
"profileVersion",
"modelFingerprint",
"frame",
"actionChannels",
"observationChannels",
"capabilities",
},
"descriptor",
)
version(value["protocolVersion"])
if value["frame"] != "x-forward-y-left-z-up":
raise RobotError("UNSUPPORTED", "坐标系不支持")
text(value["profileId"])
integer(value["profileVersion"], 1)
if not isinstance(value["modelFingerprint"], str) or not re.fullmatch(
r"[a-f0-9]{64}", value["modelFingerprint"]
):
invalid("需要 SHA-256 模型指纹")
channels(value["actionChannels"])
channels(value["observationChannels"])
caps = record(
value["capabilities"], {"reset", "lockstep", "cameras", "training"}, "capabilities"
)
if any(type(v) is not bool for v in caps.values()) or any(
caps[k] is not False for k in ("lockstep", "cameras", "training")
):
raise RobotError("UNSUPPORTED", "V1 不支持相机、锁步或训练")
return value
def values(value, specs, clamp=False):
record(value, {c["id"] for c in specs}, "values")
result = {}
for channel in specs:
number = finite(value[channel["id"]])
result[channel["id"]] = (
max(channel["min"], min(channel["max"], number)) if clamp else number
)
return result
def action(value, desc):
record(value, {"protocolVersion", *IDENTITY, "actionSeq", "values"}, "action")
version(value["protocolVersion"])
identity(value)
integer(value["actionSeq"], 1)
return {**value, "values": values(value["values"], desc["actionChannels"], True)}
def action_result(value, desc):
record(value, {*IDENTITY, "actionSeq", "values", "simTime"}, "action result")
identity(value)
integer(value["actionSeq"], 1)
finite(value["simTime"])
if value["simTime"] < 0:
invalid("仿真时间必须非负")
values(value["values"], desc["actionChannels"])
return value
def observation(value, desc):
record(
value,
{
"protocolVersion",
"sessionId",
"modelEpoch",
"sequence",
"simTime",
"appliedActionSeq",
"paused",
"values",
},
"observation",
)
version(value["protocolVersion"])
text(value["sessionId"])
integer(value["modelEpoch"])
integer(value["sequence"])
integer(value["appliedActionSeq"])
finite(value["simTime"])
if value["simTime"] < 0 or type(value["paused"]) is not bool:
invalid("仿真时钟/暂停标记无效")
values(value["values"], desc["observationChannels"])
return value
def same_identity(actual, expected, keys=IDENTITY):
if any(actual.get(key) != expected.get(key) for key in keys):
raise RobotError("STALE", "会话、模型代次或控制租约已失效")
def loads(raw):
def pairs(items):
result = {}
for key, value in items:
if key in result:
invalid("重复 JSON 字段")
result[key] = value
return result
try:
return json.loads(
raw, parse_constant=lambda _: invalid("禁止 NaN/Infinity"), object_pairs_hook=pairs
)
except (ValueError, TypeError, RecursionError) as exc:
raise RobotError("INVALID_MESSAGE", "无效 JSON") from exc
@@ -0,0 +1,463 @@
"""Independent loopback broker. No simulation, training or LeRobot imports."""
import asyncio
import contextlib
import hmac
import re
import secrets
import time
from collections import deque
from aiohttp import WSMsgType, web
from . import protocol as p
from .protocol import RobotError
MAX_BYTES = 65536
PREFIX = "/api/control/v1"
ORIGIN = re.compile(r"http://(?:127\.0\.0\.1|localhost)(?::[0-9]{1,5})?\Z")
HOST = re.compile(r"(?:127\.0\.0\.1|localhost)(?::[0-9]{1,5})?\Z")
BROKER = web.AppKey("broker", object)
class Broker:
def __init__(self, token):
if (
not isinstance(token, str)
or not 16 <= len(token) <= 4096
or not all(33 <= ord(c) <= 126 for c in token)
):
raise ValueError("控制 token 必须为16–4096字符的可打印 ASCII,且不含空白")
self.token = token
self.ws = None
self.descriptor = None
self.observation = None
self.observed_at = 0.0
self.authorization_generation = 0
self.blocked_generation = -1
self.authorized = False
self.lease = None
self.last_action_at = 0.0
self.last_action_seq = 0
self.rate = deque()
self.pending = {}
def robot(self):
if self.ws is None or self.ws.closed or self.descriptor is None:
raise RobotError("DISCONNECTED", "浏览器机器人未连接")
return self.descriptor
def fresh_observation(self):
self.robot()
# Paused samples stop advancing by design; do not hide the required play
# step behind a freshness error merely because the user waited to connect.
if self.observation is not None and self.observation["paused"]:
raise RobotError("PAUSED", "仿真已暂停;请先在浏览器点击“播放”,再点击“允许外部控制”")
if self.observation is None or time.monotonic() - self.observed_at > 0.5:
raise RobotError(
"STALE",
"机器人观测超过 500ms 未更新;请保持仿真页面可见,"
"检查仿真是否卡顿,再播放并重新允许外部控制",
)
return self.observation
async def state(self, packet):
p.record(packet, {"type", "observation", "enabled", "authorizationGeneration"})
if type(packet["enabled"]) is not bool:
p.invalid("授权标记必须为 bool")
generation = p.integer(packet["authorizationGeneration"])
if generation < self.authorization_generation:
raise RobotError("STALE", "授权代次倒退")
observed = p.observation(packet["observation"], self.robot())
old = self.observation
if old:
p.same_identity(observed, old, ("sessionId",))
if observed["modelEpoch"] < old["modelEpoch"] or (
observed["modelEpoch"] == old["modelEpoch"]
and observed["sequence"] < old["sequence"]
):
raise RobotError("STALE", "观测代次/序号倒退")
# Sequence may restart only in a new epoch. Same-sample heartbeats aren't fresh.
if (
old is None
or observed["modelEpoch"] > old["modelEpoch"]
or observed["sequence"] > old["sequence"]
):
self.observation = observed
self.observed_at = time.monotonic()
if self.lease and generation != self.authorization_generation:
# Cancel the previous owner without consuming the new explicit grant.
await self.revoke("浏览器已重新授权", notify=False)
self.authorization_generation = generation
self.authorized = packet["enabled"] and generation > self.blocked_generation
if self.lease and (
not self.authorized
or observed["paused"]
or any(observed[k] != self.lease[k] for k in ("sessionId", "modelEpoch"))
):
await self.revoke("浏览器已撤销控制/重置模型", notify=False)
async def rpc(self, operation, payload):
self.robot()
if operation == "action":
for key, (future, op) in list(self.pending.items()):
if op == "action":
if not future.done():
future.set_exception(RobotError("SUPERSEDED", "已由更新的目标替代"))
self.pending.pop(key, None)
if len(self.pending) >= 8:
raise RobotError("CONFLICT", "待确认请求已满")
request_id = secrets.token_hex(16)
future = asyncio.get_running_loop().create_future()
ws, lease, generation = self.ws, self.lease, self.authorization_generation
self.pending[request_id] = (future, operation)
try:
await asyncio.wait_for(
self.ws.send_json(
{"type": "request", "id": request_id, "op": operation, "payload": payload}
),
1,
)
return await asyncio.wait_for(future, 1)
except TimeoutError as exc:
if (
self.ws is ws
and self.lease == lease
and self.authorization_generation == generation
):
await self.revoke("动作应用确认超时")
raise RobotError("TIMEOUT", "浏览器未在一秒内确认应用") from exc
finally:
self.pending.pop(request_id, None)
if not future.done():
future.cancel()
elif not future.cancelled():
future.exception() # Also consume failures delivered during a blocked send.
async def revoke(self, reason, notify=True):
self.lease = None
self.authorized = False
self.blocked_generation = self.authorization_generation
for future, _ in self.pending.values():
if not future.done():
future.set_exception(RobotError("DISCONNECTED", reason))
self.pending.clear()
if notify and self.ws is not None and not self.ws.closed and self.observation:
with contextlib.suppress(ConnectionError, TimeoutError):
await asyncio.wait_for(
self.ws.send_json(
{
"type": "stop",
"reason": reason,
"sessionId": self.observation["sessionId"],
"authorizationGeneration": self.authorization_generation,
}
),
0.1,
)
def require_lease(self, request):
provided = request.headers.get("X-Control-Lease", "")
if (
not self.lease
or not provided.isascii()
or not hmac.compare_digest(provided.encode(), self.lease["leaseId"].encode())
):
raise RobotError("UNAUTHORIZED", "控制租约无效")
return dict(self.lease)
async def monitor(self):
while True:
await asyncio.sleep(0.05)
if self.lease and (
time.monotonic() - self.last_action_at > 0.5
or time.monotonic() - self.observed_at > 0.5
):
await self.revoke("动作或观测看门狗超时 (500ms)")
@web.middleware
async def security(request, handler):
broker = request.app[BROKER]
try:
if not HOST.fullmatch(request.headers.get("Host", "")):
raise RobotError("UNAUTHORIZED", "Host 不被允许")
origin = request.headers.get("Origin")
if origin is not None and not ORIGIN.fullmatch(origin):
raise RobotError("UNAUTHORIZED", "Origin 不被允许")
if request.query:
raise RobotError("INVALID_MESSAGE", "禁止 URL 查询参数及 URL 中的 token")
if request.path != "/ws/control/v1":
expected = f"Bearer {broker.token}".encode()
provided = request.headers.get("Authorization", "")
if not provided.isascii() or not hmac.compare_digest(provided.encode(), expected):
raise RobotError("UNAUTHORIZED", "需要本机控制 Bearer token")
elif origin is None:
raise RobotError("UNAUTHORIZED", "浏览器 WebSocket 必须提供 Origin")
return await handler(request)
except RobotError as exc:
status = {
"UNAUTHORIZED": 401,
"DISCONNECTED": 503,
"TIMEOUT": 504,
"INVALID_MESSAGE": 400,
"UNSUPPORTED": 400,
}.get(exc.code, 409)
return web.json_response({"error": exc.as_dict()}, status=status)
except web.HTTPRequestEntityTooLarge:
return web.json_response(
{"error": {"code": "INVALID_MESSAGE", "message": "消息不能超过64KiB"}}, status=413
)
async def body(request):
if request.content_type != "application/json":
p.invalid("需要 application/json")
return p.loads(await request.read())
async def health(request):
b = request.app[BROKER]
return web.json_response(
{
"protocolVersion": 1,
"backendConnected": b.descriptor is not None,
"authorized": b.authorized,
"hasLease": b.lease is not None,
}
)
async def robot(request):
return web.json_response(request.app[BROKER].robot())
async def observation(request):
return web.json_response(request.app[BROKER].fresh_observation())
async def claim(request):
b = request.app[BROKER]
data = p.record(await body(request), {"sessionId", "modelEpoch", "modelFingerprint"})
p.text(data["sessionId"])
p.integer(data["modelEpoch"])
b.robot()
observed = b.fresh_observation()
p.same_identity(data, observed, ("sessionId", "modelEpoch"))
if data["modelFingerprint"] != b.descriptor["modelFingerprint"]:
raise RobotError("INCOMPATIBLE_MODEL", "模型指纹已变化")
if b.lease:
raise RobotError("CONFLICT", "已有一个 Python 控制者")
if not b.authorized:
raise RobotError("UNAUTHORIZED", "请先在浏览器显式允许外部控制")
lease = {
"sessionId": data["sessionId"],
"modelEpoch": data["modelEpoch"],
"leaseId": secrets.token_hex(24),
}
b.lease = lease
b.last_action_at = time.monotonic()
b.last_action_seq = 0
b.rate.clear()
try:
result = p.record(
await b.rpc(
"claim",
{
**lease,
"modelFingerprint": data["modelFingerprint"],
"authorizationGeneration": b.authorization_generation,
},
),
p.IDENTITY,
)
p.same_identity(result, lease)
if b.lease != lease:
raise RobotError("STALE", "租约在申请期间失效")
return web.json_response(result)
except BaseException:
if b.lease == lease:
await b.revoke("租约申请失败")
raise
async def action(request):
b = request.app[BROKER]
lease = b.require_lease(request)
b.fresh_observation()
data = p.action(await body(request), b.robot())
p.same_identity(data, lease)
if data["actionSeq"] <= b.last_action_seq:
raise RobotError("STALE", "拒绝重复/乱序动作")
now = time.monotonic()
while b.rate and now - b.rate[0] >= 1:
b.rate.popleft()
if len(b.rate) >= 100:
raise RobotError("CONFLICT", "动作频率不能超过100Hz")
b.rate.append(now)
b.last_action_seq = data["actionSeq"]
b.last_action_at = now
try:
result = p.action_result(await b.rpc("action", data), b.robot())
p.same_identity(result, data, (*p.IDENTITY, "actionSeq"))
except RobotError as exc:
if exc.code != "SUPERSEDED" and b.lease == lease:
await b.revoke("动作确认失败")
raise
if b.lease != lease:
raise RobotError("STALE", "动作确认来自失效租约")
return web.json_response(result)
async def release(request):
b = request.app[BROKER]
lease = b.require_lease(request)
try:
await b.rpc("release", lease)
finally:
if b.lease == lease:
await b.revoke("控制者断开连接")
return web.json_response({"released": True})
async def reset(request):
b = request.app[BROKER]
lease = b.require_lease(request)
p.record(await body(request), set())
if not b.robot()["capabilities"]["reset"]:
raise RobotError("UNSUPPORTED", "机器人不支持 reset")
try:
result = p.observation(await b.rpc("reset", lease), b.robot())
if (
result["sessionId"] != lease["sessionId"]
or result["modelEpoch"] <= lease["modelEpoch"]
or not result["paused"]
):
p.invalid("reset 未返回新代次的暂停观测")
finally:
if b.lease == lease:
await b.revoke("reset 后需要重新授权")
return web.json_response(result)
async def websocket(request):
b = request.app[BROKER]
ws = web.WebSocketResponse(
max_msg_size=MAX_BYTES, heartbeat=2, receive_timeout=5, compress=False
)
await ws.prepare(request)
registered = False
times = deque()
try:
# Outer deadline: WS ping/pong must NOT restart the authentication clock.
first = await asyncio.wait_for(ws.receive(), 5)
if first.type != WSMsgType.TEXT:
raise RobotError("UNAUTHORIZED", "必须在5秒内通过认证")
auth = p.record(p.loads(first.data), {"type", "token", "protocolVersion"})
p.version(auth["protocolVersion"])
if (
auth["type"] != "auth"
or not isinstance(auth["token"], str)
or not auth["token"].isascii()
or not hmac.compare_digest(auth["token"].encode(), b.token.encode())
):
raise RobotError("UNAUTHORIZED", "WebSocket token 无效")
if b.ws is not None:
raise RobotError("CONFLICT", "已有浏览器后端连接")
b.ws = ws # Reserve the single backend before any further await.
await ws.send_json({"type": "authenticated"})
while True:
message = (
await asyncio.wait_for(ws.receive(), 5) if not registered else await ws.receive()
)
if message.type != WSMsgType.TEXT:
break
now = time.monotonic()
while times and now - times[0] > 1:
times.popleft()
if len(times) >= 256:
raise RobotError("CONFLICT", "浏览器消息过于频繁")
times.append(now)
packet = p.loads(message.data)
if not isinstance(packet, dict):
p.invalid("消息必须是对象")
kind = packet.get("type")
if kind == "register" and not registered:
p.record(
packet,
{"type", "descriptor", "observation", "enabled", "authorizationGeneration"},
)
b.descriptor = p.descriptor(packet["descriptor"])
await b.state({k: v for k, v in packet.items() if k != "descriptor"})
registered = True
await ws.send_json({"type": "ready"})
elif kind == "state" and registered:
await b.state(packet)
elif kind == "result" and registered:
p.record(packet, {"type", "id", "ok", "value"})
p.text(packet["id"])
if type(packet["ok"]) is not bool:
p.invalid("ok 必须为 bool")
pending = b.pending.get(packet["id"])
if pending and not pending[0].done():
if packet["ok"]:
pending[0].set_result(packet["value"])
else:
error = p.record(packet["value"], {"code", "message"})
if (
not isinstance(error["code"], str)
or not isinstance(error["message"], str)
or len(error["message"]) > 1024
):
p.invalid("错误格式无效")
pending[0].set_exception(RobotError(error["code"], error["message"]))
else:
p.invalid("未注册后端或未知消息类型")
except (RobotError, TimeoutError, ConnectionError) as exc:
error = (
exc
if isinstance(exc, RobotError)
else RobotError("DISCONNECTED", "浏览器连接中断/超时")
)
if not ws.closed:
with contextlib.suppress(ConnectionError):
await ws.send_json({"type": "error", "error": error.as_dict()})
finally:
if b.ws is ws:
await b.revoke("浏览器已断开", notify=False)
b.ws = b.descriptor = b.observation = None
b.authorization_generation = 0
b.blocked_generation = -1
await ws.close()
return ws
def create_app(token):
app = web.Application(middlewares=[security], client_max_size=MAX_BYTES)
app[BROKER] = Broker(token)
app.add_routes(
[
web.get(f"{PREFIX}/health", health),
web.get(f"{PREFIX}/robot", robot),
web.get(f"{PREFIX}/observation", observation),
web.post(f"{PREFIX}/lease", claim),
web.delete(f"{PREFIX}/lease", release),
web.post(f"{PREFIX}/action", action),
web.post(f"{PREFIX}/reset", reset),
web.get("/ws/control/v1", websocket),
]
)
async def lifetime(application):
b = application[BROKER]
task = asyncio.create_task(b.monitor())
yield
task.cancel()
with contextlib.suppress(asyncio.CancelledError):
await task
await b.revoke("桥接服务关闭")
if b.ws is not None:
await b.ws.close()
app.cleanup_ctx.append(lifetime)
return app
+65
View File
@@ -0,0 +1,65 @@
import copy
import json
import unittest
from pathlib import Path
from mujoco_control_bridge import protocol as p
from mujoco_control_bridge.client import SimRobotClient
FIXTURE = json.loads(
(Path(__file__).resolve().parents[2] / "contracts/fixtures/single-joint.json").read_text()
)
class ProtocolTests(unittest.TestCase):
def test_shared_fixture(self):
descriptor = p.descriptor(FIXTURE["descriptor"])
p.observation(FIXTURE["observation"], descriptor)
action = p.action(FIXTURE["validAction"], descriptor)
p.same_identity(action, FIXTURE["identity"])
for invalid in FIXTURE["invalidActions"]:
with self.subTest(invalid["label"]), self.assertRaises(p.RobotError) as caught:
action = p.action({**FIXTURE["validAction"], **invalid["patch"]}, descriptor)
p.same_identity(action, FIXTURE["identity"])
self.assertEqual(caught.exception.code, invalid["code"])
def test_finite_clamp_and_extra_fields(self):
for value in (float("nan"), float("inf"), True, "0", 10**1000):
with self.subTest(value=str(value)[:20]), self.assertRaises(p.RobotError):
p.values({"slider.position": value}, FIXTURE["descriptor"]["actionChannels"])
self.assertEqual(
p.values({"slider.position": 20}, FIXTURE["descriptor"]["actionChannels"], True),
{"slider.position": 1},
)
for raw in ('{"x":1,"x":2}', '{"x":NaN}', '{"x":Infinity}', "["):
with self.assertRaises(p.RobotError):
p.loads(raw)
def test_unknown_units_capabilities_versions(self):
for mutation in ("unit", "frame", "capability", "fingerprint"):
descriptor = copy.deepcopy(FIXTURE["descriptor"])
if mutation == "unit":
descriptor["actionChannels"][0]["unit"] = "deg"
elif mutation == "frame":
descriptor["frame"] = "unknown"
elif mutation == "fingerprint":
descriptor["modelFingerprint"] = "not-sha"
else:
descriptor["capabilities"]["training"] = True
with self.subTest(mutation), self.assertRaises(p.RobotError):
p.descriptor(descriptor)
def test_only_local_endpoints_and_token_not_in_url(self):
for endpoint in (
"https://127.0.0.1",
"http://evil.test",
"http://user:token@127.0.0.1",
"http://localhost/?token=secret",
"http://localhost/path",
):
with self.subTest(endpoint), self.assertRaises(ValueError):
SimRobotClient(endpoint, token="test-only-not-a-real-secret")
if __name__ == "__main__":
unittest.main()
+322
View File
@@ -0,0 +1,322 @@
import asyncio
import contextlib
import copy
import json
import time
import unittest
from collections import deque
from pathlib import Path
from aiohttp import ClientSession, WSMsgType, WSServerHandshakeError
from aiohttp.test_utils import TestServer
from mujoco_control_bridge import RobotError, SimRobotClient
from mujoco_control_bridge.server import BROKER, PREFIX, create_app
FIXTURE = json.loads(
(Path(__file__).resolve().parents[2] / "contracts/fixtures/single-joint.json").read_text()
)
TOKEN = "unit-test-token-not-for-real-use"
ORIGIN = "http://127.0.0.1:5173"
class Backend:
"""One-channel numeric peer: no LeKiwi knowledge, never used for physics acceptance."""
def __init__(self, session, url):
self.session, self.url = session, url
self.obs = copy.deepcopy(FIXTURE["observation"])
self.enabled = True
self.generation = 1
self.acknowledge = True
self.calls = []
async def connect(self):
self.ws = await self.session.ws_connect(self.url + "/ws/control/v1", origin=ORIGIN)
await self.ws.send_json({"type": "auth", "token": TOKEN, "protocolVersion": 1})
assert (await self.ws.receive_json())["type"] == "authenticated"
await self.ws.send_json(
{**self.state(), "type": "register", "descriptor": FIXTURE["descriptor"]}
)
assert (await self.ws.receive_json())["type"] == "ready"
self.task = asyncio.create_task(self.consume())
def state(self):
return {
"type": "state",
"observation": self.obs,
"enabled": self.enabled,
"authorizationGeneration": self.generation,
}
async def push(self):
await self.ws.send_json(self.state())
async def consume(self):
async for raw in self.ws:
if raw.type != WSMsgType.TEXT:
break
msg = json.loads(raw.data)
if msg["type"] == "stop":
self.enabled = False
self.obs["paused"] = True
self.obs["sequence"] += 1
await self.push()
elif msg["type"] == "request":
self.calls.append(msg)
payload, op = msg["payload"], msg["op"]
if not self.acknowledge:
continue
if op == "claim":
result = {k: payload[k] for k in ("sessionId", "modelEpoch", "leaseId")}
elif op == "action":
result = {
k: payload[k]
for k in ("sessionId", "modelEpoch", "leaseId", "actionSeq", "values")
}
result["simTime"] = self.obs["simTime"] + 0.002
self.obs["simTime"] += 0.002
self.obs["sequence"] += 1
self.obs["appliedActionSeq"] = payload["actionSeq"]
elif op == "reset":
self.obs["modelEpoch"] += 1
self.obs["sequence"] = 1
self.obs["paused"] = True
self.obs["simTime"] = 0
self.enabled = False
result = self.obs
else:
self.enabled = False
self.obs["sequence"] += 1
self.obs["paused"] = True
result = {"released": True}
await self.ws.send_json(
{"type": "result", "id": msg["id"], "ok": True, "value": result}
)
await self.push()
async def close(self):
await self.ws.close()
await self.task
class BridgeTests(unittest.IsolatedAsyncioTestCase):
async def asyncSetUp(self):
self.server = TestServer(create_app(TOKEN))
await self.server.start_server()
self.url = str(self.server.make_url("")).rstrip("/")
self.http = ClientSession(headers={"Authorization": f"Bearer {TOKEN}"})
self.backend = Backend(self.http, self.url)
await self.backend.connect()
self.claim_data = {
"sessionId": "sim-test",
"modelEpoch": 2,
"modelFingerprint": FIXTURE["descriptor"]["modelFingerprint"],
}
async def asyncTearDown(self):
await self.backend.close()
await self.http.close()
await self.server.close()
async def claim(self):
response = await self.http.post(self.url + PREFIX + "/lease", json=self.claim_data)
self.assertEqual(response.status, 200, await response.text())
return await response.json()
async def test_malformed_tokens_fail_without_encoding_errors(self):
for token in ("x" * 15, "a" * 4097, "中文" * 16, "a" * 16 + " "):
with self.assertRaises(ValueError):
create_app(token)
with self.assertRaises(ValueError):
SimRobotClient(self.url, token)
response = await self.http.get(
self.url + PREFIX + "/health", headers={"Authorization": "Bearer " + "é" * 20}
)
self.assertEqual(response.status, 401)
async with self.http.ws_connect(self.url + "/ws/control/v1", origin=ORIGIN) as ws:
await ws.send_json({"type": "auth", "protocolVersion": 1, "token": "x" * 16 + "\ud800"})
self.assertEqual((await ws.receive_json())["error"]["code"], "UNAUTHORIZED")
async def test_real_sync_sdk_single_joint_and_reset(self):
client = SimRobotClient(self.url, TOKEN)
desc = await asyncio.to_thread(client.connect)
self.assertEqual(len(desc["actionChannels"]), 1)
result = await asyncio.to_thread(client.send_action, {"slider.position": 40})
self.assertEqual(result["values"], {"slider.position": 1})
obs = await asyncio.to_thread(client.get_observation)
self.assertEqual(obs["values"], {"slider.position": 0.12})
self.assertEqual(obs["appliedActionSeq"], 1)
reset = await asyncio.to_thread(client.reset)
self.assertEqual(reset["modelEpoch"], 3)
self.assertTrue(reset["paused"])
self.assertFalse(client.is_connected)
async def test_single_browser_single_writer_and_stale_action(self):
lease = await self.claim()
conflict = await self.http.post(self.url + PREFIX + "/lease", json=self.claim_data)
self.assertEqual(conflict.status, 409)
other = await self.http.ws_connect(self.url + "/ws/control/v1", origin=ORIGIN)
await other.send_json({"type": "auth", "token": TOKEN, "protocolVersion": 1})
self.assertEqual((await other.receive_json())["error"]["code"], "CONFLICT")
await other.close()
packet = {**FIXTURE["validAction"], **lease}
headers = {"X-Control-Lease": lease["leaseId"]}
good = await self.http.post(self.url + PREFIX + "/action", json=packet, headers=headers)
self.assertEqual(good.status, 200)
stale = await self.http.post(self.url + PREFIX + "/action", json=packet, headers=headers)
self.assertEqual((await stale.json())["error"]["code"], "STALE")
self.assertIsNotNone(self.server.app[BROKER].lease)
async def test_host_origin_auth_and_url_token_rejected(self):
for headers in (
{"Host": "evil.test"},
{"Origin": "http://evil.test"},
{"Authorization": "Bearer wrong"},
):
response = await self.http.get(self.url + PREFIX + "/health", headers=headers)
self.assertEqual(response.status, 401)
response = await self.http.get(self.url + PREFIX + "/health?token=secret")
self.assertEqual(response.status, 400)
with self.assertRaises(WSServerHandshakeError):
await self.http.ws_connect(self.url + "/ws/control/v1", origin="http://evil.test")
ws = await self.http.ws_connect(self.url + "/ws/control/v1", origin=ORIGIN)
await ws.send_json({"type": "auth", "token": "wrong", "protocolVersion": 1})
self.assertEqual((await ws.receive_json())["error"]["code"], "UNAUTHORIZED")
await ws.close()
self.assertIsNotNone(self.server.app[BROKER].descriptor)
async def test_watchdog_and_old_authorization_cannot_reconnect(self):
await self.claim()
await asyncio.sleep(0.62)
self.assertIsNone(self.server.app[BROKER].lease)
self.assertFalse(self.backend.enabled)
# Replayed enabled heartbeat with the same generation must not reauthorize.
self.backend.enabled = True
self.backend.obs["paused"] = False
self.backend.obs["sequence"] += 1
await self.backend.push()
response = await self.http.post(self.url + PREFIX + "/lease", json=self.claim_data)
self.assertEqual(response.status, 401)
self.backend.generation += 1
await self.backend.push()
await self.claim()
async def test_old_failed_request_cannot_revoke_a_new_authorization(self):
lease = await self.claim()
self.backend.acknowledge = False
pending = asyncio.create_task(
self.http.post(
self.url + PREFIX + "/action",
json={**FIXTURE["validAction"], **lease},
headers={"X-Control-Lease": lease["leaseId"]},
)
)
await asyncio.sleep(0.03)
self.backend.generation += 1
self.backend.obs["sequence"] += 1
await self.backend.push()
failed = await pending
self.assertEqual(failed.status, 503)
self.assertTrue(self.server.app[BROKER].authorized)
self.assertIsNone(self.server.app[BROKER].lease)
self.backend.acknowledge = True
new_lease = await self.claim()
self.assertNotEqual(new_lease["leaseId"], lease["leaseId"])
async def test_paused_observation_reports_pause_even_after_freshness_expires(self):
broker = self.server.app[BROKER]
self.backend.obs["paused"] = True
self.backend.obs["sequence"] += 1
await broker.state(self.backend.state())
for age in (0.0, 1.0):
with self.subTest(age=age):
broker.observed_at = time.monotonic() - age
client = SimRobotClient(self.url, TOKEN)
with self.assertRaises(RobotError) as raised:
await asyncio.to_thread(client.connect)
self.assertEqual(raised.exception.code, "PAUSED")
self.assertIn("播放", str(raised.exception))
self.assertFalse(client.is_connected)
response = await self.http.post(self.url + PREFIX + "/lease", json=self.claim_data)
self.assertEqual(response.status, 409)
self.assertEqual((await response.json())["error"]["code"], "PAUSED")
self.assertIsNone(broker.lease)
self.assertFalse(self.backend.calls)
async def test_stale_observation_is_not_refreshed_by_heartbeat(self):
await asyncio.sleep(0.53)
await self.backend.push()
response = await self.http.get(self.url + PREFIX + "/observation")
self.assertEqual((await response.json())["error"]["code"], "STALE")
client = SimRobotClient(self.url, TOKEN)
with self.assertRaises(RobotError) as raised:
await asyncio.to_thread(client.connect)
self.assertEqual(raised.exception.code, "STALE")
self.assertFalse(client.is_connected)
self.assertIsNone(self.server.app[BROKER].lease)
async def test_rate_limit_and_http_frame_size(self):
lease = await self.claim()
self.server.app[BROKER].rate = deque([time.monotonic()] * 100)
response = await self.http.post(
self.url + PREFIX + "/action",
json={**FIXTURE["validAction"], **lease},
headers={"X-Control-Lease": lease["leaseId"]},
)
self.assertEqual(response.status, 409)
response = await self.http.post(self.url + PREFIX + "/lease", json={"padding": "x" * 70000})
self.assertEqual(response.status, 413)
async def test_no_ack_cancels_pending_and_cannot_keep_lease(self):
client = SimRobotClient(self.url, TOKEN)
await asyncio.to_thread(client.connect)
self.backend.acknowledge = False
with self.assertRaises(RobotError):
await asyncio.to_thread(client.send_action, {"slider.position": 0.2})
self.assertFalse(client.is_connected)
self.assertFalse(self.server.app[BROKER].pending)
self.assertIsNone(self.server.app[BROKER].lease)
async def test_latest_request_supersedes_pending(self):
lease = await self.claim()
self.backend.acknowledge = False
headers = {"X-Control-Lease": lease["leaseId"]}
one = asyncio.create_task(
self.http.post(
self.url + PREFIX + "/action",
json={**FIXTURE["validAction"], **lease},
headers=headers,
)
)
await asyncio.sleep(0.03)
two = asyncio.create_task(
self.http.post(
self.url + PREFIX + "/action",
json={**FIXTURE["validAction"], **lease, "actionSeq": 2},
headers=headers,
)
)
self.assertEqual((await (await one).json())["error"]["code"], "SUPERSEDED")
self.assertLessEqual(len(self.server.app[BROKER].pending), 1)
self.backend.acknowledge = True
# A pending write receives an explicit failure on disconnect, not a false ACK.
await self.backend.ws.close()
with contextlib.suppress(ConnectionError):
response = await two
self.assertEqual(response.status, 503)
async def test_unauthenticated_ws_timeout_and_oversize(self):
ws = await self.http.ws_connect(self.url + "/ws/control/v1", origin=ORIGIN)
error = await ws.receive_json(timeout=6)
self.assertEqual(error["type"], "error")
await ws.close()
# A too-large peer cannot replace the current backend.
ws = await self.http.ws_connect(self.url + "/ws/control/v1", origin=ORIGIN)
await ws.send_str("x" * 70000)
await ws.receive(timeout=1)
await ws.close()
self.assertIsNotNone(self.server.app[BROKER].descriptor)
if __name__ == "__main__":
unittest.main()
+111
View File
@@ -0,0 +1,111 @@
# 机器人接口 V1:实时控制与任务层分离
## 能力与兼容矩阵
| 层 | 已验证内容 | 不支持 / 不承诺 |
| ------------------------------------ | ------------------------------------------------------------------------------- | ----------------------------------------------- |
| `RobotDescriptor/Action/Observation` | 通用 SI 标量、单关节与 LeKiwi 后端 | 任意远程可执行插件 |
| LeKiwi profile v1 | 三轮底盘、五个臂关节、夹爪、实测反馈、reset | 实机标定、可靠抓取、崎岖地形或 sim-to-real 保证 |
| `lerobot_robot_mujoco` | 实际 LeRobot 0.6.1 的 Robot/Config、发现、工厂、九维控制循环 | 冒充硬件 ZMQ 服务、官方所有 CLI 原样运行 |
| 仿真 | 官方浏览器 MuJoCo WASM 3.11.0 | 用原生 MuJoCo 测试代替浏览器物理验收 |
| 环境 | Ubuntu 24.04 x86_64、Python 3.12、CPU torch 2.11.0+cpu / torchvision 0.26.0+cpu | 修改既有训练 `.venv` 或加载物理机器人 |
| V1 能力标志 | `lockstep=false`、`cameras=false`、`training=false` | 相机、LeRobot 数据采集、Gym/RL 训练 |
普通 MJCF/URDF、Pyodide 控制器和 Go2 ONNX/训练链保持独立。未知模型不会因关节数相似自动套用 LeKiwi。平台自己的有界遥测 CSV/JSON 仍可使用,但不是 LeRobot dataset。
## 运行链路
```text
LeRobot Robot / 通用 SimRobotClient(同步 HTTP)
→ 本机 aiohttp broker(认证、租约、限流)
→ 浏览器 ExternalControlClient(WS,仅数值消息)
→ RobotRuntime(校验、最新目标邮箱、watchdog)
→ MuJoCoRobotAdapter + ModelBindings
→ 固定 dt 的 mj_step → 实测 observation / applied ACK
```
`ModelBindings` 缓存关节/执行器名称、qpos/dof 地址和标量传动约束,复用于 Python 和 Go2 绑定。Embind 临时查询句柄及时释放;关节 ID 不直接当作 qpos/dof 地址。
`ControlArbiter` 只有 `manual | python | policy | external` 一个所有者。每次 claim 创建新的身份票据,旧 Python `dispose/step`、旧 ONNX 结果或旧 socket 回调不能覆盖新控制者。非所有者退出不清除另一个控制者的目标。
外控使用同一 JS 线程上的约 120 Hz 物理调度器,仍按模型固定 dt 积分并遵循单次 8 ms / 100 步追帧预算;网络事件只入队,不在物理步内 await。普通手动/Python/ONNX 保留原渲染驱动步进。观测约 30 Hz,WS 独立发送;React 完整快照约 5 Hz,不用 UI 帧率充当控制/观测时钟。
外控绘制上限 30 FPS。检测到 SwiftShader/llvmpipe 等软件渲染时,profile 关闭阴影,外控绘制上限 5 FPS,为物理/通信留出主线程时间;几何和接触动力学不变。仍非硬实时:主线程卡顿/系统休眠时不能保证及时回调,恢复后先拒绝过期动作。
## 协议与身份
权威结构:[JSON Schema](../contracts/robot-v1.schema.json)、[共享 fixture](../contracts/fixtures/single-joint.json)。不要根据本页重新发明字段名称。
- descriptor:`protocolVersion=1`、`profileId/profileVersion`、`modelFingerprint`、`frame`、`actionChannels/observationChannels`、`capabilities`。
- 通道:`id/unit/min/max/mode`。只接受有限标量;未知字段、额外通道、重复通道、bool、NaN/Infinity 被拒绝。
- 身份:`sessionId + modelEpoch + leaseId`。重置递增 epoch;重载新建 session;租约只对当前授权代次有效。
- action:身份、`protocolVersion=1`、递增 `actionSeq` 和完整 `values`。
- observation:`protocolVersion/sessionId/modelEpoch/sequence/simTime/appliedActionSeq/paused/values`。`sequence` 可在新 epoch 中重启,同 epoch 不可倒退。
- action result:身份、`actionSeq/simTime/values`。它在物理步之后确认 **接受的目标**,不是实际位置;实际状态必须读 observation。
### HTTP 与 WS
HTTP 前缀 `/api/control/v1`,均需要 `Authorization: Bearer …`:
| 方法 / 路径 | 作用 |
| ------------------ | --------------------------------------------- |
| GET `/health` | 本机桥状态 |
| GET `/robot` | 当前 descriptor |
| GET `/observation` | 新鲜的实测状态 |
| POST `/lease` | 用 session/epoch/fingerprint 申请单写入者租约 |
| DELETE `/lease` | 释放,带 `X-Control-Lease` |
| POST `/action` | 完整 action,带 `X-Control-Lease` |
| POST `/reset` | 请求新 epoch 的暂停状态,带 `X-Control-Lease` |
WS `/ws/control/v1`:先发送 `{type:"auth",token:…}`;注册 descriptor/状态,再交换带 request id 的 claim/action/release/reset RPC、结果和状态更新。以 `ExternalControlClient.ts` 和 `server.py` 为实现依据,SDK 用户无需手写 WS。
错误码:`INVALID_MESSAGE`、`INCOMPATIBLE_MODEL`、`UNSUPPORTED`、`UNAUTHORIZED`、`CONFLICT`、`STALE`、`PAUSED`、`TIMEOUT`、`SUPERSEDED`、`DISCONNECTED`。缺能力显式失败,不伪造零图像或假训练支持。
## LeKiwi 映射与校验
唯一参数源:[lekiwi-v1.json](../robot_profiles/lekiwi-v1.json),Python 包通过 package-data 发布同一份 JSON。
- canonical 机体系:X 前、Y 左、Z 上;CAD +Y 前向经 -π/2 绕 Z 转换。
- 五个臂角:LeRobot 度 ↔ profile 符号/零偏修正后的 rad;夹爪 0–100 ↔ ratio 0–1 ↔ 关节 -0.18…0.9 rad。CAD 夹爪轴反向,使递增开度对应物理张开;闭合限位不再允许两指交叉。
- 底盘 `x.vel/y.vel` 为 m/s,LeRobot `theta.vel` 为 deg/s,通用接口为 rad/s。
- 轮顺序 left/back/right,半径 0.05 m、基座半径 0.125 m;最大 4.601942363656923 rad/s,三轮同比缩放而非分别削顶。
- LeRobot 九维反馈使用实测轮速里程计以兼容上游;通用 observation 另有真实机身位姿/速度,因此碰撞/打滑时两者可能不同。
- LeRobot 部分动作仅保持最后 **确认** 的臂/夹爪目标,省略底盘速度为零。连接初始保持值来自实测观测。拒绝 `use_degrees=False`、相机配置、物理校准目录以及非法 scalar。
URDF 和整臂 18 个视觉 STL 必须匹配固定来源 SHA-256;导出 MJCF 必须含受支持的 profile、version、source SHA、`platform_lekiwi_collision_revision=4` 和当前 `platform_lekiwi_collision_recipe_sha256`(旧配方,包括 revision 3,必须从原始 URDF 重新转换)。再校验编译后的浮动根、关节/执行器、传动、轴向、限位、增益和力限等。缓存编辑继承当前显式 profile,校验失败保留旧场景但不恢复外部授权。
启用机器人 profile 后,关节角仅作实测显示:禁止 `setJointPosition()` 或 `resetJoints()` 直接写 qpos,禁用关节拖动/单独重置关节,也禁止忽略关节限位;完整仿真 reset 的撤权/epoch 行为不变。旧滑条是暂停时的姿态编辑而非物理运动,任何碰撞体都不能防止这种直接插入。需要运动时播放仿真,通过执行器目标或外控驱动;普通非 profile 模型的姿态编辑不变,外控仍排斥手动执行器写入。
`modelFingerprint` 是最终输入 XML 字节的 SHA-256,不是语义哈希或所有 mesh 的合并内容哈希;资源另有 `source-manifest.json`。导出保留 `platform_robot_profile`、`platform_robot_profile_version`、`platform_robot_source_sha256`,并添加 `platform_robot_source_fingerprint` 追溯已加载源。重新序列化/导入会重新计算指纹;标记不是签名,也不能代替运行时校验。需要限定精确模型时设置插件的 `expected_model_fingerprint`。
## 停止、冻结与回滚
超过 500 ms 没有有效动作、观测过期、用户暂停/停止、控制者离线、页面隐藏/退出、模型替换都会撤销 lease/授权,清除待应用目标,停止轮目标、保持实测臂姿态并暂停。重新出现的 WS 事件也不能续期已经过期的租约;第一恢复物理步再次检查期限。心跳不等于新观测。
`authorizationGeneration` 隔离旧 release/stop/error/reset 的清理回调。新授权后迟到的旧消息不能把新 lease 清掉。reset 的 observation 序号在新 epoch 中可以从 1 重新开始;同 epoch 回退仍然拒绝。
浏览器 token 只在当前页面内存。本机 bearer token 不防已经获得 token 的本机恶意进程;不应开放到局域网/公网。详见 [桥接安全边界](../control_bridge/README.md)。
## 验证、性能与升级
- 普通 CI:合成单关节真实 SDK→桥接→浏览器 WASM;协议、身份、超时、队列和现有功能测试。不下载 LeKiwi,不安装 LeRobot。
- 独立 `lekiwi-compatibility` CI:固定资产 revision、固定 CPU 环境、实际上游发现/工厂、完整物理和工作台测试;依赖缺失即失败,不以 skip 通过。
- revision 4 工作台实测(本机 Ubuntu 24.04、WASM 3.11.0、SwiftShader):60.022 s / 1800 次动作,仿真推进约 59.636 s;整轮“观测→动作确认→诊断回读”平均 22.6 ms、P95 75.2 ms、最大 151.9 ms;最大观测仿真时间间隔 136 ms,超时/丢弃为 0。外控活跃样本的软件渲染平均约 4.90 FPS;绘制间累计物理耗时最高 139.6 ms,60 次采样中 59 次出现 `overBudget`(至少一次调度仍有待追帧时间),不能宣称无超预算或硬实时。热身后 JS 堆采样约 132.56 MiB、WASM 堆容量 564592640 字节,首末采样无增长。完整碰撞与 1 ms 步长比旧简化模型开销更大;500 ms 看门狗、8 ms/100 步调度预算不变,这不是跨机器性能保证。
- 专用测试记录逐秒 FPS、step budget、JS 堆、WASM 堆容量及实际机体移动。JSON、物理轨迹和截图输出到 `build/e2e/lekiwi/`;堆容量不等同于实际已用内存,60 秒稳定不等于长期无泄漏证明。
- revision 4 从整臂 18 个视觉网格(包括焊接舵机/附件)离线 CoACD 生成 538 个凸包,再按配合区边界切分成 1,220 个碰撞凸包,替代旧胶囊/手工分段,显式启用 5,581 对相邻结构接触。仅保留有几何边界的装配配合例外,不整对排除相邻 body。生成/缓存、固定依赖、来源/许可证与审核边界见 [碰撞数据说明](../robot_profiles/NOTICE.md)。
- `Mirror / Square` 回归使用实际可达的肩旋 0/±0.8 rad:肘目标 −1.3 rad 被挡在约 −0.07953 rad,接触点离轴约 71.8 mm(不是轴承锁死);峰值软穿入约 0.158 mm,稳定约 0.046 mm。反向到 +0.3 rad 后实测约 +0.29135,接触解除。
- 上臂/臂座组件测试中,安装板或肩部结构夹片先于 `Base_08q` 本体阻挡:抬升目标 +0.6 rad,实测约 +0.119(肩旋 0)/+0.243(肩旋 ±0.8);反向到 −0.3 后实测约 −0.2913、接触解除。完整几何下部分抬升/腕俯仰目标会提前遇阻;没有缩小声明的关节范围来隐藏问题。肩旋配合区跨边界凸包切分后,±0.942 rad 双向扫掠实测可达且无配合区接触,不再把轴承锁死冒充结构阻挡。
- 夹爪闭合/半开/全开的指间距约 1.47/41.10/64.88 mm,三者无指间穿入接触,张开空隙不误封堵;静态 16 mm 球体阻挡闭合,峰值软穿入约 0.320 mm、稳定低于 0.020 mm。间距使用独立三角面距离 oracle:WASM 3.11 的一次薄凸包 `mj_geomDistance` 查询返回零,但两见证点相距超过 50 mm,不能拿该零值作真实间距;没有为通过测试而改 solver 或放宽间隙断言。
- 另有原始 STL 六向极值的 108 次覆盖探针、六关节各两个目标(上下限的 60%)的 0.4 rad/s 扫掠及数值稳定性检查。这些是有限场景验证,不是全表面几何误差证明、所有关节组合的穷举、自动避障或可靠抓取保证。
- 浏览器真实物理包括 30 秒站稳、±0.1 m/s 两秒位移约 0.195–0.199 m、双向转动、臂/夹爪以及墙体阻挡;另外测试真实 PTY 键盘输入、Pyodide 旧回调、重载/缓存回滚、reset、崩溃、隐藏事件、CDP 暂停整个 JS 执行后恢复。
- 本次整臂回归:110 个文件 / 482 个 Vitest、7 个离线生成器单测;专用 LeKiwi 物理/插件/工作台 **14 个 E2E 全部通过,无跳过**,最终证据在 `build/e2e/lekiwi-full-collision-final/`。16 个桥接、19 个 LeRobot/键盘 Python 单测通过,命令见示例 README。旧普通浏览器与训练服务覆盖率记录不是本次重新执行结果,不混作当前碰撞验收。
- 已知限制:开发诊断强制注入严重重叠 qpos 后的原生几何查询曾触发 WASM 2 GiB 上限/中止;不是受支持的运动路径,未修复引擎对任意穿透初态的健壮性。正常入口已禁止 profile qpos 瞬移,不应把有限动态回归解释为任意姿态保证。详见碰撞数据说明。
- CI 配置增加隔离的离线分解重建检查;上述数字来自本机执行,不声称已运行远端 GitHub Actions。固定 revision 下载与本地参考目录重建得到相同 ZIP;wheel 已验证包含 canonical profile 和许可证。
升级上游/profile/依赖时:审查许可证与源码差异 → 更新 source hash/版本和映射 → 重建资产及 wheel → 跑跨语言 golden、真实物理、完整 60 秒及旧功能回归 → 更新约束文件、环境记录和 CHANGELOG。不要仅改版本号后绕过模型校验。
## 后续任务 / RL 层(尚未实现)
新增后端需要实现 `RobotAdapter` 的 describe/validateAction/applyAction/readObservation/safeStop/reset/dispose,使用可信代码显式注册;无需修改通用桥接。不要把可执行工厂放入用户导入 JSON。
在其上单独增加 task 层:原生 MuJoCo 后端、`step(action,n_substeps)`、seed/reset、reward/terminated/truncated、Gym wrapper,再做批量训练/策略部署。需要先验证原生与 WASM 的模型、单位、动作语义一致。当前实时浏览器 bridge 不能冒充确定性锁步或高吞吐训练环境。
+143
View File
@@ -0,0 +1,143 @@
# LeKiwi 仿真示例
目标:通过统一机器人接口连接 LeRobot 0.6.1 的上层控制逻辑。此目录不包含实体硬件驱动、相机或 RL 训练任务。
## 可重建模型输入
```bash
source .venv/bin/activate
python examples/lekiwi/prepare_assets.py --source /path/to/LeKiwi
# 或明确允许下载固定版本所需文件(不会克隆整个仓库)
python examples/lekiwi/prepare_assets.py --download
```
生成 `build/lekiwi/lekiwi-v1.zip`、原始 URDF/资源副本、SHA-256 清单及 Apache-2.0 许可证。原参考仓库保持只读;常规前端构建不下载任何资产。
适用源:SIGRobotics-UIUC/LeKiwi 的 `efa608d7ee5a495a4803b1d28cd0c955b4f1e033`;URDF SHA-256 见 `robot_profiles/lekiwi-v1.json`。未知变体拒绝套用。
## 动力学边界
- CAD 轮 STL 单个超过 300,000 个三角形,超出 MuJoCo 的 STL 上限。因此显式 profile 在中间编译前替换这些几何,之后生成简化轮毂与每轮 12 个被动滚子;不是保留高多边形轮子的视觉模型。
- 九个主执行器:5 个臂位置伺服、夹爪位置伺服、3 个轮速度伺服;另外 36 个滚子 hinge **不加电机**。仅通过物理接触驱动车体,不强写底盘位姿/速度。
- CAD +Y 朝前,在 canonical 基座中旋转 -90°。简化三轮按理想 0.125 m 轮距、0.05 m 半径布置;并非沿用 CAD 不等距的轮轴位置。
- 原始 CAD 的 17.324 kg 惯性估计不用于动力学。仿真采用 2.2 kg 基座、按 profile 给出的臂段/轮质量、简化碰撞和保守限位。惯性、摩擦、增益均为**仿真估计**,不代表实体标定。
- 当前碰撞配方为 **revision 4**:从整臂 18 个原始视觉网格(含固定舵机/附件)离线 CoACD 分解成 538 个凸包,再按轴承配合区切分成 1,220 个独立碰撞凸包,替代旧臂胶囊和手工分段。保持视觉坐标变换、夹爪空隙、零附加碰撞质量与显式惯性。工具隔离在 `build/venvs/collision`,浏览器仅使用生成数据。来源、许可证、缓存/参数和重建方法见 [碰撞数据说明](../../robot_profiles/NOTICE.md)。
- 不再整对排除相邻连杆:显式启用 5,581 对结构凸包接触,包括 `SO_ARM100_08k_Mirror-v1` / `SO_ARM100_08k_116_Square-v1`。跨区域凸包先切分,只对完全落入关节轴承/舵机配合区域的部分保留局部例外,以免装配重叠锁死关节;非相邻碰撞保持启用。细化为 1 ms 物理步长,未放宽 500 ms 安全阈值。凸分解仍有误差,不等于逐三角形精确碰撞或所有姿态无穿入保证。
- 自碰撞会阻挡不可能到达的目标,动作确认值不等于实测关节角;遇阻时目标/实测不一致是正常反馈。这不是自动避障或无碰撞路径规划,也没有关闭全臂自碰撞、缩小肩关节范围来隐藏问题。
- CAD 夹爪轴已反向,使开度增大确实张开;内部关节范围为 -0.18…0.9 rad,LeRobot 的 0–100 开度接口不变。当前配方闭合端测试间隙约 1.47 mm,另有指面间接触保护;不会通过关闭自碰撞来允许两指交叉。
- 平地低速控制、静态障碍阻挡和夹爪指尖/物体接触经过真实浏览器 WASM 验证;不保证崎岖地形、可靠抓取、相机、训练或 sim-to-real 策略迁移。MuJoCo 使用软接触,受力时允许小量穿入,瞬态大小取决于速度/载荷;这与缺少碰撞体导致整体穿过不同,高速/薄物体也不属于已验证范围。
### 更新碰撞体后必须重新转换模型
1. 退出 Python 控制脚本,停止外控;刷新前端页面加载新代码。
2. **重新导入 `build/lekiwi/lekiwi-v1.zip` 中的原始 URDF**,显式选择 **LeKiwi v1(仿真专用)**,重新“转换并加载”。源 ZIP 没变,不必重新下载;不要继续使用旧的已转换 MJCF/缓存模型。旧 MJCF 会提示从原始 URDF 重新转换。
3. 在检查器勾选“显示碰撞几何”,可查看橙色凸包,确认覆盖两侧指尖、臂座、`Square`、`Mirror`、其他臂段和固定附件;此开关仅影响显示,不开关物理碰撞。
4. 重新连接桥接、播放并允许外部控制,再启动键盘脚本。`v` 张开、`b` 闭合;可以先用 `--gripper-speed 25` 低速检查。
### 网页关节滑条不是物理运动
旧关节滑条是暂停后直接写 `qpos` 的姿态编辑器,会绕过物理积分,碰撞体不能阻止这种“瞬移”。现在启用机器人 profile 时,**关节角只读、禁用关节拖动/单独重置关节、禁止直接位姿写入和忽略关节限位**;要运动请播放后调整“执行器”目标,或使用键盘/外控。普通非 profile 模型仍保留原姿态编辑功能。“重置仿真”仍可用,并保留撤销外控和 epoch 更新。外控期间继续禁止手动执行器写入。
## 完整演示
从仓库根目录执行:
```bash
source .venv/bin/activate # Python 3.12
python -m pip install -e ./control_bridge
python examples/lekiwi/setup_lerobot.py # 固定 CPU 依赖,仅写 build/venvs/lerobot/
npm ci
npm run dev
```
1. 将 `build/lekiwi/lekiwi-v1.zip` 导入工作台,在 URDF 对话框显式选择 **LeKiwi v1(仿真专用)**,点击“转换并加载”。若已导入可兼容 MJCF,在“控制台 → 开源项目 / 外部控制”选择 profile 并重新编译。
2. 另一终端用 `.venv/bin/python -m mujoco_control_bridge` 启动本机服务。复制启动时显示的 token,在工作台填入 `http://127.0.0.1:8766` 和 token,点击“连接桥接”。
3. 点击“播放”,再点击“允许外部控制”。外控锁定 1×;手动关节/执行器写入不可用,原 Python/ONNX 停止。
4. 第三个终端设置同一 token,运行真正的 LeRobot 插件示例:
```bash
read -rsp '控制 token: ' MUJOCO_CONTROL_TOKEN; echo
export MUJOCO_CONTROL_TOKEN
export MUJOCO_CONTROL_ENDPOINT=http://127.0.0.1:8766
env -u PYTHONPATH PYTHONNOUSERSITE=1 build/venvs/lerobot/bin/python \
examples/lekiwi/demo_control.py --duration 60
```
脚本保留“读状态 → 合成动作 → 发送 → 节拍等待”的上游 30 Hz 循环形状,九个通道一起运动,输出确认目标、实测反馈、时间和延迟统计。不运行硬件构造函数,不初始化串口/相机/GPU。
脚本退出会自动暂停。重新演示必须再次播放并授权;重置、模型重载、页面隐藏/退出、控制进程崩溃和超时也不会自动恢复。
键盘演示(包括从 `demo_control.py` 切换过来)必须再次完成 **播放 → 允许外部控制 → 启动脚本**。授权不会自动播放;确认按钮已变为“暂停”、外控状态为“等待 Python 控制者”,再执行:
```bash
env -u PYTHONPATH PYTHONNOUSERSITE=1 build/venvs/lerobot/bin/python \
examples/lekiwi/teleoperate_sim.py
```
### 底盘 + 机械臂键盘控制
原 LeKiwi 仓库主要提供硬件/CAD/URDF,其 README 的遥操作方案是 **WASD + leader arm**:键盘控制底盘,SO-ARM leader 的六路位置控制从臂。当前仿真已经实现五个臂关节和夹爪的执行器、单位换算与反馈;这里用键盘关节点动替代实体 leader,无需安装硬件驱动或更换桥接;但更新碰撞配方仍须按上文重新转换模型。
在运行脚本的 Linux 交互终端使用小写按键:
| 按键 | 功能 | LeRobot 动作通道 |
| -------------- | ---------------------------------------------- | ----------------------- |
| `w / s` | 底盘前进 / 后退 | `x.vel` |
| `a / d` | 底盘左移 / 右移 | `y.vel` |
| `z / x` | 底盘左转 / 右转 | `theta.vel` |
| `r / f` | 底盘速度升档 / 降档(不改变臂速度) | — |
| `u / j` | 肩部旋转角增大 / 减小 | `arm_shoulder_pan.pos` |
| `i / k` | 肩部俯仰角增大 / 减小 | `arm_shoulder_lift.pos` |
| `o / l` | 肘部角度增大 / 减小 | `arm_elbow_flex.pos` |
| `t / g` | 腕部俯仰角增大 / 减小 | `arm_wrist_flex.pos` |
| `y / h` | 腕部旋转角增大 / 减小 | `arm_wrist_roll.pos` |
| `v / b` | 夹爪张开 / 闭合 | `arm_gripper.pos` |
| 空格 | 清除按键脉冲,底盘停止、机械臂保持最后确认目标 | — |
| `q` / `Ctrl+C` | 退出、释放控制权并暂停仿真 | — |
- 臂按键是**关节空间点动**,不是末端 XYZ/逆运动学控制。五个关节使用度,夹爪使用 0–100 开度;角度正负不代表相机画面中的上下左右。
- 初始目标来自实测姿态,不会启动即归零。每轮 30 Hz 将底盘速度与臂增量合成一条动作;关节/夹爪限幅由现有 profile 执行,下一轮从**确认后的目标**累加,避免到限位后积累不可见的超限目标。
- 默认臂速度为 20 度/秒、夹爪速度为 50 百分点/秒,可用下列参数调慢(上限分别为 90 和 100,必须大于零)。控制循环卡顿不会补发大幅角度跳变。
```bash
env -u PYTHONPATH PYTHONNOUSERSITE=1 build/venvs/lerobot/bin/python \
examples/lekiwi/teleoperate_sim.py --arm-speed 5 --gripper-speed 25
```
终端输入是 **180 ms 脉冲**,不是系统级按下/松开监听;点按为小步调整,长按依赖系统键盘重复,组合键只是重叠脉冲。脉冲过期后底盘速度归零、机械臂保持最后确认目标,脚本仍持续发送动作。空格不会退出授权,也不会将机械臂归零;真正停止仿真请按 `q` 或在页面停止外控。请并排显示浏览器和终端,保持仿真页面可见;隐藏/最小化页面会撤销授权。
底盘和机械臂必须由**同一个控制循环**合成动作,不能分别启动两个 Python 控制者;外控期间也不能同时用网页执行器滑条写入。碰到其他臂段/安装板后不会继续到达目标;完整 CAD 下可用运动范围可能比声明的关节上下限窄,遇阻应反向点动,而不是扩大限位或禁用碰撞。
### 后续接实体 leader 的接入点
若要复用上游 SO100/SO101 leader,保留 `lekiwi_sim` 作为被控机器人,只替换机械臂输入源:在 leader 已连接、完成硬件校准且 `use_degrees=True` 的前提下,将 `leader.get_action()` 的 `shoulder_pan.pos` 等六路键名加 `arm_` 前缀,再与底盘动作合并后调用同一个 `robot.send_action()`。不要启动实体 LeKiwi follower/ZMQ 服务,也不要把归一化的 -100…100 臂角当成度。
这只是接口接入说明,当前终端脚本未实现串口 leader 模式,也未做硬件验证;接入前还需核对真实 leader 与仿真 profile 的零位/方向、限位和每步速度限制。当前模型不保证可靠抓取或 sim-to-real 一致性。
## 连接时提示“机器人观测超过 500ms 未更新”
这表示本机桥接可访问,但未收到浏览器端新的机器人观测;不是 LeRobot 安装或 Python 导入错误。桥接连接成功不等于仿真正在运行。
1. 将浏览器和终端并排显示,保持仿真页面可见,不要切换到其他浏览器标签、最小化或完全遮住仿真窗口。页面隐藏会撤销授权;若显示“未连接”,先重新连接桥接。
2. 查看“机器人观测状态”。若显示“已暂停(非实时观测)”或播放按钮仍为“播放”,先点击 **播放**,再点击 **允许外部控制**。前一个脚本退出、停止外控或超时后都会暂停,不能只重新运行 Python。
3. 确认仿真时间持续增加、观测年龄低于 500 ms,状态显示“等待 Python 控制者”,然后在终端重跑脚本。等待 Python 连接本身没有 500 ms 倒计时;这个阈值检查的是观测新鲜度,取得租约后还会检查动作是否持续更新。
4. 若正在播放且页面可见时观测仍持续过期,检查页面错误和主线程卡顿;必要时重新加载模型并连接、播放、授权。不要通过增大 SDK HTTP 超时来绕过:HTTP 超时与 500 ms 观测/动作安全看门狗是两回事。
旧版桥接会将暂停超过 500 ms 也报成“观测过期”;修复后会明确提示“仿真已暂停;请先在浏览器点击‘播放’,再点击‘允许外部控制’”。更新代码后需重启 `python -m mujoco_control_bridge`,并在页面重新连接、播放、授权;若服务生成了新 token,请同时更新浏览器和终端中的 token。
## 回归和证据
```bash
source .venv/bin/activate
npm run test:control-bridge
build/venvs/lerobot/bin/python -m unittest discover -s integrations/lerobot/tests -v
LEROBOT_PYTHON="$PWD/build/venvs/lerobot/bin/python" npm run test:e2e:lekiwi
# 仅物理测试(不需 LeRobot)
npx playwright test -c web_platform/playwright.lekiwi.config.ts lekiwi.physics.spec.ts lekiwi.gripper.spec.ts lekiwi.armCollision.spec.ts lekiwi.fullCollision.spec.ts
```
专用套件不是 mock:整臂 18 个视觉网格的 108 次独立 CAD 表面接触探针,六关节各两个有界目标扫掠,`Mirror/Square` 主体接触阻挡/脱离及禁止直接 `qpos` 写入;真实 WASM 3.11.0 的 30 仿真秒站稳、正负三轴运动、臂/夹爪、墙体阻挡、两侧 CAD 指尖探针接触、开度与实际指间距同向、张开空隙不误碰撞、闭合被物体阻挡以及指面自碰撞、上臂下压时与自身臂座组件的真实接触/穿入量及反向脱离(完整模型中安装板或肩部结构夹片先于底座本体接触);真实 Python/LeRobot、PTY 终端底盘 + 六路臂/夹爪正反点动、无输入保持与退出撤权、完整工作台文件导入→编译→授权→60 秒控制;另含旧控制回调、reset、新 epoch、崩溃、重载/回滚、隐藏和 JS 冻结恢复。
`build/e2e/lekiwi/` 保存 XML、物理轨迹、60 秒 JSON(RTT、新鲜度、FPS/步进预算、JS/WASM 堆容量)和截图。普通 E2E 不下载资产或安装 LeRobot;专用 CI 缺依赖即失败,不静默跳过。
软件 WebGL 比物理计算慢:外控物理/传输独立调度;检测到 SwiftShader 等软件渲染时关闭阴影、3D 显示上限 5 FPS,观测仍约 30 Hz。不能把此模式当作锁步 RL 或硬实时系统。性能实测、兼容矩阵和限制见 [机器人接口](../../docs/robot-interface.md)。
+98
View File
@@ -0,0 +1,98 @@
"""Hardware-free LeRobot loop: observe -> compose action -> send, at 30 Hz.
Only the robot implementation and input source differ from the upstream example.
This is real-time control, NOT lockstep RL or dataset recording.
"""
import argparse
import json
import math
import os
import time
from importlib.metadata import version
from lerobot.robots.config import RobotConfig
from lerobot.robots.utils import make_robot_from_config
from lerobot.utils.import_utils import register_third_party_plugins
def make_robot(endpoint):
register_third_party_plugins()
config_type = RobotConfig.get_choice_class("lekiwi_sim")
return make_robot_from_config(config_type(endpoint=endpoint, id="lekiwi-sim-demo"))
def run_demo(endpoint, duration=10.0):
if not math.isfinite(duration) or not 1 <= duration <= 3600:
raise ValueError("演示时长必须为1–3600秒")
robot = make_robot(endpoint)
robot.connect()
try:
start = time.monotonic()
first = robot.get_sim_observation()
latencies, gaps, max_motion = [], [], 0.0
steps = math.ceil(duration * 30)
previous_sim_time = first["simTime"]
for i in range(steps):
tick = time.monotonic()
observation = robot.get_observation()
t = i / 30
action = {
"arm_shoulder_pan.pos": 10 * math.sin(t),
"arm_shoulder_lift.pos": 6 * math.sin(t + 0.2),
"arm_elbow_flex.pos": 8 * math.sin(t + 0.4),
"arm_wrist_flex.pos": 6 * math.sin(t + 0.6),
"arm_wrist_roll.pos": 12 * math.sin(t + 0.8),
"arm_gripper.pos": 50 + 25 * math.sin(t),
"x.vel": 0.05 * math.cos(t),
"y.vel": 0.05 * math.sin(t),
"theta.vel": 10 * math.sin(t / 2),
}
accepted = robot.send_action(action)
platform = robot.get_sim_observation()
latencies.append((time.monotonic() - tick) * 1000)
gaps.append(platform["simTime"] - previous_sim_time)
previous_sim_time = platform["simTime"]
max_motion = max(
max_motion,
math.hypot(
platform["values"]["base.x"] - first["values"]["base.x"],
platform["values"]["base.y"] - first["values"]["base.y"],
),
)
time.sleep(max(0, start + (i + 1) / 30 - time.monotonic()))
ordered = sorted(latencies)
return {
"lerobot": version("lerobot"),
"torch": version("torch"),
"steps": steps,
"elapsedSeconds": time.monotonic() - start,
"simSeconds": platform["simTime"] - first["simTime"],
"rttMeanMs": sum(latencies) / len(latencies),
"rttP95Ms": ordered[int(0.95 * (len(ordered) - 1))],
"rttMaxMs": max(latencies),
"maxObservationSimGap": max(gaps),
"maxTranslationM": max_motion,
"observation": observation,
"accepted": accepted,
"sequence": platform["sequence"],
"appliedActionSeq": platform["appliedActionSeq"],
"timeouts": 0,
"droppedRequests": 0,
}
finally:
robot.disconnect()
def main():
parser = argparse.ArgumentParser(description="LeKiwi 仿真:先播放并在浏览器显式授权")
parser.add_argument(
"--endpoint", default=os.environ.get("MUJOCO_CONTROL_ENDPOINT", "http://127.0.0.1:8766")
)
parser.add_argument("--duration", type=float, default=10)
args = parser.parse_args()
print(json.dumps(run_demo(args.endpoint, args.duration), ensure_ascii=False))
if __name__ == "__main__":
main()
@@ -0,0 +1,51 @@
"""Offline CAD collisions for the fixed arm pedestal and shoulder-lift link.
Use the same source-bound triangle clipping as the jaw recipe. Separate convex
slices avoid spanning the pedestal's stepped silhouette or the link's full length
with one broad hull. These are conservative parts, not triangle-mesh collisions.
"""
import argparse
import json
from pathlib import Path
from generate_gripper_collisions import ROOT, generate
# Original STL coordinates, millimeters; seams overlap by 1 mm.
PARTS = [
("base_foot", "Base_08q-v1", [(1, 1, 16.0)]),
("base_column", "Base_08q-v1", [(1, -1, -15.0), (1, 1, 59.0)]),
("base_crown", "Base_08q-v1", [(1, -1, -58.0)]),
("upper_arm_end", "SO_ARM100_08k_116_Square-v1", [(0, 1, -28.0)]),
("upper_arm_beam", "SO_ARM100_08k_116_Square-v1", [(0, -1, 29.0), (0, 1, 50.0)]),
("upper_arm_hinge", "SO_ARM100_08k_116_Square-v1", [(0, -1, -49.0)]),
]
SOURCE_HASHES = {
"Base_08q-v1.stl": "a05be37db52657615ac69423fe577979efa0cd0634a5e9546bbc85537f003172",
"SO_ARM100_08k_116_Square-v1.stl": (
"64fdc308759ff58756e3c39a0ea8018d6b37b551bc3d29238b6e86bc5acae666"
),
}
def main():
parser = argparse.ArgumentParser(description="从固定版本 STL 重建 LeKiwi 底座/上臂碰撞体")
parser.add_argument("--source", type=Path, default=ROOT / "build/lekiwi/URDF/meshes")
parser.add_argument(
"--output", type=Path, default=ROOT / "robot_profiles/lekiwi-arm-collision.json"
)
parser.add_argument("--check", action="store_true", help="只验证已签入的碰撞数据可重建")
args = parser.parse_args()
result = generate(args.source, definitions=PARTS, source_hashes=SOURCE_HASHES, revision=3)
if args.check:
if json.loads(args.output.read_text()) != result:
raise ValueError("底座/上臂碰撞数据与固定 CAD/生成器不一致")
else:
args.output.parent.mkdir(parents=True, exist_ok=True)
args.output.write_text(json.dumps(result, indent=2) + "\n")
status = "验证通过" if args.check else args.output
print(f"LeKiwi 底座/上臂碰撞体:{len(result['parts'])} 个分段凸包,{status}")
if __name__ == "__main__":
main()
+494
View File
@@ -0,0 +1,494 @@
"""Offline collision-from-visuals cooking for the complete LeKiwi arm subtree.
Run in build/venvs/collision, NOT the training or LeRobot environment. CoACD is
never imported by the browser/bridge. Each convex part is a separate MJCF mesh.
"""
import argparse
import hashlib
import importlib.metadata
import json
import os
import xml.etree.ElementTree as ET
from pathlib import Path
# Fix the numerical execution environment before importing numpy/CoACD.
os.environ["OMP_NUM_THREADS"] = "4"
os.environ["OPENBLAS_NUM_THREADS"] = "1"
ROOT = Path(__file__).resolve().parents[2]
CONFIG = ROOT / "robot_profiles/lekiwi-collision-source.json"
VERSIONS = {"coacd": "1.0.14", "trimesh": "4.12.2", "numpy": "2.1.3", "scipy": "1.17.0"}
PARAMETERS = {
"threshold": 0.003,
"real_metric": True,
"preprocess_mode": "auto",
"preprocess_resolution": 60,
"resolution": 1000,
"mcts_nodes": 12,
"mcts_iterations": 60,
"mcts_max_depth": 3,
"decimate": True,
"max_ch_vertex": 96,
"seed": 42,
}
def digest(data):
return hashlib.sha256(data).hexdigest()
def visuals(source, config):
"""Select every visual in the subtree, including welded motors/accessories."""
urdf = (source / "LeKiwi.urdf").read_bytes()
if digest(urdf) != config["urdfSha256"]:
raise ValueError("URDF 与固定版本不符")
robot = ET.fromstring(urdf)
parents = {
j.find("child").get("link"): j.find("parent").get("link") for j in robot.findall("joint")
}
result = []
for link in robot.findall("link"):
ancestor = link.get("name")
visited = set()
while ancestor in parents and ancestor != config["rootLink"]:
if ancestor in visited:
raise ValueError("URDF 运动链存在环")
visited.add(ancestor)
ancestor = parents[ancestor]
if ancestor != config["rootLink"]:
continue
for visual in link.findall("visual"):
mesh = visual.find("geometry/mesh")
if mesh is None:
raise ValueError("当前固定资产只支持 STL 视觉网格,不能静默遗漏几何")
filename = mesh.get("filename")
path = (source / filename).resolve()
if not path.is_relative_to(source.resolve()) or filename not in config["meshes"]:
raise ValueError(f"未批准的源网格:{filename}")
sha = digest(path.read_bytes())
if sha != config["meshes"][filename]:
raise ValueError(f"源网格 SHA-256 不匹配:{filename}")
scale = [float(x) for x in mesh.get("scale", "1 1 1").split()]
if len(scale) != 3 or not all(0 < x < float("inf") for x in scale):
raise ValueError("无效的视觉网格缩放")
result.append(
{
"link": link.get("name"),
"visual": visual.get("name"),
"mesh": filename,
"sha256": sha,
"scale": scale,
}
)
if not result or {v["mesh"] for v in result} != set(config["meshes"]):
raise ValueError("整臂视觉覆盖清单不完整")
return result
def swept_bounds(vertices, axis):
"""Conservative axial/radial intervals for a full revolution, not pose sampling."""
import numpy as np
from scipy.spatial import ConvexHull
axial = vertices @ axis
reference = np.eye(3)[int(np.argmin(np.abs(axis)))]
x = np.cross(axis, reference)
x /= np.linalg.norm(x)
y = np.cross(axis, x)
projected = vertices @ np.column_stack([x, y])
hull = ConvexHull(projected)
radial_min = 0.0
if hull.equations[:, 2].max() > 0:
a = projected[hull.vertices]
b = np.roll(a, -1, axis=0)
edge = b - a
t = np.clip(-(a * edge).sum(axis=1) / (edge * edge).sum(axis=1), 0, 1)
radial_min = float(np.linalg.norm(a + t[:, None] * edge, axis=1).min())
return (
float(axial.min()),
float(axial.max()),
radial_min,
float(np.linalg.norm(projected, axis=1).max()),
)
def intervals_overlap(a, b, margin=0.0001):
# 0.1 mm conservative allowance for serialized/compiled frame precision.
return (
max(a[0], b[0]) <= min(a[1], b[1]) + margin and max(a[2], b[2]) <= min(a[3], b[3]) + margin
)
def assembly_frames(source, config, coverage):
"""URDF joint/visual frames for both geometric partitioning and pair policy."""
import numpy as np
from scipy.spatial.transform import Rotation
robot = ET.parse(source / "LeKiwi.urdf").getroot()
joints = {j.get("name"): j for j in robot.findall("joint")}
parents = {j.find("child").get("link"): j for j in joints.values()}
links = {link.get("name"): link for link in robot.findall("link")}
poses = {}
def origin(node):
result = np.eye(4)
if node is not None:
result[:3, 3] = [float(x) for x in node.get("xyz", "0 0 0").split()]
result[:3, :3] = Rotation.from_euler(
"xyz", [float(x) for x in node.get("rpy", "0 0 0").split()]
).as_matrix()
return result
def pose(link):
if link not in poses:
j = parents.get(link)
poses[link] = (
np.eye(4)
if j is None
else pose(j.find("parent").get("link")) @ origin(j.find("origin"))
)
return poses[link]
def owner(link):
if link == config["rootLink"]:
return "base"
j = parents[link]
return owner(j.find("parent").get("link")) if j.get("type") == "fixed" else j.get("name")
by_visual = {}
for item in coverage:
item["weldJoint"] = owner(item["link"])
visual = next(
v for v in links[item["link"]].findall("visual") if v.get("name") == item["visual"]
)
by_visual[item["visual"]] = (
item["weldJoint"],
pose(item["link"]) @ origin(visual.find("origin")),
)
result = []
for name, core in config["assemblyCores"].items():
if not (0 < core["radiusM"] <= 0.025 and 0 < core["axialHalfExtentM"] <= 0.06):
raise ValueError("装配配合区域超出已审核尺寸预算")
j = joints[name]
parent = owner(j.find("parent").get("link"))
transform = pose(j.find("child").get("link"))
axis = transform[:3, :3] @ np.array([float(x) for x in j.find("axis").get("xyz").split()])
axis /= np.linalg.norm(axis)
result.append((name, parent, transform, axis, core))
return by_visual, result
def joint_policy(source, config, parts, coverage):
"""Bound exceptions by joint-local geometry, never by a whole body pair."""
import numpy as np
by_visual, frames = assembly_frames(source, config, coverage)
result = []
for name, parent, transform, axis, core in frames:
members = []
groups = {parent: {}, name: {}}
for part in parts:
group, visual_pose = by_visual[part["visual"]]
if group not in (parent, name):
continue
vertices = np.fromstring(part["vertices"], sep=" ").reshape(-1, 3)
offsets = vertices @ visual_pose[:3, :3].T + visual_pose[:3, 3] - transform[:3, 3]
bounds = swept_bounds(offsets, axis)
groups[group][part["name"]] = bounds
if (
bounds[3] <= core["radiusM"]
and max(abs(bounds[0]), abs(bounds[1])) <= core["axialHalfExtentM"]
):
members.append(part["name"])
core_set = set(members)
pairs = []
filtered = pruned = 0
for a, bound_a in groups[parent].items():
for b, bound_b in groups[name].items():
if a in core_set or b in core_set:
filtered += 1
elif not intervals_overlap(bound_a, bound_b):
pruned += 1
else:
pairs.append([a, b])
if not pairs:
raise ValueError(f"不能关闭整对相邻连杆碰撞:{name}")
result.append(
{
"joint": name,
"parent": parent,
**core,
"coreParts": members,
"pairs": pairs,
"coreFilteredPairs": filtered,
"sweptPrunedPairs": pruned,
}
)
return result
def clip_convex(vertices, normal, offset):
"""Intersect a convex hull with a half-space, retaining all crossing edges."""
import numpy as np
from scipy.spatial import ConvexHull, QhullError
signed = vertices @ normal - offset
if signed.max() <= 1e-10:
return vertices
if signed.min() >= -1e-10:
return None
hull = ConvexHull(vertices)
result = list(vertices[signed <= 0])
for triangle in hull.simplices:
for i in range(3):
a, b = triangle[i], triangle[(i + 1) % 3]
if signed[a] * signed[b] < 0:
result.append(
vertices[a] + (vertices[b] - vertices[a]) * signed[a] / (signed[a] - signed[b])
)
points = np.unique(np.asarray(result), axis=0)
if len(points) < 4:
return None
try:
# Remove edge/triangulation interpolation points BEFORE rounding;
# rounding collinear points first invents tiny zigzag facet vertices.
points = np.unique(np.round(points[ConvexHull(points).vertices], 6), axis=0)
if len(points) < 4:
return None
hull = ConvexHull(points)
except QhullError:
return None # sub-micrometre cutting sliver collapsed by serialization
if hull.volume <= 1e-15:
return None
return points[hull.vertices]
def split_core(vertices, planes):
"""Partition, not delete: core and every outside fragment retain external contact."""
result = []
pending = vertices
for normal, offset in planes:
outside = clip_convex(pending, -normal, -offset)
if outside is not None:
result.append(outside)
pending = clip_convex(pending, normal, offset)
if pending is None:
return result
result.append(pending)
return result
def partition_assembly_cores(source, config, parts, coverage):
"""Split straddling hulls so a bearing contact cannot lock a structural hull.
A 16-sided inscribed prism stays inside each reviewed cylinder. The 2um
inset keeps rounding from reclassifying its core as structural. No region
is removed; only whole resulting core parts receive local mating exceptions.
"""
import numpy as np
from scipy.spatial import ConvexHull
by_visual, frames = assembly_frames(source, config, coverage)
shapes = [
(part["visual"], np.fromstring(part["vertices"], sep=" ").reshape(-1, 3)) for part in parts
]
for name, parent, transform, axis, core in frames:
x = np.cross(axis, np.eye(3)[int(np.argmin(np.abs(axis)))])
x /= np.linalg.norm(x)
y = np.cross(axis, x)
normals = [np.cos(t) * x + np.sin(t) * y for t in np.arange(16) * (2 * np.pi / 16)] + [
axis,
-axis,
]
distances = [(core["radiusM"] - 0.000002) * np.cos(np.pi / 16)] * 16 + [
core["axialHalfExtentM"] - 0.000002
] * 2
divided = []
for visual, vertices in shapes:
group, pose = by_visual[visual]
if group not in (parent, name):
divided.append((visual, vertices))
continue
origin = pose[:3, 3] - transform[:3, 3]
bounds = swept_bounds(vertices @ pose[:3, :3].T + origin, axis)
if (
(
bounds[3] <= core["radiusM"]
and max(abs(bounds[0]), abs(bounds[1])) <= core["axialHalfExtentM"]
)
or bounds[2] > core["radiusM"]
or bounds[0] > core["axialHalfExtentM"]
or bounds[1] < -core["axialHalfExtentM"]
):
divided.append((visual, vertices))
continue
planes = [
(pose[:3, :3].T @ n, distance - n @ origin)
for n, distance in zip(normals, distances, strict=True)
]
pieces = split_core(vertices, planes)
original = ConvexHull(vertices)
volume = sum(ConvexHull(piece).volume for piece in pieces)
if abs(volume - original.volume) > max(original.volume * 0.01, original.area * 0.00001):
raise ValueError(f"配合区切分体积误差超出微米量化预算:{visual}")
divided.extend((visual, piece) for piece in pieces)
shapes = divided
result = []
for item in coverage:
pieces = [v for visual, v in shapes if visual == item["visual"]]
pieces.sort(key=lambda v: tuple(v.mean(axis=0)))
item["coacdParts"] = item["parts"]
item["parts"] = len(pieces)
if not 1 <= len(pieces) <= 256:
raise ValueError("装配区域分割超出预算")
for i, vertices in enumerate(pieces):
if len(vertices) > 256:
raise ValueError(f"装配分割凸包顶点超出预算:{item['visual']} / {len(vertices)}")
result.append(
{
"name": f"{item['link']}__h{i:03d}",
"visual": item["visual"],
"vertices": " ".join(
f"{value:.6f}" for point in sorted(map(tuple, vertices)) for value in point
),
}
)
return result
def cook(source, config, cache):
import coacd
import numpy as np
import trimesh
from scipy.spatial import ConvexHull
for name, version in VERSIONS.items():
if importlib.metadata.version(name) != version:
raise ValueError(f"请使用离线固定依赖:{name}=={version}")
coacd.set_log_level("info")
parts = []
coverage = []
for visual in visuals(source, config):
key = digest(
json.dumps(
{
"source": visual["sha256"],
"scale": visual["scale"],
"parameters": PARAMETERS,
"versions": VERSIONS,
"threads": 4,
},
sort_keys=True,
).encode()
)
cached = cache / f"{key}.json"
mesh = trimesh.load(source / visual["mesh"], force="mesh")
mesh.apply_scale(visual["scale"])
if cached.exists():
hulls = json.loads(cached.read_text())
else:
print(f"生成 {visual['mesh']}", flush=True)
raw = coacd.run_coacd(coacd.Mesh(mesh.vertices, mesh.faces), **PARAMETERS)
hulls = [sorted(np.round(v, 6).tolist()) for v, _ in raw]
hulls.sort(key=lambda vertices: tuple(np.mean(vertices, axis=0)))
cache.mkdir(parents=True, exist_ok=True)
temporary = cached.with_suffix(".tmp")
temporary.write_text(json.dumps(hulls))
temporary.replace(cached)
if not 1 <= len(hulls) <= 128:
raise ValueError(f"凸包数量超出预算:{visual['mesh']}")
# An explicit sampled coverage diagnostic, not a certified Hausdorff bound.
samples = np.concatenate([mesh.vertices, mesh.triangles_center])[::3]
outside = np.full(len(samples), np.inf)
for index, vertices in enumerate(hulls):
v = np.asarray(vertices)
if (
v.ndim != 2
or v.shape[1] != 3
or not 4 <= len(v) <= PARAMETERS["max_ch_vertex"]
or not np.isfinite(v).all()
):
raise ValueError("凸包顶点损坏或超出预算")
if (v.min(axis=0) < mesh.bounds[0] - 0.002).any() or (
v.max(axis=0) > mesh.bounds[1] + 0.002
).any():
raise ValueError("凸包超出源网格边界预算")
hull = ConvexHull(v)
if hull.volume <= 1e-15:
raise ValueError("退化凸包")
signed_planes = (samples @ hull.equations[:, :3].T + hull.equations[:, 3]).max(axis=1)
outside = np.minimum(outside, signed_planes)
parts.append(
{
"name": f"{visual['link']}__h{index:03d}",
"visual": visual["visual"],
"vertices": " ".join(f"{x:.6f}" for x in v.ravel()),
}
)
if outside.max() > 0.0015:
raise ValueError(f"源表面采样覆盖不合格:{visual['mesh']}")
coverage.append(
{
**visual,
"parts": len(hulls),
"samples": len(samples),
"coacdSampleOutsidePlanesMaxM": float(max(0, outside.max())),
"sourceWatertight": bool(mesh.is_watertight),
"cacheKey": key,
}
)
print(
f"完成 {visual['link']}: {len(hulls)} hulls; sampled plane gap={outside.max():.6g} m",
flush=True,
)
parts = partition_assembly_cores(source, config, parts, coverage)
policy = joint_policy(source, config, parts, coverage)
geometry_sha = digest(json.dumps(parts, sort_keys=True, separators=(",", ":")).encode())
physics = {"timestep": 0.001, "solref": [0.002, 1], "solimp": [0.95, 0.99, 0.001]}
recipe_sha = digest(
json.dumps(
{"geometry": geometry_sha, "policy": policy, "physics": physics},
sort_keys=True,
separators=(",", ":"),
).encode()
)
return {
"revision": 4,
"geometrySha256": geometry_sha,
"recipeSha256": recipe_sha,
"physics": physics,
"jointPolicy": policy,
"source": config,
"generator": {
"versions": VERSIONS,
"parameters": PARAMETERS,
"threads": 4,
"corePartition": {"sides": 16, "insetM": 0.000002},
},
"coverage": coverage,
"parts": parts,
}
def main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument("--source", type=Path, default=ROOT / "build/lekiwi/URDF")
parser.add_argument("--cache", type=Path, default=ROOT / "build/collision-cache")
parser.add_argument(
"--output", type=Path, default=ROOT / "robot_profiles/lekiwi-full-collision.json"
)
parser.add_argument("--check", action="store_true")
args = parser.parse_args()
result = cook(args.source, json.loads(CONFIG.read_text()), args.cache)
if args.check:
if json.loads(args.output.read_text()) != result:
raise ValueError("已生成碰撞数据与源文件/算法参数不一致")
else:
args.output.parent.mkdir(parents=True, exist_ok=True)
args.output.write_text(json.dumps(result, indent=2) + "\n")
print(f"整臂碰撞覆盖 {len(result['coverage'])} 个视觉网格 / {len(result['parts'])} 个凸包")
if __name__ == "__main__":
main()
@@ -0,0 +1,117 @@
"""Offline, deterministic convex parts from the pinned CAD (requires numpy/scipy).
Runtime imports the checked-in JSON only; no extra browser/Python teleop dependency.
Do not use one hull for the entire gripper: that would fill its open jaw space.
"""
import argparse
import hashlib
import json
import struct
from pathlib import Path
import numpy as np
from scipy.spatial import ConvexHull
ROOT = Path(__file__).resolve().parents[2]
# Cuts in the original STL's millimeters, with 1 mm overlap at each seam.
# Each plane keeps sign * coordinate[axis] <= limit.
PARTS = [
("fixed_palm", "Wrist_Roll_08c-v1", [(2, 1, 39.0)]),
("fixed_finger", "Wrist_Roll_08c-v1", [(2, -1, -38.0)]),
("moving_hinge", "Moving_Jaw_08d-v1", [(1, -1, 22.0)]),
("moving_finger", "Moving_Jaw_08d-v1", [(1, 1, -21.0), (1, -1, 60.0)]),
("moving_tip", "Moving_Jaw_08d-v1", [(1, 1, -59.0)]),
]
# Bound to prepare_assets.py's pinned source; never silently regenerate for another CAD.
SOURCE_HASHES = {
"Wrist_Roll_08c-v1.stl": "87507f73f485c2cacb3dc83924a712069fbd68f72b82d4a31a2cfe7f58c9e4c9",
"Moving_Jaw_08d-v1.stl": "71caabee267376791210950b3b4e2f7968d9b57b92ed3f57711c03f2b5666912",
}
def clip_polygon(polygon, axis, sign, limit):
result = []
for previous, current in zip(polygon[-1:] + polygon[:-1], polygon, strict=True):
a = sign * previous[axis] - limit
b = sign * current[axis] - limit
if (a <= 0) != (b <= 0):
result.append(previous + (current - previous) * (a / (a - b)))
if b <= 0:
result.append(current)
return result
def generate(source, *, definitions=PARTS, source_hashes=SOURCE_HASHES, revision=2):
"""Shared CAD-frame convex clipping; callers bind their own source hashes/cuts."""
parts = []
for name, mesh, planes in definitions:
raw = (source / (mesh + ".stl")).read_bytes()
digest = hashlib.sha256(raw).hexdigest()
if digest != source_hashes[mesh + ".stl"]:
raise ValueError(f"不支持的碰撞体 CAD:{mesh}.stl ({digest})")
count = struct.unpack_from("<I", raw, 80)[0]
if len(raw) != 84 + count * 50:
raise ValueError(f"无效的二进制 STL:{mesh}")
records = np.frombuffer(
raw,
offset=84,
dtype=np.dtype(
[
("normal", "<f4", (3,)),
("vertex", "<f4", (3, 3)),
("attribute", "<u2"),
]
),
)
points = []
for triangle in records["vertex"]:
polygon = [point.astype(float) for point in triangle]
for axis, sign, limit in planes:
if polygon:
polygon = clip_polygon(polygon, axis, sign, limit)
points.extend(polygon)
# Clip triangles, not just vertices, so long faces crossing cuts leave no holes.
points = np.unique(np.round(points, 3), axis=0) # 1 micrometer grid in CAD mm.
vertices = points[ConvexHull(points).vertices] / 1000 # MJCF meters.
vertices = vertices[np.lexsort(vertices.T[::-1])]
parts.append(
{
"name": name,
"visual": mesh + "_visual",
"vertices": " ".join(f"{value:.6f}" for value in vertices.flat),
}
)
return {
"revision": revision,
"source": {
"repository": "https://github.com/SIGRobotics-UIUC/LeKiwi",
"revision": "efa608d7ee5a495a4803b1d28cd0c955b4f1e033",
"license": "Apache-2.0",
},
"sources": source_hashes,
"parts": parts,
}
def main():
parser = argparse.ArgumentParser(description="从固定版本 STL 重建 LeKiwi 分段夹爪碰撞体")
parser.add_argument("--source", type=Path, default=ROOT / "build/lekiwi/URDF/meshes")
parser.add_argument(
"--output", type=Path, default=ROOT / "robot_profiles/lekiwi-gripper-collision.json"
)
parser.add_argument("--check", action="store_true", help="只验证已签入的碰撞数据可重建")
args = parser.parse_args()
result = generate(args.source)
if args.check:
if json.loads(args.output.read_text()) != result:
raise ValueError("夹爪碰撞数据与固定 CAD/生成器不一致")
else:
args.output.parent.mkdir(parents=True, exist_ok=True)
args.output.write_text(json.dumps(result, indent=2) + "\n")
status = "验证通过" if args.check else args.output
print(f"LeKiwi 夹爪碰撞体:{len(result['parts'])} 个分段凸包,{status}")
if __name__ == "__main__":
main()
+116
View File
@@ -0,0 +1,116 @@
#!/usr/bin/env python3
"""Rebuild a provenance-bound LeKiwi input ZIP without modifying the source repo."""
import argparse
import hashlib
import json
import urllib.request
import xml.etree.ElementTree as ET
import zipfile
from pathlib import Path, PurePosixPath
ROOT = Path(__file__).resolve().parents[2]
PROFILE = json.loads((ROOT / "robot_profiles/lekiwi-v1.json").read_text())
SOURCE = PROFILE["source"]
MAX_TOTAL = 128 * 1024 * 1024
def digest(data: bytes) -> str:
return hashlib.sha256(data).hexdigest()
def prepare(source: Path | None, output: Path, download: bool = False) -> Path:
if source is not None:
source = source.resolve()
if source.name == "URDF":
source = source.parent
if not (source / "URDF/LeKiwi.urdf").is_file():
raise ValueError("Source must be the LeKiwi repository or its URDF directory")
if output.resolve().is_relative_to(source):
raise ValueError("Output must not modify the reference repository")
elif not download:
raise ValueError("Pass --source or explicitly opt in with --download")
files: dict[str, bytes] = {}
total = 0
def read(relative: str) -> bytes:
nonlocal total
path = PurePosixPath(relative)
if path.is_absolute() or ".." in path.parts or "\\" in relative:
raise ValueError(f"Unsafe asset path: {relative}")
if source is not None:
file = (source / relative).resolve()
if not file.is_relative_to(source) or file.stat().st_size > MAX_TOTAL:
raise ValueError(f"Invalid asset: {relative}")
data = file.read_bytes()
else:
url = f"https://raw.githubusercontent.com/SIGRobotics-UIUC/LeKiwi/{SOURCE['revision']}/{relative}"
with urllib.request.urlopen(url, timeout=60) as response:
data = response.read(MAX_TOTAL + 1)
total += len(data)
if total > MAX_TOTAL:
raise ValueError("Assets exceed 128 MiB")
files[relative] = data
return data
urdf = read("URDF/LeKiwi.urdf")
if digest(urdf) != SOURCE["urdfSha256"]:
raise ValueError("URDF revision/hash is not supported by lekiwi-v1")
tree = ET.fromstring(urdf)
for mesh in sorted({m.get("filename", "") for m in tree.iter("mesh")}):
if not mesh.startswith("meshes/") or not mesh.endswith(".stl"):
raise ValueError(f"Unexpected mesh reference: {mesh}")
read(f"URDF/{mesh}")
read("LICENSE.txt")
read("CITATION.cff")
manifest = {
"profileId": PROFILE["id"],
"profileVersion": PROFILE["version"],
"source": SOURCE,
"files": {p: digest(data) for p, data in files.items()},
}
files["source-manifest.json"] = json.dumps(manifest, indent=2).encode()
files["robot-profile.json"] = json.dumps(
{"id": PROFILE["id"], "version": PROFILE["version"]}
).encode()
files["SIMULATION-NOTICE.md"] = (
"# LeKiwi simulation input\n\nSource: "
+ SOURCE["repository"]
+ "\n\nRevision: "
+ SOURCE["revision"]
+ "\n\nOriginal URDF/STL files are unmodified, Apache-2.0. "
"The platform applies an explicit simulation-only profile after conversion: estimated "
"mass/inertia/limits/servos and simplified passive-roller contacts. Not a calibrated "
"hardware model; no cameras, training or grasping guarantee.\n"
).encode()
output.mkdir(parents=True, exist_ok=True)
for relative, data in files.items():
target = output / relative
if target.is_symlink() or not target.resolve().is_relative_to(output.resolve()):
raise ValueError("Unsafe output path")
target.parent.mkdir(parents=True, exist_ok=True)
target.write_bytes(data)
archive = output / "lekiwi-v1.zip"
if archive.is_symlink() or not archive.resolve().is_relative_to(output.resolve()):
raise ValueError("Unsafe archive output path")
with zipfile.ZipFile(archive, "w", zipfile.ZIP_DEFLATED) as bundle:
for relative, data in files.items():
info = zipfile.ZipInfo(relative, date_time=(1980, 1, 1, 0, 0, 0))
info.compress_type = zipfile.ZIP_DEFLATED
info.external_attr = 0o100644 << 16
bundle.writestr(info, data, compresslevel=9)
return archive
def main() -> None:
parser = argparse.ArgumentParser(description=__doc__)
group = parser.add_mutually_exclusive_group(required=True)
group.add_argument("--source", type=Path)
group.add_argument("--download", action="store_true")
parser.add_argument("--output", type=Path, default=ROOT / "build/lekiwi")
args = parser.parse_args()
print(prepare(args.source, args.output, args.download))
if __name__ == "__main__":
main()
@@ -0,0 +1,5 @@
# Offline cooking only. Do not install into training/LeRobot environments.
coacd==1.0.14
trimesh==4.12.2
numpy==2.1.3
scipy==1.17.0
+68
View File
@@ -0,0 +1,68 @@
#!/usr/bin/env python3
"""安装经验证的 CPU LeRobot 环境;不修改当前解释器、上游源码或训练环境。"""
import argparse
import json
import os
import subprocess
import sys
import venv
from pathlib import Path
ROOT = Path(__file__).resolve().parents[2]
def main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument("--venv", type=Path, default=ROOT / "build/venvs/lerobot")
args = parser.parse_args()
target = args.venv.resolve()
if sys.version_info[:2] != (3, 12):
parser.error("此兼容环境仅验证 Python 3.12,请使用 Python 3.12 执行安装脚本")
if not target.is_relative_to(ROOT / "build") or target == Path(sys.prefix).resolve():
parser.error("目标必须位于仓库 build/ 下,并且不能是当前环境")
env = {**os.environ, "PYTHONNOUSERSITE": "1", "PYTHONPATH": ""}
if not (target / "bin/python").exists():
venv.EnvBuilder(with_pip=True).create(target)
python = str(target / "bin/python")
prefix = subprocess.check_output(
[python, "-I", "-c", "import sys; print(sys.prefix)"], env=env, text=True
).strip()
config = (target / "pyvenv.cfg").read_text().lower()
if Path(prefix).resolve() != target or "include-system-site-packages = false" not in config:
parser.error("目标不是独立 venv;拒绝对共享/其他 Python 环境执行 pip")
def run(*arguments):
subprocess.run([python, "-m", "pip", *arguments], env=env, cwd=ROOT, check=True)
run(
"install",
"--index-url",
"https://download.pytorch.org/whl/cpu",
"torch==2.11.0+cpu",
"torchvision==0.26.0+cpu",
)
constraints = "integrations/lerobot/requirements-cpu.txt"
run("install", "-r", constraints)
run(
"install",
"--no-build-isolation",
"-c",
constraints,
"-e",
"control_bridge",
"-e",
"integrations/lerobot",
)
run("check")
# Record versions, not pip freeze's private VCS origins or credentials.
result = subprocess.check_output(
[python, "-m", "pip", "list", "--format=json"], env=env, text=True
)
report = ROOT / "build/lerobot-environment.json"
report.write_text(json.dumps({"python": sys.version, "packages": json.loads(result)}, indent=2))
print(f"完成:{python}\n版本记录:{report}\n请在此环境执行插件测试和示例。")
if __name__ == "__main__":
main()
+132
View File
@@ -0,0 +1,132 @@
"""Linux terminal base + joint-space arm teleop; no hardware or pynput required."""
import argparse
import math
import os
import select
import sys
import termios
import time
import tty
CONTROL_HZ = 30
KEY_PULSE_SECONDS = 0.18
# Positive / negative in LeRobot units: degrees, or 0–100 gripper opening.
ARM_KEYS = {
"arm_shoulder_pan.pos": ("u", "j"),
"arm_shoulder_lift.pos": ("i", "k"),
"arm_elbow_flex.pos": ("o", "l"),
"arm_wrist_flex.pos": ("t", "g"),
"arm_wrist_roll.pos": ("y", "h"),
"arm_gripper.pos": ("v", "b"),
}
class KeyboardController:
"""Compose base velocities and incremental arm targets in one writer/lease."""
def __init__(self, robot, arm_speed=20.0, gripper_speed=50.0):
self.robot = robot
self.arm_speed = arm_speed
self.gripper_speed = gripper_speed
self.targets = robot.get_observation() # Never jump to a hard-coded zero pose.
self.active = {}
self.speed_keys = {robot.teleop_keys[k] for k in ("speed_up", "speed_down")}
self.motion_keys = {
robot.teleop_keys[k]
for k in ("forward", "backward", "left", "right", "rotate_left", "rotate_right")
} | {key for pair in ARM_KEYS.values() for key in pair}
def step(self, keys, now):
keys = set(keys)
if " " in keys:
self.active.clear()
pressed = set() # Space wins over every movement/speed key in the batch.
else:
self.active = {key: expiry for key, expiry in self.active.items() if expiry > now}
self.active.update(dict.fromkeys(keys & self.motion_keys, now + KEY_PULSE_SECONDS))
# Speed changes are input events, not a 180 ms pulse repeated every frame.
pressed = set(self.active) | (keys & self.speed_keys)
self.robot.get_observation() # Require fresh feedback even while holding still.
action = self.robot._from_keyboard_to_base_action(pressed)
for channel, (positive, negative) in ARM_KEYS.items():
direction = int(positive in pressed) - int(negative in pressed)
if direction:
speed = self.gripper_speed if channel == "arm_gripper.pos" else self.arm_speed
# At most one control tick per update: no large catch-up jump after a stall.
action[channel] = self.targets[channel] + direction * speed / CONTROL_HZ
# The plugin holds omitted arm channels and clamps via the browser descriptor.
# Accumulate from confirmed targets only, including any joint/gripper clipping.
self.targets = self.robot.send_action(action)
return self.targets
def read_keys(fd):
keys = []
# Bound input draining so queued terminal data cannot starve the control watchdog.
for _ in range(64):
if not select.select([fd], [], [], 0)[0]:
break
value = os.read(fd, 1)
if not value:
raise EOFError("终端输入已断开")
keys.append(value.decode(errors="ignore"))
return keys
def main():
parser = argparse.ArgumentParser(description="LeKiwi 底盘 + 机械臂终端遥操作(仿真专用)")
parser.add_argument(
"--endpoint", default=os.environ.get("MUJOCO_CONTROL_ENDPOINT", "http://127.0.0.1:8766")
)
parser.add_argument(
"--arm-speed", type=float, default=20.0, help="臂关节点动速度,0–90 度/秒,默认20"
)
parser.add_argument(
"--gripper-speed", type=float, default=50.0, help="夹爪点动速度,0–100 百分点/秒,默认50"
)
args = parser.parse_args()
for name, speed, limit in (
("--arm-speed", args.arm_speed, 90),
("--gripper-speed", args.gripper_speed, 100),
):
if not math.isfinite(speed) or not 0 < speed <= limit:
parser.error(f"{name} 必须为大于0且不超过{limit}的有限数值")
if not sys.stdin.isatty():
raise ValueError("请在交互式终端运行;自动测试请用 demo_control.py")
# Keep --help and argument validation usable without importing the LeRobot stack.
from demo_control import make_robot
fd = sys.stdin.fileno()
previous = termios.tcgetattr(fd)
robot = make_robot(args.endpoint)
try:
robot.connect()
controller = KeyboardController(robot, args.arm_speed, args.gripper_speed)
tty.setcbreak(fd)
print(
"底盘:w/s 前后,a/d 左右,z/x 旋转,r/f 底盘调速;空格停止点动,q 退出。\n"
"机械臂(前键增大/后键减小):u/j 肩转,i/k 肩俯仰,o/l 肘,t/g 腕俯仰,"
"y/h 腕旋转;v/b 夹爪开/合。\n"
f"臂 {args.arm_speed:g} 度/秒,夹爪 {args.gripper_speed:g} 百分点/秒。"
"终端按键为180ms脉冲;脉冲结束后底盘停止、机械臂保持目标。",
flush=True,
)
while True:
start = time.monotonic()
keys = read_keys(fd)
if robot.teleop_keys["quit"] in keys:
return
controller.step(keys, start)
time.sleep(max(0, 1 / CONTROL_HZ - (time.monotonic() - start)))
except KeyboardInterrupt:
pass
finally:
try:
robot.disconnect()
finally:
termios.tcsetattr(fd, termios.TCSADRAIN, previous)
if __name__ == "__main__":
main()
@@ -0,0 +1,175 @@
"""Offline cooker contracts: no real CAD download or CoACD run required here."""
import copy
import hashlib
import importlib.util
import tempfile
import unittest
from pathlib import Path
import numpy as np
SCRIPT = Path(__file__).resolve().parents[1] / "generate_full_collisions.py"
spec = importlib.util.spec_from_file_location("collision_cooker", SCRIPT)
cooker = importlib.util.module_from_spec(spec)
spec.loader.exec_module(cooker)
def cube(center, size=0.004):
return (
np.array([[x, y, z] for x in (-size, size) for y in (-size, size) for z in (-size, size)])
+ center
)
class CookingTests(unittest.TestCase):
def setUp(self):
self.temp = tempfile.TemporaryDirectory()
self.addCleanup(self.temp.cleanup)
self.source = Path(self.temp.name)
(self.source / "meshes").mkdir()
(self.source / "meshes/m.stl").write_bytes(b"source")
links = "".join(
f'<link name="{name}"><visual name="{name}_visual"><geometry>'
'<mesh filename="meshes/m.stl" scale=".001 .001 .001"/>'
"</geometry></visual></link>"
for name in ("base", "motor", "arm")
)
self.xml = (
'<robot name="fixture">'
+ links
+ """
<joint name="fixed" type="fixed"><parent link="base"/><child link="motor"/></joint>
<joint name="hinge" type="continuous">
<parent link="motor"/><child link="arm"/><axis xyz="0 0 1"/>
</joint>
</robot>"""
)
self.config = {
"rootLink": "base",
"meshes": {"meshes/m.stl": hashlib.sha256(b"source").hexdigest()},
"assemblyCores": {
"hinge": {"radiusM": 0.015, "axialHalfExtentM": 0.02, "reason": "fixture bearing"}
},
}
self.save_source()
def save_source(self):
data = self.xml.encode()
(self.source / "LeKiwi.urdf").write_bytes(data)
self.config["urdfSha256"] = hashlib.sha256(data).hexdigest()
def test_covers_fixed_and_moving_visuals_and_preserves_scale(self):
items = cooker.visuals(self.source, self.config)
self.assertEqual(
[x["visual"] for x in items], ["base_visual", "motor_visual", "arm_visual"]
)
self.assertTrue(all(x["scale"] == [0.001] * 3 for x in items))
def test_rejects_changed_source_urdf_and_mesh(self):
(self.source / "LeKiwi.urdf").write_text("modified")
with self.assertRaisesRegex(ValueError, "URDF"):
cooker.visuals(self.source, self.config)
self.save_source()
(self.source / "meshes/m.stl").write_bytes(b"modified")
with self.assertRaisesRegex(ValueError, "SHA-256"):
cooker.visuals(self.source, self.config)
def test_rejects_unsupported_scale_and_path_escape(self):
for scale in ("nan .001 .001", "-.001 .001 .001", ".001 .001", "inf 1 1"):
original = self.xml
self.xml = self.xml.replace(".001 .001 .001", scale)
self.save_source()
with self.assertRaisesRegex(ValueError, "缩放"):
cooker.visuals(self.source, self.config)
self.xml = original
self.xml = self.xml.replace("meshes/m.stl", "../escape.stl")
self.save_source()
with self.assertRaisesRegex(ValueError, "未批准"):
cooker.visuals(self.source, self.config)
def test_rejects_missing_or_incomplete_coverage(self):
self.config["meshes"]["meshes/extra.stl"] = "unknown"
with self.assertRaisesRegex(ValueError, "覆盖"):
cooker.visuals(self.source, self.config)
self.config["rootLink"] = "not_present"
self.config["meshes"] = {}
with self.assertRaisesRegex(ValueError, "覆盖"):
cooker.visuals(self.source, self.config)
def test_radial_minimum_uses_polygon_edges_not_only_vertices(self):
bounds = cooker.swept_bounds(cube([2.0, 0, 0], 0.5), np.array([0, 0, 1.0]))
self.assertAlmostEqual(bounds[2], 1.5)
self.assertGreater(np.linalg.norm(cube([2.0, 0, 0], 0.5)[:, :2], axis=1).min(), bounds[2])
self.assertTrue(cooker.intervals_overlap(bounds, (-0.1, 0.1, 1.51, 1.52)))
self.assertFalse(cooker.intervals_overlap(bounds, (1.0, 2.0, 1.51, 1.52)))
for angle in np.linspace(-np.pi, np.pi, 37):
rotation = np.array(
[[np.cos(angle), -np.sin(angle), 0], [np.sin(angle), np.cos(angle), 0], [0, 0, 1]]
)
np.testing.assert_allclose(
cooker.swept_bounds(cube([2.0, 0, 0], 0.5) @ rotation.T, np.array([0, 0, 1.0])),
bounds,
atol=1e-12,
)
def test_core_partition_preserves_volume_and_separates_mating_from_structure(self):
from scipy.spatial import ConvexHull
coverage = cooker.visuals(self.source, self.config)
parts = [
{
"visual": item["visual"],
"name": item["link"],
"vertices": " ".join(map(str, cube([0.012, 0, 0], 0.01).ravel())),
}
for item in coverage
]
for item in coverage:
item["parts"] = 1
divided = cooker.partition_assembly_cores(self.source, self.config, parts, coverage)
policy = cooker.joint_policy(self.source, self.config, divided, coverage)[0]
for visual in (item["visual"] for item in coverage):
group = [piece for piece in divided if piece["visual"] == visual]
volume = 0
for piece in group:
hull = ConvexHull(np.fromstring(piece["vertices"], sep=" ").reshape(-1, 3))
volume += hull.volume
# The old unsplit hull locked the bearing although this point is inside its core.
if np.max(hull.equations[:, :3] @ [0.012, 0, 0] + hull.equations[:, 3]) < 1e-8:
self.assertIn(piece["name"], policy["coreParts"])
if np.max(hull.equations[:, :3] @ [0.021, 0, 0] + hull.equations[:, 3]) < 1e-8:
self.assertNotIn(piece["name"], policy["coreParts"])
self.assertAlmostEqual(volume / (0.02**3), 1, delta=0.001)
self.assertGreater(len(group), 1)
self.assertTrue(policy["pairs"])
def test_exceptions_are_bounded_core_parts_not_whole_adjacent_bodies(self):
coverage = cooker.visuals(self.source, self.config)
parts = [
{"name": name, "visual": visual, "vertices": " ".join(map(str, cube(center).ravel()))}
for name, visual, center in [
("base_core", "base_visual", [0, 0, 0]),
("base_structural", "base_visual", [0.05, 0, 0]),
("far_axial", "base_visual", [0.05, 0, 0.2]),
("arm_core", "arm_visual", [0, 0, 0]),
("arm_structural", "arm_visual", [0.05, 0, 0]),
]
]
policy = cooker.joint_policy(self.source, self.config, parts, coverage)[0]
self.assertEqual(policy["coreParts"], ["base_core", "arm_core"])
self.assertEqual(policy["pairs"], [["base_structural", "arm_structural"]])
self.assertGreater(policy["sweptPrunedPairs"], 0)
self.assertEqual([x["weldJoint"] for x in coverage], ["base", "base", "hinge"])
bad = copy.deepcopy(self.config)
bad["assemblyCores"]["hinge"]["radiusM"] = 1
with self.assertRaisesRegex(ValueError, "尺寸预算"):
cooker.joint_policy(self.source, bad, parts, coverage)
with self.assertRaisesRegex(ValueError, "整对"):
cooker.joint_policy(
self.source, self.config, [p for p in parts if "core" in p["name"]], coverage
)
if __name__ == "__main__":
unittest.main()
+202
View File
@@ -0,0 +1,202 @@
Apache License
Version 2.0, January 2004
http://www.apache.org/licenses/
TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
1. Definitions.
"License" shall mean the terms and conditions for use, reproduction,
and distribution as defined by Sections 1 through 9 of this document.
"Licensor" shall mean the copyright owner or entity authorized by
the copyright owner that is granting the License.
"Legal Entity" shall mean the union of the acting entity and all
other entities that control, are controlled by, or are under common
control with that entity. For the purposes of this definition,
"control" means (i) the power, direct or indirect, to cause the
direction or management of such entity, whether by contract or
otherwise, or (ii) ownership of fifty percent (50%) or more of the
outstanding shares, or (iii) beneficial ownership of such entity.
"You" (or "Your") shall mean an individual or Legal Entity
exercising permissions granted by this License.
"Source" form shall mean the preferred form for making modifications,
including but not limited to software source code, documentation
source, and configuration files.
"Object" form shall mean any form resulting from mechanical
transformation or translation of a Source form, including but
not limited to compiled object code, generated documentation,
and conversions to other media types.
"Work" shall mean the work of authorship, whether in Source or
Object form, made available under the License, as indicated by a
copyright notice that is included in or attached to the work
(an example is provided in the Appendix below).
"Derivative Works" shall mean any work, whether in Source or Object
form, that is based on (or derived from) the Work and for which the
editorial revisions, annotations, elaborations, or other modifications
represent, as a whole, an original work of authorship. For the purposes
of this License, Derivative Works shall not include works that remain
separable from, or merely link (or bind by name) to the interfaces of,
the Work and Derivative Works thereof.
"Contribution" shall mean any work of authorship, including
the original version of the Work and any modifications or additions
to that Work or Derivative Works thereof, that is intentionally
submitted to Licensor for inclusion in the Work by the copyright owner
or by an individual or Legal Entity authorized to submit on behalf of
the copyright owner. For the purposes of this definition, "submitted"
means any form of electronic, verbal, or written communication sent
to the Licensor or its representatives, including but not limited to
communication on electronic mailing lists, source code control systems,
and issue tracking systems that are managed by, or on behalf of, the
Licensor for the purpose of discussing and improving the Work, but
excluding communication that is conspicuously marked or otherwise
designated in writing by the copyright owner as "Not a Contribution."
"Contributor" shall mean Licensor and any individual or Legal Entity
on behalf of whom a Contribution has been received by Licensor and
subsequently incorporated within the Work.
2. Grant of Copyright License. Subject to the terms and conditions of
this License, each Contributor hereby grants to You a perpetual,
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
copyright license to reproduce, prepare Derivative Works of,
publicly display, publicly perform, sublicense, and distribute the
Work and such Derivative Works in Source or Object form.
3. Grant of Patent License. Subject to the terms and conditions of
this License, each Contributor hereby grants to You a perpetual,
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
(except as stated in this section) patent license to make, have made,
use, offer to sell, sell, import, and otherwise transfer the Work,
where such license applies only to those patent claims licensable
by such Contributor that are necessarily infringed by their
Contribution(s) alone or by combination of their Contribution(s)
with the Work to which such Contribution(s) was submitted. If You
institute patent litigation against any entity (including a
cross-claim or counterclaim in a lawsuit) alleging that the Work
or a Contribution incorporated within the Work constitutes direct
or contributory patent infringement, then any patent licenses
granted to You under this License for that Work shall terminate
as of the date such litigation is filed.
4. Redistribution. You may reproduce and distribute copies of the
Work or Derivative Works thereof in any medium, with or without
modifications, and in Source or Object form, provided that You
meet the following conditions:
(a) You must give any other recipients of the Work or
Derivative Works a copy of this License; and
(b) You must cause any modified files to carry prominent notices
stating that You changed the files; and
(c) You must retain, in the Source form of any Derivative Works
that You distribute, all copyright, patent, trademark, and
attribution notices from the Source form of the Work,
excluding those notices that do not pertain to any part of
the Derivative Works; and
(d) If the Work includes a "NOTICE" text file as part of its
distribution, then any Derivative Works that You distribute must
include a readable copy of the attribution notices contained
within such NOTICE file, excluding those notices that do not
pertain to any part of the Derivative Works, in at least one
of the following places: within a NOTICE text file distributed
as part of the Derivative Works; within the Source form or
documentation, if provided along with the Derivative Works; or,
within a display generated by the Derivative Works, if and
wherever such third-party notices normally appear. The contents
of the NOTICE file are for informational purposes only and
do not modify the License. You may add Your own attribution
notices within Derivative Works that You distribute, alongside
or as an addendum to the NOTICE text from the Work, provided
that such additional attribution notices cannot be construed
as modifying the License.
You may add Your own copyright statement to Your modifications and
may provide additional or different license terms and conditions
for use, reproduction, or distribution of Your modifications, or
for any such Derivative Works as a whole, provided Your use,
reproduction, and distribution of the Work otherwise complies with
the conditions stated in this License.
5. Submission of Contributions. Unless You explicitly state otherwise,
any Contribution intentionally submitted for inclusion in the Work
by You to the Licensor shall be under the terms and conditions of
this License, without any additional terms or conditions.
Notwithstanding the above, nothing herein shall supersede or modify
the terms of any separate license agreement you may have executed
with Licensor regarding such Contributions.
6. Trademarks. This License does not grant permission to use the trade
names, trademarks, service marks, or product names of the Licensor,
except as required for reasonable and customary use in describing the
origin of the Work and reproducing the content of the NOTICE file.
7. Disclaimer of Warranty. Unless required by applicable law or
agreed to in writing, Licensor provides the Work (and each
Contributor provides its Contributions) on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
implied, including, without limitation, any warranties or conditions
of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
PARTICULAR PURPOSE. You are solely responsible for determining the
appropriateness of using or redistributing the Work and assume any
risks associated with Your exercise of permissions under this License.
8. Limitation of Liability. In no event and under no legal theory,
whether in tort (including negligence), contract, or otherwise,
unless required by applicable law (such as deliberate and grossly
negligent acts) or agreed to in writing, shall any Contributor be
liable to You for damages, including any direct, indirect, special,
incidental, or consequential damages of any character arising as a
result of this License or out of the use or inability to use the
Work (including but not limited to damages for loss of goodwill,
work stoppage, computer failure or malfunction, or any and all
other commercial damages or losses), even if such Contributor
has been advised of the possibility of such damages.
9. Accepting Warranty or Additional Liability. While redistributing
the Work or Derivative Works thereof, You may choose to offer,
and charge a fee for, acceptance of support, warranty, indemnity,
or other liability obligations and/or rights consistent with this
License. However, in accepting such obligations, You may act only
on Your own behalf and on Your sole responsibility, not on behalf
of any other Contributor, and only if You agree to indemnify,
defend, and hold each Contributor harmless for any liability
incurred by, or claims asserted against, such Contributor by reason
of your accepting any such warranty or additional liability.
END OF TERMS AND CONDITIONS
APPENDIX: How to apply the Apache License to your work.
To apply the Apache License to your work, attach the following
boilerplate notice, with the fields enclosed by brackets "[]"
replaced with your own identifying information. (Don't include
the brackets!) The text should be enclosed in the appropriate
comment syntax for the file format. We also recommend that a
file or class name and description of purpose be included on the
same "printed page" as the copyright notice for easier
identification within third-party archives.
Copyright [yyyy] [name of copyright owner]
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.
+13
View File
@@ -0,0 +1,13 @@
# 归属与兼容范围
本包是独立仿真适配器,不是上游 LeRobot 的 fork 或硬件 `LeKiwiClient`/ZMQ 服务器。
- LeRobot 0.6.1:[huggingface/lerobot](https://github.com/huggingface/lerobot),Apache-2.0。
LeKiwi 运动学、键盘辅助和循环约定参考其 `lerobot/robots/lekiwi/lekiwi_client.py`。
保留上游声明:Copyright 2024 The HuggingFace Inc. team. All rights reserved.
- LeKiwi:[SIGRobotics-UIUC/LeKiwi](https://github.com/SIGRobotics-UIUC/LeKiwi),Apache-2.0,固定 revision `efa608d7ee5a495a4803b1d28cd0c955b4f1e033`。
模型资源不放入本 wheel;显式准备资源时复制其 `LICENSE.txt` 和 `CITATION.cff`。
- 本项目修改:移除硬件 I/O/校准持久化;增加认证本机仿真传输、SI/上游单位映射、部分动作/确认目标语义以及 profile 校验。
简化的质量、惯量、轮几何、碰撞、限位和伺服是仿真估计,不是硬件标定。
许可证正文见 `LICENSE`。本包不授予额外的商标或硬件认证权利。
+35
View File
@@ -0,0 +1,35 @@
# LeRobot 仿真插件
真实 LeRobot **0.6.1** 的 `Robot` 子类与 `RobotConfig`,注册名 `lekiwi_sim`,发行包名 **`lerobot_robot_mujoco`**。上游按发行包名前缀发现插件,因此不能随意改成连字符名称或只提供自定义 entry point。
```bash
source .venv/bin/activate # Python 3.12;只用它创建隔离环境
python examples/lekiwi/setup_lerobot.py
# 不要在原训练环境直接 pip install lerobot
build/venvs/lerobot/bin/python -m unittest discover -s integrations/lerobot/tests -v
```
安装脚本仅修改 `build/venvs/lerobot/`,先装官方 CPU torch/torchvision,再装 [固定兼容集](requirements-cpu.txt),最后 editable 安装桥接和本插件并执行 `pip check`。版本记录在 `build/lerobot-environment.json`;不会改上游或原 `.venv` 的训练依赖。
先按 [示例](../../examples/lekiwi/README.md) 准备模型、连接桥接并在浏览器授权。相同 token 放在 Python 的 `MUJOCO_CONTROL_TOKEN` 环境变量中:
```python
from lerobot.robots.config import RobotConfig
from lerobot.robots.utils import make_robot_from_config
from lerobot.utils.import_utils import register_third_party_plugins
register_third_party_plugins()
config_type = RobotConfig.get_choice_class('lekiwi_sim')
robot = make_robot_from_config(config_type(endpoint='http://127.0.0.1:8766', id='sim-demo'))
robot.connect()
try:
measured = robot.get_observation()
accepted = robot.send_action({'x.vel': 0.05})
diagnostic = robot.get_sim_observation()
finally:
robot.disconnect()
```
这是单步 API 演示;实际控制须持续刷新(示例为 30 Hz),不能在两条动作之间阻塞超过 500 ms。`send_action` 返回确认目标,`get_observation` 才是实测反馈。五个臂通道为度,夹爪 0–100,底盘平移 m/s、转动 deg/s。部分动作保持最后确认的臂/夹爪目标,省略的底盘速度为零。
仅接受 `use_degrees=True`、`cameras={}`;拒绝物理校准目录和校准文件 I/O。`is_calibrated=True` 表示固定仿真 profile 可用,不代表硬件标定。提供 `stop_base`、键盘辅助函数和完整 SI 诊断,但没有实体 leader、串口、ZMQ、相机、dataset 或训练实现。协议/来源/升级流程见 [机器人接口](../../docs/robot-interface.md),许可证和上游归属见 [NOTICE](NOTICE.md)。
+19
View File
@@ -0,0 +1,19 @@
[build-system]
requires = ["setuptools>=77,<82"]
build-backend = "setuptools.build_meta"
[project]
# Upstream 0.6.1 discovers the literal distribution Name prefix, not entry points.
name = "lerobot_robot_mujoco"
version = "0.1.0"
description = "Simulation-only LeKiwi adapter for LeRobot 0.6.1 and MuJoCo Web"
requires-python = ">=3.12"
license = "Apache-2.0"
license-files = ["LICENSE", "NOTICE.md"]
dependencies = ["lerobot==0.6.1", "mujoco-control-bridge==0.1.0"]
[tool.setuptools.packages.find]
where = ["src"]
[tool.setuptools.package-data]
lerobot_robot_mujoco = ["lekiwi-v1.json"]
+52
View File
@@ -0,0 +1,52 @@
# Ubuntu 24.04 x86_64 / Python 3.12;固定兼容集,不含原训练环境。
# 先从官方 CPU 索引安装 torch/torchvision,再安装本文件;见 setup_lerobot.py。
Farama-Notifications==0.0.6
Jinja2==3.1.6
MarkupSafe==3.0.3
PyYAML==6.0.3
aiohappyeyeballs==2.7.1
aiohttp==3.14.3
aiosignal==1.4.0
anyio==4.15.1
attrs==26.1.0
certifi==2026.7.22
charset-normalizer==3.5.1
click==8.5.0
cloudpickle==3.1.2
cmake==4.1.3
draccus==0.11.6
einops==0.8.2
filelock==3.32.3
frozenlist==1.8.0
fsspec==2026.7.0
gymnasium==1.3.0
h11==0.16.0
hf-xet==1.6.0
httpcore==1.0.9
httpx==0.28.1
huggingface_hub==1.32.0
idna==3.20
lerobot==0.6.1
mergedeep==1.3.4
mpmath==1.3.0
multidict==6.8.0
mypy_extensions==1.1.0
networkx==3.6.1
numpy==2.2.6
opencv-python-headless==4.13.0.92
packaging==25.0
pillow==12.3.0
propcache==0.5.4
requests==2.34.2
safetensors==0.8.0
setuptools==78.1.0
sympy==1.14.0
termcolor==3.3.0
toml==0.10.2
torch==2.11.0+cpu
torchvision==0.26.0+cpu
tqdm==4.70.1
typing-inspect==0.9.0
typing_extensions==4.16.0
urllib3==2.8.0
yarl==1.25.1
@@ -0,0 +1,4 @@
from .config_lekiwi_sim import LeKiwiSimConfig
from .lekiwi_sim import LeKiwiSim
__all__ = ["LeKiwiSim", "LeKiwiSimConfig"]
@@ -0,0 +1,48 @@
from dataclasses import dataclass, field
from urllib.parse import urlparse
from lerobot.robots.config import RobotConfig
@RobotConfig.register_subclass("lekiwi_sim")
@dataclass
class LeKiwiSimConfig(RobotConfig):
endpoint: str = "http://127.0.0.1:8766"
# Prefer MUJOCO_CONTROL_TOKEN. Never include this value in repr/diagnostics.
token: str | None = field(default=None, repr=False)
use_degrees: bool = True
cameras: dict = field(default_factory=dict)
expected_model_fingerprint: str | None = None
teleop_keys: dict[str, str] = field(
default_factory=lambda: {
"forward": "w",
"backward": "s",
"left": "a",
"right": "d",
"rotate_left": "z",
"rotate_right": "x",
"speed_up": "r",
"speed_down": "f",
"quit": "q",
}
)
def __post_init__(self):
if self.use_degrees is not True:
raise ValueError("V1 仅支持 use_degrees=True,不支持归一化臂位置")
if self.cameras:
raise ValueError("V1 不支持相机;请使用 cameras={}")
if self.calibration_dir is not None:
raise ValueError("模拟器使用版本化 profile,不接受实体校准目录")
url = urlparse(self.endpoint)
if (
url.scheme != "http"
or url.hostname not in {"127.0.0.1", "localhost"}
or url.username
or url.password
or url.query
or url.fragment
or url.path not in {"", "/"}
):
raise ValueError("仅支持不含凭据/查询参数的本机 HTTP 桥接地址")
super().__post_init__()
@@ -0,0 +1 @@
../../../../robot_profiles/lekiwi-v1.json
@@ -0,0 +1,217 @@
"""Real LeRobot Robot implementation, with no hardware bus/camera/ZMQ backend."""
import json
import math
from importlib.resources import files
from numbers import Real
from lerobot.robots.robot import Robot
from lerobot.utils.decorators import check_if_already_connected, check_if_not_connected
from mujoco_control_bridge import RobotError, SimRobotClient
from .config_lekiwi_sim import LeKiwiSimConfig
PROFILE = json.loads(files(__package__).joinpath("lekiwi-v1.json").read_text())
ARM = tuple(f"{spec['joint']}.pos" for spec in PROFILE["arm"])
BASE = ("x.vel", "y.vel", "theta.vel")
FEATURES = (*ARM, *BASE)
def scalar(value):
"""Accept upstream numpy/torch scalar actions, but never bool/strings/vectors."""
if (
not isinstance(value, Real)
and getattr(value, "ndim", None) == 0
and callable(getattr(value, "item", None))
):
value = value.item()
if isinstance(value, bool) or not isinstance(value, Real):
raise RobotError("INVALID_MESSAGE", "动作必须是有限标量,不能是 bool/字符串/数组")
value = float(value)
if not math.isfinite(value):
raise RobotError("INVALID_MESSAGE", "动作不允许 NaN/Infinity")
return value
def to_sim(key, value):
value = scalar(value)
if key == "arm_gripper.pos":
return value / 100
if key in ARM:
i = ARM.index(key)
return (
PROFILE["mapping"]["armSigns"][i] * math.radians(value)
+ PROFILE["mapping"]["armOffsetsRad"][i]
)
return math.radians(value) if key == "theta.vel" else value
def from_sim(key, value):
if key == "arm_gripper.pos":
return value * 100
if key in ARM:
i = ARM.index(key)
return math.degrees(
(value - PROFILE["mapping"]["armOffsetsRad"][i]) / PROFILE["mapping"]["armSigns"][i]
)
return math.degrees(value) if key == "theta.vel" else value
class LeKiwiSim(Robot):
config_class = LeKiwiSimConfig
name = "lekiwi_sim"
def __init__(self, config: LeKiwiSimConfig):
# Robot.__init__ reads/creates physical calibration files. This simulation
# implements the same base metadata, deliberately without those side effects.
self.config = config
self.id, self.robot_type = config.id, self.name
self.calibration = {}
self.calibration_dir = self.calibration_fpath = None
self.cameras = {}
self._client = None
self._targets = {}
self.teleop_keys = dict(config.teleop_keys)
self.speed_levels = [
{"xy": 0.1, "theta": 30},
{"xy": 0.2, "theta": 60},
{"xy": 0.3, "theta": 90},
]
self.speed_index = 0
@property
def action_features(self):
return dict.fromkeys(FEATURES, float)
@property
def observation_features(self):
return self.action_features
@property
def is_connected(self):
return self._client is not None and self._client.is_connected
@property
def is_calibrated(self):
return True # Means the fixed SIMULATION profile is in use, not hardware calibration.
def calibrate(self):
pass
def configure(self):
pass
def _load_calibration(self, fpath=None):
raise RobotError("UNSUPPORTED", "模拟 profile 不读取硬件校准文件")
def _save_calibration(self, fpath=None):
raise RobotError("UNSUPPORTED", "模拟 profile 不生成硬件校准文件")
def _validate_descriptor(self, descriptor):
if (
descriptor["profileId"] != PROFILE["id"]
or descriptor["profileVersion"] != PROFILE["version"]
):
raise RobotError("INCOMPATIBLE_MODEL", "需要 lekiwi-v1 profile v1")
if (
self.config.expected_model_fingerprint
and descriptor["modelFingerprint"] != self.config.expected_model_fingerprint
):
raise RobotError("INCOMPATIBLE_MODEL", "模型指纹不符合固定配置")
channels = descriptor["actionChannels"]
if [c["id"] for c in channels] != list(FEATURES):
raise RobotError("INCOMPATIBLE_MODEL", "LeKiwi 通道集合/顺序不匹配")
for channel in channels:
key = channel["id"]
unit = (
"ratio"
if key == "arm_gripper.pos"
else "rad"
if key in ARM
else "rad/s"
if key == "theta.vel"
else "m/s"
)
mode = (
"opening" if key == "arm_gripper.pos" else "position" if key in ARM else "velocity"
)
if channel["unit"] != unit or channel["mode"] != mode:
raise RobotError("INCOMPATIBLE_MODEL", f"LeKiwi 通道单位/模式不匹配:{key}")
observations = {c["id"]: c for c in descriptor["observationChannels"]}
if any(key not in observations for key in FEATURES) or any(
observations[c["id"]]["unit"] != c["unit"] for c in channels
):
raise RobotError("INCOMPATIBLE_MODEL", "LeKiwi 观测通道/单位不匹配")
@check_if_already_connected
def connect(self, calibrate=True):
self.disconnect() # Clear an expired client, without reusing its old lease.
client = SimRobotClient(self.config.endpoint, self.config.token)
self._validate_descriptor(client.describe())
try:
self._validate_descriptor(
client.connect()
) # Recheck after the transactional handshake.
measured = client.get_observation()["values"]
self._targets = {key: measured[key] for key in ARM}
self._client = client
except BaseException:
client.disconnect()
raise
@check_if_not_connected
def get_observation(self):
# Base velocities are measured-wheel odometry, matching upstream LeKiwi.
# Actual body twist remains available through the generic SDK, not extra ML features.
values = self._client.get_observation()["values"]
return {key: float(from_sim(key, values[key])) for key in FEATURES}
@check_if_not_connected
def get_sim_observation(self):
"""SI diagnostics and epoch metadata, deliberately not LeRobot training features."""
return self._client.get_observation()
@check_if_not_connected
def send_action(self, action):
if not isinstance(action, dict) or not set(action).issubset(FEATURES):
raise RobotError("INVALID_MESSAGE", "动作包含未知通道")
values = {**self._targets, **dict.fromkeys(BASE, 0.0)}
values.update({key: to_sim(key, value) for key, value in action.items()})
accepted = self._client.send_action(values)["values"]
# Only confirmed targets become the next partial-action hold state.
self._targets = {key: accepted[key] for key in ARM}
return {key: float(from_sim(key, accepted[key])) for key in FEATURES}
@check_if_not_connected
def stop_base(self):
return self.send_action({})
def _from_keyboard_to_base_action(self, pressed_keys):
"""Same w/s/a/d/z/x and r/f semantics as the upstream 0.6.1 example."""
keys = set(pressed_keys)
self.speed_index = min(
2,
max(
0,
self.speed_index
+ int(self.teleop_keys["speed_up"] in keys)
- int(self.teleop_keys["speed_down"] in keys),
),
)
speed = self.speed_levels[self.speed_index]
pairs = [
("x.vel", "forward", "backward", speed["xy"]),
("y.vel", "left", "right", speed["xy"]),
("theta.vel", "rotate_left", "rotate_right", speed["theta"]),
]
return {
channel: amount
* (int(self.teleop_keys[positive] in keys) - int(self.teleop_keys[negative] in keys))
for channel, positive, negative, amount in pairs
}
def disconnect(self):
if self._client is not None:
self._client.disconnect()
self._client = None
self._targets = {}
@@ -0,0 +1,188 @@
"""Keyboard composition uses the real LeRobot plugin; only the transport is fake."""
import math
import runpy
import subprocess
import sys
import unittest
from pathlib import Path
from types import SimpleNamespace
from unittest.mock import Mock, patch
from lerobot_robot_mujoco import LeKiwiSim, LeKiwiSimConfig
from lerobot_robot_mujoco.lekiwi_sim import ARM, BASE, PROFILE
from mujoco_control_bridge import RobotError
from test_plugin import FakeClient
SCRIPT = Path(__file__).resolve().parents[3] / "examples/lekiwi/teleoperate_sim.py"
DEMO = runpy.run_path(str(SCRIPT))
KeyboardController = DEMO["KeyboardController"]
ARM_KEYS = DEMO["ARM_KEYS"]
class KeyboardTeleopTests(unittest.TestCase):
def setUp(self):
transport = patch("lerobot_robot_mujoco.lekiwi_sim.SimRobotClient", FakeClient)
transport.start()
self.addCleanup(transport.stop)
self.robot = LeKiwiSim(LeKiwiSimConfig())
self.robot.connect()
self.addCleanup(self.robot.disconnect)
self.controller = KeyboardController(self.robot)
def test_every_arm_key_moves_only_its_joint_in_degrees_or_percent(self):
self.assertEqual(set(ARM_KEYS), set(ARM))
all_keys = [key for pair in ARM_KEYS.values() for key in pair]
self.assertEqual(len(set(all_keys)), 12)
self.assertFalse(set(all_keys) & set(self.robot.teleop_keys.values()))
for channel, pair in ARM_KEYS.items():
for key, direction in zip(pair, (1, -1), strict=True):
with self.subTest(channel=channel, key=key):
self.robot.disconnect()
self.robot.connect()
controller = KeyboardController(self.robot, arm_speed=12, gripper_speed=30)
initial = dict(controller.targets)
result = controller.step([key], 0)
for other in ARM:
delta = direction * (1 if channel == "arm_gripper.pos" else 0.4)
self.assertAlmostEqual(
result[other], initial[other] + (delta if other == channel else 0)
)
self.assertEqual({key: result[key] for key in BASE}, dict.fromkeys(BASE, 0))
def test_compose_all_six_arm_targets_and_base_in_one_action(self):
initial = dict(self.controller.targets)
result = self.controller.step("wauiotyv", 0)
self.assertEqual(result["x.vel"], 0.1)
self.assertEqual(result["y.vel"], 0.1)
self.assertEqual(result["theta.vel"], 0)
for channel in ARM:
speed = 50 if channel == "arm_gripper.pos" else 20
self.assertAlmostEqual(result[channel], initial[channel] + speed / 30)
self.assertEqual(len(self.robot._client.sent), 1)
self.assertEqual(len(self.robot._client.sent[0]), 9)
def test_opposing_keys_cancel_without_resetting_arm(self):
initial = dict(self.controller.targets)
keys = [key for pair in ARM_KEYS.values() for key in pair] + list("wsadzx")
result = self.controller.step(keys, 0)
for channel in ARM:
self.assertEqual(result[channel], initial[channel])
self.assertEqual({key: result[key] for key in BASE}, dict.fromkeys(BASE, 0))
def test_expired_pulses_stop_base_and_hold_confirmed_not_measured_arm(self):
first = self.controller.step("wu", 0)
second = self.controller.step([], 0.1)
self.assertGreater(second[ARM[0]], first[ARM[0]])
expired = self.controller.step([], 0.19)
self.assertEqual(expired["x.vel"], 0)
for channel in ARM:
self.assertEqual(expired[channel], second[channel])
self.assertNotEqual(expired[ARM[0]], self.robot.get_observation()[ARM[0]])
self.assertEqual(self.controller.step([], 100), expired) # No catch-up jump.
self.assertFalse(self.controller.active)
def test_space_clears_motion_and_holds_pose_even_with_other_keys_in_batch(self):
first = self.controller.step("wuv", 0)
stopped = self.controller.step(" wur", 0.05)
self.assertEqual(self.robot.speed_index, 0)
for channel in ARM:
self.assertEqual(stopped[channel], first[channel])
self.assertEqual(stopped["x.vel"], 0)
self.assertEqual(self.controller.step([], 0.1), stopped)
restarted = self.controller.step("u", 0.12)
self.assertGreater(restarted[ARM[0]], stopped[ARM[0]])
def test_confirmed_clipping_prevents_joint_and_gripper_target_windup(self):
for i in range(300):
limited = self.controller.step("uv", i / 30)
self.assertAlmostEqual(limited[ARM[0]], math.degrees(PROFILE["arm"][0]["max"]))
self.assertEqual(limited["arm_gripper.pos"], 100)
self.controller.step(" ", 10)
reversed_action = self.controller.step("jb", 10.01)
self.assertAlmostEqual(reversed_action[ARM[0]], limited[ARM[0]] - 20 / 30)
self.assertAlmostEqual(reversed_action["arm_gripper.pos"], 100 - 50 / 30)
for i in range(600):
limited = self.controller.step("jb", 11 + i / 30)
self.assertAlmostEqual(limited[ARM[0]], math.degrees(PROFILE["arm"][0]["min"]))
self.assertEqual(limited["arm_gripper.pos"], 0)
def test_base_speed_changes_once_per_input_event_not_each_pulse_tick(self):
self.controller.step("r", 0)
for now in (0.03, 0.06, 0.1, 0.17):
self.controller.step([], now)
self.assertEqual(self.robot.speed_index, 1)
self.controller.step("f", 0.2)
self.assertEqual(self.robot.speed_index, 0)
def test_failed_send_does_not_commit_unconfirmed_targets(self):
before = dict(self.controller.targets)
with (
patch.object(self.robot, "send_action", side_effect=RobotError("TIMEOUT", "test")),
self.assertRaises(RobotError),
):
self.controller.step("u", 0)
self.assertEqual(self.controller.targets, before)
def test_main_always_releases_lease_and_restores_terminal(self):
self.robot.disconnect()
main = DEMO["main"]
for outcome in (["q"], KeyboardInterrupt(), EOFError("test"), RobotError("STALE", "test")):
with self.subTest(outcome=outcome):
reader = Mock()
if isinstance(outcome, BaseException):
reader.side_effect = outcome
else:
reader.return_value = outcome
with (
patch.dict(main.__globals__, {"read_keys": reader}),
patch.dict(
sys.modules,
{"demo_control": SimpleNamespace(make_robot=lambda _: self.robot)},
),
patch.object(sys, "argv", [str(SCRIPT)]),
patch.object(sys, "stdin") as stdin,
patch("termios.tcgetattr", return_value=["saved"]),
patch("termios.tcsetattr") as restore,
patch("tty.setcbreak"),
patch("builtins.print"),
):
stdin.isatty.return_value = True
stdin.fileno.return_value = 123
if isinstance(outcome, Exception):
with self.assertRaises(type(outcome)):
main()
else:
main()
self.assertFalse(self.robot.is_connected)
self.assertIsNone(self.robot._client)
restore.assert_called_once_with(123, DEMO["termios"].TCSADRAIN, ["saved"])
def test_input_drain_is_bounded_and_eof_is_not_an_infinite_loop(self):
with patch("select.select", return_value=([123], [], [])):
with patch("os.read", return_value=b"u") as read:
self.assertEqual(DEMO["read_keys"](123), ["u"] * 64)
self.assertEqual(read.call_count, 64)
with patch("os.read", return_value=b""), self.assertRaises(EOFError):
DEMO["read_keys"](123)
def test_invalid_speeds_rejected_before_loading_robot_or_connecting(self):
for flag, value in (
("--arm-speed", "nan"),
("--arm-speed", "91"),
("--gripper-speed", "inf"),
("--gripper-speed", "0"),
):
with self.subTest(flag=flag, value=value):
result = subprocess.run(
[sys.executable, str(SCRIPT), flag, value],
capture_output=True,
text=True,
timeout=5,
)
self.assertEqual(result.returncode, 2)
self.assertIn(f"{flag} 必须", result.stderr)
if __name__ == "__main__":
unittest.main()
+201
View File
@@ -0,0 +1,201 @@
import copy
import importlib.metadata
import json
import math
import os
import subprocess
import sys
import unittest
from pathlib import Path
from unittest.mock import patch
import numpy as np
import torch
from lerobot.robots.robot import Robot
from lerobot.robots.utils import make_robot_from_config
from lerobot.utils.errors import DeviceNotConnectedError
from lerobot_robot_mujoco import LeKiwiSim, LeKiwiSimConfig
from lerobot_robot_mujoco.lekiwi_sim import ARM, BASE, FEATURES, PROFILE, from_sim, to_sim
from mujoco_control_bridge import RobotError
from mujoco_control_bridge.protocol import values
def descriptor():
channels = []
for i, key in enumerate(FEATURES):
gripper = key == "arm_gripper.pos"
base = key in BASE
limit = PROFILE["baseAngularLimit"] if key == "theta.vel" else PROFILE["baseLinearLimit"]
channels.append(
{
"id": key,
"unit": "ratio"
if gripper
else "rad/s"
if key == "theta.vel"
else "m/s"
if base
else "rad",
"mode": "opening" if gripper else "velocity" if base else "position",
"min": 0 if gripper else -limit if base else PROFILE["arm"][i]["min"],
"max": 1 if gripper else limit if base else PROFILE["arm"][i]["max"],
}
)
return {
"protocolVersion": 1,
"profileId": "lekiwi-v1",
"profileVersion": 1,
"modelFingerprint": "a" * 64,
"frame": "x-forward-y-left-z-up",
"capabilities": {"reset": True, "lockstep": False, "cameras": False, "training": False},
"actionChannels": channels,
"observationChannels": copy.deepcopy(channels),
}
class FakeClient:
def __init__(self, *_):
self.is_connected = False
self.sent = []
def describe(self):
return descriptor()
def connect(self):
self.is_connected = True
return self.describe()
def get_observation(self):
return {"values": {**dict.fromkeys(FEATURES, 0.1), "arm_gripper.pos": 0.25}}
def send_action(self, value):
self.sent.append(value)
return {"values": values(value, self.describe()["actionChannels"], True)}
def disconnect(self):
self.is_connected = False
class LeRobotPluginTests(unittest.TestCase):
def test_actual_upstream_factory_and_no_hardware_calibration(self):
self.assertEqual(importlib.metadata.version("lerobot"), "0.6.1")
with patch.object(Path, "mkdir", side_effect=AssertionError("hardware calibration I/O")):
robot = make_robot_from_config(LeKiwiSimConfig())
self.assertIsInstance(robot, Robot)
self.assertIsInstance(robot, LeKiwiSim)
self.assertEqual(tuple(robot.action_features), FEATURES)
self.assertEqual(robot.observation_features, robot.action_features)
self.assertTrue(robot.is_calibrated)
self.assertFalse(robot.is_connected)
self.assertEqual(robot.cameras, {})
self.assertFalse(torch.cuda.is_initialized())
with self.assertRaises(DeviceNotConnectedError):
robot.get_observation()
def test_fresh_process_plugin_discovery(self):
result = subprocess.run(
[
sys.executable,
"-c",
"""
import sys
from lerobot.utils.import_utils import register_third_party_plugins
from lerobot.robots.config import RobotConfig
from lerobot.robots.utils import make_robot_from_config
assert 'lerobot_robot_mujoco' not in sys.modules
register_third_party_plugins()
config = RobotConfig.get_choice_class('lekiwi_sim')()
robot = make_robot_from_config(config)
assert robot.name == 'lekiwi_sim'
assert len(robot.action_features) == 9
assert not any(m in sys.modules for m in ('serial','zmq','pyrealsense2','scservo_sdk'))
""",
],
env={**os.environ, "PYTHONPATH": "", "PYTHONNOUSERSITE": "1"},
capture_output=True,
text=True,
)
self.assertEqual(result.returncode, 0, result.stderr)
def test_profile_resource_matches_canonical(self):
root = Path(__file__).resolve().parents[3]
self.assertEqual(PROFILE, json.loads((root / "robot_profiles/lekiwi-v1.json").read_text()))
def test_bidirectional_units_and_upstream_scalar_values(self):
for key in FEATURES:
for value in (-30.0, 0.0, 30.0):
self.assertAlmostEqual(from_sim(key, to_sim(key, value)), value)
self.assertAlmostEqual(to_sim(ARM[0], 90), math.pi / 2)
self.assertEqual(to_sim("arm_gripper.pos", 50), 0.5)
self.assertAlmostEqual(to_sim("theta.vel", 30), math.pi / 6)
self.assertAlmostEqual(to_sim(ARM[0], np.float32(10)), math.radians(10))
self.assertAlmostEqual(to_sim(ARM[0], torch.tensor(10.0)), math.radians(10))
for bad in (
True,
np.bool_(True),
"2",
[1],
np.array([1]),
torch.tensor([1.0]),
float("nan"),
float("inf"),
):
with self.subTest(value=repr(bad)), self.assertRaises(RobotError):
to_sim(ARM[0], bad)
def test_partial_action_holds_confirmed_arm_and_stops_omitted_base(self):
with patch("lerobot_robot_mujoco.lekiwi_sim.SimRobotClient", FakeClient):
robot = LeKiwiSim(LeKiwiSimConfig())
robot.connect()
first = robot.send_action({ARM[0]: 180, "arm_gripper.pos": 150, "x.vel": 0.1})
self.assertAlmostEqual(first[ARM[0]], math.degrees(PROFILE["arm"][0]["max"]))
self.assertEqual(first["arm_gripper.pos"], 100)
self.assertEqual(first["x.vel"], 0.1)
second = robot.send_action({ARM[1]: 5})
self.assertEqual(second[ARM[0]], first[ARM[0]])
self.assertEqual(second["arm_gripper.pos"], 100)
self.assertEqual(second["x.vel"], 0)
self.assertEqual(second["theta.vel"], 0)
self.assertEqual(robot.get_observation()["arm_gripper.pos"], 25)
self.assertNotEqual(robot.get_observation()[ARM[0]], first[ARM[0]])
with self.assertRaises(RobotError):
robot.send_action({"unknown": 1})
robot.stop_base()
robot.disconnect()
robot.disconnect()
def test_reject_unsupported_configuration_and_descriptor(self):
for kwargs in (
{"use_degrees": False},
{"cameras": {"camera": object()}},
{"calibration_dir": Path("/tmp/never-touch")},
{"endpoint": "http://token@localhost"},
):
with self.assertRaises(ValueError):
LeKiwiSimConfig(**kwargs)
config = LeKiwiSimConfig(token="secret-not-real")
self.assertNotIn("secret-not-real", repr(config))
robot = LeKiwiSim(config)
for field in ("profileId", "profileVersion", "unit", "mode"):
desc = descriptor()
if field == "profileVersion":
desc[field] = 2
elif field == "profileId":
desc[field] = "other"
else:
desc["actionChannels"][0][field] = "wrong"
with self.assertRaises(RobotError):
robot._validate_descriptor(desc)
def test_keyboard_helpers_match_upstream_key_semantics(self):
robot = LeKiwiSim(LeKiwiSimConfig())
self.assertEqual(
robot._from_keyboard_to_base_action(["w", "a", "z"]),
{"x.vel": 0.1, "y.vel": 0.1, "theta.vel": 30},
)
self.assertEqual(robot._from_keyboard_to_base_action(["w", "s", "r"])["x.vel"], 0)
self.assertEqual(robot._from_keyboard_to_base_action(["d"])["y.vel"], -0.2)
if __name__ == "__main__":
unittest.main()
@@ -0,0 +1,54 @@
"""Run the installed upstream's pure methods, without constructing a hardware robot."""
import ast
import importlib.metadata
import json
import unittest
from pathlib import Path
from typing import Any
import numpy as np
class UpstreamKinematicsTests(unittest.TestCase):
def test_actual_061_methods_match_shared_wheel_fixture(self):
self.assertEqual(importlib.metadata.version("lerobot"), "0.6.1")
path = importlib.metadata.distribution("lerobot").locate_file(
"lerobot/robots/lekiwi/lekiwi.py"
)
tree = ast.parse(path.read_text())
robot = next(
node for node in tree.body if isinstance(node, ast.ClassDef) and node.name == "LeKiwi"
)
names = {"_degps_to_raw", "_raw_to_degps", "_body_to_wheel_raw", "_wheel_raw_to_body"}
methods = [
node for node in robot.body if isinstance(node, ast.FunctionDef) and node.name in names
]
self.assertEqual({node.name for node in methods}, names)
# Only the four audited numeric methods. No imports, constructors, buses or cameras.
cls = ast.ClassDef(
name="PureKinematics",
bases=[],
keywords=[],
body=methods,
decorator_list=[],
type_params=[],
)
namespace = {"np": np, "Any": Any}
module = ast.fix_missing_locations(ast.Module(body=[cls], type_ignores=[]))
exec(compile(module, str(path), "exec"), namespace)
pure = namespace["PureKinematics"]()
fixture = json.loads(
(
Path(__file__).resolve().parents[3] / "contracts/fixtures/lekiwi-kinematics.json"
).read_text()
)
for case in fixture["cases"]:
with self.subTest(body=case["body"]):
raw = pure._body_to_wheel_raw(*case["body"])
self.assertEqual(list(raw), fixture["wheelOrder"])
self.assertEqual(list(raw.values()), case["wheelRaw"])
if max(abs(v) for v in case["wheelRaw"]) < 3000:
body = pure._wheel_raw_to_body(*case["wheelRaw"])
for observed, expected in zip(body.values(), case["body"], strict=True):
self.assertAlmostEqual(observed, expected, delta=0.02)
+2 -2
View File
@@ -1,12 +1,12 @@
{
"name": "mujoco-web-platform",
"version": "0.9.5",
"version": "0.10.1",
"lockfileVersion": 3,
"requires": true,
"packages": {
"": {
"name": "mujoco-web-platform",
"version": "0.9.5",
"version": "0.10.1",
"license": "Apache-2.0",
"dependencies": {
"@monaco-editor/react": "^4.7.0",
+8 -4
View File
@@ -1,6 +1,6 @@
{
"name": "mujoco-web-platform",
"version": "0.9.5",
"version": "0.10.1",
"description": "基于 MuJoCo WebAssembly 的本地机器人仿真与控制平台",
"private": true,
"type": "module",
@@ -12,14 +12,18 @@
"lint": "eslint web_platform/src web_platform/e2e",
"test": "vitest run --config web_platform/vite.config.ts",
"test:e2e": "playwright test -c web_platform/playwright.config.ts",
"test:e2e:lekiwi": "playwright test -c web_platform/playwright.lekiwi.config.ts",
"control-bridge": "python3 -m mujoco_control_bridge",
"test:control-bridge": "python3 -m unittest discover -s control_bridge/tests -v",
"setup:lerobot": "python3 examples/lekiwi/setup_lerobot.py",
"training-server": "python3 training_server/server.py",
"test:training-server": "python3 -m unittest discover -s training_server/tests",
"check": "npm run typecheck && npm run lint && npm run check:format && npm run test:coverage && npm run test:training-server && npm run build",
"check": "npm run typecheck && npm run lint && npm run check:format && npm run test:coverage && npm run test:training-server && npm run test:control-bridge && npm run build",
"format": "prettier --write .",
"check:format": "prettier --check .",
"test:coverage": "vitest run --coverage --config web_platform/vite.config.ts",
"lint:python": "python3 -m ruff check training_server",
"format:python": "python3 -m ruff format training_server"
"lint:python": "python3 -m ruff check training_server control_bridge integrations/lerobot/src integrations/lerobot/tests examples/lekiwi",
"format:python": "python3 -m ruff format training_server control_bridge integrations/lerobot/src integrations/lerobot/tests examples/lekiwi"
},
"license": "Apache-2.0",
"devDependencies": {
+1
View File
@@ -1,2 +1,3 @@
-r training_server/requirements.txt
ruff==0.16.5
-e ./control_bridge
+201
View File
@@ -0,0 +1,201 @@
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 [yyyy] [name of copyright owner]
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.
+52
View File
@@ -0,0 +1,52 @@
# LeKiwi 碰撞数据来源与重建
## 当前配方:revision 4
`lekiwi-full-collision.json` 是 [SIGRobotics-UIUC/LeKiwi](https://github.com/SIGRobotics-UIUC/LeKiwi) 的派生模型数据,来源 revision 为 `efa608d7ee5a495a4803b1d28cd0c955b4f1e033`,按 [Apache-2.0](LeKiwi.LICENSE.txt) 分发。固定 URDF/STL SHA-256 与装配例外配置见 `lekiwi-collision-source.json`,生成结果也保存完整 `source`。
本项目修改:从 `Base_08q-v1` 子树的**全部 18 个视觉网格**离线 CoACD 得到 538 个凸包,再按装配配合区切分为 1,220 个独立碰撞凸包,包含固定焊接的舵机、安装板、相机支架及相机外壳;替代旧臂胶囊和手工分段配方。此处“整臂”不包含底盘/全向轮;它们仍使用独立简化配方。
- 使用 [CoACD](https://github.com/SarahWeiii/CoACD) 1.0.14,`real_metric=true`、`threshold=0.003`、固定 seed=42、4 个 OpenMP 线程,CoACD 阶段每包最多 96 个顶点,切分后预算为 256(当前最多 167);完整参数和版本存入 `generator`。**3 mm 是分解参数,不是已证明的全表面误差上限。**
- 输入按 URDF `scale` 转成米,输出保留原视觉网格局部坐标、顶点取整到 1 μm。运行时继承 visual geom 的 body/pose,不跨越两指合成凸包;碰撞体 `mass=0`,沿用显式惯性估计。
- 浏览器只读取生成数据,导入时校验源 STL 哈希、视觉覆盖清单和运动刚体归属;不会在线运行 CoACD,也不修改参考仓库、训练或 LeRobot 环境。
- 配方使用 1 ms 物理步长、CAD 接触 `solref=".002 1"`;不是加大安全超时或放宽关节范围。几何、关节过滤和这些接触参数共同形成 `recipeSha256`,导出 MJCF 携带 revision 与配方指纹。标记用于兼容性检查,不是防篡改签名。
### 相邻关节不能整对排除
MuJoCo 的默认父子过滤保持开启;显式加入 **5,581 对结构凸包接触**,包括 `Mirror / Square` 和两指指面。没有整对相邻 body 的 `<exclude>`。
唯一局部装配例外:一个**完整凸包**必须落在该关节审核过的轴承/舵机配合圆柱内,才可忽略它与直接相邻运动组的配合接触。半径分别为肩旋 23、肩俯仰/肘/夹爪 18.5、腕俯仰 16、腕旋 12.1 mm,轴向半长 55 mm;不是“中立位重叠就排除”。这些部件与外界及其他非相邻臂段仍参与碰撞。
跨越配合区边界的凸包先按内接 16 边棱柱切开(2 μm 内缩以容纳取整误差),**配合区和外侧各片都保留**,不挖掉对外碰撞几何。生成器检查切分体积的量化误差;单测覆盖体积保持和内部/结构区分类。否则同一个凸包跨越轴承与结构区,即使中立位不碰撞,转动后仍可能误锁轴承。CAD 审核在肩旋约 0.35 rad 发现了这种情况;修复后真实 WASM 肩旋可双向到达 ±0.942 rad,且没有配合区接触。
另外以整周旋转的轴向/径向包络保守剔除不可能相交的凸包对,保留 0.1 mm 数值余量。各关节的成员、显式接触对、配合例外/扫掠剔除数量都在 `jointPolicy`,改参数后必须重新审核和跑物理回归,不能只改指纹。
### 离线环境和重建
从仓库根目录执行,先按 [示例说明](../examples/lekiwi/README.md) 准备固定源资产:
```bash
python3 -m venv build/venvs/collision
env -u PYTHONPATH PYTHONNOUSERSITE=1 build/venvs/collision/bin/python \
-m pip install -r examples/lekiwi/requirements-collision.txt
env -u PYTHONPATH PYTHONNOUSERSITE=1 build/venvs/collision/bin/python \
examples/lekiwi/generate_full_collisions.py --check
# 确需写入重建数据时去掉 --check
```
默认源目录 `build/lekiwi/URDF`,缓存 `build/collision-cache`,均可通过 CLI 覆盖。缓存键包含源 STL 哈希、缩放、工具版本、参数与线程数;关节过滤每次重算。`--check` 比较完整生成结果,不改已发布 JSON;使用新的 `--cache` 目录可验证从零分解,而非只命中缓存。本机已完成一次无缓存重建一致性验证,不承诺任意平台/工具版本逐字节相同。
```bash
env -u PYTHONPATH PYTHONNOUSERSITE=1 build/venvs/collision/bin/python \
-m unittest discover -s examples/lekiwi/tests -v
```
生成器拒绝缺失/变更源、非法缩放、退化凸包、超预算包数/顶点数及采样覆盖不合格。`coverage.coacdSampleOutsidePlanesMaxM` 是抽样点相对 CoACD 阶段凸包平面的诊断值,**不是 Hausdorff 距离或连续全表面保证**。`coverage.coacdParts/parts` 分别记录切分前后数量。浏览器额外从原始 STL 独立取每个视觉网格的六向极值,做 108 次球体接触探测;另测六关节双向目标扫掠、相邻连杆主体阻挡/脱离、中立位、两指空隙/物体阻挡及真实 60 秒控制性能。仍未穷举多关节配置空间。
### 历史数据
`lekiwi-gripper-collision.json` / `generate_gripper_collisions.py`(revision 2)和 `lekiwi-arm-collision.json` / `generate_arm_collisions.py`(revision 3)保留历史重建用途,同样源自上述 Apache-2.0 资产;当前前端不再导入它们。旧方案是 STL 平面裁剪、1 mm 接缝重叠后取凸包,并非 CoACD。
旧 MJCF(包括 revision 3)必须从原始 URDF 重新转换。自动凸分解、局部装配例外和软接触仍是近似,不保证所有薄壁间隙、任意高速动作、可靠抓取或实体标定;观察到提前接触时,应检查具名接触对与原 CAD,不能靠扩大 body 排除或关闭自碰撞掩盖。当前仅支持上述固定 LeKiwi 资产,不是任意 URDF 的免审核导入开关。
开发期强制把 qpos 写入严重重叠姿态的几何查询曾使 WASM 触及 2 GiB 内存上限并原生中止;这不是支持的控制路径,也未在此修复引擎本身。profile 的普通关节编辑入口已禁止瞬移;验收使用有界执行器运动、正常初始姿态和有限场景,不能由此承诺任意穿透初态稳定。诊断失败记录保存在 `build/e2e/lekiwi-full-collision-geometry-diagnostic/`。
File diff suppressed because one or more lines are too long
@@ -0,0 +1,60 @@
{
"repository": "https://github.com/SIGRobotics-UIUC/LeKiwi",
"revision": "efa608d7ee5a495a4803b1d28cd0c955b4f1e033",
"urdfSha256": "36e102375e1e6ffdc1ca2d343c7406284b3cabc77ae16bfc001a3da89fddc6e1",
"license": "Apache-2.0",
"lerobotVersion": "0.6.1",
"rootLink": "Base_08q-v1",
"meshes": {
"meshes/Base_08q-v1.stl": "a05be37db52657615ac69423fe577979efa0cd0634a5e9546bbc85537f003172",
"meshes/WaveShare_Mounting_Plate_01d-v1.stl": "abde2af470078f125c6e001e64abcae7c4307bff02f27c3a10dc2a87f7039bed",
"meshes/Rotation_Pitch_08i-v1.stl": "747a37e0c99c8fa85ad52ccf9fbfbb945468d8f9f590a1b7faadf5188d741ba9",
"meshes/STS3215_03a-v1.stl": "07695009c2e9ef352787fe98814d85eb936228a1843a0c76d22ab1bb3d37eb92",
"meshes/SO_ARM100_08k_Asym_Mirror_Clip-v1.stl": "0f336a7f3940c943554252032ae8e351196353b8aa983827f045c4d7aa4cc52d",
"meshes/Passive_Horn_01-v1.stl": "fca61eb1fb63b4bb04ff0c80d395ae10ae69787a7da7e15414234d39b8b0ab7d",
"meshes/STS3215_03a-v1-1.stl": "07695009c2e9ef352787fe98814d85eb936228a1843a0c76d22ab1bb3d37eb92",
"meshes/SO_ARM100_08k_116_Square-v1.stl": "64fdc308759ff58756e3c39a0ea8018d6b37b551bc3d29238b6e86bc5acae666",
"meshes/STS3215_03a-v1-2.stl": "07695009c2e9ef352787fe98814d85eb936228a1843a0c76d22ab1bb3d37eb92",
"meshes/SO_ARM100_08k_Mirror-v1.stl": "f6a2abf909ddd6e27c6fef5791937497a7686221dc1d877a7b30820b91cc1ca6",
"meshes/STS3215_03a-v1-3.stl": "07695009c2e9ef352787fe98814d85eb936228a1843a0c76d22ab1bb3d37eb92",
"meshes/Wrist_Roll_Pitch_08i-v1.stl": "964cef356d5a120cd7d65018fcbd89a7f2dcd5f7a503d415720f94c452eb5da6",
"meshes/STS3215_03a_Wrist_Roll-v1.stl": "b390f54c455a7900687af22432c8f742f4060d3c1afb90091c2fbf1dc3a9b978",
"meshes/Wrist_Roll_08c-v1.stl": "87507f73f485c2cacb3dc83924a712069fbd68f72b82d4a31a2cfe7f58c9e4c9",
"meshes/STS3215_03a-v1-4.stl": "07695009c2e9ef352787fe98814d85eb936228a1843a0c76d22ab1bb3d37eb92",
"meshes/Moving_Jaw_08d-v1.stl": "71caabee267376791210950b3b4e2f7968d9b57b92ed3f57711c03f2b5666912",
"meshes/Wrist-Camera-Mount-v11.stl": "2bb123c754eff42c57c4605a533b94d14e7b51889a813764f798b7878489a98a",
"meshes/Camera-Model-v3-1.stl": "f5ff94543c19e91a6d5ffb540e2d25347d813d191793405ff2e4178230dec21e"
},
"assemblyCores": {
"arm_shoulder_pan": {
"radiusM": 0.023,
"axialHalfExtentM": 0.055,
"reason": "Bounded mating envelope with straddling hull partitioning: CAD clearance audit found a false bearing contact at radius 19.1mm; verify both pan directions remain free, while outside-core structural contacts stay enabled."
},
"arm_shoulder_lift": {
"radiusM": 0.0185,
"axialHalfExtentM": 0.055,
"reason": "Adjacent joint bearing/servo mating core only; outside-core structural hull pairs remain enabled."
},
"arm_elbow_flex": {
"radiusM": 0.0185,
"axialHalfExtentM": 0.055,
"reason": "Adjacent joint bearing/servo mating core only; outside-core structural hull pairs remain enabled."
},
"arm_wrist_flex": {
"radiusM": 0.016,
"axialHalfExtentM": 0.055,
"reason": "Adjacent joint bearing/servo mating core only; outside-core structural hull pairs remain enabled."
},
"arm_wrist_roll": {
"radiusM": 0.0121,
"axialHalfExtentM": 0.055,
"reason": "Adjacent joint bearing/servo mating core only; outside-core structural hull pairs remain enabled."
},
"arm_gripper": {
"radiusM": 0.0185,
"axialHalfExtentM": 0.055,
"reason": "Adjacent joint bearing/servo mating core only; outside-core structural hull pairs remain enabled."
}
}
}
File diff suppressed because it is too large Load Diff
File diff suppressed because one or more lines are too long
+98
View File
@@ -0,0 +1,98 @@
{
"id": "lekiwi-v1",
"version": 1,
"source": {
"repository": "https://github.com/SIGRobotics-UIUC/LeKiwi",
"revision": "efa608d7ee5a495a4803b1d28cd0c955b4f1e033",
"urdfSha256": "36e102375e1e6ffdc1ca2d343c7406284b3cabc77ae16bfc001a3da89fddc6e1",
"license": "Apache-2.0",
"lerobotVersion": "0.6.1"
},
"baseBody": "base_plate_layer1-v5",
"frame": "x-forward-y-left-z-up",
"mapping": {
"cadYawRad": -1.5707963267948966,
"armSigns": [1, 1, 1, 1, 1],
"armOffsetsRad": [0, 0, 0, 0, 0],
"gripperAxisSign": -1
},
"controlHz": 30,
"watchdogMs": 500,
"wheelRadius": 0.05,
"baseRadius": 0.125,
"wheelMaxRadS": 4.601942363656923,
"baseLinearLimit": 0.2,
"baseAngularLimit": 1.0471975511965976,
"wheels": ["base_left_wheel", "base_back_wheel", "base_right_wheel"],
"arm": [
{
"joint": "arm_shoulder_pan",
"min": -1.57,
"max": 1.57,
"neutral": 0,
"kp": 40,
"kv": 3,
"force": 8
},
{
"joint": "arm_shoulder_lift",
"min": -1.4,
"max": 1.4,
"neutral": 0,
"kp": 60,
"kv": 4,
"force": 10
},
{
"joint": "arm_elbow_flex",
"min": -1.4,
"max": 1.4,
"neutral": 0,
"kp": 50,
"kv": 3,
"force": 8
},
{
"joint": "arm_wrist_flex",
"min": -1.4,
"max": 1.4,
"neutral": 0,
"kp": 30,
"kv": 2,
"force": 5
},
{
"joint": "arm_wrist_roll",
"min": -3.14,
"max": 3.14,
"neutral": 0,
"kp": 20,
"kv": 1,
"force": 3
},
{
"joint": "arm_gripper",
"min": -0.18,
"max": 0.9,
"neutral": 0,
"kp": 15,
"kv": 1,
"force": 2
}
],
"simulation": {
"baseMass": 2.2,
"baseInertia": [0.014, 0.014, 0.022],
"armMasses": [0.1, 0.25, 0.22, 0.08, 0.09, 0.025],
"wheelMass": 0.06,
"rollerCount": 12,
"rollerMass": 0.003,
"wheelKv": 0.5,
"wheelTorqueLimit": 2.0
},
"parameterNotes": {
"kinematics": "Wheel radius/base radius and proportional wheel saturation follow LeRobot 0.6.1; raw 3000 ticks/s converts to wheelMaxRadS. CAD frame mapping is explicit in model enhancement. The CAD gripper axis is reversed so increasing canonical angle opens the jaws; the -0.18 rad closing limit leaves approximately 0.5 mm fingertip clearance.",
"dynamics": "Simulation-only estimated collision, mass, inertia, joint limits and servo gains. NOT measured hardware calibration or a sim-to-real model.",
"scope": "Flat ground low-speed control; no cameras, training, rough terrain or reliable grasping claim."
}
}
+10 -1
View File
@@ -12,6 +12,7 @@ src/
├── map/ 地图加载、生成、编辑和面板
├── project/ 工程导入、转换、文件树和工作区
├── rl/ ONNX 策略运行时、任务绑定、类型和面板
├── robot/ 通用机器人契约、profile、实时协议与外部控制面板
├── simulation/ MuJoCo 会话、物理适配器和仿真控制组件
├── telemetry/ 数据源抽象、记录器、导出和数据面板
├── training/ 本地训练/调参客户端、类型、共享连接和面板
@@ -41,4 +42,12 @@ SimulationSession snapshot → app → viewer / 各业务面板
主工作台由 `index.html → src/main.tsx` 启动;自调参工作台由 Vite MPA 入口 `tuning.html → src/tuning/main.tsx` 启动,避免把 MuJoCo/Three.js 主应用依赖打入监控页面。两页仅通过训练 HTTP API和严格同源的短消息交接训练服务凭据/策略导入请求,不在 URL 中传 token。
测试文件使用 `*.test.ts(x)` 与被测模块共置;端到端测试统一保存在 `e2e/`。
## 机器人接口边界
`robot/types + validation` 定义跨语言契约,`robot_profiles/` 是唯一参数源;可信 `RobotAdapter` 封装物理模型,`ModelBindings` 为机器人/Python/Go2 共用索引。`ControlArbiter` 在 Session 边界授予身份票据,所有写入都经过所有者检查,不依赖 UI 灰化。
外部 Python SDK → 独立 `control_bridge` → `ExternalControlClient` → `RobotRuntime` 最新目标邮箱。网络事件不直接改 qpos/qvel;物理步前施加目标、步后确认,观测是实测状态。外控的有界固定-dt 调度器独立于 renderer,React 快照不是传输时钟。暂停时由轻量生命周期轮询补一次状态快照,避免 UI 停留在旧授权状态。
`app/hooks/useExternalControl` 只负责连接、内存凭据、显式授权及隐藏/卸载清理。profile 变更走既有候选编译事务;回滚不恢复 lease。协议、指纹、单位、软件渲染预算和后续独立 task/RL 层详见 [机器人接口](../docs/robot-interface.md)。
测试文件使用 `*.test.ts(x)` 与被测模块共置;端到端测试统一保存在 `e2e/`。`physics/runner.html` 是仅开发服务器提供的真实 WASM 夹具,不进入生产构建;普通桥接使用小型通用模型,完整 LeKiwi / LeRobot / 60 秒 UI 回归通过独立配置和 CPU 环境运行。
+10 -1
View File
@@ -17,6 +17,7 @@
- 导入 mjlab 导出的 `policy.onnx`,在浏览器本地执行 Go2-W 平衡/速度策略推理
- 从图形界面向本机训练桥接服务发起 mjlab 强化学习训练、查看进度/日志、停止任务并导入训练生成的 ONNX;可在独立 TensorBoard 风格页面运行 DeepSeek 奖励函数自调参
- 可配置仿真遥测记录,实时查看速度、机身姿态、位置、驱动力等指标并导出 CSV/JSON
- 通用机器人 profile、外部控制授权和独立本机桥接;首个适配为 LeKiwi + 实际 LeRobot 0.6.1
- FPS、物理耗时和主线程步进预算提示
## 开发
@@ -43,7 +44,7 @@ npm run test:e2e
npm run check
```
E2E 默认使用系统安装的 Google Chrome。若没有 Chrome,可修改 `playwright.config.ts` 或运行 `npx playwright install chromium` 后移除 `channel: 'chrome'`。
E2E 使用 Playwright Chromium,先运行 `npx playwright install chromium`。普通桥接用例需要 Python 3.12 与 `pip install -e control_bridge`;可设置 `BRIDGE_PYTHON`,默认优先项目 `.venv/bin/python`。完整 LeKiwi 套件单独运行 `npm run test:e2e:lekiwi`,不纳入普通 E2E。
## 生产构建与本地静态部署
@@ -110,6 +111,14 @@ Python 控制器是可信的单文件脚本,必须同步定义 `step(ctx, stat
当前 Python 与 MuJoCo 都运行在主线程,以保证闭环调用严格位于 `mj_step` 前。仅运行可信脚本;死循环仍可能阻塞页面。Pyodide 及 Python 标准库由 npm 包随生产构建离线发布,不从 CDN 下载;暂不支持第三方 Python 包、`pip` 或多文件 import。
## 开源项目 / 外部机器人控制
右侧“控制台”包含 profile 选择、校验/重编译、桥地址/token、连接、明确授权和停止,以及所有者、epoch、动作确认与观测年龄。URDF 导入对话框可直接选择 LeKiwi v1;普通模型默认不启用,导出 MJCF 重新导入也必须明确选择。模型变更失败保留旧场景,但不恢复授权;profile 的固定伺服参数不能用通用面板修改。
先播放再授权。接管会停止 Python/ONNX、锁定 1×,执行器和关节写入被禁止;暂停、重置、隐藏页面、离开、超时或重载后必须重新授权。控制 token 仅在当前页面内存,不保存到工程或浏览器存储。
外控物理与约30Hz观测独立于渲染/React。软件 WebGL 会关闭 profile 阴影并把外控显示降至5FPS(硬件上限30FPS),避免主线程渲染拖慢控制;仍不提供硬实时或锁步保证。完整准备、实际 LeRobot 示例和限制见 [LeKiwi 文档](../examples/lekiwi/README.md)、[统一接口](../docs/robot-interface.md)。此控制桥不是训练服务,当前 profile 明确 `training=false/cameras=false/lockstep=false`。
## 本地强化学习训练
训练仍由本机 Python/mjlab 进程执行,但可以从右侧“控制 → 本地强化学习训练”直接发起和管理。仓库已经内置默认 Go2 任务的训练代码与资产;先按 [`training_server/README.md`](../training_server/README.md) 安装训练依赖,再使用对应 Python 启动本地桥接服务:
+46
View File
@@ -0,0 +1,46 @@
import { expect, test } from '@playwright/test';
import { startBridge } from './fixtures/controlBridge';
import type {} from '../physics/runner';
test('通用单关节:真实 Python SDK ↔ 本机桥接 ↔ 浏览器 WASM,断开后安全保持', async ({ page }) => {
const bridge = await startBridge();
try {
await page.goto('http://127.0.0.1:4174/physics/runner.html');
await page.waitForFunction(() => Boolean(window.lekiwiPhysics));
await page.evaluate(() => window.lekiwiPhysics.bootSingle());
await page.evaluate(
({ endpoint, token }) => window.lekiwiPhysics.connectBridge(endpoint, token),
{ endpoint: bridge.endpoint, token: bridge.token },
);
const run = await bridge.python(`
import json, os, time
from mujoco_control_bridge import SimRobotClient
with SimRobotClient(os.environ['MUJOCO_CONTROL_ENDPOINT']) as robot:
start=time.monotonic()
for i in range(60):
accepted=robot.send_action({'slider.position': .5})
obs=robot.get_observation()
time.sleep(max(0,start+(i+1)/30-time.monotonic()))
print(json.dumps({'accepted':accepted['values'], 'measured':obs['values'], 'seq':obs['appliedActionSeq']}))
`);
const result = JSON.parse(run.stdout.trim()) as {
accepted: Record<string, number>;
measured: Record<string, number>;
seq: number;
};
expect(result.accepted['slider.position']).toBe(0.5);
expect(result.measured['slider.position']).toBeGreaterThan(0.48);
expect(result.measured['slider.position']).toBeLessThan(0.51);
expect(result.seq).toBeGreaterThan(55);
await expect
.poll(
async () =>
(await page.evaluate(() => window.lekiwiPhysics.externalState())).observation?.paused,
)
.toBe(true);
expect(bridge.errors()).toBe('');
} finally {
await page.evaluate(() => window.lekiwiPhysics?.dispose()).catch(() => {});
await bridge.stop();
}
});
@@ -0,0 +1,82 @@
import { spawn, execFile, type ChildProcess } from 'node:child_process';
import { randomBytes } from 'node:crypto';
import { existsSync } from 'node:fs';
import { resolve } from 'node:path';
import { promisify } from 'node:util';
const execFileAsync = promisify(execFile);
export const bridgePython =
process.env.BRIDGE_PYTHON ??
(existsSync(resolve('.venv/bin/python')) ? resolve('.venv/bin/python') : 'python3');
export async function stopProcess(child: ChildProcess): Promise<void> {
if (child.exitCode !== null || child.signalCode !== null) return;
await new Promise<void>((resolve) => {
const timer = setTimeout(() => child.kill('SIGKILL'), 3000);
child.once('exit', () => {
clearTimeout(timer);
resolve();
});
child.kill('SIGTERM');
});
}
export async function startBridge() {
const token = randomBytes(32).toString('hex');
const child = spawn(bridgePython, ['-m', 'mujoco_control_bridge', '--port', '0'], {
env: { ...process.env, PYTHONPATH: '', PYTHONNOUSERSITE: '1', MUJOCO_CONTROL_TOKEN: token },
stdio: ['ignore', 'pipe', 'pipe'],
});
let output = '',
errors = '';
child.stderr.on('data', (chunk: Buffer) => {
errors = (errors + chunk.toString()).slice(-65536);
});
try {
const endpoint = await new Promise<string>((resolve, reject) => {
const timeout = setTimeout(() => reject(new Error(`桥接启动超时:${errors}`)), 15000);
child.once('error', (error) => {
clearTimeout(timeout);
reject(error);
});
child.once('exit', () => {
clearTimeout(timeout);
reject(new Error(`桥接启动失败:${errors}`));
});
child.stdout.on('data', (chunk: Buffer) => {
output += chunk.toString();
const ready = output.match(/CONTROL_BRIDGE_READY (\{[^\n]+\})/);
if (ready) {
clearTimeout(timeout);
resolve((JSON.parse(ready[1]) as { endpoint: string }).endpoint);
}
});
});
return {
endpoint,
token,
stop: () => stopProcess(child),
errors: () => errors,
python: async (
script: string,
python = bridgePython,
environment: Record<string, string> = {},
) => {
const { stdout, stderr } = await execFileAsync(python, ['-c', script], {
env: {
...process.env,
PYTHONPATH: '',
PYTHONNOUSERSITE: '1',
...environment,
MUJOCO_CONTROL_TOKEN: token,
MUJOCO_CONTROL_ENDPOINT: endpoint,
},
timeout: 120000,
maxBuffer: 4 * 1024 * 1024,
});
return { stdout, stderr };
},
};
} catch (error) {
await stopProcess(child);
throw error;
}
}
@@ -0,0 +1,42 @@
import { expect, test } from '@playwright/test';
import { resolve } from 'node:path';
import { writeFile } from 'node:fs/promises';
import type {} from '../physics/runner';
test('真实 WASM:上臂不能穿过自身臂座组件,接触后仍能反向离开', async ({ page }, info) => {
const errors: string[] = [];
page.on('pageerror', (error) => errors.push(error.message));
await page.goto('/physics/runner.html');
await page.waitForFunction(() => Boolean(window.lekiwiPhysics));
const initial = await page.evaluate(
(path) => window.lekiwiPhysics.boot(path),
`/@fs${resolve('build/lekiwi')}`,
);
try {
const samples = await page.evaluate(() => window.lekiwiPhysics.armSelfCollisionChecks());
console.log('arm/base contacts', JSON.stringify(samples));
const evidence = info.outputPath('arm-base-contacts.json');
await writeFile(evidence, JSON.stringify({ version: initial.version, samples }, null, 2));
await info.attach('arm-base-contacts', { path: evidence, contentType: 'application/json' });
expect(initial.version).toBe('3.11.0');
for (const sample of samples) {
expect(Math.abs(sample.neutral.actual), `neutral, pan=${sample.pan}`).toBeLessThan(0.01);
expect(sample.neutral.contacts).toEqual([]);
// Full CAD changes the first obstruction: WaveShare plate at pan=0,
// rotating shoulder clip at +/-0.8. Keep real contact/penetration/release
// assertions; original Base_08q coverage is independently probed from STL.
expect(sample.blocked.maxContacts, `blocked, pan=${sample.pan}`).toBeGreaterThan(0);
expect(sample.blocked.contacts.length).toBeGreaterThan(0);
expect(sample.blocked.actual).toBeLessThan(sample.blocked.target - 0.15);
expect(sample.blocked.maxPenetration).toBeLessThan(0.002);
for (const contact of sample.blocked.contacts)
expect(contact.distance).toBeGreaterThan(-0.0005);
expect(sample.blocked.finite).toBe(true);
expect(Math.abs(sample.released.actual + 0.3)).toBeLessThan(0.01);
expect(sample.released.contacts).toEqual([]);
}
expect(errors).toEqual([]);
} finally {
await page.evaluate(() => window.lekiwiPhysics.dispose());
}
});
+52
View File
@@ -0,0 +1,52 @@
import { expect, test } from '@playwright/test';
import { resolve } from 'node:path';
import { startBridge } from './fixtures/controlBridge';
import type {} from '../physics/runner';
test('LeKiwi:真实通用 Python SDK 双向控制,3秒30Hz,无 LeRobot 依赖', async ({ page }) => {
const bridge = await startBridge();
try {
await page.goto('/physics/runner.html');
await page.waitForFunction(() => Boolean(window.lekiwiPhysics));
await page.evaluate(
(path) => window.lekiwiPhysics.boot(path),
`/@fs${resolve(process.cwd(), 'build/lekiwi')}`,
);
await page.evaluate(
({ endpoint, token }) => window.lekiwiPhysics.connectBridge(endpoint, token),
{ endpoint: bridge.endpoint, token: bridge.token },
);
const run = await bridge.python(`
import json, os, time
from mujoco_control_bridge import SimRobotClient
with SimRobotClient(os.environ['MUJOCO_CONTROL_ENDPOINT']) as robot:
values={c['id']:0.0 for c in robot.describe()['actionChannels']}
values.update({'arm_shoulder_pan.pos':.1,'arm_gripper.pos':.5})
before=robot.get_observation()['values']
start=time.monotonic(); latency=[]
for i in range(90):
values.update({'x.vel':.08 if i<30 else 0,'y.vel':.08 if 30<=i<60 else 0,'theta.vel':.3 if i>=60 else 0})
tick=time.monotonic(); robot.send_action(values); obs=robot.get_observation()
latency.append((time.monotonic()-tick)*1000)
time.sleep(max(0,start+(i+1)/30-time.monotonic()))
print(json.dumps({'before':before,'after':obs['values'],'rttMeanMs':sum(latency)/len(latency),'rttMaxMs':max(latency),'seq':obs['appliedActionSeq']}))
`);
const result = JSON.parse(run.stdout.trim());
expect(result.after['base.x'] - result.before['base.x']).toBeGreaterThan(0.04);
expect(result.after['base.y'] - result.before['base.y']).toBeGreaterThan(0.04);
expect(result.after['base.yaw']).toBeGreaterThan(0.2);
expect(result.after['arm_gripper.pos']).toBeCloseTo(0.5, 1);
expect(result.seq).toBeGreaterThan(85);
console.log('SDK bridge timing', { meanMs: result.rttMeanMs, maxMs: result.rttMaxMs });
await expect
.poll(
async () =>
(await page.evaluate(() => window.lekiwiPhysics.externalState())).observation?.paused,
)
.toBe(true);
expect(bridge.errors()).toBe('');
} finally {
await page.evaluate(() => window.lekiwiPhysics?.dispose()).catch(() => {});
await bridge.stop();
}
});
@@ -0,0 +1,91 @@
import { expect, test } from '@playwright/test';
import { resolve } from 'node:path';
import { writeFile } from 'node:fs/promises';
import type {} from '../physics/runner';
test('真实 WASM:相邻 Mirror/Square 连杆主体阻挡、反向脱离,机器人禁止 qpos 瞬移', async ({
page,
}, info) => {
const errors: string[] = [];
page.on('pageerror', (e) => errors.push(e.message));
await page.goto('/physics/runner.html');
await page.waitForFunction(() => Boolean(window.lekiwiPhysics));
await page.evaluate((path) => window.lekiwiPhysics.boot(path), `/@fs${resolve('build/lekiwi')}`);
try {
const result = await page.evaluate(() => window.lekiwiPhysics.adjacentArmChecks());
console.log('adjacent arm', JSON.stringify(result));
await writeFile(info.outputPath('adjacent-arm-contacts.json'), JSON.stringify(result, null, 2));
expect(result.teleportAccepted).toBe(false);
expect(result.jointResetError).toMatch(/profile.*瞬移/);
expect(result.editable).toBe(false);
expect(result.before).toEqual(result.after);
for (const sample of result.samples) {
expect(sample.neutral.contacts).toEqual([]);
expect(Math.abs(sample.neutral.panActual - sample.pan)).toBeLessThan(0.01);
expect(Math.abs(sample.neutral.actual)).toBeLessThan(0.015);
expect(sample.blocked.maxContacts).toBeGreaterThan(0);
expect(sample.blocked.contacts.length).toBeGreaterThan(0);
expect(sample.blocked.actual).toBeGreaterThan(sample.blocked.target + 0.5);
expect(sample.blocked.minContactRadius).toBeGreaterThan(0.04); // structural contact, NOT the bearing core
expect(sample.blocked.maxPenetration).toBeLessThan(0.002);
expect(sample.blocked.finite).toBe(true);
for (const c of sample.blocked.contacts) expect(c.distance).toBeGreaterThan(-0.0005);
expect(Math.abs(sample.released.actual - 0.3)).toBeLessThan(0.015);
expect(sample.released.contacts).toEqual([]);
}
expect(errors).toEqual([]);
} finally {
await page.evaluate(() => window.lekiwiPhysics.dispose());
}
});
test('真实 WASM:六关节双向有界目标扫掠,无整体穿入或数值重置', async ({ page }, info) => {
const errors: string[] = [];
page.on('pageerror', (e) => errors.push(e.message));
await page.goto('/physics/runner.html');
await page.waitForFunction(() => Boolean(window.lekiwiPhysics));
await page.evaluate((path) => window.lekiwiPhysics.boot(path), `/@fs${resolve('build/lekiwi')}`);
try {
const result = await page.evaluate(() => window.lekiwiPhysics.jointSweepChecks());
await writeFile(info.outputPath('joint-sweeps.json'), JSON.stringify(result, null, 2));
console.log('joint sweeps', JSON.stringify(result));
expect(result).toHaveLength(12);
for (const sample of result) {
expect(sample.finite, sample.joint).toBe(true);
expect(sample.elapsed).toBeCloseTo(sample.expectedTime, 6);
expect(sample.maxPenetration, sample.joint).toBeLessThan(0.002);
if (sample.joint === 'arm_shoulder_pan') {
expect(Math.abs(sample.actual - sample.target)).toBeLessThan(0.01);
expect(sample.maxContacts).toBe(0); // core/structural straddlers must not lock the bearing
}
if (Math.abs(sample.actual - sample.target) > 0.05)
expect(sample.maxContacts, sample.joint).toBeGreaterThan(0);
}
expect(errors).toEqual([]);
} finally {
await page.evaluate(() => window.lekiwiPhysics.dispose());
}
});
test('真实 WASM:从原始 STL 独立取六向极值,整臂 18 个视觉网格均有外部物体接触覆盖', async ({
page,
}, info) => {
const errors: string[] = [];
page.on('pageerror', (e) => errors.push(e.message));
await page.goto('/physics/runner.html');
await page.waitForFunction(() => Boolean(window.lekiwiPhysics));
await page.evaluate((path) => window.lekiwiPhysics.boot(path), `/@fs${resolve('build/lekiwi')}`);
try {
const result = await page.evaluate(() => window.lekiwiPhysics.visualCollisionCoverage());
await writeFile(info.outputPath('arm-visual-coverage.json'), JSON.stringify(result, null, 2));
expect(result).toHaveLength(18);
for (const sample of result) {
expect(sample.probes).toHaveLength(6);
for (const probe of sample.probes)
expect(probe.contacts.length, `${sample.visual}: ${probe.point}`).toBeGreaterThan(0);
}
expect(errors).toEqual([]);
} finally {
await page.evaluate(() => window.lekiwiPhysics.dispose());
}
});
+55
View File
@@ -0,0 +1,55 @@
import { expect, test } from '@playwright/test';
import { resolve } from 'node:path';
import { writeFile } from 'node:fs/promises';
import type {} from '../physics/runner';
test('真实 WASM:夹爪两侧指尖覆盖、开度方向和物体阻挡,不能只检查关节数值', async ({
page,
}, info) => {
const errors: string[] = [];
page.on('pageerror', (error) => errors.push(error.message));
await page.goto('/physics/runner.html');
await page.waitForFunction(() => Boolean(window.lekiwiPhysics));
await page.evaluate((path) => window.lekiwiPhysics.boot(path), `/@fs${resolve('build/lekiwi')}`);
try {
const result = await page.evaluate(() => window.lekiwiPhysics.gripperCollisionChecks());
console.log('gripper contacts', JSON.stringify(result));
await writeFile(info.outputPath('gripper-contacts.json'), JSON.stringify(result, null, 2));
await writeFile(
info.outputPath('gripper-scene.xml'),
await page.evaluate(() => window.lekiwiPhysics.xml()),
);
expect(result.finite).toBe(true);
for (const tip of result.coverage) {
expect(
tip.contacts.some((c) =>
c.other.startsWith(
tip.name === 'fixed_tip'
? '__lekiwi_cad_Wrist_Roll_08c-v1__'
: '__lekiwi_cad_Moving_Jaw_08d-v1__',
),
),
tip.name,
).toBe(true);
}
for (const state of result.openings) {
expect(Math.abs(state.actual - state.target)).toBeLessThan(0.01);
expect(state.contacts).toEqual([]);
}
expect(result.openings[0].separation).toBeGreaterThan(0);
expect(result.openings[0].separation).toBeLessThan(0.002);
expect(result.openings[1].separation).toBeGreaterThan(result.openings[0].separation + 0.02);
expect(result.openings[2].separation).toBeGreaterThan(result.openings[1].separation + 0.01);
expect(result.gapContacts).toEqual([]); // a hull must not seal the open space
expect(result.obstruction.maxContacts).toBeGreaterThan(0);
expect(result.obstruction.maxPenetration).toBeLessThan(0.0015);
expect(result.obstruction.actual).toBeGreaterThan(result.obstruction.target + 0.03);
expect(result.obstruction.contacts.length).toBeGreaterThan(0);
for (const contact of result.obstruction.contacts)
expect(contact.distance).toBeGreaterThan(-0.0002);
expect(result.fingerSelfContacts).toBeGreaterThan(0);
expect(errors).toEqual([]);
} finally {
await page.evaluate(() => window.lekiwiPhysics.dispose());
}
});
+215
View File
@@ -0,0 +1,215 @@
import { expect, test } from '@playwright/test';
import { resolve } from 'node:path';
import { existsSync } from 'node:fs';
import { writeFile } from 'node:fs/promises';
import { startBridge } from './fixtures/controlBridge';
import type {} from '../physics/runner';
const python = process.env.LEROBOT_PYTHON ?? resolve('build/venvs/lerobot/bin/python');
test('真实终端输入:底盘与六路臂/夹爪点动、反向、保持和退出撤权', async ({ page }, info) => {
expect(existsSync(python), '需要真实的独立 LeRobot 环境').toBe(true);
const bridge = await startBridge();
try {
await page.goto('/physics/runner.html');
await page.waitForFunction(() => Boolean(window.lekiwiPhysics));
await page.evaluate(
(path) => window.lekiwiPhysics.boot(path),
`/@fs${resolve('build/lekiwi')}`,
);
await page.evaluate(
({ endpoint, token }) => window.lekiwiPhysics.connectBridge(endpoint, token),
{ endpoint: bridge.endpoint, token: bridge.token },
);
const result = await bridge.python(
`
import json, os, pty, select, subprocess, sys, termios, time
from urllib.request import Request, urlopen
def observe():
request=Request(os.environ['MUJOCO_CONTROL_ENDPOINT']+'/api/control/v1/observation',
headers={'Authorization':'Bearer '+os.environ['MUJOCO_CONTROL_TOKEN']})
with urlopen(request,timeout=2) as response:
return json.load(response)
master,slave=pty.openpty()
original_terminal=termios.tcgetattr(slave)
# This checks free-space hold, not servo compliance under self contact. The
# complete CAD assembly now blocks shoulder_lift near .12rad at this posture.
# Use 5 deg/s for this free-space test; keep hold/reversal thresholds unchanged.
# armCollision/fullCollision.spec separately exercise physical obstruction.
process=subprocess.Popen([sys.executable,'-u',os.environ['KEYBOARD_DEMO'],'--arm-speed','5'],stdin=slave,stdout=subprocess.PIPE,stderr=subprocess.PIPE)
try:
assert select.select([process.stdout],[],[],10)[0], 'terminal demo did not start'
assert 'w/s' in process.stdout.readline().decode()
initial=observe()
for _ in range(10):
os.write(master,b'wuiotyv');time.sleep(.1)
# No input for longer than the watchdog: base stops, arm holds, actions continue.
time.sleep(.35)
held=observe()
time.sleep(.4)
held_again=observe()
for _ in range(6):
os.write(master,b'jklghb');time.sleep(.1)
os.write(master,b' ')
time.sleep(.35)
reversed_pose=observe()
os.write(master,b'q')
_,err=process.communicate(timeout=5)
assert process.returncode==0,err.decode()
assert termios.tcgetattr(slave)==original_terminal, 'terminal settings were not restored'
print(json.dumps({'exit':process.returncode,'initial':initial,'held':held,
'heldAgain':held_again,'reversed':reversed_pose}))
finally:
if process.poll() is None:
process.kill();process.wait()
os.close(master);os.close(slave)
`,
python,
{ KEYBOARD_DEMO: resolve('examples/lekiwi/teleoperate_sim.py') },
);
const report = JSON.parse(result.stdout.trim());
const reportPath = info.outputPath('keyboard-teleop.json');
await writeFile(reportPath, JSON.stringify(report, null, 2));
await info.attach('keyboard-teleop.json', {
path: reportPath,
contentType: 'application/json',
});
expect(report.exit).toBe(0);
expect(report.held.values['base.x'] - report.initial.values['base.x']).toBeGreaterThan(0.01);
expect(Math.abs(report.heldAgain.values['x.vel'])).toBeLessThan(0.02);
expect(report.heldAgain.appliedActionSeq).toBeGreaterThan(report.held.appliedActionSeq);
for (const joint of [
'arm_shoulder_pan',
'arm_shoulder_lift',
'arm_elbow_flex',
'arm_wrist_flex',
'arm_wrist_roll',
'arm_gripper',
]) {
const key = `${joint}.pos`;
expect(report.held.values[key] - report.initial.values[key], `${joint} 正向`).toBeGreaterThan(
0.03,
);
expect(
Math.abs(report.heldAgain.values[key] - report.held.values[key]),
`${joint} 保持`,
).toBeLessThan(0.03);
expect(
report.heldAgain.values[key] - report.reversed.values[key],
`${joint} 反向`,
).toBeGreaterThan(0.03);
}
await expect
.poll(
async () =>
(await page.evaluate(() => window.lekiwiPhysics.externalState())).observation?.paused,
)
.toBe(true);
const final = await page.evaluate(() => window.lekiwiPhysics.externalState());
expect(final.control).toMatchObject({ enabled: false, connected: false });
expect(bridge.errors()).toBe('');
} finally {
await page.evaluate(() => window.lekiwiPhysics?.dispose()).catch(() => {});
await bridge.stop();
}
});
test('真实 LeRobot 0.6.1 工厂/插件:无硬件 demo 与实际部分动作/限幅回传', async ({ page }) => {
expect(
existsSync(python),
'请先创建隔离 LeRobot 环境或设置 LEROBOT_PYTHON;兼容门槛不能 skipped',
).toBe(true);
const bridge = await startBridge();
try {
await page.goto('/physics/runner.html');
await page.waitForFunction(() => Boolean(window.lekiwiPhysics));
await page.evaluate(
(path) => window.lekiwiPhysics.boot(path),
`/@fs${resolve('build/lekiwi')}`,
);
await page.evaluate(
({ endpoint, token }) => window.lekiwiPhysics.connectBridge(endpoint, token),
{ endpoint: bridge.endpoint, token: bridge.token },
);
const run = await bridge.python(
`
import os, runpy, sys
sys.argv=['demo_control.py','--duration','3']
runpy.run_path(os.environ['LEKIWI_DEMO'],run_name='__main__')
`,
python,
{ LEKIWI_DEMO: resolve('examples/lekiwi/demo_control.py') },
);
const result = JSON.parse(run.stdout.trim());
expect(result.lerobot).toBe('0.6.1');
expect(result.steps).toBe(90);
expect(result.maxTranslationM).toBeGreaterThan(0.05);
expect(result.timeouts).toBe(0);
expect(Object.keys(result.observation)).toHaveLength(9);
await expect
.poll(
async () =>
(await page.evaluate(() => window.lekiwiPhysics.externalState())).observation?.paused,
)
.toBe(true);
// The previous demo paused the simulation. An old paused sample must explain
// the missing play step, not misreport it as a running simulation freeze.
const paused = await bridge.python(
`
import json, os, time
from mujoco_control_bridge import RobotError
from lerobot_robot_mujoco import LeKiwiSim, LeKiwiSimConfig
time.sleep(.6)
robot=LeKiwiSim(LeKiwiSimConfig(endpoint=os.environ['MUJOCO_CONTROL_ENDPOINT']))
try:
robot.connect()
except RobotError as error:
assert not robot.is_connected
print(json.dumps({'code':error.code,'message':str(error)}))
else:
robot.disconnect()
raise AssertionError('paused simulation must refuse a new controller')
`,
python,
);
expect(JSON.parse(paused.stdout.trim())).toMatchObject({
code: 'PAUSED',
message: expect.stringContaining('播放'),
});
await page.evaluate(() => window.lekiwiPhysics.authorize());
const partial = await bridge.python(
`
import json, os, torch, sys
from lerobot.utils.import_utils import register_third_party_plugins
from lerobot.robots.config import RobotConfig
from lerobot.robots.utils import make_robot_from_config
register_third_party_plugins()
robot=make_robot_from_config(RobotConfig.get_choice_class('lekiwi_sim')(endpoint=os.environ['MUJOCO_CONTROL_ENDPOINT']))
robot.connect()
try:
first=robot.send_action({'arm_shoulder_pan.pos':180,'arm_gripper.pos':150,'x.vel':.1})
second=robot.send_action({'arm_shoulder_lift.pos':5})
observed=robot.get_observation()
assert not torch.cuda.is_initialized()
assert not any(m in sys.modules for m in ('serial','zmq','scservo_sdk','pyrealsense2'))
print(json.dumps({'first':first,'second':second,'observed':observed}))
finally:
robot.disconnect()
`,
python,
);
const values = JSON.parse(partial.stdout.trim());
expect(values.first['arm_gripper.pos']).toBe(100);
expect(values.first['arm_shoulder_pan.pos']).toBeCloseTo((1.57 * 180) / Math.PI);
expect(values.second['arm_shoulder_pan.pos']).toBe(values.first['arm_shoulder_pan.pos']);
expect(values.second['x.vel']).toBe(0);
expect(values.second['arm_gripper.pos']).toBe(100);
expect(
Math.abs(values.observed['arm_shoulder_pan.pos'] - values.first['arm_shoulder_pan.pos']),
).toBeGreaterThan(1);
expect(bridge.errors()).toBe('');
} finally {
await page.evaluate(() => window.lekiwiPhysics?.dispose()).catch(() => {});
await bridge.stop();
}
});
+97
View File
@@ -0,0 +1,97 @@
import { expect, test } from '@playwright/test';
import { resolve } from 'node:path';
import { writeFile } from 'node:fs/promises';
import type {} from '../physics/runner';
test('真实 WASM 3.11:LeKiwi 站稳、全向轮、臂夹爪与障碍阻挡', async ({ page }, info) => {
const errors: string[] = [];
page.on('pageerror', (e) => errors.push(e.message));
page.on('console', (msg) => {
if (msg.type() === 'error') console.log(msg.text());
});
await page.goto('/physics/runner.html');
await page.waitForFunction(() => Boolean(window.lekiwiPhysics));
const assetBase = `/@fs${resolve(process.cwd(), 'build/lekiwi')}`;
const initial = await page.evaluate((path) => window.lekiwiPhysics.boot(path), assetBase);
console.log('initial', initial);
expect(initial.version).toBe('3.11.0');
expect(initial.nu).toBe(9);
expect(initial.njnt).toBe(46);
await writeFile(
info.outputPath('lekiwi-profile.xml'),
await page.evaluate(() => window.lekiwiPhysics.xml()),
);
const stable = await page.evaluate(() => window.lekiwiPhysics.step(30));
console.log('stable', stable);
expect(stable.finite).toBe(true);
expect(Math.abs(stable.roll)).toBeLessThan(0.15);
expect(Math.abs(stable.pitch)).toBeLessThan(0.15);
const traces: unknown[] = [{ initial, stable }];
for (const [x, y, w] of [
[0.1, 0, 0],
[-0.1, 0, 0],
[0, 0.1, 0],
[0, -0.1, 0],
[0, 0, Math.PI / 6],
[0, 0, -Math.PI / 6],
]) {
const start = await page.evaluate(() => window.lekiwiPhysics.reset());
const end = await page.evaluate(
([x, y, w]) => window.lekiwiPhysics.drive(x, y, w, 2),
[x, y, w],
);
console.log('drive', x, y, w, 'start', start.position, start.yaw, 'end', end.position, end.yaw);
traces.push({ command: [x, y, w], start, end });
expect(end.finite).toBe(true);
if (x || y) {
expect(Math.abs(end.position[0] - start.position[0] - x * 2)).toBeLessThan(0.05);
expect(Math.abs(end.position[1] - start.position[1] - y * 2)).toBeLessThan(0.05);
} else expect(Math.abs(end.yaw - start.yaw - w * 2)).toBeLessThan((10 * Math.PI) / 180);
}
await page.evaluate(() => window.lekiwiPhysics.reset());
// Free-space tracking; negative elbow folding now has its own obstruction test.
const targets = [0.1, 0.1, 0.1, 0.1, 0.1, 0.3];
const arm = await page.evaluate((values) => window.lekiwiPhysics.arm(values, 2), targets);
console.log('arm', arm.arm);
traces.push({ targets, arm });
arm.arm.forEach((value, i) => expect(Math.abs(value - targets[i])).toBeLessThan(0.015));
await page.evaluate(() => window.lekiwiPhysics.wall());
const blocked = await page.evaluate(() => window.lekiwiPhysics.drive(0.1, 0, 0, 5));
console.log('blocked', blocked);
const contacts = await page.evaluate(() => window.lekiwiPhysics.contactPairs());
traces.push({ blocked, contacts });
expect(contacts.some((c) => c.geom1 === 'test_wall' || c.geom2 === 'test_wall')).toBe(true);
expect(blocked.maxContacts).toBeGreaterThan(0);
expect(blocked.position[0]).toBeLessThan(0.22);
// The complete arm/accessory outline reaches the wall before the old chassis proxy.
expect(blocked.position[0]).toBeGreaterThan(0.02);
expect(blocked.finite).toBe(true);
await writeFile(info.outputPath('physics-traces.json'), JSON.stringify(traces, null, 2));
expect(errors).toEqual([]);
await page.evaluate(() => window.lekiwiPhysics.dispose());
});
test('真实 Python/WASM:控制权、旧回调隔离、安全保持、reset 与导出重载', async ({ page }) => {
await page.goto('/physics/runner.html');
await page.waitForFunction(() => Boolean(window.lekiwiPhysics));
await page.evaluate(
(path) => window.lekiwiPhysics.boot(path),
`/@fs${resolve(process.cwd(), 'build/lekiwi')}`,
);
const result = await page.evaluate(() => window.lekiwiPhysics.runtimeChecks());
expect(result.pythonTarget).toBe(0.1);
expect(result.accepted.values['arm_shoulder_pan.pos']).toBe(0.3);
expect(result.afterOldDispose).toBe(0.3);
expect(result.manualBlocked).toBe(true);
expect(result.held).toBeCloseTo(result.actual, 10);
expect(result.paused?.enabled).toBe(false);
expect(result.pausedOwner).toBe('manual');
expect(result.cancel).toBe('DISCONNECTED');
expect(result.reset?.modelEpoch).toBe(1);
expect(result.reset?.paused).toBe(true);
const reloaded = await page.evaluate(() => window.lekiwiPhysics.reimport());
expect(reloaded.nu).toBe(9);
expect(reloaded.njnt).toBe(46);
expect(reloaded.robot?.profileId).toBe('lekiwi-v1');
await page.evaluate(() => window.lekiwiPhysics.dispose());
});
+317
View File
@@ -0,0 +1,317 @@
import { expect, test, type Page } from '@playwright/test';
import { resolve } from 'node:path';
import { writeFile } from 'node:fs/promises';
import { startBridge } from './fixtures/controlBridge';
async function loadWorkspace(page: Page) {
await page.goto('/');
await page
.locator('input[type="file"]')
.first()
.setInputFiles(resolve('build/lekiwi/lekiwi-v1.zip'));
const dialog = page.getByRole('dialog', { name: '配置 URDF 仿真组件' });
await expect(dialog).toBeVisible({ timeout: 30000 });
await expect(dialog.getByLabel('URDF 机器人 profile')).toHaveValue('');
await dialog.getByLabel('URDF 机器人 profile').selectOption('lekiwi-v1');
await expect(dialog.getByRole('checkbox', { name: /添加传感器/ })).toBeDisabled();
await dialog.getByRole('button', { name: '转换并加载' }).click();
await expect(page.getByRole('button', { name: '▶ 播放' })).toBeEnabled({ timeout: 60000 });
await page.getByRole('tab', { name: '控制台', exact: true }).click();
await expect(page.getByRole('region', { name: '开源机器人控制' })).toContainText('lekiwi-v1');
}
async function connect(page: Page, bridge: Awaited<ReturnType<typeof startBridge>>) {
await page.getByLabel('本机桥接地址', { exact: true }).fill(bridge.endpoint);
await page.getByLabel('控制 token', { exact: true }).fill(bridge.token);
await page.getByRole('button', { name: '连接桥接', exact: true }).click();
await expect(page.getByLabel('桥接连接状态', { exact: true })).toHaveText('已连接');
}
async function authorize(page: Page) {
await page.getByRole('button', { name: '▶ 播放' }).click();
await page.getByRole('button', { name: '允许外部控制', exact: true }).click();
await expect(page.getByLabel('当前控制所有者', { exact: true })).toHaveText('外部');
}
async function state(page: Page) {
return page.evaluate(async () => {
const path = '/src/stores/useAppStore.ts';
const { useAppStore } = (await import(
/* @vite-ignore */ path
)) as typeof import('../src/stores/useAppStore');
const s = useAppStore.getState();
return {
paused: s.paused,
owner: s.snapshot?.controlOwner,
time: s.snapshot?.time ?? 0,
epoch: s.snapshot?.externalControl?.modelEpoch,
sessionId: s.snapshot?.externalControl?.sessionId,
wasmHeapBytes: s.snapshot?.wasmHeapBytes,
fps: s.fps,
stepMs: s.stepMs,
memoryMb: s.memoryMb,
overBudget: s.overBudget,
actuators: s.snapshot?.actuators.map((a) => ({ name: a.jointName, target: a.value })),
joints: s.snapshot?.joints.map((j) => ({ name: j.name, value: j.value })),
diagnostic: s.diagnostic?.summary,
};
});
}
test('完整工作台 + 真实 LeRobot:文件导入、profile、授权和60秒30Hz控制', async ({ page }, info) => {
const bridge = await startBridge(),
errors: string[] = [];
page.on('pageerror', (error) => errors.push(error.message));
try {
await loadWorkspace(page);
// Profile pose editing is disabled even before an external controller owns it.
await expect(page.getByRole('button', { name: '关节拖动', exact: true })).toBeDisabled();
await connect(page, bridge);
await authorize(page);
await expect(page.getByLabel('仿真速度', { exact: true })).toBeDisabled();
await expect(page.getByLabel('仿真速度', { exact: true })).toHaveValue('1');
await expect(page.getByRole('button', { name: '关节拖动', exact: true })).toBeDisabled();
const consolePanel = page.getByRole('tabpanel', { name: '控制台' });
await consolePanel.getByRole('button', { name: /执行器实时控制/ }).click();
await expect(consolePanel.getByRole('slider').first()).toBeDisabled();
await consolePanel.getByRole('button', { name: /执行器实时控制/ }).click();
const metrics: Awaited<ReturnType<typeof state>>[] = [];
let done = false;
const run = bridge
.python(
`
import os, runpy, sys
sys.argv=['demo_control.py','--duration','60']
runpy.run_path(os.environ['LEKIWI_DEMO'],run_name='__main__')
`,
process.env.LEROBOT_PYTHON ?? resolve('build/venvs/lerobot/bin/python'),
{ LEKIWI_DEMO: resolve('examples/lekiwi/demo_control.py') },
)
.finally(() => {
done = true;
});
const monitor = (async () => {
while (!done) {
await page.waitForTimeout(1000);
metrics.push(await state(page));
}
})();
const [result] = await Promise.all([run, monitor]).catch(async (error: unknown) => {
const path = info.outputPath('partial-metrics.json');
await writeFile(path, JSON.stringify(metrics));
await info.attach('partial-metrics.json', { path, contentType: 'application/json' });
throw error;
});
const report = JSON.parse(result.stdout.trim());
const path = info.outputPath('lekiwi-60s.json');
const graphics = await page.evaluate(() => {
const canvas = document.querySelector('main canvas') as HTMLCanvasElement;
const gl = canvas.getContext('webgl2')!,
ext = gl.getExtension('WEBGL_debug_renderer_info');
return {
size: [canvas.width, canvas.height],
renderer: ext ? String(gl.getParameter(ext.UNMASKED_RENDERER_WEBGL)) : 'unknown',
};
});
await writeFile(path, JSON.stringify({ report, metrics, graphics }, null, 2));
await info.attach('lekiwi-60s.json', { path, contentType: 'application/json' });
console.log('LeKiwi 60s evidence', report);
expect(report.steps).toBe(1800);
expect(report.elapsedSeconds).toBeGreaterThanOrEqual(60);
expect(report.elapsedSeconds).toBeLessThan(63);
expect(report.simSeconds).toBeGreaterThan(55);
expect(report.maxTranslationM).toBeGreaterThan(0.05);
expect(report.rttMaxMs).toBeLessThan(500);
expect(report.maxObservationSimGap).toBeLessThan(0.5);
expect(report.timeouts).toBe(0);
expect(report.droppedRequests).toBe(0);
expect(report.appliedActionSeq).toBeGreaterThan(1790);
expect(metrics.every((m) => Number.isFinite(m.time) && !m.diagnostic)).toBe(true);
const heaps = metrics.slice(10).flatMap((m) => (m.memoryMb === undefined ? [] : [m.memoryMb]));
if (heaps.length > 10) expect(heaps.at(-1)! - heaps[0]).toBeLessThan(64);
const wasm = metrics.slice(10).map((m) => m.wasmHeapBytes!);
expect(wasm.every((v) => Number.isFinite(v) && v > 0)).toBe(true);
expect(wasm.at(-1)! - wasm[0]).toBeLessThan(16 * 1024 * 1024);
await expect(page.getByRole('button', { name: '▶ 播放' })).toBeEnabled();
await expect(page.getByLabel('当前控制所有者', { exact: true })).toHaveText('手动');
await page.screenshot({ path: info.outputPath('lekiwi-workspace.png') });
expect(bridge.errors()).toBe('');
expect(errors).toEqual([]);
} finally {
await page.close();
await bridge.stop();
}
});
test('工作台重编译与缓存编辑失败回滚不会恢复外部授权', async ({ page }) => {
page.setDefaultTimeout(15000);
const bridge = await startBridge();
try {
await loadWorkspace(page);
await connect(page, bridge);
await authorize(page);
const old = await state(page);
const driving = bridge.python(
genericDrive +
`
try:
for i in range(900):
robot.send_action(values);time.sleep(1/30)
raise AssertionError('reload must disconnect the controller')
except RobotError as error:
print(json.dumps({'stopped':error.code}))
finally:
robot.disconnect()
`,
);
await expect(page.getByLabel('外部控制状态', { exact: true })).toHaveText('外部控制者已连接');
await page.getByRole('button', { name: '校验并重新编译 profile' }).click();
await expect.poll(async () => (await state(page)).sessionId).not.toBe(old.sessionId);
expect(JSON.parse((await driving).stdout.trim()).stopped).toBeTruthy();
await expect(page.getByLabel('桥接连接状态', { exact: true })).toHaveText('未连接');
await expect(page.getByRole('button', { name: '▶ 播放' })).toBeEnabled();
await connect(page, bridge);
await authorize(page);
const before = await state(page);
await page.getByRole('button', { name: '源代码', exact: true }).click();
const dialog = page.getByRole('dialog', { name: '转换后的 MJCF 编辑器' });
const editor = dialog.locator('.monaco-editor');
await editor.click({ position: { x: 240, y: 120 } });
await page.keyboard.press('Control+a');
await page.keyboard.press('Backspace');
await page.keyboard.insertText('<mujoco>\n <worldbody/>\n</mujoco>\n');
await dialog.getByRole('button', { name: '保存并重新载入', exact: true }).click();
await expect.poll(async () => (await state(page)).diagnostic).toBeTruthy();
expect(await state(page)).toMatchObject({
sessionId: before.sessionId,
paused: true,
owner: 'manual',
});
await dialog.getByRole('button', { name: '关闭源代码编辑器' }).click();
await expect(page.getByLabel('桥接连接状态', { exact: true })).toHaveText('未连接');
await expect(page.getByLabel('外部控制状态', { exact: true })).toHaveText('未授权');
} finally {
await bridge.stop();
}
});
test('普通模型不自动套用 profile,错误 profile 保留原场景', async ({ page }) => {
await page.goto('/');
await page
.locator('input[type="file"]')
.first()
.setInputFiles({
name: 'generic.xml',
mimeType: 'text/xml',
buffer: Buffer.from(
'<mujoco><worldbody><body><joint name="j" type="slide" axis="1 0 0"/><geom type="sphere" size=".1"/></body></worldbody><actuator><motor joint="j"/></actuator></mujoco>',
),
});
await expect(page.getByRole('button', { name: '▶ 播放' })).toBeEnabled();
await page.getByRole('tab', { name: '控制台', exact: true }).click();
await page.getByRole('button', { name: '开源项目 / 外部控制' }).click();
await expect(page.getByLabel('控制接口 profile', { exact: true })).toHaveValue('');
await expect(page.getByRole('button', { name: '连接桥接', exact: true })).toBeDisabled();
const before = await state(page);
await page.getByLabel('控制接口 profile', { exact: true }).selectOption('lekiwi-v1');
await page.getByRole('button', { name: '校验并重新编译 profile' }).click();
await expect.poll(async () => (await state(page)).diagnostic).toBeTruthy();
expect((await state(page)).actuators).toEqual(before.actuators);
await expect(page.getByRole('button', { name: '连接桥接', exact: true })).toBeDisabled();
});
const genericDrive = `
import json, os, time
from mujoco_control_bridge import SimRobotClient, RobotError
robot=SimRobotClient(os.environ['MUJOCO_CONTROL_ENDPOINT']);robot.connect()
values={c['id']:0 for c in robot.describe()['actionChannels']}
values.update({'arm_shoulder_pan.pos':.2,'arm_gripper.pos':.7,'x.vel':.1})
`;
test('工作台安全生命周期:真实 reset、新 epoch、控制者崩溃、页面隐藏与冻结恢复', async ({
page,
}) => {
const bridge = await startBridge();
try {
await loadWorkspace(page);
await connect(page, bridge);
await authorize(page);
const reset = await bridge.python(
genericDrive +
`
robot.send_action(values)
result=robot.reset()
print(json.dumps({'epoch':result['modelEpoch'],'paused':result['paused']}))
`,
);
expect(JSON.parse(reset.stdout.trim())).toEqual({ epoch: 1, paused: true });
await expect(page.getByRole('button', { name: '▶ 播放' })).toBeEnabled();
await expect(page.getByLabel('桥接连接状态', { exact: true })).toHaveText('已连接');
expect((await state(page)).epoch).toBe(1);
await authorize(page);
await bridge.python(
genericDrive +
`
robot.send_action(values)
os._exit(0) # Deliberately crash without a finalizer; watchdog must stop it.
`,
);
await expect(page.getByRole('button', { name: '▶ 播放' })).toBeEnabled({ timeout: 3000 });
const stopped = await state(page);
expect(stopped.owner).toBe('manual');
expect(
stopped.actuators?.filter((a) => a.name?.startsWith('base_')).map((a) => a.target),
).toEqual([0, 0, 0]);
const held = stopped.actuators?.find((a) => a.name === 'arm_shoulder_pan')?.target;
expect(held).toBeGreaterThan(0.05);
expect(held).toBeCloseTo(stopped.joints!.find((j) => j.name === 'arm_shoulder_pan')!.value, 8);
await authorize(page);
await page.evaluate(() => {
Object.defineProperty(document, 'visibilityState', { configurable: true, value: 'hidden' });
document.dispatchEvent(new Event('visibilitychange'));
});
await expect(page.getByLabel('桥接连接状态', { exact: true })).toHaveText('未连接');
await expect(page.getByRole('button', { name: '▶ 播放' })).toBeEnabled();
await page.evaluate(() => {
Reflect.deleteProperty(document, 'visibilityState');
document.dispatchEvent(new Event('visibilitychange'));
});
await expect(page.getByLabel('外部控制状态', { exact: true })).toHaveText('未授权');
await connect(page, bridge);
await authorize(page);
const driving = bridge.python(
genericDrive +
`
try:
for i in range(900):
robot.send_action(values);time.sleep(1/30)
raise AssertionError('Frozen browser must revoke the lease')
except RobotError as error:
print(json.dumps({'stopped':error.code}))
finally:
robot.disconnect()
`,
);
await expect(page.getByLabel('外部控制状态', { exact: true })).toHaveText('外部控制者已连接');
const before = await state(page),
cdp = await page.context().newCDPSession(page);
await cdp.send('Debugger.enable');
const paused = new Promise<void>((resolve) => cdp.once('Debugger.paused', () => resolve()));
await cdp.send('Debugger.pause');
await paused;
try {
await new Promise((resolve) => setTimeout(resolve, 800));
} finally {
await cdp.send('Debugger.resume');
await cdp.detach();
}
const frozen = await driving;
expect(JSON.parse(frozen.stdout.trim()).stopped).toBeTruthy();
await expect(page.getByRole('button', { name: '▶ 播放' })).toBeEnabled();
const after = await state(page);
expect(after.time - before.time).toBeLessThan(0.3);
expect(
after.actuators?.filter((a) => a.name?.startsWith('base_')).map((a) => a.target),
).toEqual([0, 0, 0]);
} finally {
await page.close();
await bridge.stop();
}
});
+71
View File
@@ -0,0 +1,71 @@
import { Ray, Triangle, Vector3 } from 'three';
export type Surface = { vertices: Vector3[]; faces: number[][] };
/** Independent, unsigned triangle-surface distance oracle for the small E2E tip hulls.
* Not a runtime collision detector. Includes vertex/face, edge/edge and edge/face cases.
*/
export function surfaceDistance(a: Surface, b: Surface) {
let distanceSq = Infinity;
let from = new Vector3(),
to = new Vector3();
const candidate = new Vector3(),
rayPoint = new Vector3(),
segmentPoint = new Vector3();
const consider = (p: Vector3, q: Vector3, reversed = false) => {
const distance = p.distanceToSquared(q);
if (distance < distanceSq) {
distanceSq = distance;
from = (reversed ? q : p).clone();
to = (reversed ? p : q).clone();
}
};
const triangles = (surface: Surface) =>
surface.faces.map(
([i, j, k]) => new Triangle(surface.vertices[i], surface.vertices[j], surface.vertices[k]),
);
const edges = (surface: Surface) => {
const pairs = new Map<string, [Vector3, Vector3]>();
for (const f of surface.faces)
for (let i = 0; i < 3; i++) {
const p = f[i],
q = f[(i + 1) % 3];
pairs.set([p, q].sort((x, y) => x - y).join(','), [
surface.vertices[p],
surface.vertices[q],
]);
}
return [...pairs.values()];
};
const ta = triangles(a),
tb = triangles(b),
ea = edges(a),
eb = edges(b);
for (const v of a.vertices) for (const t of tb) consider(v, t.closestPointToPoint(v, candidate));
for (const v of b.vertices)
for (const t of ta) consider(v, t.closestPointToPoint(v, candidate), true);
for (const [p, q] of ea) {
const length = p.distanceTo(q);
if (length === 0) continue;
const ray = new Ray(p, q.clone().sub(p).divideScalar(length));
for (const [v, w] of eb) {
ray.distanceSqToSegment(v, w, rayPoint, segmentPoint);
if (rayPoint.distanceTo(p) <= length) consider(rayPoint, segmentPoint);
}
for (const t of tb) {
const point = ray.intersectTriangle(t.a, t.b, t.c, false, candidate);
if (point && point.distanceTo(p) <= length) consider(point, point);
}
}
for (const [p, q] of eb) {
const length = p.distanceTo(q);
if (length === 0) continue;
const ray = new Ray(p, q.clone().sub(p).divideScalar(length));
for (const t of ta) {
const point = ray.intersectTriangle(t.a, t.b, t.c, false, candidate);
if (point && point.distanceTo(p) <= length) consider(point, point);
}
}
if (!Number.isFinite(distanceSq)) throw new Error('表面距离查询缺少有效三角形');
return { distance: Math.sqrt(distanceSq), from: from.toArray(), to: to.toArray() };
}
+9
View File
@@ -0,0 +1,9 @@
<!doctype html>
<html lang="zh">
<base href="/" />
<meta charset="utf-8" /><title>LeKiwi WASM physics fixture (development only)</title>
<body>
<p>真实 WASM 动力学测试,无生产入口。</p>
<script type="module" src="/physics/runner.ts"></script>
</body>
</html>
+822
View File
@@ -0,0 +1,822 @@
import { MainThreadPhysicsAdapter } from '../src/simulation/PhysicsAdapter';
import { SimulationSession } from '../src/simulation/SimulationSession';
import { ModelBindings } from '../src/simulation/ModelBindings';
import { LEKIWI_PROFILE as p, bodyToWheels } from '../src/robot/profiles/lekiwi';
import type { ProjectManifest } from '../src/project/types';
import { ExternalControlClient } from '../src/robot/ExternalControlClient';
import { validateDescriptor, validateValues } from '../src/robot/validation';
import { sha256 } from '../src/robot/registry';
import single from '../../contracts/fixtures/single-joint.json';
import { Quaternion, Vector3 } from 'three';
import { surfaceDistance, type Surface } from './meshDistance';
const adapter = new MainThreadPhysicsAdapter();
let bindings: ModelBindings;
let currentManifest: ProjectManifest;
let frame: number | undefined;
let bridge: ExternalControlClient | undefined;
function session() {
if (!adapter.session) throw new Error('not initialized');
return adapter.session;
}
function read() {
const s = session(),
id = bindings.resolve(bindings.bodies, p.baseBody);
const quat = Array.from(s.data.xquat.slice(id * 4, id * 4 + 4), Number);
const [w, x, y, z] = quat;
return {
time: Number(s.data.time),
position: Array.from(s.data.xpos.slice(id * 3, id * 3 + 3), Number),
quaternion: quat,
roll: Math.atan2(2 * (w * x + y * z), 1 - 2 * (x * x + y * y)),
pitch: Math.asin(Math.max(-1, Math.min(1, 2 * (w * y - z * x)))),
yaw: Math.atan2(2 * (w * z + x * y), 1 - 2 * (y * y + z * z)),
arm: p.arm.map((j) => Number(s.data.qpos[bindings.scalarJoint(j.joint).qposAddress])),
wheel: p.wheels.map((j) => Number(s.data.qvel[bindings.scalarJoint(j).qvelAddress])),
finite: Array.from(s.data.qpos, Number).every(Number.isFinite),
contacts: Number(s.data.ncon),
nu: s.model.nu,
njnt: s.model.njnt,
};
}
function contactPairs() {
const s = session();
const result: {
geom1: string;
geom2: string;
body1: number;
body2: number;
distance: number;
position: number[];
}[] = [];
for (let i = 0; i < s.data.ncon; i++) {
const contact = s.data.contact.get(i)!;
if (
contact.geom1 < 0 ||
contact.geom2 < 0 ||
contact.geom1 >= s.model.ngeom ||
contact.geom2 >= s.model.ngeom
) {
const message = `非法接触几何 id: ${contact.geom1}/${contact.geom2}; ${i}/${s.data.ncon}`;
contact.delete();
throw new Error(message);
}
const a = s.model.geom(contact.geom1),
b = s.model.geom(contact.geom2);
try {
result.push({
geom1: a.name,
geom2: b.name,
body1: Number(s.model.geom_bodyid[contact.geom1]),
body2: Number(s.model.geom_bodyid[contact.geom2]),
distance: contact.dist,
position: Array.from(contact.pos, Number),
});
} finally {
a.delete();
b.delete();
contact.delete();
}
}
return result;
}
function step(seconds: number) {
const s = session(),
steps = Math.round(seconds / Number(s.model.opt.timestep));
let maxContacts = 0;
for (let i = 0; i < steps; i++) {
s.singleStep();
maxContacts = Math.max(maxContacts, Number(s.data.ncon));
}
return { ...read(), maxContacts };
}
const harness = {
async bootSingle() {
const data = new TextEncoder().encode(
'<mujoco><option gravity="0 0 0" timestep=".002"/><worldbody><body pos="0 0 1"><joint name="slider" type="slide" axis="1 0 0" range="-1 1"/><geom type="sphere" size=".1" mass="1"/></body></worldbody><actuator><position name="servo" joint="slider" kp="40" kv="15" ctrlrange="-1 1"/></actuator></mujoco>',
);
const manifest: ProjectManifest = {
id: 'single',
name: 'single',
entries: [{ path: 'single.xml', format: 'mjcf', label: 'single' }],
maps: [],
files: [{ path: 'single.xml', data, size: data.length, mimeType: '', source: 'file' }],
totalBytes: data.length,
};
await adapter.load(manifest, 'single.xml', {
urdfMode: 'mjcf',
baseMode: 'fixed',
map: { kind: 'none' },
});
const s = session(),
b = new ModelBindings(s.model),
j = b.scalarActuator('servo', 'slider', 'position');
const descriptor = validateDescriptor({
...single.descriptor,
modelFingerprint: await sha256(data),
});
const values = (v: unknown) => validateValues(v, descriptor.actionChannels, true);
s.configureRobotAdapter({
describe: () => descriptor,
validateAction: values,
applyAction(v) {
const accepted = values(v);
s.data.ctrl[j.controlAddress] = accepted['slider.position'];
return accepted;
},
readObservation: () => ({ 'slider.position': Number(s.data.qpos[j.qposAddress]) }),
safeStop() {
s.data.ctrl[j.controlAddress] = Math.max(
-1,
Math.min(1, Number(s.data.qpos[j.qposAddress])),
);
},
reset() {
s.data.qpos[j.qposAddress] = 0;
s.data.ctrl[j.controlAddress] = 0;
},
dispose() {},
});
return descriptor;
},
async connectBridge(endpoint: string, token: string) {
bridge?.disconnect();
bridge = new ExternalControlClient(() => adapter);
session().setPaused(false);
session().setExternalControlEnabled(true);
const tick = (now: number) => {
adapter.advance(now);
frame = requestAnimationFrame(tick);
};
if (frame === undefined) frame = requestAnimationFrame(tick);
await bridge.connect(endpoint, token);
return bridge.status();
},
externalState() {
return {
control: session().externalControlStatus(),
observation: session().robotObservation(),
bridge: bridge?.status(),
};
},
authorize() {
session().setPaused(false);
session().setExternalControlEnabled(true);
bridge?.sync();
},
pause() {
session().setPaused(true);
bridge?.sync();
},
async boot(assetBase: string) {
const response = await fetch(`${assetBase}/source-manifest.json`);
if (!response.ok) throw new Error('Run examples/lekiwi/prepare_assets.py first');
const source = (await response.json()) as { files: Record<string, string> };
const files = await Promise.all(
Object.keys(source.files)
.filter((f) => f.startsWith('URDF/'))
.map(async (path) => {
const response = await fetch(`${assetBase}/${path}`);
if (!response.ok) throw new Error(`Missing ${path}`);
const data = new Uint8Array(await response.arrayBuffer());
return { path, data, size: data.byteLength, source: 'file' as const, mimeType: '' };
}),
);
const manifest: ProjectManifest = {
id: 'lekiwi-physics',
name: 'LeKiwi physics',
files,
entries: [{ path: 'URDF/LeKiwi.urdf', format: 'urdf', label: 'LeKiwi' }],
maps: [],
totalBytes: files.reduce((s, f) => s + f.size, 0),
};
currentManifest = manifest;
await adapter.load(manifest, 'URDF/LeKiwi.urdf', {
robotProfileId: p.id,
urdfMode: 'mjcf',
baseMode: 'floating',
map: { kind: 'none' },
enhancements: { addActuators: false, addSensors: false, sensorType: 'camera' },
});
bindings = new ModelBindings(session().model);
return { version: session().module.mj_versionString(), ...read() };
},
reset() {
session().reset();
return step(1);
},
step,
read,
contactPairs,
adjacentArmChecks() {
const s = session(),
joint = bindings.scalarJoint('arm_elbow_flex');
const upper = bindings.resolve(bindings.bodies, 'SO_ARM100_08k_116_Square-v1');
const forearm = bindings.resolve(bindings.bodies, 'SO_ARM100_08k_Mirror-v1');
const touching = () =>
contactPairs().filter(
(c) =>
(c.body1 === upper && c.body2 === forearm) || (c.body2 === upper && c.body1 === forearm),
);
s.reset();
const before = Array.from(s.data.qpos, Number);
const teleportAccepted = s.setJointPosition(joint.id, -1.3);
let jointResetError = '';
try {
s.resetJoints();
} catch (error) {
jointResetError = String(error);
}
const after = Array.from(s.data.qpos, Number);
const editable = s.snapshot().joints.find((j) => j.id === joint.id)!.editable;
// Bearing/structural hull partitioning must leave these pan poses reachable.
const samples = [0, 0.8, -0.8].map((pan) => {
s.reset();
harness.arm([pan, 0, 0, 0, 0, 0], 1.5);
const neutral = {
actual: Number(s.data.qpos[joint.qposAddress]),
panActual: read().arm[0],
contacts: touching(),
};
s.setActuator(bindings.resolve(bindings.actuators, 'arm_elbow_flex_servo'), -1.3);
let maxPenetration = 0,
maxContacts = 0;
for (let i = 0; i < Math.ceil(3 / Number(s.model.opt.timestep)); i++) {
s.singleStep();
const contacts = touching();
maxContacts = Math.max(maxContacts, contacts.length);
for (const c of contacts) maxPenetration = Math.max(maxPenetration, -c.distance);
}
const contacts = touching();
const anchor = Array.from(s.data.xanchor.slice(joint.id * 3, joint.id * 3 + 3), Number);
const axis = Array.from(s.data.xaxis.slice(joint.id * 3, joint.id * 3 + 3), Number);
const radii = contacts.map((c) => {
const offset = c.position.map((x, i) => x - anchor[i]);
const axial = offset.reduce((sum, x, i) => sum + x * axis[i], 0);
return Math.hypot(...offset.map((x, i) => x - axial * axis[i]));
});
const blocked = {
actual: Number(s.data.qpos[joint.qposAddress]),
target: -1.3,
contacts,
maxPenetration,
maxContacts,
minContactRadius: Math.min(...radii),
finite: read().finite,
};
harness.arm([pan, 0, 0.3, 0, 0, 0], 2);
return {
pan,
neutral,
blocked,
released: { actual: Number(s.data.qpos[joint.qposAddress]), contacts: touching() },
};
});
s.reset();
return { teleportAccepted, jointResetError, editable, before, after, samples };
},
jointSweepChecks() {
const s = session(),
dt = Number(s.model.opt.timestep);
const results = [];
for (const spec of p.arm)
for (const target of [spec.min * 0.6, spec.max * 0.6]) {
s.reset();
step(0.5);
const motor = bindings.resolve(bindings.actuators, `${spec.joint}_servo`);
const joint = bindings.scalarJoint(spec.joint);
const initial = Number(s.data.qpos[joint.qposAddress]);
const duration = Math.abs(target - initial) / 0.4; // bounded 0.4 rad/s target ramp
let maxPenetration = 0,
maxContacts = 0;
const start = Number(s.data.time),
steps = Math.ceil((duration + 0.75) / dt);
for (let i = 0; i < steps; i++) {
if (i % 10 === 0)
s.setActuator(motor, initial + (target - initial) * Math.min(1, (i * dt) / duration));
s.singleStep();
const contacts = contactPairs().filter(
(c) => c.geom1.startsWith('__lekiwi_cad_') && c.geom2.startsWith('__lekiwi_cad_'),
);
maxContacts = Math.max(maxContacts, contacts.length);
for (const c of contacts) maxPenetration = Math.max(maxPenetration, -c.distance);
}
results.push({
joint: spec.joint,
target,
actual: Number(s.data.qpos[joint.qposAddress]),
maxPenetration,
maxContacts,
finite: read().finite,
elapsed: Number(s.data.time) - start,
expectedTime: steps * dt,
contacts: contactPairs().filter(
(c) => c.geom1.startsWith('__lekiwi_cad_') && c.geom2.startsWith('__lekiwi_cad_'),
),
});
}
s.reset();
return results;
},
visualCollisionCoverage() {
const xml = new DOMParser().parseFromString(harness.xml(), 'application/xml');
const arm = xml.querySelector('body[name="Base_08q-v1"]')!;
const original = new DOMParser().parseFromString(
new TextDecoder().decode(
currentManifest.files.find((f) => f.path === 'URDF/LeKiwi.urdf')!.data,
),
'application/xml',
);
const visuals = Array.from(original.querySelectorAll('link > visual')).filter((v) =>
arm.querySelector(`geom[name="${v.getAttribute('name')}"]`),
);
const probeBody = xml.createElement('body');
probeBody.setAttribute('name', '__coverage_probe_body');
probeBody.setAttribute('mocap', 'true');
probeBody.setAttribute('pos', '10 10 10');
const geom = xml.createElement('geom');
geom.setAttribute('name', '__coverage_probe');
geom.setAttribute('type', 'sphere');
geom.setAttribute('size', '.0015');
probeBody.append(geom);
xml.querySelector('worldbody')!.append(probeBody);
const path = 'URDF/lekiwi-coverage-test.xml';
adapter.workspace!.writeGenerated(
path,
new TextEncoder().encode(new XMLSerializer().serializeToString(xml)),
);
const module = session().module;
session().dispose();
adapter.session = new SimulationSession(module, adapter.workspace!.path(path));
const s = session();
bindings = new ModelBindings(s.model);
const mocap = Number(
s.model.body_mocapid[bindings.resolve(bindings.bodies, '__coverage_probe_body')],
);
const samples = visuals.map((v) => {
const name = v.getAttribute('name')!,
link = v.parentElement!.getAttribute('name')!;
const sourceMesh = v.querySelector('mesh')!;
const scale = (sourceMesh.getAttribute('scale') ?? '1 1 1').split(/\s+/).map(Number);
const file = currentManifest.files.find(
(f) => f.path === `URDF/${sourceMesh.getAttribute('filename')}`,
)!;
const data = new DataView(file.data.buffer, file.data.byteOffset, file.data.byteLength);
const extrema: number[][] = [];
for (let triangle = 0; triangle < data.getUint32(80, true); triangle++)
for (let vertex = 0; vertex < 3; vertex++) {
const point = [0, 1, 2].map(
(a) => data.getFloat32(84 + triangle * 50 + 12 + vertex * 12 + a * 4, true) * scale[a],
);
for (let a = 0; a < 3; a++)
for (let sign = 0; sign < 2; sign++) {
const i = a * 2 + sign,
factor = sign ? 1 : -1;
if (!extrema[i] || point[a] * factor > extrema[i][a] * factor) extrema[i] = point;
}
}
const visual = xml.querySelector(`geom[name="${name}"]`)!;
const body = bindings.resolve(bindings.bodies, visual.parentElement!.getAttribute('name')!);
const q = (visual.getAttribute('quat') ?? '1 0 0 0').split(/\s+/).map(Number);
const pos = (visual.getAttribute('pos') ?? '0 0 0').split(/\s+/).map(Number);
const probes = extrema.map((point) => {
const local = new Vector3(...point)
.applyQuaternion(new Quaternion(q[1], q[2], q[3], q[0]))
.add(new Vector3(...pos))
.toArray();
const world = [0, 1, 2].map(
(row) =>
Number(s.data.xpos[body * 3 + row]) +
local.reduce(
(sum, value, col) => sum + value * Number(s.data.xmat[body * 9 + row * 3 + col]),
0,
),
);
s.data.mocap_pos.set(world, mocap * 3);
module.mj_forward(s.model, s.data);
const contacts = contactPairs().filter(
(c) =>
(c.geom1 === '__coverage_probe' && c.geom2.startsWith(`__lekiwi_cad_${link}__`)) ||
(c.geom2 === '__coverage_probe' && c.geom1.startsWith(`__lekiwi_cad_${link}__`)),
);
return { point, contacts };
});
return { visual: name, probes };
});
s.data.mocap_pos.set([10, 10, 10], mocap * 3);
module.mj_forward(s.model, s.data);
return samples;
},
drive(x: number, y: number, w: number, seconds: number) {
const wheels = bodyToWheels(x, y, w);
p.wheels.forEach((j, i) =>
session().setActuator(bindings.resolve(bindings.actuators, `${j}_servo`), wheels[i]),
);
return step(seconds);
},
arm(targets: number[], seconds: number) {
p.arm.forEach((j, i) =>
session().setActuator(bindings.resolve(bindings.actuators, `${j.joint}_servo`), targets[i]),
);
return step(seconds);
},
wall() {
const xml = new DOMParser().parseFromString(
new TextDecoder().decode(adapter.exportMjcf()),
'application/xml',
);
const wall = xml.createElement('geom');
for (const [k, v] of Object.entries({
name: 'test_wall',
type: 'box',
pos: '.32 0 .12',
size: '.03 .5 .12',
}))
wall.setAttribute(k, v);
xml.querySelector('worldbody')!.append(wall);
adapter.workspace!.writeGenerated(
'URDF/lekiwi-test.xml',
new TextEncoder().encode(new XMLSerializer().serializeToString(xml)),
);
const module = session().module;
session().dispose();
adapter.session = new SimulationSession(
module,
adapter.workspace!.path('URDF/lekiwi-test.xml'),
);
bindings = new ModelBindings(session().model);
return step(1);
},
armSelfCollisionChecks() {
const s = session();
const supportGroups = new Set([
bindings.resolve(bindings.bodies, p.baseBody),
bindings.resolve(bindings.bodies, 'Rotation_Pitch_08i-v1'),
]);
const upperArm = bindings.resolve(bindings.bodies, 'SO_ARM100_08k_116_Square-v1');
const joint = bindings.scalarJoint('arm_shoulder_lift');
const geomNames = Array.from({ length: s.model.ngeom }, (_, i) => {
const geom = s.model.geom(i);
try {
return geom.name;
} finally {
geom.delete();
}
});
const contacts = () => {
const touching: { distance: number; geom1: string; geom2: string }[] = [];
for (let i = 0; i < s.data.ncon; i++) {
const contact = s.data.contact.get(i)!;
try {
// The fixed base/mounting plate and rotating shoulder clip can all
// stop the upper arm before it reaches the original Base/Square pair.
const a = Number(s.model.body_weldid[Number(s.model.geom_bodyid[contact.geom1])]);
const b = Number(s.model.body_weldid[Number(s.model.geom_bodyid[contact.geom2])]);
if (
((supportGroups.has(a) && b === upperArm) ||
(a === upperArm && supportGroups.has(b))) &&
geomNames[contact.geom1].startsWith('__lekiwi_cad_') &&
geomNames[contact.geom2].startsWith('__lekiwi_cad_')
)
touching.push({
distance: contact.dist,
geom1: geomNames[contact.geom1],
geom2: geomNames[contact.geom2],
});
} finally {
contact.delete();
}
}
return touching;
};
const samples = [0, 0.8, -0.8].map((pan) => {
s.reset();
harness.arm([pan, 0, 0, 0, 0, 0], 1.5);
const neutral = { actual: Number(s.data.qpos[joint.qposAddress]), contacts: contacts() };
const target = 0.6;
s.setActuator(bindings.resolve(bindings.actuators, 'arm_shoulder_lift_servo'), target);
let maxContacts = 0,
maxPenetration = 0;
for (let i = 0; i < Math.ceil(3 / Number(s.model.opt.timestep)); i++) {
s.singleStep();
const touching = contacts();
maxContacts = Math.max(maxContacts, touching.length);
for (const contact of touching)
maxPenetration = Math.max(maxPenetration, -contact.distance);
}
const blocked = {
actual: Number(s.data.qpos[joint.qposAddress]),
target,
contacts: contacts(),
allArmContacts: contactPairs().filter(
(c) => c.geom1.startsWith('__lekiwi_cad_') && c.geom2.startsWith('__lekiwi_cad_'),
),
maxContacts,
maxPenetration,
finite: read().finite,
};
harness.arm([pan, -0.3, 0, 0, 0, 0], 2);
const released = { actual: Number(s.data.qpos[joint.qposAddress]), contacts: contacts() };
return { pan, neutral, blocked, released };
});
s.reset();
return samples;
},
gripperCollisionChecks() {
const xml = new DOMParser().parseFromString(harness.xml(), 'application/xml');
const probe = xml.createElement('body');
probe.setAttribute('name', 'gripper_probe_body');
probe.setAttribute('mocap', 'true');
probe.setAttribute('pos', '10 10 10');
const sphere = xml.createElement('geom');
sphere.setAttribute('name', 'gripper_probe');
sphere.setAttribute('type', 'sphere');
sphere.setAttribute('size', '.008');
probe.append(sphere);
xml.querySelector('worldbody')!.append(probe);
const path = 'URDF/lekiwi-gripper-test.xml';
adapter.workspace!.writeGenerated(
path,
new TextEncoder().encode(new XMLSerializer().serializeToString(xml)),
);
const module = session().module;
session().dispose();
adapter.session = new SimulationSession(module, adapter.workspace!.path(path));
const s = session();
bindings = new ModelBindings(s.model);
const geoms = new Map<string, number>();
for (let i = 0; i < s.model.ngeom; i++) {
const geom = s.model.geom(i);
try {
geoms.set(geom.name, i);
} finally {
geom.delete();
}
}
const probeId = geoms.get('gripper_probe')!;
const fixedBody = bindings.resolve(bindings.bodies, 'Wrist_Roll_08c-v1');
const movingBody = bindings.resolve(bindings.bodies, 'Moving_Jaw_08d-v1');
const motor = bindings.resolve(bindings.actuators, 'arm_gripper_servo');
const joint = bindings.scalarJoint('arm_gripper');
const spec = p.arm[5];
const sourcePoint = (visualName: string, point: number[]) => {
// Independent CAD surface samples, NOT points taken from the collision recipe.
const visual = xml.querySelector(`geom[name="${visualName}"]`)!;
const body = bindings.resolve(bindings.bodies, visual.parentElement!.getAttribute('name')!);
const q = (visual.getAttribute('quat') ?? '1 0 0 0').split(/\s+/).map(Number);
const pos = (visual.getAttribute('pos') ?? '0 0 0').split(/\s+/).map(Number);
const local = new Vector3(...point)
.multiplyScalar(0.001)
.applyQuaternion(new Quaternion(q[1], q[2], q[3], q[0]))
.add(new Vector3(...pos))
.toArray();
return [0, 1, 2].map(
(row) =>
Number(s.data.xpos[body * 3 + row]) +
local.reduce(
(sum, value, col) => sum + value * Number(s.data.xmat[body * 9 + row * 3 + col]),
0,
),
);
};
const contacts = () => {
const result: { other: string; distance: number }[] = [];
for (let i = 0; i < s.data.ncon; i++) {
const contact = s.data.contact.get(i)!;
try {
if (contact.geom1 !== probeId && contact.geom2 !== probeId) continue;
const other = contact.geom1 === probeId ? contact.geom2 : contact.geom1;
result.push({
other: [...geoms].find(([, id]) => id === other)![0],
distance: contact.dist,
});
} finally {
contact.delete();
}
}
return result;
};
const place = (position: number[]) => {
s.data.mocap_pos.set(position);
module.mj_forward(s.model, s.data);
return contacts();
};
step(0.5);
const coverage = [
{ name: 'fixed_tip', visual: 'Wrist_Roll_08c-v1_visual', point: [-12, 0, 99] },
{ name: 'moving_tip', visual: 'Moving_Jaw_08d-v1_visual', point: [-10, -78, 0] },
].map(({ name, visual, point }) => ({ name, contacts: place(sourcePoint(visual, point)) }));
place([10, 10, 10]);
// Select actual collision parts contacted by independent CAD tip probes.
// No hull indices or generator vertices are used as the test oracle.
const fingerIds = coverage.map((sample, i) =>
sample.contacts
.filter((c) =>
c.other.startsWith(
`__lekiwi_cad_${i === 0 ? 'Wrist_Roll_08c-v1' : 'Moving_Jaw_08d-v1'}__`,
),
)
.map((c) => geoms.get(c.other)!),
);
const surface = (geom: number): Surface => {
const m = s.model,
d = s.data,
mesh = Number(m.geom_dataid[geom]);
const va = Number(m.mesh_vertadr[mesh]),
fa = Number(m.mesh_faceadr[mesh]);
return {
vertices: Array.from({ length: Number(m.mesh_vertnum[mesh]) }, (_, i) => {
const local = [0, 1, 2].map((c) => Number(m.mesh_vert[3 * (va + i) + c]));
return new Vector3(
...[0, 1, 2].map(
(r) =>
Number(d.geom_xpos[3 * geom + r]) +
local.reduce((sum, v, c) => sum + v * Number(d.geom_xmat[9 * geom + 3 * r + c]), 0),
),
);
}),
faces: Array.from({ length: Number(m.mesh_facenum[mesh]) }, (_, i) =>
[0, 1, 2].map((c) => Number(m.mesh_face[3 * (fa + i) + c])),
),
};
};
const closestPair = () => {
// Independent triangle geometry: WASM 3.11 mj_geomDistance returned scalar 0
// for a separated thin hull pair (its witness points were >50 mm apart).
// Do not silently discard that pair, loosen gap assertions or tune the solver.
const surfaces = fingerIds.map((ids) => ids.map(surface));
let closest: ReturnType<typeof surfaceDistance> | undefined;
for (const fixed of surfaces[0])
for (const moving of surfaces[1]) {
const result = surfaceDistance(fixed, moving);
if (!closest || result.distance < closest.distance) closest = result;
}
if (!closest) throw new Error('CAD 指尖没有对应碰撞体');
return closest;
};
const openings = [0, 0.5, 1].map((opening) => {
const target = spec.min + opening * (spec.max - spec.min);
s.setActuator(motor, target);
step(1);
return {
opening,
target,
actual: Number(s.data.qpos[joint.qposAddress]),
separation: closestPair().distance,
contacts: contactPairs().filter(
(c) =>
(c.body1 === fixedBody && c.body2 === movingBody) ||
(c.body1 === movingBody && c.body2 === fixedBody),
),
};
});
// Put a rigid object in the jaw space of the half-open pose; then close on it.
s.setActuator(motor, (spec.min + spec.max) / 2);
step(1);
const closest = closestPair();
const midpoint = [0, 1, 2].map((axis) => (closest.from[axis] + closest.to[axis]) / 2);
s.setActuator(motor, spec.max);
step(1);
const gapContacts = place(midpoint);
s.setActuator(motor, spec.min);
let maxContacts = 0,
maxPenetration = 0;
for (let i = 0; i < Math.ceil(2 / Number(s.model.opt.timestep)); i++) {
s.singleStep();
const touching = contacts();
maxContacts = Math.max(maxContacts, touching.length);
for (const contact of touching) maxPenetration = Math.max(maxPenetration, -contact.distance);
}
const obstruction = {
maxContacts,
maxPenetration,
actual: Number(s.data.qpos[joint.qposAddress]),
target: spec.min,
contacts: contacts(),
};
place([10, 10, 10]);
// Deliberately overlap the fingers to check structural pairs bypass the
// default parent filter. This is a query, never a normal control command.
s.data.qpos[joint.qposAddress] = -0.24;
module.mj_forward(s.model, s.data);
let fingerSelfContacts = 0;
for (let i = 0; i < s.data.ncon; i++) {
const contact = s.data.contact.get(i)!;
try {
if (
(s.model.geom_bodyid[contact.geom1] === fixedBody &&
s.model.geom_bodyid[contact.geom2] === movingBody) ||
(s.model.geom_bodyid[contact.geom2] === fixedBody &&
s.model.geom_bodyid[contact.geom1] === movingBody)
)
fingerSelfContacts++;
} finally {
contact.delete();
}
}
s.reset();
return {
coverage,
openings,
gapContacts,
obstruction,
fingerSelfContacts,
finite: read().finite,
};
},
async runtimeChecks() {
const s = session();
await s.loadPythonController(
`OLD = None\nMOTOR = 0\ndef init(model):\n global MOTOR\n MOTOR=model.actuator('arm_shoulder_pan_servo')\n return MOTOR\ndef step(ctx, state):\n global OLD\n OLD=ctx\n ctx.set_control(state, 0.1)\ndef dispose(state):\n if OLD is not None:\n OLD.set_control(MOTOR, 1.2)\n`,
'scoped.py',
);
s.setControllerEnabled(true);
s.singleStep();
const motor = bindings.resolve(bindings.actuators, 'arm_shoulder_pan_servo');
const pythonTarget = Number(s.data.ctrl[motor]);
s.setPaused(false);
s.setExternalControlEnabled(true);
const identity = s.claimExternalControlLease('browser-lease');
const values = Object.fromEntries(s.describeRobot()!.actionChannels.map((c) => [c.id, 0]));
values['arm_gripper.pos'] = 0.25;
values['arm_shoulder_pan.pos'] = 0.3;
values['x.vel'] = 0.1;
const pending = s.sendRobotAction({ protocolVersion: 1, ...identity, actionSeq: 1, values });
s.singleStep();
const accepted = await pending;
let manualBlocked = false;
try {
s.setActuator(motor, 1);
} catch {
manualBlocked = true;
}
s.removeController(); // old Python dispose tries to write 1.2 through its saved ctx
const afterOldDispose = Number(s.data.ctrl[motor]);
const measured = s.robotObservation();
s.setPaused(true);
const held = Number(s.data.ctrl[motor]);
const actual = Number(s.data.qpos[bindings.scalarJoint('arm_shoulder_pan').qposAddress]);
const paused = s.snapshot();
s.setPaused(false);
s.setExternalControlEnabled(true);
const nextIdentity = s.claimExternalControlLease('next-lease');
const cancel = s
.sendRobotAction({ protocolVersion: 1, ...nextIdentity, actionSeq: 1, values })
.catch((e) => e.code);
s.reset();
return {
pythonTarget,
accepted,
manualBlocked,
afterOldDispose,
measured,
held,
actual,
paused: paused.externalControl,
pausedOwner: paused.controlOwner,
reset: s.robotObservation(),
cancel: await cancel,
};
},
async reimport() {
const data = adapter.exportMjcf();
const file = {
path: 'URDF/export.xml',
data,
size: data.length,
mimeType: '',
source: 'file' as const,
};
const manifest: ProjectManifest = {
...currentManifest,
files: [...currentManifest.files, file],
entries: [...currentManifest.entries, { path: file.path, format: 'mjcf', label: 'export' }],
};
await adapter.load(manifest, file.path, {
robotProfileId: p.id,
urdfMode: 'mjcf',
baseMode: 'floating',
map: { kind: 'none' },
});
adapter.releaseRetired();
bindings = new ModelBindings(session().model);
return { robot: adapter.describeRobot(), ...read() };
},
xml() {
return new TextDecoder().decode(adapter.exportMjcf());
},
dispose() {
if (frame !== undefined) cancelAnimationFrame(frame);
frame = undefined;
bridge?.disconnect();
bridge = undefined;
adapter.dispose();
},
};
export type PhysicsHarness = typeof harness;
declare global {
interface Window {
lekiwiPhysics: PhysicsHarness;
}
}
window.lekiwiPhysics = harness;
+14 -5
View File
@@ -1,11 +1,20 @@
import { defineConfig } from '@playwright/test';
export default defineConfig({
testDir: './e2e',
testIgnore: /lekiwi.*\.spec\.ts/,
timeout: 120_000,
use: { baseURL: 'http://127.0.0.1:4173' },
webServer: {
command: 'npm run preview --prefix .. -- --host 127.0.0.1',
url: 'http://127.0.0.1:4173',
reuseExistingServer: true,
},
webServer: [
{
command: 'npm run preview --prefix .. -- --host 127.0.0.1',
url: 'http://127.0.0.1:4173',
reuseExistingServer: true,
},
{
// Synthetic real-WASM bridge fixture is intentionally not in the production bundle.
command: 'npm run dev --prefix .. -- --host 127.0.0.1 --port 4174 --strictPort',
url: 'http://127.0.0.1:4174',
reuseExistingServer: false,
},
],
});
+14
View File
@@ -0,0 +1,14 @@
import { defineConfig } from '@playwright/test';
export default defineConfig({
testDir: './e2e',
outputDir: '../build/e2e/lekiwi',
testMatch: /lekiwi.*\.spec\.ts/,
timeout: 180_000,
workers: 1,
use: { baseURL: 'http://127.0.0.1:4174' },
webServer: {
command: 'npm run dev --prefix .. -- --host 127.0.0.1 --port 4174 --strictPort',
url: 'http://127.0.0.1:4174',
reuseExistingServer: false,
},
});
+99 -7
View File
@@ -1,4 +1,6 @@
import { useThemePreference } from './hooks/useThemePreference';
import { useExternalControl } from './hooks/useExternalControl';
import { ExternalControlPanel } from '../robot/ExternalControlPanel';
import { resolvePolicyDeployment, type PolicyDeployment } from '../rl/deployment';
/* Zustand 的 action 引用稳定;初始化 viewer 与导入回调有意只创建一次。 */
/* eslint-disable react-hooks/exhaustive-deps */
@@ -288,6 +290,7 @@ export function App() {
setLeftOpen,
setRightOpen,
} = useSidebarLayout();
const robotProfilesRef = useRef(new Map<string, string>());
const sensorCameraFrame = useRef<HTMLDivElement>(null);
const orientationHost = useRef<HTMLDivElement>(null);
const [mapCommitState, setMapCommitState] = useState<'idle' | 'submitting' | 'failed'>('idle');
@@ -298,6 +301,7 @@ export function App() {
[generatedMjcf, setGeneratedMjcf] = useState<string>(),
[generatedMjcfPath, setGeneratedMjcfPath] = useState<string>(),
[pendingUrdfPath, setPendingUrdfPath] = useState<string>(),
[pendingUrdfProfile, setPendingUrdfProfile] = useState<string>(),
[pendingUrdfMounts, setPendingUrdfMounts] = useState<string[]>([]),
[removeConfirmOpen, setRemoveConfirmOpen] = useState(false),
[fullscreen, setFullscreen] = useState(false),
@@ -351,6 +355,16 @@ export function App() {
[jointAdvanced, setJointAdvanced] = useState(false),
[ignoreJointLimits, setIgnoreJointLimits] = useState(false),
[angleUnit, setAngleUnit] = useState<'rad' | 'deg'>('rad');
const publishExternalSnapshot = useCallback(() => {
const snapshot = adapter.current.snapshot() ?? undefined;
const store = useAppStore.getState();
store.setSnapshot(snapshot);
if (snapshot?.paused !== undefined) store.setPaused(snapshot.paused);
if (snapshot?.speed !== undefined) store.setSpeed(snapshot.speed);
setControllerStatus(snapshot?.controller);
setPolicyStatus(snapshot?.rlPolicy);
}, []);
const externalControl = useExternalControl(adapter, publishExternalSnapshot);
const showCollision = displayOptions.showCollision,
setShowCollision = (value: boolean) =>
setDisplayOptions((options) => ({ ...options, showCollision: value }));
@@ -458,6 +472,8 @@ export function App() {
);
if (snapshot) {
state.setSnapshot(snapshot);
if (snapshot.paused !== undefined) state.setPaused(snapshot.paused);
if (snapshot.speed !== undefined) state.setSpeed(snapshot.speed);
setControllerStatus(snapshot.controller);
setPolicyStatus(snapshot.rlPolicy);
if (snapshot.controller?.error || snapshot.rlPolicy?.error) {
@@ -591,7 +607,11 @@ export function App() {
);
const previousState = useAppStore.getState();
const previousEntry = previousState.selectedEntry;
const previousPaused = previousState.paused;
const previousSnapshot = adapter.current.snapshot();
const previousPaused =
previousSnapshot?.controlOwner === 'external' ||
(previousSnapshot?.paused ?? previousState.paused);
externalControl.disconnect('模型正在重载,请重新连接并授权');
loadInFlight.current = true;
setIgnoreJointLimits(false);
setControllerStatus(undefined);
@@ -605,8 +625,8 @@ export function App() {
value: 0.4,
});
state.setDiagnostic(undefined);
setGeneratedMjcf(undefined);
setGeneratedMjcfPath(undefined);
// Keep the current source editor through a failed transactional compile,
// so a rejected profile edit can be corrected rather than losing its UI.
adapter.current.setPaused(true);
state.setPaused(true);
let attachedViewer: MuJoCoViewer | null = null;
@@ -618,6 +638,7 @@ export function App() {
urdfMode: requestedMode ?? urdfModeRef.current,
baseMode: baseModeRef.current,
enhancements: urdfEnhancementsRef.current,
robotProfileId: robotProfilesRef.current.get(path),
mapAssets: sceneAssets,
trainingDeployment: requestedDeployment,
trainingPolicy: requestedPolicy,
@@ -756,6 +777,7 @@ export function App() {
const entry = manifest.current?.entries.find((candidate) => candidate.path === path);
if (entry?.format === 'urdf' && urdfModeRef.current === 'mjcf') {
setPendingUrdfMounts(urdfLinkNames(manifest.current, path));
setPendingUrdfProfile(robotProfilesRef.current.get(path));
setPendingUrdfPath(path);
return;
}
@@ -763,20 +785,59 @@ export function App() {
},
[loadEntry],
);
const confirmUrdfOptions = (options: UrdfEnhancementOptions) => {
const confirmUrdfOptions = (options: UrdfEnhancementOptions, profileId?: string) => {
const path = pendingUrdfPath;
if (!path) return;
const choices = robotProfilesRef.current,
previous = choices.get(path),
previousBase = baseModeRef.current;
urdfEnhancementsRef.current = options;
if (profileId) {
robotProfilesRef.current.set(path, profileId);
baseModeRef.current = 'floating';
setBaseMode('floating');
} else robotProfilesRef.current.delete(path);
setPendingUrdfPath(undefined);
setPendingUrdfMounts([]);
void loadEntry(path);
void loadEntry(path).then((loaded) => {
if (!loaded && robotProfilesRef.current === choices) {
if (previous) choices.set(path, previous);
else choices.delete(path);
baseModeRef.current = previousBase;
setBaseMode(previousBase);
}
});
};
const skipUrdfOptions = () =>
confirmUrdfOptions({ addActuators: false, addSensors: false, sensorType: 'camera' });
const applyRobotProfile = async (profileId?: string) => {
const path = useAppStore.getState().selectedEntry;
if (!path || loadInFlight.current) return;
const choices = robotProfilesRef.current,
previous = choices.get(path),
previousMode = urdfModeRef.current,
previousBase = baseModeRef.current;
if (profileId) {
robotProfilesRef.current.set(path, profileId);
urdfModeRef.current = 'mjcf';
setUrdfMode('mjcf');
baseModeRef.current = 'floating';
setBaseMode('floating');
} else robotProfilesRef.current.delete(path);
if (!(await loadEntry(path)) && robotProfilesRef.current === choices) {
if (previous) robotProfilesRef.current.set(path, previous);
else robotProfilesRef.current.delete(path);
urdfModeRef.current = previousMode;
setUrdfMode(previousMode);
baseModeRef.current = previousBase;
setBaseMode(previousBase);
}
};
const ingest = useCallback(
async (files: File[], lockOwned = false) => {
if (importInFlight.current && !lockOwned) return;
importInFlight.current = true;
externalControl.disconnect('正在导入工程,请重新连接并授权');
state.setLoading(true);
setImportProgress({
title: '正在导入工程',
@@ -812,6 +873,7 @@ export function App() {
value: 0.39,
});
manifest.current = next;
robotProfilesRef.current = new Map();
setProjectMaps(next.maps);
setCommittedEditorDocuments(manifestEditorDocuments(next));
setProjectSidebarTab('assets');
@@ -887,6 +949,8 @@ export function App() {
if (state.projectName) setRemoveConfirmOpen(true);
};
const confirmRemoveProject = () => {
externalControl.disconnect('工程已关闭');
robotProfilesRef.current = new Map();
setMapCommitState('idle');
void viewer.current?.setVisualMaps([]);
viewer.current?.attach(null);
@@ -931,6 +995,7 @@ export function App() {
if (entry?.format !== 'urdf') return;
if (value === 'mjcf') {
setPendingUrdfMounts(urdfLinkNames(manifest.current, entry.path));
setPendingUrdfProfile(robotProfilesRef.current.get(entry.path));
setPendingUrdfPath(entry.path);
} else void loadEntry(entry.path, value);
};
@@ -1883,7 +1948,14 @@ export function App() {
state.setSpeed(value);
adapter.current.setSpeed(value);
};
const mode = (value: InteractionMode) => state.setMode(value);
const jointPoseEditingDisabled = Boolean(
state.snapshot?.robot ||
(state.snapshot?.controlOwner && state.snapshot.controlOwner !== 'manual'),
);
const mode = (value: InteractionMode) => {
if (value === 'joint' && jointPoseEditingDisabled) return;
state.setMode(value);
};
const resetJoints = () => {
adapter.current.resetJoints();
state.setPaused(true);
@@ -2186,6 +2258,8 @@ export function App() {
};
const saveCachedSource = async (path: string, text: string) => {
if (!manifest.current) return;
const profileId = adapter.current.describeRobot()?.profileId;
if (profileId) robotProfilesRef.current.set(path, profileId);
manifest.current = upsertCachedMjcf(manifest.current, path, text);
state.setProject(
manifest.current.name,
@@ -2426,6 +2500,17 @@ export function App() {
active={workspaceTool ?? 'controls'}
snapshot={state.snapshot}
loading={state.loading}
externalControl={
state.snapshot && (
<ExternalControlPanel
key={state.snapshot.externalControl?.sessionId ?? state.selectedEntry}
snapshot={state.snapshot}
loading={state.loading}
bridge={externalControl}
onApplyProfile={(id) => void applyRobotProfile(id)}
/>
)
}
ignoreJointLimits={ignoreJointLimits}
jointAdvanced={jointAdvanced}
angleUnit={angleUnit}
@@ -2631,12 +2716,17 @@ export function App() {
ready={Boolean(state.snapshot)}
speed={state.speed}
loading={state.loading}
externalControl={state.snapshot?.controlOwner === 'external'}
onTogglePause={togglePause}
onStep={singleStep}
onReset={reset}
onSpeed={changeSpeed}
/>
<ViewerToolDock mode={state.mode} onModeChange={mode} />
<ViewerToolDock
mode={state.mode}
onModeChange={mode}
jointDisabled={jointPoseEditingDisabled}
/>
<MapViewportToolbar
visible={mapEditingActive && Boolean(state.snapshot)}
interactionActive={state.mode === 'select'}
@@ -2827,9 +2917,11 @@ export function App() {
</PanelWidthBudgetContext>
{pendingUrdfPath && (
<UrdfImportOptionsDialog
key={pendingUrdfPath}
open
path={pendingUrdfPath}
mountBodies={pendingUrdfMounts}
initialProfileId={pendingUrdfProfile}
onConfirm={confirmUrdfOptions}
onSkip={skipUrdfOptions}
/>
@@ -5,6 +5,7 @@ export function SimulationControls({
ready,
speed,
loading,
externalControl = false,
onTogglePause,
onStep,
onReset,
@@ -14,6 +15,7 @@ export function SimulationControls({
ready: boolean;
speed: number;
loading: boolean;
externalControl?: boolean;
onTogglePause: () => void;
onStep: () => void;
onReset: () => void;
@@ -34,7 +36,7 @@ export function SimulationControls({
tooltip="单步(暂停时可用)"
aria-label="单步"
onClick={onStep}
disabled={!ready || !paused}
disabled={!ready || !paused || externalControl}
>
<StepForward className="h-3.5 w-3.5" />
</IconButton>
@@ -49,7 +51,7 @@ export function SimulationControls({
<Select
aria-label="仿真速度"
value={speed}
disabled={loading}
disabled={loading || externalControl}
onChange={(event) => onSpeed(Number(event.target.value))}
className="w-[70px]"
>
@@ -2,6 +2,7 @@ import { useState, type ReactNode } from 'react';
import { Camera, Settings2 } from 'lucide-react';
import type { CameraDirection, UrdfEnhancementOptions } from '../../project/urdfToMjcf';
import { Button, Dialog, Select, Tooltip } from '../../components/ui';
import { ROBOT_PROFILES } from '../../robot/registry';
function OptionCard({
checked,
@@ -44,15 +45,18 @@ export function UrdfImportOptionsDialog({
open,
path,
mountBodies = [],
initialProfileId = '',
onConfirm,
onSkip,
}: {
open: boolean;
path?: string;
mountBodies?: string[];
onConfirm: (options: UrdfEnhancementOptions) => void;
initialProfileId?: string;
onConfirm: (options: UrdfEnhancementOptions, profileId?: string) => void;
onSkip: () => void;
}) {
const [profileId, setProfileId] = useState(initialProfileId);
const [options, setOptions] = useState<UrdfEnhancementOptions>(() => ({
addActuators: true,
addSensors: true,
@@ -77,8 +81,20 @@ export function UrdfImportOptionsDialog({
className="max-w-xl"
footer={
<div className="flex justify-end gap-2">
<Button onClick={onSkip}>不添加,直接加载</Button>
<Button variant="primary" onClick={() => onConfirm(options)}>
<Button onClick={onSkip} disabled={Boolean(profileId)}>
不添加,直接加载
</Button>
<Button
variant="primary"
onClick={() =>
profileId
? onConfirm(
{ addActuators: false, addSensors: false, sensorType: 'camera' },
profileId,
)
: onConfirm(options)
}
>
转换并加载
</Button>
</div>
@@ -88,90 +104,116 @@ export function UrdfImportOptionsDialog({
导入 <strong className="text-text-primary">{path}</strong> 的仿真组件
</p>
<div className="mt-4 space-y-3">
<OptionCard
checked={options.addActuators}
onChange={(addActuators) => setOptions((value) => ({ ...value, addActuators }))}
icon={<Settings2 className="h-4 w-4" />}
title="为关节添加驱动器"
description="生成不限幅 motor 控制输入:hinge 为 N·m,slide 为 N。已有驱动器不重复添加。"
/>
<OptionCard
checked={options.addSensors}
onChange={(addSensors) => setOptions((value) => ({ ...value, addSensors }))}
icon={<Camera className="h-4 w-4" />}
title="添加传感器"
description="在浮动基座添加三轴陀螺仪和三轴加速度计(6轴 IMU),并添加一台 640×480 固定摄像头。"
/>
{options.addSensors && (
<div className="border-t border-border pt-3">
<div className="mb-2 text-xs font-medium text-text-primary">摄像头安装参数</div>
<label className="block text-xs text-text-secondary">
<span className="mb-1 block">固连 Body</span>
<Select
aria-label="摄像头固连 Body"
className="w-full"
value={options.cameraMountBody ?? ''}
onChange={(event) =>
setOptions((value) => ({
...value,
cameraMountBody: event.target.value || undefined,
}))
}
>
{mountBodies.length ? (
mountBodies.map((name) => (
<option key={name} value={name}>
{name}
</option>
))
) : (
<option value="">自动选择头部/末端 Body</option>
)}
</Select>
</label>
<div className="mt-3 grid grid-cols-3 gap-2">
{(['X', 'Y', 'Z'] as const).map((axis, index) => (
<label key={axis} className="text-xs text-text-secondary">
<span className="mb-0.5 flex items-center gap-1">
<span className={`axis-badge axis-${axis.toLowerCase()}`}>{axis}</span>
位置(m)
</span>
<input
aria-label={`摄像头位置 ${axis}`}
className="field technical-value h-8 w-full px-1.5 text-xs"
type="number"
step="0.01"
value={(options.cameraPosition ?? [0.1, 0, 0.05])[index]}
onChange={(event) => setPosition(index, Number(event.target.value))}
/>
</label>
))}
</div>
<label className="mt-3 block text-xs text-text-secondary">
<span className="mb-1 block">镜头朝向(Body 局部轴)</span>
<Select
aria-label="摄像头朝向"
className="w-full"
value={options.cameraDirection ?? '+X'}
onChange={(event) =>
setOptions((value) => ({
...value,
cameraDirection: event.target.value as CameraDirection,
}))
}
>
{(['+X', '-X', '+Y', '-Y', '+Z', '-Z'] as CameraDirection[]).map((direction) => (
<option key={direction}>{direction}</option>
))}
</Select>
</label>
<Tooltip content="位置和朝向相对于所选 Body;ROS 摄像头通常 +X 朝前、+Z 朝上">
<span tabIndex={0} className="mt-2 inline-block text-xs text-text-tertiary">
安装坐标说明
</span>
</Tooltip>
</div>
<label className="block text-sm">
机器人 profile(可选)
<Select
aria-label="URDF 机器人 profile"
value={profileId}
className="mt-1 w-full"
onChange={(e) => setProfileId(e.target.value)}
>
<option value="">不启用(通用转换)</option>
{ROBOT_PROFILES.map((p) => (
<option key={p.id} value={p.id}>
{p.label}
</option>
))}
</Select>
</label>
{profileId && (
<p className="text-xs text-warning">
将校验固定模型版本,采用浮动基座、简化被动滚子/碰撞和九路伺服;不使用下方通用组件,不启用相机。
</p>
)}
<fieldset
disabled={Boolean(profileId)}
className={profileId ? 'space-y-3 opacity-50' : 'space-y-3'}
>
<OptionCard
checked={options.addActuators}
onChange={(addActuators) => setOptions((value) => ({ ...value, addActuators }))}
icon={<Settings2 className="h-4 w-4" />}
title="为关节添加驱动器"
description="生成不限幅 motor 控制输入:hinge 为 N·m,slide 为 N。已有驱动器不重复添加。"
/>
<OptionCard
checked={options.addSensors}
onChange={(addSensors) => setOptions((value) => ({ ...value, addSensors }))}
icon={<Camera className="h-4 w-4" />}
title="添加传感器"
description="在浮动基座添加三轴陀螺仪和三轴加速度计(6轴 IMU),并添加一台 640×480 固定摄像头。"
/>
{options.addSensors && (
<div className="border-t border-border pt-3">
<div className="mb-2 text-xs font-medium text-text-primary">摄像头安装参数</div>
<label className="block text-xs text-text-secondary">
<span className="mb-1 block">固连 Body</span>
<Select
aria-label="摄像头固连 Body"
className="w-full"
value={options.cameraMountBody ?? ''}
onChange={(event) =>
setOptions((value) => ({
...value,
cameraMountBody: event.target.value || undefined,
}))
}
>
{mountBodies.length ? (
mountBodies.map((name) => (
<option key={name} value={name}>
{name}
</option>
))
) : (
<option value="">自动选择头部/末端 Body</option>
)}
</Select>
</label>
<div className="mt-3 grid grid-cols-3 gap-2">
{(['X', 'Y', 'Z'] as const).map((axis, index) => (
<label key={axis} className="text-xs text-text-secondary">
<span className="mb-0.5 flex items-center gap-1">
<span className={`axis-badge axis-${axis.toLowerCase()}`}>{axis}</span>
位置(m)
</span>
<input
aria-label={`摄像头位置 ${axis}`}
className="field technical-value h-8 w-full px-1.5 text-xs"
type="number"
step="0.01"
value={(options.cameraPosition ?? [0.1, 0, 0.05])[index]}
onChange={(event) => setPosition(index, Number(event.target.value))}
/>
</label>
))}
</div>
<label className="mt-3 block text-xs text-text-secondary">
<span className="mb-1 block">镜头朝向(Body 局部轴)</span>
<Select
aria-label="摄像头朝向"
className="w-full"
value={options.cameraDirection ?? '+X'}
onChange={(event) =>
setOptions((value) => ({
...value,
cameraDirection: event.target.value as CameraDirection,
}))
}
>
{(['+X', '-X', '+Y', '-Y', '+Z', '-Z'] as CameraDirection[]).map((direction) => (
<option key={direction}>{direction}</option>
))}
</Select>
</label>
<Tooltip content="位置和朝向相对于所选 Body;ROS 摄像头通常 +X 朝前、+Z 朝上">
<span tabIndex={0} className="mt-2 inline-block text-xs text-text-tertiary">
安装坐标说明
</span>
</Tooltip>
</div>
)}
</fieldset>
</div>
<p className="mt-4 text-xs text-text-tertiary">
自动组件只写入浏览器内生成的 MJCF,不会修改本地 URDF 文件;使用“原生
@@ -9,11 +9,18 @@ const tools: ToolbarItem<InteractionMode>[] = [
export function ViewerToolDock({
mode,
onModeChange,
jointDisabled = false,
}: {
mode: InteractionMode;
jointDisabled?: boolean;
onModeChange: (mode: InteractionMode) => void;
}) {
return (
<ToolbarToggleGroup items={tools} value={mode} onChange={onModeChange} label="视口交互模式" />
<ToolbarToggleGroup
items={tools.map((tool) => ({ ...tool, disabled: tool.value === 'joint' && jointDisabled }))}
value={mode}
onChange={onModeChange}
label="视口交互模式"
/>
);
}
@@ -1,4 +1,4 @@
import { useState } from 'react';
import { useState, type ReactNode } from 'react';
import type { TrainingSceneCompiler } from '../../map/trainingMap';
import type { PolicyDeployment } from '../../rl/deployment';
import type { PlacedMapAsset } from '../../map/types';
@@ -37,6 +37,7 @@ export function WorkspaceToolsPanel({
active,
snapshot,
loading,
externalControl,
ignoreJointLimits,
jointAdvanced,
angleUnit,
@@ -82,6 +83,7 @@ export function WorkspaceToolsPanel({
active: WorkspaceTool;
snapshot?: SimulationSnapshot;
loading: boolean;
externalControl?: ReactNode;
ignoreJointLimits: boolean;
jointAdvanced: boolean;
angleUnit: 'rad' | 'deg';
@@ -125,10 +127,21 @@ export function WorkspaceToolsPanel({
onDataRecordingExport: (format: 'csv' | 'json') => void;
}) {
const [trainingSummary, setTrainingSummary] = useState('');
const manualDisabled =
loading || Boolean(snapshot?.controlOwner && snapshot.controlOwner !== 'manual');
const resolvedControllerStatus = controllerStatus ?? snapshot?.controller;
const resolvedPolicyStatus = policyStatus ?? snapshot?.rlPolicy;
const controls = snapshot ? (
<>
{externalControl && (
<CollapsibleSection
title="开源项目 / 外部控制"
defaultOpen={Boolean(snapshot.robot)}
keepMounted
>
{externalControl}
</CollapsibleSection>
)}
<CollapsibleSection
title="执行器实时控制"
defaultOpen={false}
@@ -143,6 +156,8 @@ export function WorkspaceToolsPanel({
<ActuatorControl
key={actuator.id}
showParameterNotice={false}
disabled={manualDisabled}
parametersDisabled={Boolean(snapshot.robot)}
actuator={actuator}
onControl={(value) => onActuator(actuator.id, value)}
onParameters={(parameters) => onActuatorParameters(actuator.id, parameters)}
@@ -159,10 +174,13 @@ export function WorkspaceToolsPanel({
badge={<ConsoleSectionBadges count={snapshot.joints.length} />}
>
<div className="mb-4 grid grid-cols-2 gap-2">
<Button onClick={onResetJoints}>重置关节</Button>
<Button onClick={onResetJoints} disabled={manualDisabled}>
重置关节
</Button>
<Button
variant={ignoreJointLimits ? 'primary' : 'secondary'}
aria-pressed={ignoreJointLimits}
disabled={manualDisabled}
onClick={onToggleJointLimits}
>
忽略关节限位
@@ -302,13 +320,19 @@ export function WorkspaceToolsPanel({
/>
}
>
<LocalTrainingPanel
onStatusChange={setTrainingSummary}
onPolicyReady={onImportPolicy}
compileScene={compileTrainingScene}
sceneMaps={trainingSceneMaps}
sceneDirty={trainingSceneDirty}
/>
{snapshot.robot && !snapshot.robot.capabilities.training ? (
<p className="text-xs text-text-secondary">
此机器人接口 V1 不支持训练。既有 Go2 训练任务不受影响,请使用对应模型。
</p>
) : (
<LocalTrainingPanel
onStatusChange={setTrainingSummary}
onPolicyReady={onImportPolicy}
compileScene={compileTrainingScene}
sceneMaps={trainingSceneMaps}
sceneDirty={trainingSceneDirty}
/>
)}
</CollapsibleSection>
</>
) : (
@@ -0,0 +1,120 @@
import { useCallback, useEffect, useMemo, useRef, useState, type RefObject } from 'react';
import type { PhysicsAdapter } from '../../simulation/PhysicsAdapter';
import { ExternalControlClient, type BridgeStatus } from '../../robot/ExternalControlClient';
/** UI lifecycle only. Neither physics stepping nor observation transport depends on React. */
export function useExternalControl(adapter: RefObject<PhysicsAdapter>, changed: () => void) {
const [status, setStatus] = useState<BridgeStatus>({ phase: 'disconnected' });
const [endpoint, setEndpoint] = useState('http://127.0.0.1:8766');
const [token, setToken] = useState(''); // Memory only: never store/export/log a bearer token.
const [observationAgeMs, setAge] = useState<number>();
const changedRef = useRef(changed),
alive = useRef(true);
changedRef.current = changed;
const client = useMemo(
() =>
new ExternalControlClient(
() => adapter.current,
(next) => {
if (alive.current) {
setStatus(next);
changedRef.current();
}
},
),
[adapter],
);
const disconnect = useCallback(
(reason = '用户关闭桥接') => {
client.disconnect(reason);
adapter.current.stopExternalControl(reason);
if (alive.current) changedRef.current();
},
[adapter, client],
);
const stop = useCallback(() => {
adapter.current.stopExternalControl('用户停止外部控制,请重新授权');
client.sync();
changedRef.current();
}, [adapter, client]);
const authorize = useCallback(() => {
try {
if (document.visibilityState === 'hidden') throw new Error('页面隐藏时不能授权外部控制');
if (client.status().phase !== 'ready') throw new Error('请先连接本机桥接');
adapter.current.setExternalControlEnabled(true);
client.sync();
setStatus(client.status());
changedRef.current();
} catch (error: unknown) {
setStatus((s) => ({ ...s, error: error instanceof Error ? error.message : '授权失败' }));
}
}, [adapter, client]);
const connect = useCallback(async () => {
try {
if (
window.location.protocol !== 'http:' ||
!['127.0.0.1', 'localhost'].includes(window.location.hostname)
)
throw new Error('V1 需要本机 HTTP 前端;不支持远程页面或 HTTPS 混合内容');
await client.connect(endpoint, token);
} catch (error: unknown) {
if (alive.current)
setStatus({
phase: 'disconnected',
error: error instanceof Error ? error.message : '连接失败',
});
}
}, [client, endpoint, token]);
useEffect(() => {
alive.current = true;
const visibility = () => {
if (document.visibilityState === 'hidden') disconnect('页面隐藏,已暂停并撤销授权');
};
const leaving = () => disconnect('页面退出,已撤销授权');
document.addEventListener('visibilitychange', visibility);
window.addEventListener('pagehide', leaving);
window.addEventListener('beforeunload', leaving);
let previousLifecycle = '';
const timer = setInterval(() => {
const control = adapter.current.externalControlStatus();
setAge(control?.observationAgeMs);
const lifecycle = control
? JSON.stringify([
control.sessionId,
control.modelEpoch,
control.authorizationGeneration,
control.enabled,
control.connected,
control.error,
adapter.current.robotObservation()?.paused,
])
: '';
// The viewer intentionally stops taking full snapshots while paused. Publish
// one on an asynchronous stop/reset/claim, rather than polling the whole model.
if (lifecycle !== previousLifecycle) {
previousLifecycle = lifecycle;
changedRef.current();
}
}, 200);
return () => {
alive.current = false;
clearInterval(timer);
document.removeEventListener('visibilitychange', visibility);
window.removeEventListener('pagehide', leaving);
window.removeEventListener('beforeunload', leaving);
disconnect('工作台已关闭');
};
}, [adapter, disconnect]);
return {
status,
endpoint,
setEndpoint,
token,
setToken,
observationAgeMs,
connect,
disconnect,
authorize,
stop,
};
}
@@ -3,6 +3,7 @@ import { IconButton } from './IconButton';
export interface ToolbarItem<T extends string> {
value: T;
label: string;
disabled?: boolean;
icon: ComponentType<{ className?: string }>;
}
export function ToolbarToggleGroup<T extends string>({
@@ -28,6 +29,7 @@ export function ToolbarToggleGroup<T extends string>({
<IconButton
key={item.value}
active={item.value === value}
disabled={item.disabled}
tooltip={item.label}
aria-label={item.label}
onClick={() => onChange(item.value)}
@@ -0,0 +1,72 @@
import type { ProjectManifest } from '../types';
import { LEKIWI_PROFILE } from '../../robot/profiles/lekiwi';
import { sha256, validateLeKiwiSource } from '../../robot/registry';
import { RobotError } from '../../robot/types';
import {
LEKIWI_COLLISION_REVISION,
LEKIWI_COLLISION_RECIPE_HASH,
LEKIWI_COLLISION_SOURCE_HASHES,
prepareLeKiwiUrdf,
} from './lekiwi';
async function validateCollisionSources(manifest: ProjectManifest, entryPath: string) {
const directory = entryPath.slice(0, entryPath.lastIndexOf('/') + 1);
await Promise.all(
Object.entries(LEKIWI_COLLISION_SOURCE_HASHES).map(async ([path, expected]) => {
const file = manifest.files.find((candidate) => candidate.path === directory + path);
if (!file || (await sha256(file.data)) !== expected)
throw new RobotError('INCOMPATIBLE_MODEL', `碰撞体源网格缺失或 SHA-256 不匹配:${path}`);
}),
);
}
/** Explicit, source-bound preprocessing; never executes profile content from imports. */
export async function prepareRobotProject(
manifest: ProjectManifest,
entryPath: string,
profileId: string,
): Promise<ProjectManifest> {
if (profileId !== LEKIWI_PROFILE.id) throw new RobotError('UNSUPPORTED', '机器人 profile 未注册');
const source = manifest.files.find((f) => f.path === entryPath);
if (!source) throw new RobotError('INCOMPATIBLE_MODEL', '找不到机器人模型');
if (manifest.entries.find((e) => e.path === entryPath)?.format === 'mjcf') {
const xml = new DOMParser().parseFromString(
new TextDecoder().decode(source.data),
'application/xml',
);
if (
xml.querySelector('parsererror') ||
xml.querySelector('custom text[name="platform_robot_profile"]')?.getAttribute('data') !==
LEKIWI_PROFILE.id ||
xml
.querySelector('custom text[name="platform_robot_profile_version"]')
?.getAttribute('data') !== String(LEKIWI_PROFILE.version) ||
xml
.querySelector('custom text[name="platform_robot_source_sha256"]')
?.getAttribute('data') !== LEKIWI_PROFILE.source.urdfSha256
)
throw new RobotError('INCOMPATIBLE_MODEL', 'MJCF 缺少受支持的机器人 profile 来源');
if (
xml
.querySelector('custom text[name="platform_lekiwi_collision_revision"]')
?.getAttribute('data') !== String(LEKIWI_COLLISION_REVISION) ||
xml
.querySelector('custom text[name="platform_lekiwi_collision_recipe_sha256"]')
?.getAttribute('data') !== LEKIWI_COLLISION_RECIPE_HASH
)
throw new RobotError(
'INCOMPATIBLE_MODEL',
'LeKiwi 碰撞配方已更新;请从原始 URDF 重新转换,旧 MJCF 不会自动升级',
);
// Never enhance twice. Compiled topology/actuators are checked by the backend.
await validateCollisionSources(manifest, entryPath);
return manifest;
}
await validateLeKiwiSource(source.data);
await validateCollisionSources(manifest, entryPath);
const data = prepareLeKiwiUrdf(source.data);
const files = manifest.files.map((f) =>
f === source ? { ...f, data, size: data.byteLength } : f,
);
return { ...manifest, files, totalBytes: files.reduce((sum, f) => sum + f.size, 0) };
}
@@ -0,0 +1,197 @@
import { webcrypto } from 'node:crypto';
import {
enhanceLeKiwiMjcf,
LEKIWI_COLLISION_REVISION,
LEKIWI_COLLISION_RECIPE_HASH,
type LeKiwiJointGeometry,
} from './lekiwi';
import { prepareRobotProject } from './index';
import { LEKIWI_PROFILE as p } from '../../robot/profiles/lekiwi';
import collision from '../../../../robot_profiles/lekiwi-full-collision.json';
import type { ProjectManifest } from '../types';
const encode = (xml: string) => new TextEncoder().encode(xml);
function source() {
const geometry: LeKiwiJointGeometry[] = p.arm.map((joint, i) => ({
name: joint.joint,
body: `arm_${i}`,
position: [i * 0.1, 0, 0],
rotation: [1, 0, 0, 0, 1, 0, 0, 0, 1],
}));
const visuals = (group: string) =>
collision.coverage
.filter((c) => c.weldJoint === group)
.map(
(c) =>
`<body name="${c.link}_fixture"><geom name="${c.visual}" type="mesh" mesh="source_${c.link}" group="1" pos=".01 .02 .03" quat=".5 .5 .5 .5"/></body>`,
)
.join('');
const arm = (i: number): string =>
i === p.arm.length
? ''
: `<body name="arm_${i}">
<inertial mass="1" pos="0 0 0" diaginertia="1 1 1"/>
<joint name="${p.arm[i].joint}" axis="${i === 5 ? '0 -0.906308 -0.422618' : '1 0 0'}"/>
${visuals(p.arm[i].joint)}${arm(i + 1)}</body>`;
const wheels = p.wheels
.map((name) => `<body name="${name}"><joint name="${name}"/></body>`)
.join('');
return {
data: encode(`<mujoco><asset/><worldbody><body name="${p.baseBody}">
<body name="${collision.source.rootLink}">${visuals('base')}${arm(0)}</body>${wheels}
</body></worldbody></mujoco>`),
geometry,
};
}
function enhanced() {
const { data, geometry } = source();
return new DOMParser().parseFromString(
new TextDecoder().decode(enhanceLeKiwiMjcf(data, geometry)),
'application/xml',
);
}
it('covers every arm visual including welded motors/accessories, in its visual frame without added mass', () => {
const doc = enhanced();
expect(collision.coverage).toHaveLength(18);
expect(collision.coverage.map((c) => c.mesh).sort()).toEqual(
Object.keys(collision.source.meshes).sort(),
);
const geoms = Array.from(doc.querySelectorAll('geom[name^="__lekiwi_cad_"]'));
expect(geoms).toHaveLength(collision.parts.length);
const meshes = new Map(
Array.from(doc.querySelectorAll('asset mesh')).map((m) => [m.getAttribute('name'), m]),
);
for (const geom of geoms) {
expect(geom.getAttribute('mass')).toBe('0');
expect(geom.getAttribute('solref')).toBe(collision.physics.solref.join(' '));
expect(geom.getAttribute('solimp')).toBe(collision.physics.solimp.join(' '));
expect(geom.getAttribute('pos')).toBe('.01 .02 .03');
expect(geom.getAttribute('quat')).toBe('.5 .5 .5 .5');
expect(geom.getAttribute('contype')).toBe('2');
expect(geom.getAttribute('conaffinity')).toBe('3');
expect(geom.getAttribute('group')).toBe('3');
const vertices = meshes
.get(geom.getAttribute('mesh'))!
.getAttribute('vertex')!
.split(/\s+/)
.map(Number);
expect(vertices.length).toBeGreaterThanOrEqual(12);
expect(vertices.every(Number.isFinite)).toBe(true);
expect(Math.max(...vertices.map(Math.abs))).toBeLessThan(0.12);
}
for (const spec of p.arm)
expect(doc.querySelector(`geom[name="__lekiwi_${spec.joint}_collision"]`)).toBeNull();
const axis = doc
.querySelector('joint[name="arm_gripper"]')!
.getAttribute('axis')!
.split(/\s+/)
.map(Number);
expect(axis).toEqual([0, 0.906308, 0.422618]);
expect(p.arm[5].min).toBe(-0.18);
expect(LEKIWI_COLLISION_REVISION).toBe(4);
expect(doc.querySelector('option')!.getAttribute('timestep')).toBe(
String(collision.physics.timestep),
);
expect(
doc.querySelector('text[name="platform_lekiwi_collision_recipe_sha256"]')!.getAttribute('data'),
).toBe(LEKIWI_COLLISION_RECIPE_HASH);
});
it('enables structural adjacent contacts (including elbow and fingers), not body-wide exclusions', () => {
const doc = enhanced();
expect(doc.querySelectorAll('contact exclude')).toHaveLength(0);
const pairs = Array.from(doc.querySelectorAll('contact pair'));
expect(pairs).toHaveLength(collision.jointPolicy.reduce((sum, j) => sum + j.pairs.length, 0));
for (const pair of pairs)
expect(pair.getAttribute('solref')).toBe(collision.physics.solref.join(' '));
expect(
pairs.some(
(pair) =>
pair.getAttribute('geom1')!.includes('SO_ARM100_08k_116_Square-v1') &&
pair.getAttribute('geom2')!.includes('SO_ARM100_08k_Mirror-v1'),
),
).toBe(true);
expect(
pairs.some(
(pair) =>
pair.getAttribute('geom1')!.includes('Wrist_Roll_08c-v1') &&
pair.getAttribute('geom2')!.includes('Moving_Jaw_08d-v1'),
),
).toBe(true);
for (const policy of collision.jointPolicy) {
expect(policy.pairs.length).toBeGreaterThan(0);
expect(policy.radiusM).toBeLessThanOrEqual(0.025);
for (const pair of policy.pairs)
expect(pair.some((part) => policy.coreParts.includes(part))).toBe(false);
}
});
it.each([
'Base_08q-v1_visual',
'SO_ARM100_08k_Mirror-v1_visual',
'SO_ARM100_08k_116_Square-v1_visual',
'Moving_Jaw_08d-v1_visual',
])('rejects missing/unexpected visual coverage: %s', (name) => {
const { data, geometry } = source();
expect(() =>
enhanceLeKiwiMjcf(encode(new TextDecoder().decode(data).replace(name, 'unrelated')), geometry),
).toThrow(/覆盖清单/);
});
it('rejects a visual attached to the wrong moving body', () => {
const { data, geometry } = source();
const wrong = new TextDecoder()
.decode(data)
.replace(
'<body name="SO_ARM100_08k_Mirror-v1_fixture">',
'<body name="SO_ARM100_08k_Mirror-v1_fixture"><joint name="unrelated"/>',
);
expect(() => enhanceLeKiwiMjcf(encode(wrong), geometry)).toThrow(/运动链/);
});
function manifest(revision?: number, hash = LEKIWI_COLLISION_RECIPE_HASH): ProjectManifest {
const data = encode(`<mujoco><custom>
<text name="platform_robot_profile" data="${p.id}"/>
<text name="platform_robot_profile_version" data="${p.version}"/>
<text name="platform_robot_source_sha256" data="${p.source.urdfSha256}"/>
<text name="platform_lekiwi_collision_recipe_sha256" data="${hash}"/>
${revision === undefined ? '' : `<text name="platform_lekiwi_collision_revision" data="${revision}"/>`}
</custom></mujoco>`);
return {
id: 'model',
name: 'model',
maps: [],
entries: [{ path: 'model.xml', format: 'mjcf', label: 'model' }],
files: [{ path: 'model.xml', data, size: data.length, source: 'file', mimeType: '' }],
totalBytes: data.length,
};
}
it.each([undefined, 1, 2, 3])(
'requires reconversion of legacy collision revision %s',
async (revision) => {
await expect(prepareRobotProject(manifest(revision), 'model.xml', p.id)).rejects.toThrow(
/原始 URDF.*重新转换/,
);
},
);
it('rejects a mismatched cooking fingerprint and missing visual assets', async () => {
vi.stubGlobal('crypto', webcrypto);
try {
await expect(prepareRobotProject(manifest(4, 'incorrect'), 'model.xml', p.id)).rejects.toThrow(
/重新转换/,
);
await expect(prepareRobotProject(manifest(4), 'model.xml', p.id)).rejects.toThrow(/源网格缺失/);
const changed = manifest(4);
changed.files.push(
...Object.keys(collision.source.meshes).map((path) => ({
path,
data: encode('modified STL'),
size: 12,
source: 'file' as const,
mimeType: '',
})),
);
await expect(prepareRobotProject(changed, 'model.xml', p.id)).rejects.toThrow(/SHA-256 不匹配/);
} finally {
vi.unstubAllGlobals();
}
});
@@ -0,0 +1,52 @@
import { webcrypto } from 'node:crypto';
import { prepareLeKiwiUrdf, enhanceLeKiwiMjcf } from './lekiwi';
import { prepareRobotProject } from './index';
import type { ProjectManifest } from '../types';
const encode = (xml: string) => new TextEncoder().encode(xml);
describe('explicit LeKiwi profile preparation', () => {
it('replaces only oversized wheels and preserves source bytes', () => {
const xml =
'<robot><link name="wheel"><visual><geometry><mesh filename="meshes/4-Omni-Directional-Wheel_Single_Body-v1.stl"/></geometry></visual></link><link name="arm"><visual><geometry><mesh filename="meshes/arm.stl"/></geometry></visual></link></robot>';
const data = encode(xml),
result = new TextDecoder().decode(prepareLeKiwiUrdf(data));
expect(new TextDecoder().decode(data)).toBe(xml);
expect(result).toContain('strippath="false"');
expect(result).toContain('<sphere');
expect(result).toContain('meshes/arm.stl');
expect(result).not.toContain('Omni-Directional');
});
it('rejects invalid topology and repeated enhancement', () => {
expect(() => enhanceLeKiwiMjcf(encode('<mujoco><worldbody/></mujoco>'), [])).toThrow(/基座/);
expect(() =>
enhanceLeKiwiMjcf(
encode(
'<mujoco><custom><text name="platform_robot_profile" data="lekiwi-v1"/></custom></mujoco>',
),
[],
),
).toThrow(/重复/);
});
it('does not select unknown profiles or accept a same-named arbitrary URDF', async () => {
vi.stubGlobal('crypto', webcrypto);
const data = encode('<robot name="LeKiwi"/>');
const manifest: ProjectManifest = {
id: 'x',
name: 'x',
files: [{ path: 'LeKiwi.urdf', data, size: data.length, mimeType: '', source: 'file' }],
entries: [{ path: 'LeKiwi.urdf', format: 'urdf', label: 'x' }],
maps: [],
totalBytes: data.length,
};
try {
await expect(prepareRobotProject(manifest, 'LeKiwi.urdf', 'unknown')).rejects.toThrow(
/未注册/,
);
await expect(prepareRobotProject(manifest, 'LeKiwi.urdf', 'lekiwi-v1')).rejects.toThrow(
/版本不符/,
);
} finally {
vi.unstubAllGlobals();
}
});
});
@@ -0,0 +1,396 @@
import { LEKIWI_PROFILE as p, WHEEL_TANGENTS } from '../../robot/profiles/lekiwi';
import { RobotError } from '../../robot/types';
import collision from '../../../../robot_profiles/lekiwi-full-collision.json';
export const LEKIWI_COLLISION_REVISION = collision.revision;
export const LEKIWI_COLLISION_RECIPE_HASH = collision.recipeSha256;
export const LEKIWI_COLLISION_SOURCE_HASHES = collision.source.meshes;
const decoder = new TextDecoder(),
encoder = new TextEncoder();
export interface LeKiwiJointGeometry {
name: string;
body: string;
position: number[];
rotation: number[];
}
function parse(data: Uint8Array): Document {
const doc = new DOMParser().parseFromString(decoder.decode(data), 'application/xml');
if (doc.querySelector('parsererror'))
throw new RobotError('INCOMPATIBLE_MODEL', 'LeKiwi XML 无效');
return doc;
}
function serialize(doc: Document): Uint8Array {
return encoder.encode(new XMLSerializer().serializeToString(doc));
}
function element(doc: Document, tag: string, attributes: Record<string, string | number>) {
const node = doc.createElement(tag);
for (const [name, value] of Object.entries(attributes)) node.setAttribute(name, String(value));
return node;
}
/** The CAD wheel STL has >300k faces, above MuJoCo's STL limit. This explicit,
* source-hash-gated preprocessing removes only the oversized wheel visual/collision.
* The original source files remain intact; the simulation recipe replaces wheels.
*/
export function prepareLeKiwiUrdf(data: Uint8Array): Uint8Array {
const doc = parse(data),
robot = doc.documentElement;
let mujoco = robot.querySelector(':scope > mujoco');
if (!mujoco) {
mujoco = element(doc, 'mujoco', {});
robot.prepend(mujoco);
}
let compiler = mujoco.querySelector('compiler');
if (!compiler) {
compiler = element(doc, 'compiler', {});
mujoco.append(compiler);
}
compiler.setAttribute('strippath', 'false');
for (const mesh of doc.querySelectorAll('mesh[filename]')) {
if (mesh.getAttribute('filename')!.includes('4-Omni-Directional-Wheel_Single_Body'))
mesh.replaceWith(element(doc, 'sphere', { radius: p.wheelRadius }));
}
return serialize(doc);
}
/** CAD-frame convex parts: preserve the source visual's body/pose and explicit inertia.
* Do not approximate offset bodies by a capsule along joint origins, or bridge jaws.
*/
function addCadCollisions(
doc: Document,
cad: Element,
prefix: string,
parts: readonly { name: string; visual: string; vertices: string }[],
): void {
const asset = doc.querySelector('mujoco > asset');
if (!asset) throw new RobotError('INCOMPATIBLE_MODEL', 'LeKiwi 缺少 CAD 碰撞网格资源');
// Resolve original visuals once; repeated DOM queries grow quadratically as
// generated hulls are appended (more than a thousand in the complete recipe).
const visuals = new Map(
Array.from(cad.querySelectorAll('geom[name]')).map((geom) => [
geom.getAttribute('name')!,
geom,
]),
);
for (const part of parts) {
const visual = visuals.get(part.visual);
if (!visual || visual.getAttribute('type') !== 'mesh')
throw new RobotError('INCOMPATIBLE_MODEL', `缺少碰撞体的源视觉几何:${part.visual}`);
const name = `__lekiwi_${prefix}_${part.name}`;
asset.append(element(doc, 'mesh', { name, vertex: part.vertices }));
const geom = visual.cloneNode(true) as Element;
for (const [key, value] of Object.entries({
name: `${name}_collision`,
mesh: name,
group: '3',
contype: '2',
conaffinity: '3',
mass: '0',
friction: '0.8 0.005 0.0001',
priority: '1',
solref: collision.physics.solref.join(' '),
solimp: collision.physics.solimp.join(' '),
rgba: '.8 .4 .1 .25',
}))
geom.setAttribute(key, value);
visual.parentElement!.append(geom);
}
}
function addArmCollisions(doc: Document, cad: Element, contact: Element): void {
const arm = cad.querySelector(`body[name="${collision.source.rootLink}"]`);
const expected = new Set(collision.coverage.map((item) => item.visual));
const actual = Array.from(arm?.querySelectorAll('geom[group="1"]') ?? []);
if (
actual.length !== expected.size ||
actual.some((geom) => !expected.has(geom.getAttribute('name')!))
)
throw new RobotError('INCOMPATIBLE_MODEL', '整臂视觉网格与碰撞覆盖清单不一致');
// Validate the rigid-body ownership too, rather than attaching shapes by name
// to an unrelated body after a broken conversion.
for (const item of collision.coverage) {
let body: Element | null = cad.querySelector(`geom[name="${item.visual}"]`)!.parentElement;
let group = 'base';
while (body && body !== cad) {
const joint = body.querySelector(':scope > joint');
if (joint) {
group = joint.getAttribute('name')!;
break;
}
body = body.parentElement;
}
if (group !== item.weldJoint)
throw new RobotError('INCOMPATIBLE_MODEL', `碰撞体所属运动链不匹配:${item.visual}`);
}
addCadCollisions(doc, cad, 'cad', collision.parts);
// Parent filtering stays ON: explicitly enable only structural hull pairs.
// Offline policy ignores bounded bearing-core parts, not whole adjacent links.
// Axial/radial swept bounds prune pairs impossible over an entire revolution.
for (const policy of collision.jointPolicy)
for (const [a, b] of policy.pairs)
contact.append(
element(doc, 'pair', {
geom1: `__lekiwi_cad_${a}_collision`,
geom2: `__lekiwi_cad_${b}_collision`,
solref: collision.physics.solref.join(' '),
solimp: collision.physics.solimp.join(' '),
condim: 3,
}),
);
}
/** Simulation-only recipe, applied after MuJoCo's own URDF conversion. No URDF FK parser. */
export function enhanceLeKiwiMjcf(
data: Uint8Array,
geometry: readonly LeKiwiJointGeometry[],
): Uint8Array {
const doc = parse(data),
root = doc.documentElement;
if (root.querySelector('custom text[name="platform_robot_profile"]'))
throw new RobotError('INCOMPATIBLE_MODEL', '已增强的 LeKiwi 不能重复应用 profile');
const world = root.querySelector('worldbody');
const cad = world?.querySelector(`:scope > body[name="${p.baseBody}"]`);
if (!world || !cad) throw new RobotError('INCOMPATIBLE_MODEL', 'LeKiwi 缺少预期基座');
const joints = new Map(
Array.from(cad.querySelectorAll('joint[name]')).map((j) => [j.getAttribute('name')!, j]),
);
if (![...p.arm.map((j) => j.joint), ...p.wheels].every((name) => joints.has(name)))
throw new RobotError('INCOMPATIBLE_MODEL', 'LeKiwi 驱动关节不完整');
// Disable CAD collision meshes (visuals retained). CAD mass estimates are not
// trustworthy: replace inertials only on the dynamic joints with explicit estimates.
for (const geom of cad.querySelectorAll('geom')) {
geom.setAttribute('contype', '0');
geom.setAttribute('conaffinity', '0');
geom.setAttribute('mass', '0');
if (geom.getAttribute('group') !== '1') geom.remove();
}
const armInertials = new Map(
p.arm.map((joint) => {
const body = joints.get(joint.joint)!.parentElement!;
return [
joint.joint,
body.querySelector(':scope > inertial')?.cloneNode(true) as Element | undefined,
];
}),
);
for (const inertial of cad.querySelectorAll('inertial')) inertial.remove();
for (const joint of cad.querySelectorAll(':scope > freejoint, :scope > joint')) joint.remove();
for (const wheel of p.wheels) joints.get(wheel)!.parentElement!.remove();
root.querySelector('actuator')?.remove();
const actuators = element(doc, 'actuator', {});
root.append(actuators);
// Explicit canonical frame: CAD +Y is forward; turn CAD by -90 degrees inside
// a new canonical base. Wheel placement is the ideal r/R geometry, not an
// undocumented reinterpretation of the CAD's unequal axle positions.
cad.setAttribute('name', '__lekiwi_cad_base__');
cad.setAttribute('pos', '0 0 -0.01786');
cad.setAttribute(
'quat',
`${Math.cos(p.mapping.cadYawRad / 2)} 0 0 ${Math.sin(p.mapping.cadYawRad / 2)}`,
);
const base = element(doc, 'body', { name: p.baseBody, pos: '0 0 0.0515' });
base.append(element(doc, 'freejoint', { name: '__platform_base_freejoint__' }));
base.append(
element(doc, 'inertial', {
pos: '0 0 0.04',
mass: p.simulation.baseMass,
diaginertia: p.simulation.baseInertia.join(' '),
}),
);
base.append(
element(doc, 'geom', {
name: '__lekiwi_chassis_collision',
type: 'cylinder',
size: '.112 .025',
pos: '0 0 .04',
group: 3,
contype: 2,
conaffinity: 3,
friction: '0.8 0.002 0.0001',
rgba: '.2 .3 .4 .2',
}),
);
cad.replaceWith(base);
base.append(cad);
let option: Element | null = root.querySelector('option');
if (!option) {
option = element(doc, 'option', {});
root.prepend(option);
}
for (const [k, v] of Object.entries({
timestep: String(collision.physics.timestep),
integrator: 'implicitfast',
cone: 'elliptic',
iterations: '60',
}))
option.setAttribute(k, v);
const contact = root.querySelector('contact') ?? element(doc, 'contact', {});
if (!contact.parentElement) root.append(contact);
const masses = p.simulation.armMasses;
p.arm.forEach((spec, i) => {
const joint = joints.get(spec.joint)!,
body = joint.parentElement!;
joint.setAttribute('limited', 'true');
joint.setAttribute('range', `${spec.min} ${spec.max}`);
joint.setAttribute('damping', '.02');
joint.setAttribute('armature', '.002');
if (spec.joint === 'arm_gripper') {
// CAD positive rotation CLOSES the fingers. Canonical positive must OPEN.
joint.setAttribute(
'axis',
joint
.getAttribute('axis')!
.split(/\s+/)
.map((v) => Number(v) * p.mapping.gripperAxisSign)
.join(' '),
);
}
const old = armInertials.get(spec.joint);
if (!old) throw new RobotError('INCOMPATIBLE_MODEL', `缺少惯性:${spec.joint}`);
const scale = masses[i] / Number(old.getAttribute('mass'));
old.setAttribute('mass', String(masses[i]));
for (const key of ['diaginertia', 'fullinertia'])
if (old.hasAttribute(key))
old.setAttribute(
key,
old
.getAttribute(key)!
.split(/\s+/)
.map((v) => Number(v) * scale)
.join(' '),
);
body.prepend(old);
if (!geometry.some((j) => j.name === spec.joint && j.body === body.getAttribute('name')))
throw new RobotError('INCOMPATIBLE_MODEL', `缺少运动学:${spec.joint}`);
actuators.append(
element(doc, 'position', {
name: `${spec.joint}_servo`,
joint: spec.joint,
kp: spec.kp,
kv: spec.kv,
ctrllimited: 'true',
ctrlrange: `${spec.min} ${spec.max}`,
forcelimited: 'true',
forcerange: `${-spec.force} ${spec.force}`,
}),
);
});
addArmCollisions(doc, cad, contact);
// 12 passive rollers per wheel; axes tangent to the rim. Free axial rolling
// arises from passive hinge motion, not a fake planar force or qvel assignment.
p.wheels.forEach((name, i) => {
const [tx, ty] = WHEEL_TANGENTS[i],
axle = [-ty, tx, 0];
const wheel = element(doc, 'body', {
name: `__lekiwi_${name}`,
pos: `${p.baseRadius * ty} ${-p.baseRadius * tx} 0`,
});
wheel.append(
element(doc, 'joint', {
name,
type: 'hinge',
axis: axle.join(' '),
limited: 'false',
damping: '.0002',
armature: '.00005',
}),
);
wheel.append(
element(doc, 'inertial', {
pos: '0 0 0',
mass: p.simulation.wheelMass,
diaginertia: '.00004 .00004 .00004',
}),
);
wheel.append(
element(doc, 'geom', {
name: `__lekiwi_${name}_hub`,
type: 'cylinder',
size: '.033 .012',
zaxis: axle.join(' '),
contype: 0,
conaffinity: 0,
group: 1,
mass: 0,
rgba: '.12 .15 .18 1',
}),
);
for (let j = 0; j < p.simulation.rollerCount; j++) {
const angle = (2 * Math.PI * j) / p.simulation.rollerCount,
s = Math.sin(angle),
c = Math.cos(angle),
r = p.wheelRadius - 0.009;
const axis = [c * tx, c * ty, -s];
const roller = element(doc, 'body', {
name: `__lekiwi_${name}_roller_${j}`,
pos: `${r * s * tx} ${r * s * ty} ${r * c}`,
});
roller.append(
element(doc, 'joint', {
name: `__lekiwi_${name}_passive_${j}`,
axis: axis.join(' '),
limited: 'false',
damping: '.000005',
armature: '.0000001',
}),
);
roller.append(
element(doc, 'geom', {
name: `__lekiwi_${name}_contact_${j}`,
type: 'capsule',
size: '.009 .006',
zaxis: axis.join(' '),
mass: p.simulation.rollerMass,
group: 0,
contype: 4,
conaffinity: 1,
friction: '1 .001 .0001',
condim: 3,
solref: '.008 1',
solimp: '.9 .95 .001',
rgba: '.25 .29 .33 1',
}),
);
wheel.append(roller);
}
base.append(wheel);
actuators.append(
element(doc, 'velocity', {
name: `${name}_servo`,
joint: name,
kv: p.simulation.wheelKv,
ctrllimited: 'true',
ctrlrange: `${-p.wheelMaxRadS} ${p.wheelMaxRadS}`,
forcelimited: 'true',
forcerange: `${-p.simulation.wheelTorqueLimit} ${p.simulation.wheelTorqueLimit}`,
}),
);
});
const custom = root.querySelector('custom') ?? element(doc, 'custom', {});
if (!custom.parentElement) root.append(custom);
custom.append(element(doc, 'text', { name: 'platform_robot_profile', data: p.id }));
custom.append(
element(doc, 'text', {
name: 'platform_lekiwi_collision_revision',
data: LEKIWI_COLLISION_REVISION,
}),
);
custom.append(
element(doc, 'text', {
name: 'platform_lekiwi_collision_recipe_sha256',
data: LEKIWI_COLLISION_RECIPE_HASH,
}),
);
custom.append(
element(doc, 'text', { name: 'platform_robot_profile_version', data: String(p.version) }),
);
custom.append(
element(doc, 'text', { name: 'platform_robot_source_sha256', data: p.source.urdfSha256 }),
);
return serialize(doc);
}
@@ -1,4 +1,5 @@
import type { MjData, MjModel } from '@mujoco/mujoco';
import { ModelBindings } from '../../simulation/ModelBindings';
import { buildGo2wObservation, GO2W_VELOCITY_TASK } from '../tasks/go2wVelocity';
import type { JointBinding, RLCommand } from '../types';
import type { PolicyRuntimeBindings } from './OnnxPolicyRuntime';
@@ -38,42 +39,12 @@ export class Go2wPolicyBindings implements PolicyRuntimeBindings {
private readonly setActuator: (id: number, value: number) => void,
private readonly effortLimits?: readonly number[],
) {
const jointIds = new Map<string, number>(),
actuatorIds = new Map<string, number>(),
sensorIds = new Map<string, number>(),
bodyIds = new Map<string, number>();
for (let id = 0; id < model.njnt; id += 1) {
const item = model.jnt(id);
try {
if (item.name) jointIds.set(item.name, id);
} finally {
item.delete();
}
}
for (let id = 0; id < model.nactuator; id += 1) {
const item = model.actuator(id);
try {
if (item.name) actuatorIds.set(item.name, id);
} finally {
item.delete();
}
}
for (let id = 0; id < model.nsensor; id += 1) {
const item = model.sensor(id);
try {
if (item.name) sensorIds.set(item.name, id);
} finally {
item.delete();
}
}
for (let id = 0; id < model.nbody; id += 1) {
const item = model.body(id);
try {
if (item.name) bodyIds.set(item.name, id);
} finally {
item.delete();
}
}
const {
joints: jointIds,
actuators: actuatorIds,
sensors: sensorIds,
bodies: bodyIds,
} = new ModelBindings(model);
this.baseBodyId =
bodyIds.get('base_link') ?? bodyIds.get('base') ?? this.findFloatingBaseBody();
this.baseFreeJointId = this.findFreeJoint(this.baseBodyId);
@@ -0,0 +1,83 @@
import fixture from '../../../contracts/fixtures/single-joint.json';
import { ExternalControlClient } from './ExternalControlClient';
import { validateDescriptor, validateObservation } from './validation';
function setup() {
const descriptor = validateDescriptor(fixture.descriptor);
const control = {
enabled: true,
connected: false,
authorizationGeneration: 1,
modelEpoch: 2,
sessionId: 'sim-test',
lastActionSeq: 0,
};
const backend = {
describeRobot: () => descriptor,
robotObservation: () => validateObservation(fixture.observation, descriptor),
externalControlStatus: () => ({ ...control }),
claimExternalControlLease: (leaseId: string) => {
control.connected = true;
return { ...fixture.identity, leaseId };
},
sendRobotAction: vi.fn(),
stopExternalControl: vi.fn(),
reset: vi.fn(),
};
const client = new ExternalControlClient(() => backend);
// Exercise the request capability boundary independently of the socket handshake.
Object.assign(client, { statusValue: { phase: 'ready' } });
const request = (op: string, payload: unknown) =>
(client as unknown as { request(op: string, payload: unknown): Promise<unknown> }).request(
op,
payload,
);
return {
backend,
client,
control,
request,
claim: () =>
request('claim', {
...fixture.identity,
modelFingerprint: descriptor.modelFingerprint,
authorizationGeneration: 1,
}),
};
}
describe('ExternalControlClient scope boundaries', () => {
it('old lease cannot reset or release after a new authorization (even same model/epoch)', async () => {
const s = setup();
await s.claim();
s.control.authorizationGeneration++;
for (const op of ['reset', 'release'])
await expect(s.request(op, fixture.identity)).rejects.toMatchObject({ code: 'STALE' });
expect(s.backend.reset).not.toHaveBeenCalled();
expect(s.backend.stopExternalControl).not.toHaveBeenCalled();
});
it('old lease cannot reset the model after switching to a local controller', async () => {
const s = setup();
await s.claim();
s.control.enabled = false;
s.control.connected = false;
await expect(s.request('reset', fixture.identity)).rejects.toMatchObject({ code: 'STALE' });
expect(s.backend.reset).not.toHaveBeenCalled();
});
it('requires local URL without credentials/path/query and refuses unsupported operations', async () => {
const s = setup();
await s.claim();
await expect(s.request('execute', fixture.identity)).rejects.toMatchObject({
code: 'UNSUPPORTED',
});
for (const endpoint of [
'http://evil.test',
'https://127.0.0.1',
'http://localhost/?token=x',
'http://user:token@localhost',
'http://localhost/path',
])
await expect(s.client.connect(endpoint, 'not-a-real-token')).rejects.toMatchObject({
code: 'UNAUTHORIZED',
});
});
});
@@ -0,0 +1,231 @@
import fixture from '../../../contracts/fixtures/single-joint.json';
import { ExternalControlClient } from './ExternalControlClient';
import { validateDescriptor, validateObservation } from './validation';
class Socket {
static OPEN = 1;
static latest: Socket;
readyState = 1;
bufferedAmount = 0;
onopen?: () => void;
onerror?: () => void;
onclose?: () => void;
onmessage?: (event: { data: unknown }) => void;
sent: Record<string, unknown>[] = [];
constructor(readonly url: URL) {
Socket.latest = this;
}
send(raw: string) {
this.sent.push(JSON.parse(raw));
}
close() {
this.readyState = 3;
}
receive(message: unknown) {
this.onmessage?.({ data: JSON.stringify(message) });
}
}
const flush = () => vi.advanceTimersByTimeAsync(0);
function setup() {
vi.useFakeTimers();
vi.stubGlobal('WebSocket', Socket);
const descriptor = validateDescriptor(fixture.descriptor),
observation = validateObservation(fixture.observation, descriptor);
const control = {
enabled: true,
connected: false,
authorizationGeneration: 1,
modelEpoch: observation.modelEpoch,
sessionId: observation.sessionId,
lastActionSeq: 0,
};
const backend = {
describeRobot: () => descriptor,
robotObservation: () => observation,
externalControlStatus: () => control,
claimExternalControlLease: (leaseId: string) => {
control.connected = true;
return { ...fixture.identity, leaseId };
},
sendRobotAction: vi.fn().mockResolvedValue({
...fixture.identity,
actionSeq: 1,
simTime: 0.002,
values: fixture.validAction.values,
}),
stopExternalControl: vi.fn(),
reset: vi.fn(),
};
const client = new ExternalControlClient(() => backend);
const connect = async () => {
const pending = client.connect('http://127.0.0.1:8766', 'unit-token-for-testing');
const socket = Socket.latest;
socket.onopen?.();
socket.receive({ type: 'authenticated' });
socket.receive({ type: 'ready' });
await pending;
return socket;
};
const claim = (socket: Socket) =>
socket.receive({
type: 'request',
id: 'claim-1',
op: 'claim',
payload: {
...fixture.identity,
modelFingerprint: descriptor.modelFingerprint,
authorizationGeneration: control.authorizationGeneration,
},
});
return { client, backend, control, observation, connect, claim };
}
afterEach(() => {
vi.clearAllTimers();
vi.useRealTimers();
vi.unstubAllGlobals();
});
describe('ExternalControlClient transport', () => {
it('auth/register/ready, periodic measured state and valid action/reset/release RPC', async () => {
const s = setup(),
socket = await s.connect();
expect(socket.url.toString()).toBe('ws://127.0.0.1:8766/ws/control/v1');
expect(socket.sent[0]).toMatchObject({ type: 'auth', token: 'unit-token-for-testing' });
expect(socket.sent[1]).toMatchObject({ type: 'register', observation: fixture.observation });
expect(s.client.status().phase).toBe('ready');
await vi.advanceTimersByTimeAsync(34);
expect(socket.sent.at(-1)).toMatchObject({ type: 'state', observation: fixture.observation });
s.claim(socket);
await flush();
socket.receive({ type: 'request', id: 'action-1', op: 'action', payload: fixture.validAction });
await flush();
expect(s.backend.sendRobotAction).toHaveBeenCalledWith(fixture.validAction);
expect(socket.sent.at(-1)).toMatchObject({ id: 'action-1', ok: true });
socket.receive({ type: 'request', id: 'reset-1', op: 'reset', payload: fixture.identity });
await flush();
expect(s.backend.reset).toHaveBeenCalledOnce();
s.claim(socket);
await flush();
socket.receive({ type: 'request', id: 'release-1', op: 'release', payload: fixture.identity });
await flush();
expect(socket.sent.at(-1)).toMatchObject({
id: 'release-1',
ok: true,
value: { released: true },
});
s.client.disconnect();
expect(socket.readyState).toBe(3);
});
it('old generation stops are ignored; current stop invalidates the capability', async () => {
const s = setup(),
socket = await s.connect();
s.claim(socket);
await flush();
s.control.authorizationGeneration++;
socket.receive({
type: 'stop',
reason: 'old',
sessionId: s.observation.sessionId,
authorizationGeneration: 1,
});
await flush();
expect(s.backend.stopExternalControl).not.toHaveBeenCalled();
socket.receive({
type: 'stop',
reason: 'current',
sessionId: s.observation.sessionId,
authorizationGeneration: 2,
});
await flush();
expect(s.backend.stopExternalControl).toHaveBeenCalledWith('current');
socket.receive({
type: 'request',
id: 'old-action',
op: 'action',
payload: fixture.validAction,
});
await flush();
expect(socket.sent.at(-1)).toMatchObject({
id: 'old-action',
ok: false,
value: { code: 'STALE' },
});
s.client.disconnect();
});
it('delayed ACK never reaches a replacement socket', async () => {
const s = setup(),
old = await s.connect();
s.claim(old);
await flush();
let finish!: (value: unknown) => void;
s.backend.sendRobotAction.mockImplementationOnce(
() =>
new Promise((resolve) => {
finish = resolve;
}),
);
old.receive({ type: 'request', id: 'delayed', op: 'action', payload: fixture.validAction });
await flush();
s.client.disconnect();
const next = await s.connect();
finish({});
await flush();
expect(
[...old.sent, ...next.sent].filter((m) => m.type === 'result' && m.id === 'delayed'),
).toHaveLength(0);
s.client.disconnect();
});
it('rejects malformed/oversized frames and bounded send buffering', async () => {
for (const input of [
new Uint8Array(2),
'x'.repeat(65537),
'{bad',
JSON.stringify({ type: 'unknown' }),
]) {
const s = setup(),
socket = await s.connect();
socket.onmessage?.({ data: input });
await flush();
expect(s.client.status().phase).toBe('disconnected');
expect(s.backend.stopExternalControl).toHaveBeenCalled();
}
const s = setup(),
socket = await s.connect();
socket.bufferedAmount = 65536;
s.client.sync();
expect(s.client.status().error).toMatch(/缓冲区/);
});
it('handshake deadline, server errors, transport errors and invalid tokens fail closed', async () => {
const s = setup();
for (const token of ['', '短'.repeat(20), 'a'.repeat(4097)])
await expect(s.client.connect('http://localhost', token)).rejects.toMatchObject({
code: 'UNAUTHORIZED',
});
await expect(s.client.connect('not a url', 'unit-token-for-testing')).rejects.toMatchObject({
code: 'INVALID_MESSAGE',
});
const pending = s.client
.connect('http://localhost', 'unit-token-for-testing')
.catch((e: Error) => e.message);
await vi.advanceTimersByTimeAsync(5001);
expect(await pending).toMatch(/认证超时/);
let socket = await s.connect();
socket.receive({ type: 'error', error: { message: 'denied' } });
await flush();
expect(s.client.status().error).toBe('denied');
socket = await s.connect();
socket.onerror?.();
expect(s.client.status().phase).toBe('disconnected');
socket = await s.connect();
socket.onclose?.();
expect(s.client.status().phase).toBe('disconnected');
});
it('session replacement disconnects without revoking the replacement model', async () => {
const s = setup();
await s.connect();
s.observation.sessionId = 'replacement';
s.client.sync();
expect(s.client.status().phase).toBe('disconnected');
expect(s.backend.stopExternalControl).not.toHaveBeenCalled();
});
});
@@ -0,0 +1,262 @@
import type { PhysicsAdapter } from '../simulation/PhysicsAdapter';
import { RobotError, type RobotIdentity } from './types';
import { record, exactKeys, integer, identifier } from './validation';
export interface BridgeStatus {
phase: 'disconnected' | 'connecting' | 'ready';
error?: string;
}
type Backend = Pick<
PhysicsAdapter,
| 'describeRobot'
| 'robotObservation'
| 'externalControlStatus'
| 'claimExternalControlLease'
| 'sendRobotAction'
| 'stopExternalControl'
| 'reset'
>;
/** Bounded numeric transport only. Never owns physics or a render/UI snapshot loop. */
export class ExternalControlClient {
private socket?: WebSocket;
private timer?: ReturnType<typeof setInterval>;
private handshakeTimer?: ReturnType<typeof setTimeout>;
private sessionId?: string;
private lease?: RobotIdentity;
private leaseGeneration?: number;
private rejectConnect?: (error: Error) => void;
private statusValue: BridgeStatus = { phase: 'disconnected' };
constructor(
private readonly backend: () => Backend | undefined,
private readonly onStatus: (status: BridgeStatus) => void = () => {},
) {}
status(): BridgeStatus {
return { ...this.statusValue };
}
private statusChanged(value: BridgeStatus): void {
this.statusValue = value;
this.onStatus(value);
}
private state() {
const backend = this.backend(),
observation = backend?.robotObservation(),
control = backend?.externalControlStatus();
if (
!backend ||
!observation ||
!control ||
(this.sessionId && observation.sessionId !== this.sessionId)
)
throw new RobotError('DISCONNECTED', '模型会话已变化,请重新连接');
return {
type: 'state',
observation,
enabled: control.enabled,
authorizationGeneration: control.authorizationGeneration,
};
}
async connect(endpoint: string, token: string): Promise<void> {
this.disconnect('重新连接');
let url: URL;
try {
url = new URL(endpoint);
} catch {
return Promise.reject(new RobotError('INVALID_MESSAGE', '桥接地址无效'));
}
if (
url.protocol !== 'http:' ||
!['127.0.0.1', 'localhost'].includes(url.hostname) ||
url.username ||
url.password ||
url.search ||
url.hash ||
url.pathname !== '/'
)
return Promise.reject(
new RobotError('UNAUTHORIZED', 'V1 仅允许本机 http://127.0.0.1:port 或 localhost'),
);
if (!/^[\x21-\x7e]{16,4096}$/.test(token.trim()))
return Promise.reject(
new RobotError('UNAUTHORIZED', '控制 token 需16–4096位可打印 ASCII,不能含空白'),
);
const descriptor = this.backend()?.describeRobot(),
initial = this.state();
if (!descriptor) return Promise.reject(new RobotError('UNSUPPORTED', '请先应用机器人 profile'));
this.sessionId = initial.observation.sessionId;
url.protocol = 'ws:';
url.pathname = '/ws/control/v1';
const socket = new WebSocket(url);
this.socket = socket;
this.statusChanged({ phase: 'connecting' });
return new Promise<void>((resolve, reject) => {
this.rejectConnect = reject;
this.handshakeTimer = setTimeout(() => this.disconnect('桥接认证超时'), 5000);
socket.onopen = () => {
if (this.socket === socket)
this.send({ type: 'auth', protocolVersion: 1, token: token.trim() });
};
socket.onerror = () => {
if (this.socket === socket) this.disconnect('无法连接本机控制桥接');
};
socket.onclose = () => {
if (this.socket === socket) this.disconnect('桥接已断开,请重新授权');
};
socket.onmessage = (event) => {
if (this.socket !== socket) return;
void this.message(event.data, socket, descriptor, resolve).catch((error: unknown) => {
if (this.socket === socket)
this.disconnect(error instanceof Error ? error.message : '桥接协议错误');
});
};
});
}
private send(message: unknown): void {
const json = JSON.stringify(message),
socket = this.socket;
if (
!socket ||
socket.readyState !== WebSocket.OPEN ||
socket.bufferedAmount + new TextEncoder().encode(json).length > 65536
)
throw new RobotError('DISCONNECTED', '桥接发送缓冲区已满或已关闭');
socket.send(json);
}
private async message(
raw: unknown,
socket: WebSocket,
descriptor: unknown,
ready: () => void,
): Promise<void> {
if (typeof raw !== 'string' || new TextEncoder().encode(raw).length > 65536)
throw new RobotError('INVALID_MESSAGE', '无效桥接帧');
const message = record(JSON.parse(raw));
if (message.type === 'authenticated') {
exactKeys(message, ['type']);
this.send({ ...this.state(), type: 'register', descriptor });
} else if (message.type === 'ready') {
exactKeys(message, ['type']);
clearTimeout(this.handshakeTimer);
this.rejectConnect = undefined;
this.statusChanged({ phase: 'ready' });
if (this.timer) clearInterval(this.timer);
this.timer = setInterval(() => this.sync(), 1000 / 30);
ready();
} else if (message.type === 'error') {
const error = record(message.error);
throw new RobotError(
'DISCONNECTED',
typeof error.message === 'string' ? error.message : '桥接错误',
);
} else if (message.type === 'stop') {
exactKeys(message, ['type', 'reason', 'sessionId', 'authorizationGeneration']);
const current = this.state();
if (
message.sessionId === current.observation.sessionId &&
message.authorizationGeneration === current.authorizationGeneration
) {
this.backend()?.stopExternalControl(
typeof message.reason === 'string' ? message.reason : '服务端撤销控制',
);
this.lease = undefined;
this.sync();
}
} else if (message.type === 'request') {
exactKeys(message, ['type', 'id', 'op', 'payload']);
const id = identifier(message.id);
let value: unknown,
ok = true;
try {
value = await this.request(message.op, message.payload);
} catch (error: unknown) {
ok = false;
value = {
code: error instanceof RobotError ? error.code : 'DISCONNECTED',
message: error instanceof Error ? error.message.slice(0, 1024) : '控制请求失败',
};
}
// A delayed physics acknowledgement must never land on a replacement socket.
if (this.socket === socket) this.send({ type: 'result', id, ok, value });
} else throw new RobotError('INVALID_MESSAGE', '未知桥接消息类型');
}
private async request(op: unknown, payload: unknown): Promise<unknown> {
if (this.statusValue.phase !== 'ready') throw new RobotError('UNAUTHORIZED', '桥接握手未完成');
const current = this.state(),
backend = this.backend()!;
const data = record(payload);
if (op === 'claim') {
exactKeys(data, [
'sessionId',
'modelEpoch',
'leaseId',
'modelFingerprint',
'authorizationGeneration',
]);
identifier(data.sessionId);
identifier(data.leaseId);
integer(data.modelEpoch, 'modelEpoch');
if (
data.sessionId !== current.observation.sessionId ||
data.modelEpoch !== current.observation.modelEpoch ||
data.authorizationGeneration !== current.authorizationGeneration ||
data.modelFingerprint !== backend.describeRobot()?.modelFingerprint
)
throw new RobotError('STALE', '模型或授权代次已变化');
this.lease = backend.claimExternalControlLease(data.leaseId as string);
this.leaseGeneration = current.authorizationGeneration;
return this.lease;
}
if (
!this.lease ||
!current.enabled ||
!backend.externalControlStatus()?.connected ||
current.authorizationGeneration !== this.leaseGeneration ||
current.observation.modelEpoch !== this.lease.modelEpoch ||
data.leaseId !== this.lease.leaseId ||
data.sessionId !== this.lease.sessionId ||
data.modelEpoch !== this.lease.modelEpoch
)
throw new RobotError('STALE', '控制租约失效');
if (op === 'action') return backend.sendRobotAction(data);
exactKeys(data, ['sessionId', 'modelEpoch', 'leaseId']);
if (op === 'release') {
backend.stopExternalControl('Python 控制者已断开');
this.lease = undefined;
return { released: true };
}
if (op === 'reset') {
backend.reset();
this.lease = undefined;
return backend.robotObservation();
}
throw new RobotError('UNSUPPORTED', '不支持的机器人操作');
}
/** Also called after explicit UI authorization; never resends a stale observation as fresh. */
sync(): void {
if (this.statusValue.phase !== 'ready') return;
try {
this.send(this.state());
} catch (error: unknown) {
this.disconnect(error instanceof Error ? error.message : '桥接状态异常');
}
}
disconnect(reason = '桥接连接已关闭'): void {
clearInterval(this.timer);
clearTimeout(this.handshakeTimer);
this.timer = undefined;
const socket = this.socket;
this.socket = undefined;
this.rejectConnect?.(new RobotError('DISCONNECTED', reason));
this.rejectConnect = undefined;
socket?.close();
try {
if (this.sessionId && this.backend()?.robotObservation()?.sessionId === this.sessionId)
this.backend()?.stopExternalControl(reason);
} catch {
/* Session teardown already revoked controls before deleting WASM handles. */
}
this.sessionId = undefined;
this.lease = undefined;
this.statusChanged({ phase: 'disconnected', error: reason });
}
}
@@ -0,0 +1,171 @@
import { useState } from 'react';
import { Button, Select } from '../components/ui';
import type { SimulationSnapshot } from '../simulation/SimulationSession';
import type { BridgeStatus } from './ExternalControlClient';
import { ROBOT_PROFILES } from './registry';
export interface ExternalControlUI {
status: BridgeStatus;
endpoint: string;
setEndpoint: (value: string) => void;
token: string;
setToken: (value: string) => void;
observationAgeMs?: number;
connect: () => Promise<void>;
disconnect: (reason?: string) => void;
authorize: () => void;
stop: () => void;
}
export function ExternalControlPanel({
snapshot,
loading,
bridge,
onApplyProfile,
}: {
snapshot: SimulationSnapshot;
loading: boolean;
bridge: ExternalControlUI;
onApplyProfile: (id?: string) => void;
}) {
const [profileId, setProfileId] = useState(snapshot.robot?.profileId ?? '');
const external = snapshot.externalControl,
ready = bridge.status.phase === 'ready';
const owner = { manual: '手动', python: 'Python', policy: 'ONNX', external: '外部' }[
snapshot.controlOwner ?? 'manual'
];
return (
<section aria-label="开源机器人控制" className="space-y-3 text-xs">
<label className="block">
机器人 profile
<Select
aria-label="控制接口 profile"
className="mt-1 w-full"
value={profileId}
disabled={loading}
onChange={(e) => setProfileId(e.target.value)}
>
<option value="">不启用(普通模型)</option>
{ROBOT_PROFILES.map((p) => (
<option key={p.id} value={p.id}>
{p.label}
</option>
))}
</Select>
</label>
<Button disabled={loading} onClick={() => onApplyProfile(profileId || undefined)}>
校验并重新编译 profile
</Button>
<p className="text-text-secondary">
仅显式选择后生效;未知模型/版本拒绝接管。LeKiwi 使用简化动力学,不是实机标定。
</p>
<p>
当前 profile:<strong>{snapshot.robot?.profileId ?? '未启用'}</strong>
</p>
{snapshot.robot && (
<p>
模型指纹:
<code title={snapshot.robot.modelFingerprint}>
{snapshot.robot.modelFingerprint.slice(0, 16)}
</code>
</p>
)}
<label className="block">
本机桥接地址
<input
aria-label="本机桥接地址"
className="field mt-1 w-full px-2 py-1"
value={bridge.endpoint}
disabled={bridge.status.phase !== 'disconnected'}
onChange={(e) => bridge.setEndpoint(e.target.value)}
/>
</label>
<label className="block">
控制 token(仅当前页面内存)
<input
aria-label="控制 token"
type="password"
autoComplete="off"
className="field mt-1 w-full px-2 py-1"
value={bridge.token}
disabled={bridge.status.phase !== 'disconnected'}
onChange={(e) => bridge.setToken(e.target.value)}
/>
</label>
<div className="flex flex-wrap gap-2">
<Button
disabled={
loading ||
!snapshot.robot ||
bridge.status.phase !== 'disconnected' ||
!bridge.token.trim()
}
onClick={() => void bridge.connect()}
>
{bridge.status.phase === 'connecting' ? '连接中…' : '连接桥接'}
</Button>
<Button
disabled={bridge.status.phase === 'disconnected'}
onClick={() => bridge.disconnect()}
>
断开桥接
</Button>
</div>
<div className="flex flex-wrap gap-2">
<Button
variant="primary"
disabled={loading || !ready || snapshot.paused || Boolean(external?.enabled)}
onClick={bridge.authorize}
>
允许外部控制
</Button>
<Button disabled={!external?.enabled} onClick={bridge.stop}>
停止外部控制
</Button>
</div>
<p>
连接:
<span aria-label="桥接连接状态">
{ready ? '已连接' : bridge.status.phase === 'connecting' ? '连接中' : '未连接'}
</span>
</p>
<p>
当前控制所有者:<strong aria-label="当前控制所有者">{owner}</strong>
</p>
<p aria-label="外部控制状态">
{external?.connected
? '外部控制者已连接'
: external?.enabled
? '等待 Python 控制者'
: '未授权'}
</p>
<p aria-label="机器人观测状态">
{snapshot.paused
? '已暂停(非实时观测)'
: bridge.observationAgeMs === undefined
? '尚无观测'
: `观测年龄 ${bridge.observationAgeMs.toFixed(0)} ms${bridge.observationAgeMs > 500 ? ' · 已过期' : ''}`}
</p>
{external && (
<p className="technical-value">
epoch {external.modelEpoch} · applied seq {external.lastActionSeq}
</p>
)}
{(bridge.status.error || (external?.error && !external.enabled)) && (
<p role="status" className="text-warning">
{bridge.status.error || external?.error}
</p>
)}
<p className="text-text-tertiary">
先播放再授权。接管会停止 Python/ONNX,并锁定
1×。暂停、重载、页面隐藏和500ms超时均撤销授权;不会自动恢复旧命令。
</p>
<p className="text-text-tertiary">
物理/观测不依赖渲染帧率。外控视图上限30FPS;检测到软件渲染时关闭阴影并降至5FPS,为控制保留时间。
</p>
<p className="text-text-tertiary">
V1:底盘/臂/夹爪和状态回读;不支持相机、锁步、LeRobot 数据采集或训练。
</p>
</section>
);
}
+118
View File
@@ -0,0 +1,118 @@
import fixture from '../../../contracts/fixtures/single-joint.json';
import { RobotRuntime } from './RobotRuntime';
import type { RobotAdapter } from './types';
import { validateDescriptor, validateValues } from './validation';
function setup() {
const descriptor = validateDescriptor(fixture.descriptor);
let clock = 0,
paused = false,
measured = 0.12;
const adapter: RobotAdapter = {
describe: () => descriptor,
readObservation: () => ({ 'slider.position': measured }),
validateAction: (v) => validateValues(v, descriptor.actionChannels, true),
applyAction: vi.fn((v) => validateValues(v, descriptor.actionChannels, true)),
safeStop: vi.fn(),
reset: () => {
measured = 0;
},
dispose: vi.fn(),
};
const fault = vi.fn(() => {
paused = true;
});
const runtime = new RobotRuntime(
adapter,
'sim-test',
() => paused,
fault,
() => clock,
);
runtime.authorize();
const identity = runtime.claim('lease-test');
const action = (seq = 1) => ({ ...fixture.validAction, ...identity, actionSeq: seq });
return {
runtime,
adapter,
fault,
action,
tick: (ms: number) => {
clock += ms;
},
pause: () => {
paused = true;
},
};
}
describe('RobotRuntime bounded mailbox', () => {
it('applies only at a step, confirms after the step, observations are not targets', async () => {
const s = setup();
let confirmed = false;
const ack = s.runtime.enqueue(s.action()).then((r) => {
confirmed = true;
return r;
});
expect(s.adapter.applyAction).not.toHaveBeenCalled();
expect(s.runtime.beforeStep()).toBe(true);
expect(s.adapter.applyAction).toHaveBeenCalledOnce();
expect(confirmed).toBe(false);
s.runtime.afterStep(0.002);
expect((await ack).values['slider.position']).toBe(0.3);
expect(s.runtime.latest()?.values['slider.position']).toBe(0.12);
expect(s.runtime.latest()?.appliedActionSeq).toBe(1);
});
it('only newest pending target survives and old sequences cannot replay', async () => {
const s = setup();
const old = s.runtime.enqueue(s.action()).catch((e) => e.code);
const next = s.runtime.enqueue(s.action(2));
expect(await old).toBe('SUPERSEDED');
await expect(s.runtime.enqueue(s.action())).rejects.toMatchObject({ code: 'STALE' });
s.runtime.beforeStep();
s.runtime.afterStep(0.002);
expect((await next).actionSeq).toBe(2);
});
it('wall clock watchdog cancels stale actions before the first resumed step', async () => {
const s = setup();
const ack = s.runtime.enqueue(s.action()).catch((e) => e.code);
s.tick(501);
expect(s.runtime.beforeStep()).toBe(false);
expect(await ack).toBe('DISCONNECTED');
expect(s.adapter.applyAction).not.toHaveBeenCalled();
expect(s.fault).toHaveBeenCalledOnce();
expect(s.runtime.status().enabled).toBe(false);
await expect(s.runtime.enqueue(s.action(2))).rejects.toMatchObject({ code: 'UNAUTHORIZED' });
});
it('a socket event delivered after freeze cannot renew an expired lease', async () => {
const s = setup();
s.runtime.capture(0);
s.tick(501);
expect(s.runtime.status().observationAgeMs).toBe(501);
await expect(s.runtime.enqueue(s.action())).rejects.toMatchObject({ code: 'TIMEOUT' });
expect(s.adapter.applyAction).not.toHaveBeenCalled();
expect(s.fault).toHaveBeenCalledOnce();
expect(s.runtime.status().enabled).toBe(false);
});
it('reset/dispose reject outstanding requests and invalidate epoch', async () => {
const s = setup();
const pending = s.runtime.enqueue(s.action()).catch((e) => e.code);
s.runtime.reset();
expect(await pending).toBe('DISCONNECTED');
expect(s.runtime.status().modelEpoch).toBe(1);
s.runtime.authorize();
s.runtime.claim('new-lease');
await expect(s.runtime.enqueue(s.action(2))).rejects.toMatchObject({ code: 'STALE' });
s.runtime.dispose();
s.runtime.dispose();
expect(s.adapter.dispose).toHaveBeenCalledOnce();
});
it('paused/multiple owners denied and nonowner revocation does not write', async () => {
const s = setup();
expect(() => s.runtime.claim('other')).toThrow(/已有/);
s.pause();
await expect(s.runtime.enqueue(s.action())).rejects.toMatchObject({ code: 'PAUSED' });
vi.mocked(s.adapter.safeStop).mockClear();
s.runtime.revoke('not owner', false);
expect(s.adapter.safeStop).not.toHaveBeenCalled();
});
});
+191
View File
@@ -0,0 +1,191 @@
import {
RobotError,
type RobotAdapter,
type RobotAction,
type RobotActionResult,
type RobotIdentity,
type RobotObservation,
} from './types';
import { identifier, validateAction, validateDescriptor } from './validation';
export interface ExternalControlStatus {
enabled: boolean;
connected: boolean;
authorizationGeneration: number;
modelEpoch: number;
sessionId: string;
lastActionSeq: number;
observationAgeMs?: number;
error?: string;
}
interface Pending {
action: RobotAction;
receivedAt: number;
resolve(result: RobotActionResult): void;
reject(error: RobotError): void;
}
/** A bounded mailbox; all adapter mutation happens on the simulation thread. */
export class RobotRuntime {
private enabled = false;
private authorizationGeneration = 0;
private disposed = false;
private epoch = 0;
private lease?: string;
private sequence = 0;
private lastSeq = 0;
private appliedSeq = 0;
private lastActionAt = 0;
private pending?: Pending;
private applied?: { pending: Pending; values: Record<string, number> };
private nextObservationTime = 0;
private observation?: RobotObservation;
private observedAt?: number;
private error?: string;
readonly descriptor;
constructor(
private readonly adapter: RobotAdapter,
readonly sessionId: string,
private readonly isPaused: () => boolean,
private readonly onFault: (reason: string) => void,
private readonly now: () => number = () => performance.now(),
private readonly watchdogMs = 500,
) {
identifier(sessionId);
this.descriptor = validateDescriptor(adapter.describe());
}
status(): ExternalControlStatus {
return {
enabled: this.enabled,
connected: Boolean(this.lease),
authorizationGeneration: this.authorizationGeneration,
modelEpoch: this.epoch,
sessionId: this.sessionId,
lastActionSeq: this.appliedSeq,
observationAgeMs: this.observedAt === undefined ? undefined : this.now() - this.observedAt,
error: this.error,
};
}
authorize(): void {
if (this.disposed) throw new RobotError('DISCONNECTED', '机器人已释放');
this.revoke('重新授权');
this.enabled = true;
this.authorizationGeneration++;
this.error = undefined;
}
claim(leaseId: string): RobotIdentity {
if (!this.enabled || this.disposed)
throw new RobotError('UNAUTHORIZED', '请先在浏览器允许外部控制');
if (this.isPaused()) throw new RobotError('PAUSED', '请先播放仿真');
if (this.lease) throw new RobotError('CONFLICT', '已有外部控制者');
this.lease = identifier(leaseId);
this.lastSeq = this.appliedSeq = 0;
this.lastActionAt = this.now();
return this.identity();
}
private identity(): RobotIdentity {
return { sessionId: this.sessionId, modelEpoch: this.epoch, leaseId: this.lease ?? 'none' };
}
enqueue(value: unknown): Promise<RobotActionResult> {
if (!this.enabled || !this.lease)
return Promise.reject(new RobotError('UNAUTHORIZED', '没有外部控制授权'));
if (this.isPaused()) return Promise.reject(new RobotError('PAUSED', '仿真已暂停'));
let action: RobotAction;
try {
action = validateAction(value, this.descriptor, this.identity(), this.lastSeq);
} catch (error) {
return Promise.reject(error);
}
// A socket event buffered while the page was frozen must not renew an
// already-expired lease before the first resumed physical step.
if (this.now() - this.lastActionAt >= this.watchdogMs) {
this.revoke('外部动作超过 500 ms 未刷新');
this.onFault('外部动作超时');
return Promise.reject(new RobotError('TIMEOUT', '租约已超时,请重新授权'));
}
this.lastSeq = action.actionSeq;
this.lastActionAt = this.now();
this.pending?.reject(new RobotError('SUPERSEDED', '已由更新的目标替代'));
return new Promise((resolve, reject) => {
this.pending = { action, receivedAt: this.now(), resolve, reject };
});
}
/** Called before ANY physical step, including the first frame after a browser freeze. */
beforeStep(): boolean {
if (!this.enabled || !this.lease) return true;
if (this.now() - this.lastActionAt >= this.watchdogMs) {
this.revoke('外部动作超过 500 ms 未刷新');
this.onFault('外部动作超时');
return false;
}
if (this.pending) {
const pending = this.pending;
this.pending = undefined;
if (this.now() - pending.receivedAt >= this.watchdogMs) {
pending.reject(new RobotError('STALE', '排队动作已过期'));
return false;
}
try {
this.applied = { pending, values: this.adapter.applyAction(pending.action.values) };
} catch (error) {
pending.reject(
error instanceof RobotError ? error : new RobotError('INVALID_MESSAGE', String(error)),
);
this.revoke('应用动作失败');
this.onFault('应用动作失败');
return false;
}
}
return true;
}
afterStep(time: number): void {
if (this.applied) {
const { pending, values } = this.applied;
this.applied = undefined;
this.appliedSeq = pending.action.actionSeq;
pending.resolve({ ...this.identity(), actionSeq: this.appliedSeq, simTime: time, values });
}
if (!this.observation || time + 1e-9 >= this.nextObservationTime) this.capture(time);
}
capture(time: number): RobotObservation {
this.observedAt = this.now();
this.observation = {
protocolVersion: 1,
sessionId: this.sessionId,
modelEpoch: this.epoch,
sequence: ++this.sequence,
simTime: time,
appliedActionSeq: this.appliedSeq,
paused: this.isPaused(),
values: this.adapter.readObservation(),
};
this.nextObservationTime = time + 1 / 30;
return structuredClone(this.observation);
}
latest(): RobotObservation | undefined {
return this.observation ? structuredClone(this.observation) : undefined;
}
revoke(reason = '外部控制已停止', stop = true): void {
this.enabled = false;
this.lease = undefined;
this.error = reason;
this.pending?.reject(new RobotError('DISCONNECTED', reason));
this.pending = undefined;
this.applied?.pending.reject(new RobotError('DISCONNECTED', reason));
this.applied = undefined;
if (stop) this.adapter.safeStop();
}
reset(): void {
this.revoke('模型已重置,请重新授权');
this.epoch++;
this.sequence = this.lastSeq = this.appliedSeq = 0;
this.nextObservationTime = 0;
this.adapter.reset();
}
dispose(): void {
if (!this.disposed) {
this.revoke('模型已释放');
this.adapter.dispose();
this.disposed = true;
}
}
}
@@ -0,0 +1,13 @@
import fixture from '../../../../contracts/fixtures/lekiwi-kinematics.json';
import { bodyToWheels, LEKIWI_PROFILE } from './lekiwi';
it('matches the real LeRobot 0.6.1 forward kinematics and proportional saturation within half an encoder tick', () => {
expect(LEKIWI_PROFILE.wheels).toEqual(fixture.wheelOrder);
const radPerTick = (2 * Math.PI) / 4096;
for (const { body, wheelRaw } of fixture.cases) {
const wheel = bodyToWheels(body[0], body[1], (body[2] * Math.PI) / 180);
wheel.forEach((value, i) =>
expect(Math.abs(value / radPerTick - wheelRaw[i])).toBeLessThanOrEqual(0.500001),
);
}
});
+74
View File
@@ -0,0 +1,74 @@
import profile from '../../../../robot_profiles/lekiwi-v1.json';
import type { RobotChannel, RobotDescriptor } from '../types';
export const LEKIWI_PROFILE = profile;
export const LEKIWI_ACTION_CHANNELS: RobotChannel[] = [
...profile.arm.map((j): RobotChannel => ({
id: `${j.joint}.pos`,
unit: j.joint === 'arm_gripper' ? 'ratio' : 'rad',
mode: j.joint === 'arm_gripper' ? 'opening' : 'position',
min: j.joint === 'arm_gripper' ? 0 : j.min,
max: j.joint === 'arm_gripper' ? 1 : j.max,
})),
...['x.vel', 'y.vel', 'theta.vel'].map((id): RobotChannel => ({
id,
mode: 'velocity',
unit: id === 'theta.vel' ? 'rad/s' : 'm/s',
min: -(id === 'theta.vel' ? profile.baseAngularLimit : profile.baseLinearLimit),
max: id === 'theta.vel' ? profile.baseAngularLimit : profile.baseLinearLimit,
})),
];
// LeRobot 0.6.1: left/back/right, angles [240,0,120]-90 = [150,-90,30].
export const WHEEL_TANGENTS: readonly (readonly number[])[] = [
[-Math.sqrt(3) / 2, 0.5],
[0, -1],
[Math.sqrt(3) / 2, 0.5],
];
export function bodyToWheels(x: number, y: number, yaw: number): number[] {
const raw = WHEEL_TANGENTS.map(
([tx, ty]) => (tx * x + ty * y + profile.baseRadius * yaw) / profile.wheelRadius,
);
const scale = Math.min(1, profile.wheelMaxRadS / Math.max(...raw.map(Math.abs), 1e-12));
return raw.map((value) => value * scale);
}
export function wheelsToBody(wheels: readonly number[]): [number, number, number] {
const [l, b, r] = wheels.map((v) => v * profile.wheelRadius);
return [(r - l) / Math.sqrt(3), (l + r - 2 * b) / 3, (l + b + r) / (3 * profile.baseRadius)];
}
export function lekiwiDescriptor(modelFingerprint: string): RobotDescriptor {
return {
protocolVersion: 1,
profileId: profile.id,
profileVersion: profile.version,
modelFingerprint,
frame: 'x-forward-y-left-z-up',
actionChannels: LEKIWI_ACTION_CHANNELS.map((c) => ({ ...c })),
observationChannels: [
...LEKIWI_ACTION_CHANNELS.map((c) => ({ ...c })),
...[
'base.x',
'base.y',
'base.z',
'base.roll',
'base.pitch',
'base.yaw',
'base.vx',
'base.vy',
'base.wz',
].map((id): RobotChannel => ({
id,
min: -1e6,
max: 1e6,
mode: id.startsWith('base.v') || id === 'base.wz' ? 'velocity' : 'position',
unit:
id === 'base.wz'
? 'rad/s'
: id.startsWith('base.v')
? 'm/s'
: ['base.roll', 'base.pitch', 'base.yaw'].includes(id)
? 'rad'
: 'm',
})),
],
capabilities: { reset: true, lockstep: false, cameras: false, training: false },
};
}
+25
View File
@@ -0,0 +1,25 @@
import { RobotError, type RobotDescriptor } from './types';
import { lekiwiDescriptor, LEKIWI_PROFILE } from './profiles/lekiwi';
import { validateDescriptor } from './validation';
export const ROBOT_PROFILES = [{ id: LEKIWI_PROFILE.id, label: 'LeKiwi v1(仿真专用)' }] as const;
/** Explicit built-in registry. Project JSON cannot load code or register factories. */
export function describeProfile(id: string, modelFingerprint: string): RobotDescriptor {
if (id !== LEKIWI_PROFILE.id)
throw new RobotError('UNSUPPORTED', `不支持的机器人 profile:${id}`);
return validateDescriptor(lekiwiDescriptor(modelFingerprint));
}
export async function sha256(data: Uint8Array): Promise<string> {
const digest = await crypto.subtle.digest('SHA-256', new Uint8Array(data));
return Array.from(new Uint8Array(digest), (value) => value.toString(16).padStart(2, '0')).join(
'',
);
}
export async function validateLeKiwiSource(data: Uint8Array): Promise<void> {
if ((await sha256(data)) !== LEKIWI_PROFILE.source.urdfSha256)
throw new RobotError(
'INCOMPATIBLE_MODEL',
`LeKiwi URDF 与受支持的 ${LEKIWI_PROFILE.source.revision.slice(0, 8)} 版本不符;请使用原始 URDF,不能仅凭关节数量套用 profile`,
);
}
+76
View File
@@ -0,0 +1,76 @@
/** Wire contract v1. Core types have no React, socket or MuJoCo dependencies. */
export const ROBOT_PROTOCOL_VERSION = 1 as const;
export type RobotUnit = 'rad' | 'rad/s' | 'm' | 'm/s' | 'ratio' | 'N' | 'N.m';
export type RobotControlMode = 'position' | 'velocity' | 'effort' | 'opening';
export interface RobotChannel {
id: string;
unit: RobotUnit;
min: number;
max: number;
mode: RobotControlMode;
}
export interface RobotDescriptor {
protocolVersion: 1;
profileId: string;
profileVersion: number;
modelFingerprint: string;
frame: 'x-forward-y-left-z-up';
actionChannels: RobotChannel[];
observationChannels: RobotChannel[];
capabilities: { reset: boolean; lockstep: false; cameras: false; training: false };
}
export interface RobotIdentity {
sessionId: string;
modelEpoch: number;
leaseId: string;
}
export interface RobotAction extends RobotIdentity {
protocolVersion: 1;
actionSeq: number;
values: Record<string, number>;
}
export interface RobotObservation {
protocolVersion: 1;
sessionId: string;
modelEpoch: number;
sequence: number;
simTime: number;
appliedActionSeq: number;
paused: boolean;
values: Record<string, number>;
}
export interface RobotActionResult extends RobotIdentity {
actionSeq: number;
simTime: number;
values: Record<string, number>;
}
export type RobotErrorCode =
| 'INVALID_MESSAGE'
| 'INCOMPATIBLE_MODEL'
| 'UNSUPPORTED'
| 'UNAUTHORIZED'
| 'CONFLICT'
| 'STALE'
| 'PAUSED'
| 'TIMEOUT'
| 'SUPERSEDED'
| 'DISCONNECTED';
export class RobotError extends Error {
constructor(
readonly code: RobotErrorCode,
message: string,
) {
super(message);
this.name = 'RobotError';
}
}
export interface RobotAdapter {
describe(): RobotDescriptor;
readObservation(): Record<string, number>;
validateAction(values: unknown): Record<string, number>;
/** Returns accepted targets, NOT a claim that the robot has reached them. */
applyAction(values: Record<string, number>): Record<string, number>;
safeStop(): void;
reset(): void;
dispose(): void;
}
+106
View File
@@ -0,0 +1,106 @@
import fixture from '../../../contracts/fixtures/single-joint.json';
import { validateAction, validateDescriptor, validateObservation } from './validation';
import { RobotError } from './types';
import { bodyToWheels, wheelsToBody, lekiwiDescriptor, LEKIWI_PROFILE } from './profiles/lekiwi';
import { describeProfile } from './registry';
describe('robot v1 / shared fixtures', () => {
const descriptor = validateDescriptor(fixture.descriptor);
it('works for a non-LeKiwi single joint and roundtrips measured state', () => {
expect(validateAction(fixture.validAction, descriptor, fixture.identity, 0)).toEqual(
fixture.validAction,
);
expect(
validateObservation(JSON.parse(JSON.stringify(fixture.observation)), descriptor),
).toEqual(fixture.observation);
expect(fixture.observation.values['slider.position']).not.toBe(
fixture.validAction.values['slider.position'],
);
});
for (const test of fixture.invalidActions)
it(test.label, () => {
try {
validateAction({ ...fixture.validAction, ...test.patch }, descriptor, fixture.identity, 0);
expect.fail('expected rejection');
} catch (error) {
expect(error).toBeInstanceOf(RobotError);
expect((error as RobotError).code).toBe(test.code);
}
});
it('clamps targets and rejects late/NaN/Infinity values', () => {
expect(
validateAction(
{ ...fixture.validAction, values: { 'slider.position': 3 } },
descriptor,
fixture.identity,
0,
).values['slider.position'],
).toBe(1);
expect(() => validateAction(fixture.validAction, descriptor, fixture.identity, 1)).toThrow(
/乱序/,
);
for (const value of [NaN, Infinity, -Infinity])
expect(() =>
validateAction(
{ ...fixture.validAction, values: { 'slider.position': value } },
descriptor,
fixture.identity,
0,
),
).toThrow();
});
it('rejects unsupported capabilities, duplicate ids and invalid ranges', () => {
expect(() =>
validateDescriptor({
...fixture.descriptor,
capabilities: { ...fixture.descriptor.capabilities, cameras: true },
}),
).toThrow();
expect(() =>
validateDescriptor({
...fixture.descriptor,
actionChannels: [...descriptor.actionChannels, ...descriptor.actionChannels],
}),
).toThrow();
expect(() =>
validateDescriptor({
...fixture.descriptor,
actionChannels: [{ ...descriptor.actionChannels[0], min: 2 }],
}),
).toThrow();
expect(() => describeProfile('arbitrary-code', fixture.descriptor.modelFingerprint)).toThrow();
});
});
describe('LeKiwi kinematic contract', () => {
it('has five radian arm channels, normalized gripper and three SI velocities', () => {
const d = validateDescriptor(lekiwiDescriptor(fixture.descriptor.modelFingerprint));
expect(d.actionChannels).toHaveLength(9);
expect(d.actionChannels.map((c) => c.unit)).toEqual([
'rad',
'rad',
'rad',
'rad',
'rad',
'ratio',
'm/s',
'm/s',
'rad/s',
]);
});
it.each([
[0.1, 0, 0],
[0, 0.1, 0],
[0, 0, Math.PI / 6],
[0.08, -0.03, 0.2],
])('roundtrip %s %s %s', (x, y, w) => {
const actual = wheelsToBody(bodyToWheels(x, y, w));
actual.forEach((v, i) => expect(v).toBeCloseTo([x, y, w][i], 10));
});
it('proportionally saturates wheels instead of changing direction', () => {
const wheels = bodyToWheels(1, 0.2, 3);
expect(Math.max(...wheels.map(Math.abs))).toBeCloseTo(LEKIWI_PROFILE.wheelMaxRadS);
const [x, y, w] = wheelsToBody(wheels);
expect(y / x).toBeCloseTo(0.2);
expect(w / x).toBeCloseTo(3);
});
});
+167
View File
@@ -0,0 +1,167 @@
import {
RobotError,
type RobotAction,
type RobotChannel,
type RobotDescriptor,
type RobotIdentity,
type RobotObservation,
} from './types';
export function record(value: unknown, label = '消息'): Record<string, unknown> {
if (!value || typeof value !== 'object' || Array.isArray(value))
throw new RobotError('INVALID_MESSAGE', `${label}必须是对象`);
return value as Record<string, unknown>;
}
export function exactKeys(value: Record<string, unknown>, keys: readonly string[]): void {
if (Object.keys(value).length !== keys.length || keys.some((key) => !Object.hasOwn(value, key)))
throw new RobotError('INVALID_MESSAGE', '字段缺失或包含未知字段');
}
export function finite(value: unknown, label: string): number {
if (typeof value !== 'number' || !Number.isFinite(value))
throw new RobotError('INVALID_MESSAGE', `${label}必须是有限数`);
return value;
}
export function integer(value: unknown, label: string, minimum = 0): number {
const number = finite(value, label);
if (!Number.isSafeInteger(number) || number < minimum)
throw new RobotError('INVALID_MESSAGE', `${label}必须是安全整数且≥${minimum}`);
return number;
}
export function identifier(value: unknown): string {
if (typeof value !== 'string' || !/^[a-zA-Z0-9][a-zA-Z0-9_.:-]{0,127}$/.test(value))
throw new RobotError('INVALID_MESSAGE', '标识符无效');
return value;
}
function channels(value: unknown): RobotChannel[] {
if (!Array.isArray(value) || !value.length || value.length > 256)
throw new RobotError('INVALID_MESSAGE', '通道数量必须为1–256');
const seen = new Set<string>();
return value.map((item) => {
const c = record(item);
exactKeys(c, ['id', 'unit', 'min', 'max', 'mode']);
const id = identifier(c.id);
if (seen.has(id)) throw new RobotError('INVALID_MESSAGE', '通道名称重复');
seen.add(id);
if (
!['rad', 'rad/s', 'm', 'm/s', 'ratio', 'N', 'N.m'].includes(String(c.unit)) ||
!['position', 'velocity', 'effort', 'opening'].includes(String(c.mode))
)
throw new RobotError('INVALID_MESSAGE', '通道单位/模式不受支持');
const min = finite(c.min, 'min'),
max = finite(c.max, 'max');
if (min >= max) throw new RobotError('INVALID_MESSAGE', '通道范围必须递增');
return {
id,
unit: c.unit as RobotChannel['unit'],
mode: c.mode as RobotChannel['mode'],
min,
max,
};
});
}
export function validateDescriptor(value: unknown): RobotDescriptor {
const d = record(value);
exactKeys(d, [
'protocolVersion',
'profileId',
'profileVersion',
'modelFingerprint',
'frame',
'actionChannels',
'observationChannels',
'capabilities',
]);
if (d.protocolVersion !== 1 || d.frame !== 'x-forward-y-left-z-up')
throw new RobotError('UNSUPPORTED', '机器人协议版本或坐标系不支持');
if (typeof d.modelFingerprint !== 'string' || !/^[a-f0-9]{64}$/.test(d.modelFingerprint))
throw new RobotError('INVALID_MESSAGE', '模型指纹必须为SHA-256');
const caps = record(d.capabilities);
exactKeys(caps, ['reset', 'lockstep', 'cameras', 'training']);
if (
typeof caps.reset !== 'boolean' ||
caps.lockstep !== false ||
caps.cameras !== false ||
caps.training !== false
)
throw new RobotError('UNSUPPORTED', 'V1 不支持相机、锁步或训练');
return {
protocolVersion: 1,
profileId: identifier(d.profileId),
profileVersion: integer(d.profileVersion, 'profileVersion', 1),
modelFingerprint: d.modelFingerprint,
frame: 'x-forward-y-left-z-up',
actionChannels: channels(d.actionChannels),
observationChannels: channels(d.observationChannels),
capabilities: { reset: caps.reset, lockstep: false, cameras: false, training: false },
};
}
/** Complete channel set; reject non-numeric input rather than coercing strings/bools. */
export function validateValues(
value: unknown,
channels: readonly RobotChannel[],
clamp = false,
): Record<string, number> {
const values = record(value, '通道值');
exactKeys(
values,
channels.map((c) => c.id),
);
return Object.fromEntries(
channels.map((c) => {
const n = finite(values[c.id], c.id);
return [c.id, clamp ? Math.max(c.min, Math.min(c.max, n)) : n];
}),
);
}
export function validateAction(
value: unknown,
descriptor: RobotDescriptor,
identity: RobotIdentity,
lastSeq: number,
): RobotAction {
const a = record(value);
exactKeys(a, ['protocolVersion', 'sessionId', 'modelEpoch', 'leaseId', 'actionSeq', 'values']);
if (a.protocolVersion !== 1) throw new RobotError('UNSUPPORTED', '协议版本不匹配');
identifier(a.sessionId);
identifier(a.leaseId);
integer(a.modelEpoch, 'modelEpoch');
if (
a.sessionId !== identity.sessionId ||
a.modelEpoch !== identity.modelEpoch ||
a.leaseId !== identity.leaseId
)
throw new RobotError('STALE', '模型或控制授权已过期');
const actionSeq = integer(a.actionSeq, 'actionSeq', 1);
if (actionSeq <= lastSeq) throw new RobotError('STALE', '拒绝重复或乱序动作');
return {
protocolVersion: 1,
...identity,
actionSeq,
values: validateValues(a.values, descriptor.actionChannels, true),
};
}
export function validateObservation(value: unknown, descriptor: RobotDescriptor): RobotObservation {
const o = record(value);
exactKeys(o, [
'protocolVersion',
'sessionId',
'modelEpoch',
'sequence',
'simTime',
'appliedActionSeq',
'paused',
'values',
]);
if (o.protocolVersion !== 1 || typeof o.paused !== 'boolean' || finite(o.simTime, 'simTime') < 0)
throw new RobotError('INVALID_MESSAGE', '观测格式无效');
return {
protocolVersion: 1,
sessionId: identifier(o.sessionId),
modelEpoch: integer(o.modelEpoch, 'modelEpoch'),
sequence: integer(o.sequence, 'sequence'),
simTime: o.simTime as number,
appliedActionSeq: integer(o.appliedActionSeq, 'appliedActionSeq'),
paused: o.paused,
values: validateValues(o.values, descriptor.observationChannels),
};
}
@@ -6,15 +6,19 @@ export function ActuatorControl({
onControl,
onParameters,
showParameterNotice = true,
disabled = false,
parametersDisabled = false,
}: {
actuator: ActuatorInfo;
showParameterNotice?: boolean;
disabled?: boolean;
parametersDisabled?: boolean;
onControl: (value: number) => void;
onParameters: (parameters: ActuatorParameters) => void;
}) {
const isMotor = actuator.kind === 'motor',
isPosition = actuator.kind === 'position',
editable = isMotor || isPosition,
editable = !parametersDisabled && (isMotor || isPosition),
baseTargetScale = isPosition && actuator.jointType === 3 ? 180 / Math.PI : 1,
targetScale =
isPosition && Math.abs(actuator.gear) > 1e-9
@@ -55,7 +59,7 @@ export function ActuatorControl({
controlLabel = isPosition ? (actuator.jointType === 3 ? '角度' : '位置') : '控制',
gearSquared = actuator.gear * actuator.gear;
return (
<div className="mb-2 border-b border-border pb-2">
<fieldset disabled={disabled} className="mb-2 min-w-0 border-b border-border pb-2">
<div className="mb-1 flex min-w-0 items-start justify-between gap-2">
<div className="min-w-0">
<div className="truncate text-xs font-medium text-text-primary" title={actuator.name}>
@@ -183,11 +187,12 @@ export function ActuatorControl({
</details>
) : (
<p className="border-t border-border pt-2 text-xs leading-4 text-text-tertiary">
该驱动器不是可直接编辑的 motor/position 类型,控制值按模型原始单位显示;请在 MJCF
源码中修改专用参数。
{parametersDisabled
? '机器人 profile 固定伺服参数;目标可在手动控制模式调整。'
: '该驱动器不是可直接编辑的 motor/position 类型,控制值按模型原始单位显示;请在 MJCF 源码中修改专用参数。'}
</p>
)}
</div>
</fieldset>
);
}
function ParameterInput({
@@ -0,0 +1,80 @@
import { ControlArbiter } from './ControlArbiter';
import { SimulationSession } from './SimulationSession';
describe('ControlArbiter', () => {
it('invalidates capabilities on every owner/generation change', () => {
const arbiter = new ControlArbiter(),
manual = arbiter.current();
const first = arbiter.claim('external');
expect(arbiter.owns(manual)).toBe(false);
expect(arbiter.owns(first)).toBe(true);
const second = arbiter.claim('external');
expect(arbiter.owns(first)).toBe(false);
expect(arbiter.owns(second)).toBe(true);
expect(arbiter.owns({ ...second })).toBe(false);
arbiter.claim('policy');
expect(arbiter.owns(second)).toBe(false);
});
it('Session rejects paused authorization before switching control and allows it after play', () => {
const arbiter = new ControlArbiter(),
ticket = arbiter.claim('python');
const robotRuntime = { authorize: vi.fn(), revoke: vi.fn(), capture: vi.fn() },
startExternalClock = vi.fn();
const session = Object.assign(Object.create(SimulationSession.prototype), {
controlArbiter: arbiter,
robotRuntime,
startExternalClock,
paused: true,
speed: 2,
data: { ctrl: new Float64Array([0.4]), time: 0 },
}) as SimulationSession;
expect(() => session.setExternalControlEnabled(true)).toThrow(
expect.objectContaining({ code: 'PAUSED' }),
);
expect(arbiter.current()).toBe(ticket);
expect(robotRuntime.authorize).not.toHaveBeenCalled();
expect(robotRuntime.revoke).not.toHaveBeenCalled();
expect(startExternalClock).not.toHaveBeenCalled();
expect(session.data.ctrl[0]).toBe(0.4);
expect(session.speed).toBe(2);
expect(session.paused).toBe(true);
session.setPaused(false);
session.setExternalControlEnabled(true);
expect(arbiter.current().owner).toBe('external');
expect(robotRuntime.authorize).toHaveBeenCalledOnce();
expect(startExternalClock).toHaveBeenCalledOnce();
expect(session.speed).toBe(1);
});
it('configured robots cannot teleport joints or disable their physical limits, even in manual mode', () => {
const session = Object.assign(Object.create(SimulationSession.prototype), {
controlArbiter: new ControlArbiter(),
robotAdapter: {},
paused: false,
data: { qpos: new Float64Array([0.2]), ctrl: new Float64Array([0.4]) },
module: { mj_forward: vi.fn() },
}) as SimulationSession;
expect(session.setJointPosition(0, 1)).toBe(false);
expect(() => session.resetJoints()).toThrow(/profile.*瞬移/);
expect(() => session.setIgnoreJointLimits(true)).toThrow(/profile.*限位/);
expect(session.data.qpos[0]).toBe(0.2);
expect(session.data.ctrl[0]).toBe(0.4);
expect(session.paused).toBe(false);
expect(session.module.mj_forward).not.toHaveBeenCalled();
});
it('Session refuses direct manual writes when another owner holds control', () => {
const arbiter = new ControlArbiter();
arbiter.claim('external');
const session = Object.assign(Object.create(SimulationSession.prototype), {
controlArbiter: arbiter,
model: { njnt: 1 },
data: { ctrl: new Float64Array([0.4]) },
}) as SimulationSession;
expect(() => session.setActuator(0, 1)).toThrow(/停止当前/);
expect(session.setJointPosition(0, 1)).toBe(false);
expect(() => session.resetJoints()).toThrow();
expect(() => session.setIgnoreJointLimits(true)).toThrow();
expect(() => session.setSpeed(2)).toThrow();
expect(session.data.ctrl[0]).toBe(0.4);
});
});
@@ -0,0 +1,19 @@
export type ControlOwner = 'manual' | 'python' | 'policy' | 'external';
export interface ControlTicket {
readonly owner: ControlOwner;
readonly generation: number;
}
/** Every transition invalidates all previously handed-out write capabilities. */
export class ControlArbiter {
private ticket: ControlTicket = Object.freeze({ owner: 'manual', generation: 0 });
current(): ControlTicket {
return this.ticket;
}
claim(owner: ControlOwner): ControlTicket {
this.ticket = Object.freeze({ owner, generation: this.ticket.generation + 1 });
return this.ticket;
}
owns(ticket: ControlTicket | undefined): boolean {
return ticket !== undefined && ticket === this.ticket;
}
}
@@ -0,0 +1,61 @@
import type { MjModel } from '@mujoco/mujoco';
import { ModelBindings } from './ModelBindings';
function fakeModel() {
const deleted = vi.fn();
const joint = { name: 'slider', type: 2, qposadr: 4, dofadr: 3, delete: deleted };
const motor = {
name: 'servo',
trntype: 0,
trnid: [0, -1],
gear: [1],
gaintype: 0,
dyntype: 0,
gainprm: [10],
biastype: 1,
biasprm: [0, -10, -1],
delete: deleted,
};
const model = {
njnt: 1,
nactuator: 1,
nsensor: 0,
nbody: 1,
nu: 1,
actuator_ctrladr: [0],
jnt: () => joint,
actuator: () => motor,
body: () => ({ name: 'root', delete: deleted }),
sensor: vi.fn(),
};
return { model: model as unknown as MjModel, joint, motor, deleted };
}
describe('ModelBindings', () => {
it('resolves an arbitrary single joint, releasing every temporary handle', () => {
const f = fakeModel(),
b = new ModelBindings(f.model);
expect(f.deleted).toHaveBeenCalledTimes(3);
expect(b.scalarActuator('servo', 'slider', 'position')).toEqual({
id: 0,
qposAddress: 4,
qvelAddress: 3,
actuatorId: 0,
controlAddress: 0,
});
expect(f.deleted).toHaveBeenCalledTimes(5);
expect(() => b.resolve(b.joints, 'missing')).toThrow(/找不到/);
});
it('rejects wrong modes/transmission/gear and cleans up on errors', () => {
const f = fakeModel(),
b = new ModelBindings(f.model);
expect(() => b.scalarActuator('servo', 'slider', 'velocity')).toThrow();
f.motor.gear[0] = 2;
expect(() => b.scalarActuator('servo', 'slider', 'position')).toThrow();
f.motor.gear[0] = 1;
f.motor.trnid[0] = 8;
expect(() => b.scalarActuator('servo', 'slider', 'position')).toThrow();
f.joint.type = 0;
expect(() => b.scalarJoint('slider')).toThrow();
expect(f.deleted).toHaveBeenCalledTimes(10);
});
});
@@ -0,0 +1,97 @@
import type { MjModel } from '@mujoco/mujoco';
import { RobotError, type RobotControlMode } from '../robot/types';
export interface ScalarJointBinding {
id: number;
qposAddress: number;
qvelAddress: number;
}
export interface ScalarActuatorBinding extends ScalarJointBinding {
actuatorId: number;
controlAddress: number;
}
/** Only plain indices/addresses are retained, never owning Embind handles. */
export class ModelBindings {
readonly joints = new Map<string, number>();
readonly actuators = new Map<string, number>();
readonly sensors = new Map<string, number>();
readonly bodies = new Map<string, number>();
constructor(private readonly model: MjModel) {
for (const [count, get, names] of [
[model.njnt, (id: number) => model.jnt(id), this.joints],
[model.nactuator, (id: number) => model.actuator(id), this.actuators],
[model.nsensor, (id: number) => model.sensor(id), this.sensors],
[model.nbody, (id: number) => model.body(id), this.bodies],
] as const) {
for (let id = 0; id < count; id++) {
const item = get(id);
try {
if (item.name) {
if (names.has(item.name))
throw new RobotError('INCOMPATIBLE_MODEL', `模型名称重复:${item.name}`);
names.set(item.name, id);
}
} finally {
item.delete();
}
}
}
}
resolve(names: ReadonlyMap<string, number>, name: string): number {
const id = names.get(name);
if (id === undefined) throw new RobotError('INCOMPATIBLE_MODEL', `模型中找不到:${name}`);
return id;
}
scalarJoint(name: string): ScalarJointBinding {
const id = this.resolve(this.joints, name),
joint = this.model.jnt(id);
try {
if (Number(joint.type) !== 2 && Number(joint.type) !== 3)
throw new RobotError('INCOMPATIBLE_MODEL', `${name} 不是标量关节`);
return { id, qposAddress: Number(joint.qposadr), qvelAddress: Number(joint.dofadr) };
} finally {
joint.delete();
}
}
scalarActuator(
name: string,
jointName: string,
mode: Exclude<RobotControlMode, 'opening'>,
): ScalarActuatorBinding {
const joint = this.scalarJoint(jointName);
const actuatorId = this.resolve(this.actuators, name),
actuator = this.model.actuator(actuatorId);
try {
const controlAddress = Number(this.model.actuator_ctrladr[actuatorId] ?? actuatorId);
const next =
actuatorId + 1 < this.model.nactuator
? Number(this.model.actuator_ctrladr[actuatorId + 1])
: this.model.nu;
const gain = Number(actuator.gainprm[0]);
if (
next - controlAddress !== 1 ||
Number(actuator.trntype) !== 0 ||
Number(actuator.trnid[0]) !== joint.id ||
Number(actuator.gaintype) !== 0 ||
Number(actuator.dyntype) !== 0 ||
!Number.isFinite(gain) ||
gain <= 0 ||
Math.abs(Number(actuator.gear[0]) - 1) > 1e-8
)
throw new RobotError('INCOMPATIBLE_MODEL', `${name} 的标量 transmission/gain/gear 不匹配`);
const bias = Number(actuator.biastype),
bp = actuator.biasprm;
const valid =
mode === 'effort'
? bias === 0
: bias === 1 &&
(mode === 'position'
? Math.abs(Number(bp[1]) + gain) < 1e-6
: Math.abs(Number(bp[1])) < 1e-8 && Math.abs(Number(bp[2]) + gain) < 1e-6);
if (!valid) throw new RobotError('INCOMPATIBLE_MODEL', `${name} 不是 ${mode} 驱动器`);
return { ...joint, actuatorId, controlAddress };
} finally {
actuator.delete();
}
}
}
@@ -0,0 +1,202 @@
import type { MjData, MjModel } from '@mujoco/mujoco';
import { ModelBindings, type ScalarActuatorBinding } from './ModelBindings';
import {
LEKIWI_PROFILE as p,
bodyToWheels,
wheelsToBody,
WHEEL_TANGENTS,
} from '../robot/profiles/lekiwi';
import { describeProfile } from '../robot/registry';
import { RobotError, type RobotAdapter, type RobotDescriptor } from '../robot/types';
import { validateValues } from '../robot/validation';
/** MuJoCo backend for the explicitly registered LeKiwi recipe. No transport/UI. */
export class MuJoCoRobotAdapter implements RobotAdapter {
private readonly descriptor: RobotDescriptor;
private readonly arm: ScalarActuatorBinding[];
private readonly wheels: ScalarActuatorBinding[];
private readonly baseId: number;
private readonly freeDof: number;
private disposed = false;
constructor(
private readonly model: MjModel,
private readonly data: MjData,
profileId: string,
fingerprint: string,
) {
this.descriptor = describeProfile(profileId, fingerprint);
const b = new ModelBindings(model);
this.arm = p.arm.map((spec) => b.scalarActuator(`${spec.joint}_servo`, spec.joint, 'position'));
this.wheels = p.wheels.map((name) => b.scalarActuator(`${name}_servo`, name, 'velocity'));
this.baseId = b.resolve(b.bodies, p.baseBody);
const free = Array.from({ length: model.njnt }, (_, i) => i).filter(
(i) => Number(model.jnt_type[i]) === 0,
);
if (
free.length !== 1 ||
Number(model.jnt_bodyid[free[0]]) !== this.baseId ||
model.nu !== 9 ||
model.njnt !== 10 + 3 * p.simulation.rollerCount
)
throw new RobotError(
'INCOMPATIBLE_MODEL',
'LeKiwi 需要唯一 canonical 浮动基座和九个主驱动器',
);
this.freeDof = Number(model.jnt_dofadr[free[0]]);
const servoCheck = (
j: ScalarActuatorBinding,
gain: number,
damping: number,
min: number,
max: number,
force: number,
) => {
const a = model.actuator(j.actuatorId);
try {
const close = (actual: unknown, expected: number) =>
Math.abs(Number(actual) - expected) < 1e-5;
if (
!a.ctrllimited ||
!a.forcelimited ||
!close(a.gainprm[0], gain) ||
!close(a.biasprm[2], -damping) ||
!close(a.ctrlrange[0], min) ||
!close(a.ctrlrange[1], max) ||
!close(a.forcerange[0], -force) ||
!close(a.forcerange[1], force)
)
throw new RobotError('INCOMPATIBLE_MODEL', '伺服参数与声明的 profile 不匹配');
} finally {
a.delete();
}
};
this.wheels.forEach((j, i) => {
servoCheck(
j,
p.simulation.wheelKv,
p.simulation.wheelKv,
-p.wheelMaxRadS,
p.wheelMaxRadS,
p.simulation.wheelTorqueLimit,
);
const expected = [-WHEEL_TANGENTS[i][1], WHEEL_TANGENTS[i][0], 0];
if (expected.some((v, axis) => Math.abs(Number(model.jnt_axis[j.id * 3 + axis]) - v) > 1e-6))
throw new RobotError('INCOMPATIBLE_MODEL', '轮轴方向与 profile 不匹配');
});
this.arm.forEach((j, i) => {
const spec = p.arm[i];
servoCheck(j, spec.kp, spec.kv, spec.min, spec.max, spec.force);
if (spec.joint === 'arm_gripper') {
const expected = [0, -0.906308, -0.422618].map((v) => v * p.mapping.gripperAxisSign);
if (
expected.some((v, axis) => Math.abs(Number(model.jnt_axis[j.id * 3 + axis]) - v) > 1e-6)
)
throw new RobotError(
'INCOMPATIBLE_MODEL',
'夹爪轴方向与 profile 不匹配;请从原始 URDF 重新转换',
);
}
const joint = model.jnt(j.id);
try {
if (
!joint.limited ||
Math.abs(Number(joint.range[0]) - p.arm[i].min) > 1e-6 ||
Math.abs(Number(joint.range[1]) - p.arm[i].max) > 1e-6
)
throw new RobotError('INCOMPATIBLE_MODEL', `关节限位与 profile 不符:${p.arm[i].joint}`);
} finally {
joint.delete();
}
});
}
describe(): RobotDescriptor {
return structuredClone(this.descriptor);
}
private alive(): void {
if (this.disposed) throw new RobotError('DISCONNECTED', '机器人后端已释放');
}
validateAction(values: unknown): Record<string, number> {
this.alive();
return validateValues(values, this.descriptor.actionChannels, true);
}
applyAction(input: Record<string, number>): Record<string, number> {
const values = this.validateAction(input);
const wheel = bodyToWheels(values['x.vel'], values['y.vel'], values['theta.vel']);
this.arm.forEach((j, i) => {
const spec = p.arm[i],
value = values[`${spec.joint}.pos`];
this.data.ctrl[j.controlAddress] =
spec.joint === 'arm_gripper' ? spec.min + value * (spec.max - spec.min) : value;
});
this.wheels.forEach((j, i) => {
this.data.ctrl[j.controlAddress] = wheel[i];
});
const [x, y, w] = wheelsToBody(wheel);
return { ...values, 'x.vel': x, 'y.vel': y, 'theta.vel': w };
}
readObservation(): Record<string, number> {
this.alive();
const values: Record<string, number> = {};
this.arm.forEach((j, i) => {
const spec = p.arm[i],
value = Number(this.data.qpos[j.qposAddress]);
values[`${spec.joint}.pos`] =
spec.joint === 'arm_gripper' ? (value - spec.min) / (spec.max - spec.min) : value;
});
const [x, y, w] = wheelsToBody(this.wheels.map((j) => Number(this.data.qvel[j.qvelAddress])));
Object.assign(values, { 'x.vel': x, 'y.vel': y, 'theta.vel': w });
const q = this.data.xquat.subarray(this.baseId * 4, this.baseId * 4 + 4),
[qw, qx, qy, qz] = Array.from(q, Number);
const pos = this.data.xpos.subarray(this.baseId * 3, this.baseId * 3 + 3);
const mat = this.data.xmat.subarray(this.baseId * 9, this.baseId * 9 + 9),
dof = this.freeDof;
const local = [0, 1, 2].map((col) =>
[0, 1, 2].reduce(
(sum, row) => sum + Number(mat[row * 3 + col]) * Number(this.data.qvel[dof + row]),
0,
),
);
Object.assign(values, {
'base.x': Number(pos[0]),
'base.y': Number(pos[1]),
'base.z': Number(pos[2]),
'base.roll': Math.atan2(2 * (qw * qx + qy * qz), 1 - 2 * (qx * qx + qy * qy)),
'base.pitch': Math.asin(Math.max(-1, Math.min(1, 2 * (qw * qy - qz * qx)))),
'base.yaw': Math.atan2(2 * (qw * qz + qx * qy), 1 - 2 * (qy * qy + qz * qz)),
'base.vx': local[0],
'base.vy': local[1],
'base.wz': Number(this.data.qvel[dof + 5]),
});
return validateValues(values, this.descriptor.observationChannels);
}
safeStop(): void {
if (this.disposed) return;
// Zero on a position servo means "move to zero", not "stop".
this.arm.forEach((j, i) => {
const q = Number(this.data.qpos[j.qposAddress]),
spec = p.arm[i];
this.data.ctrl[j.controlAddress] = Number.isFinite(q)
? Math.max(spec.min, Math.min(spec.max, q))
: spec.neutral;
});
this.wheels.forEach((j) => {
this.data.ctrl[j.controlAddress] = 0;
});
}
reset(): void {
this.alive();
this.arm.forEach((j, i) => {
this.data.qpos[j.qposAddress] = p.arm[i].neutral;
this.data.ctrl[j.controlAddress] = p.arm[i].neutral;
});
this.wheels.forEach((j) => {
this.data.ctrl[j.controlAddress] = 0;
});
}
dispose(): void {
if (!this.disposed) {
this.safeStop();
this.disposed = true;
}
}
}
+107 -2
View File
@@ -8,6 +8,17 @@ import type { PolicyDeployment, TrainingTerrain } from '../rl/deployment';
import type { MainModule } from '@mujoco/mujoco';
import type { ProjectFile, ProjectManifest } from '../project/types';
import { prepareProjectForMujoco } from '../project/importer';
import { prepareRobotProject } from '../project/robotProfiles';
import { sha256 } from '../robot/registry';
import type { ExternalControlStatus } from '../robot/RobotRuntime';
import {
RobotError,
type RobotDescriptor,
type RobotObservation,
type RobotIdentity,
type RobotActionResult,
} from '../robot/types';
import { enhanceLeKiwiMjcf, type LeKiwiJointGeometry } from '../project/robotProfiles/lekiwi';
import {
enhanceConvertedMjcf,
groundConvertedMjcf,
@@ -43,6 +54,8 @@ export interface PhysicsLoadProgress {
}
export interface PhysicsLoadOptions {
/** Explicit built-in profile; never inferred from actuator count. */
robotProfileId?: string;
trainingDeployment?: PolicyDeployment;
/** Candidate policy is initialized/validated before replacing the active session. */
trainingPolicy?: { data: Uint8Array; path: string };
@@ -67,6 +80,13 @@ export interface PhysicsAdapter {
setSpeed(speed: number): void;
reset(): void;
singleStep(): void;
describeRobot(): RobotDescriptor | undefined;
robotObservation(): RobotObservation | undefined;
externalControlStatus(): ExternalControlStatus | undefined;
setExternalControlEnabled(enabled: boolean): void;
claimExternalControlLease(leaseId: string): RobotIdentity;
sendRobotAction(action: unknown): Promise<RobotActionResult>;
stopExternalControl(reason?: string): void;
setActuator(id: number, value: number): void;
setActuatorParameters(id: number, parameters: ActuatorParameters): boolean;
setJointPosition(id: number, value: number): boolean;
@@ -149,6 +169,8 @@ export class MainThreadPhysicsAdapter implements PhysicsAdapter {
options: PhysicsLoadOptions = {},
): Promise<SimulationSnapshot> {
if (this.disposed) throw new Error('物理适配器已释放');
if (this.session?.describeRobot?.())
this.session.stopExternalControl('模型正在重载,请重新授权');
const generation = ++this.loadGeneration;
const urdfMode = options.urdfMode ?? 'mjcf';
const baseMode = options.baseMode ?? 'floating';
@@ -169,7 +191,15 @@ export class MainThreadPhysicsAdapter implements PhysicsAdapter {
if (this.disposed || generation !== this.loadGeneration) throw new Error('模型加载已取消');
report(0.32, '校验并准备模型资源');
const workspace = new MemfsWorkspace(module, `${manifest.id}_stage_${generation}`);
const prepared = await prepareProjectForMujoco(manifest, entryPath);
if (
options.robotProfileId &&
(urdfMode !== 'mjcf' || baseMode !== 'floating' || options.trainingDeployment)
)
throw new Error('机器人 profile 需要浮动基座 URDF 转换,不能与 Go2 部署混用');
const profileManifest = options.robotProfileId
? await prepareRobotProject(manifest, entryPath, options.robotProfileId)
: manifest;
const prepared = await prepareProjectForMujoco(profileManifest, entryPath);
const supportFiles = prepared.manifest.files.filter(
(file) => !manifest.files.some((original) => original.path === file.path),
);
@@ -199,7 +229,37 @@ export class MainThreadPhysicsAdapter implements PhysicsAdapter {
baseMode,
);
const enhanced = enhanceConvertedMjcf(grounded, enhancements);
workspace.writeGenerated(convertedPath, enhanced.data);
let convertedData = enhanced.data;
if (options.robotProfileId) {
const geometry: LeKiwiJointGeometry[] = [];
for (let id = 0; id < intermediate.model.njnt; id++) {
const joint = intermediate.model.jnt(id);
const bodyId = Number(intermediate.model.jnt_bodyid[id]);
const body = intermediate.model.body(bodyId);
try {
geometry.push({
name: joint.name,
body: body.name,
position: Array.from(
intermediate.data.xpos.slice(bodyId * 3, bodyId * 3 + 3),
Number,
),
rotation: Array.from(
intermediate.data.xmat.slice(bodyId * 9, bodyId * 9 + 9),
Number,
),
});
} finally {
joint.delete();
body.delete();
}
}
convertedData = enhanceLeKiwiMjcf(convertedData, geometry);
warnings.unshift(
'LeKiwi v1:仿真估计参数,简化被动滚子/碰撞与九路伺服;超大轮 STL 已替换为简化视觉,不代表实机标定',
);
}
workspace.writeGenerated(convertedPath, convertedData);
modelRelativePath = convertedPath;
modelPath = workspace.path(modelRelativePath);
warnings.push(
@@ -285,6 +345,12 @@ export class MainThreadPhysicsAdapter implements PhysicsAdapter {
report(0.84, '编译模型与物理数据');
console.info('[MuJoCo] 编译模型', modelPath);
nextSession = new SimulationSession(module, modelPath, warnings);
if (options.robotProfileId) {
const fingerprint = await sha256(
new TextEncoder().encode(workspace.readText(modelRelativePath)),
);
nextSession.configureRobot(options.robotProfileId, fingerprint);
}
if (options.trainingDeployment) nextSession.configureDeployment(options.trainingDeployment);
if (entry?.format === 'urdf' && urdfMode === 'native') {
const offset = nextSession.alignLowestPointToGround();
@@ -413,6 +479,29 @@ export class MainThreadPhysicsAdapter implements PhysicsAdapter {
singleStep(): void {
this.session?.singleStep();
}
describeRobot(): RobotDescriptor | undefined {
return this.session?.describeRobot();
}
externalControlStatus(): ExternalControlStatus | undefined {
return this.session?.externalControlStatus();
}
robotObservation(): RobotObservation | undefined {
return this.session?.robotObservation();
}
setExternalControlEnabled(enabled: boolean): void {
this.session?.setExternalControlEnabled(enabled);
}
claimExternalControlLease(leaseId: string): RobotIdentity {
if (!this.session) throw new RobotError('DISCONNECTED', '没有仿真会话');
return this.session.claimExternalControlLease(leaseId);
}
sendRobotAction(action: unknown): Promise<RobotActionResult> {
if (!this.session) return Promise.reject(new RobotError('DISCONNECTED', '没有仿真会话'));
return this.session.sendRobotAction(action);
}
stopExternalControl(reason?: string): void {
this.session?.stopExternalControl(reason);
}
setActuator(id: number, value: number): void {
this.session?.setActuator(id, value);
}
@@ -539,6 +628,22 @@ export class MainThreadPhysicsAdapter implements PhysicsAdapter {
actuatorSection = document.querySelector('mujoco > actuator');
if (document.querySelector('parsererror')) return new TextEncoder().encode(source);
const snapshot = this.session.snapshot();
if (snapshot.robot) {
const custom = document.querySelector('mujoco > custom') ?? document.createElement('custom');
if (!custom.parentNode) document.documentElement.append(custom);
for (const [name, value] of [
['platform_robot_profile', snapshot.robot.profileId],
['platform_robot_profile_version', String(snapshot.robot.profileVersion)],
// Provenance of the loaded source, not a self-referential export hash.
['platform_robot_source_fingerprint', snapshot.robot.modelFingerprint],
]) {
const marker =
custom.querySelector(`text[name="${name}"]`) ?? document.createElement('text');
marker.setAttribute('name', name);
marker.setAttribute('data', value);
custom.append(marker);
}
}
if (actuatorSection) {
for (const info of snapshot.actuators) {
const element = Array.from(actuatorSection.children).find(
+25 -4
View File
@@ -92,7 +92,7 @@ export function ModelSummaryInspector({
aria-label="URDF 处理方式"
className="w-full"
value={urdfMode}
disabled={loading}
disabled={loading || Boolean(snapshot.robot)}
onChange={(event) => onUrdfMode(event.target.value as UrdfLoadMode)}
>
<option value="mjcf">转换为 MJCF(推荐)</option>
@@ -104,7 +104,7 @@ export function ModelSummaryInspector({
aria-label="URDF 基座类型"
className="w-full"
value={baseMode}
disabled={loading || urdfMode === 'native'}
disabled={loading || urdfMode === 'native' || Boolean(snapshot.robot)}
onChange={(event) => onBaseMode(event.target.value as UrdfBaseMode)}
>
<option value="floating">浮动基座(Free Joint)</option>
@@ -242,6 +242,12 @@ export function JointInspector({
meta={`${JOINT_TYPE_LABELS[joint.type] ?? `Type ${joint.type}`} · ${body?.name ?? `Body ${joint.bodyId}`}`}
/>
<CollapsibleSection title="关节属性" defaultOpen>
{snapshot.robot && (
<p className="text-xs text-text-tertiary">
机器人关节角为实测值,只读。请在下方关联 Actuator 中设置目标并点击播放;
运动由物理伺服执行,遇到碰撞时实际位置可能达不到目标。
</p>
)}
<details className="domain-details">
<summary>关节标识与轴向</summary>
<PropertyRow label="Joint ID" value={joint.id} />
@@ -278,11 +284,22 @@ export function JointInspector({
/>
</div>
<div className="grid grid-cols-2 gap-2">
<Button disabled={loading} onClick={onResetJoints}>
<Button
disabled={
loading ||
Boolean(snapshot.robot) ||
Boolean(snapshot.controlOwner && snapshot.controlOwner !== 'manual')
}
onClick={onResetJoints}
>
重置关节
</Button>
<Button
disabled={loading}
disabled={
loading ||
Boolean(snapshot.robot) ||
Boolean(snapshot.controlOwner && snapshot.controlOwner !== 'manual')
}
variant={ignoreJointLimits ? 'primary' : 'secondary'}
aria-pressed={ignoreJointLimits}
onClick={onToggleJointLimits}
@@ -315,6 +332,10 @@ export function JointInspector({
<ActuatorControl
key={actuator.id}
actuator={actuator}
disabled={
loading || Boolean(snapshot.controlOwner && snapshot.controlOwner !== 'manual')
}
parametersDisabled={Boolean(snapshot.robot)}
onControl={(value) => onActuator(actuator.id, value)}
onParameters={(parameters) => onActuatorParameters(actuator.id, parameters)}
/>
+297 -113
View File
@@ -8,6 +8,22 @@ import {
import type { MainModule, MjData, MjModel, MjvPerturb, MjvScene } from '@mujoco/mujoco';
import { meshIdFromSceneDataId } from './geometry';
import { PythonControllerRuntime } from '../controller/PythonControllerRuntime';
import { ModelBindings } from './ModelBindings';
import { ControlArbiter, type ControlOwner, type ControlTicket } from './ControlArbiter';
import { MuJoCoRobotAdapter } from './MuJoCoRobotAdapter';
import { RobotRuntime, type ExternalControlStatus } from '../robot/RobotRuntime';
import {
RobotError,
type RobotDescriptor,
type RobotAdapter,
type RobotObservation,
type RobotIdentity,
type RobotActionResult,
} from '../robot/types';
interface ControllerWriteScope {
ticket?: ControlTicket;
}
import type { ControllerBindings, ControllerCommand, ControllerStatus } from '../controller/types';
import { Go2wPolicyBindings } from '../rl/runtime/Go2wPolicyBindings';
import type { OnnxPolicyRuntime } from '../rl/runtime/OnnxPolicyRuntime';
@@ -68,6 +84,9 @@ export interface BodyInfo {
}
export interface SimulationSnapshot {
time: number;
paused?: boolean;
speed?: number;
wasmHeapBytes?: number;
qpos: number[];
qvel: number[];
ctrl: number[];
@@ -77,6 +96,9 @@ export interface SimulationSnapshot {
warnings: string[];
controller?: ControllerStatus;
rlPolicy?: RLPolicyStatus;
controlOwner?: ControlOwner;
robot?: RobotDescriptor;
externalControl?: ExternalControlStatus;
telemetry: DataRecorderStatus;
model: {
nbody: number;
@@ -118,6 +140,119 @@ export class SimulationSession {
private deploymentInitialQpos?: Float64Array;
private rlPolicyLoadGeneration = 0;
private dataRecorder!: DataRecorder;
private controlArbiter?: ControlArbiter;
private pythonScope?: ControllerWriteScope;
private policyScope?: ControllerWriteScope;
private robotAdapter?: RobotAdapter;
private robotRuntime?: RobotRuntime;
private externalTimer?: ReturnType<typeof setInterval>;
private externalFrame: FrameResult = { steps: 0, stepMs: 0, overBudget: false };
private stopExternalClock(): void {
if (this.externalTimer !== undefined) {
clearInterval(this.externalTimer);
this.externalTimer = undefined;
this.lastNow = undefined;
this.accumulator = 0;
this.externalFrame = { steps: 0, stepMs: 0, overBudget: false };
}
}
private startExternalClock(): void {
this.stopExternalClock();
this.lastNow = undefined;
this.accumulator = 0;
// Independent of slow/occluded GPU presentation. Same JS thread and same
// fixed-dt step path; network events still only enqueue numeric targets.
this.externalTimer = setInterval(() => {
if (this.disposed) return;
try {
const result = this.advancePhysics(performance.now());
this.externalFrame.steps += result.steps;
this.externalFrame.stepMs += result.stepMs;
this.externalFrame.overBudget ||= result.overBudget;
} catch (error: unknown) {
this.stopExternalControl(error instanceof Error ? error.message : '外部物理步进失败');
}
}, 1000 / 120);
}
private get arbiter(): ControlArbiter {
return (this.controlArbiter ??= new ControlArbiter());
}
configureRobot(profileId: string, fingerprint: string): void {
this.configureRobotAdapter(
new MuJoCoRobotAdapter(this.model, this.data, profileId, fingerprint),
);
}
/** Trusted backend installation point; not exposed over the bridge. */
configureRobotAdapter(backend: RobotAdapter): void {
if (this.robotRuntime) throw new RobotError('CONFLICT', '机器人已经配置');
this.robotAdapter = backend;
this.robotRuntime = new RobotRuntime(
backend,
crypto.randomUUID(),
() => this.paused,
(reason) => this.stopExternalControl(reason),
);
backend.reset();
this.module.mj_forward(this.model, this.data);
this.robotRuntime.capture(Number(this.data.time));
}
describeRobot(): RobotDescriptor | undefined {
return this.robotAdapter?.describe();
}
externalControlStatus(): ExternalControlStatus | undefined {
return this.robotRuntime?.status();
}
robotObservation(): RobotObservation | undefined {
return this.robotRuntime?.latest();
}
private clearControlTargets(): void {
if (this.robotAdapter) this.robotAdapter.safeStop();
else this.data.ctrl.fill(0);
}
private selectControl(owner: ControlOwner): ControlTicket {
this.stopExternalClock();
const ticket = this.arbiter.claim(owner);
this.pythonController?.setEnabled(false, Number(this.data.time));
this.rlPolicy?.setEnabled(false, Number(this.data.time));
this.robotRuntime?.revoke('控制权已切换', false);
this.clearControlTargets();
return ticket;
}
setExternalControlEnabled(enabled: boolean): void {
if (!enabled) {
this.stopExternalControl();
return;
}
if (!this.robotRuntime) throw new RobotError('UNSUPPORTED', '请先应用机器人 profile');
if (this.paused) throw new RobotError('PAUSED', '请先播放仿真,再允许外部控制');
if (this.ignoreJointLimits) throw new RobotError('CONFLICT', '请先恢复关节限位');
this.selectControl('external');
this.speed = 1;
this.robotRuntime.authorize();
this.startExternalClock();
}
claimExternalControlLease(leaseId: string): RobotIdentity {
if (!this.robotRuntime || this.arbiter.current().owner !== 'external')
throw new RobotError('UNAUTHORIZED', '未允许外部控制');
return this.robotRuntime.claim(leaseId);
}
sendRobotAction(action: unknown): Promise<RobotActionResult> {
if (!this.robotRuntime || this.arbiter.current().owner !== 'external')
return Promise.reject(new RobotError('UNAUTHORIZED', '未允许外部控制'));
return this.robotRuntime.enqueue(action);
}
stopExternalControl(reason = '外部控制已停止'): void {
this.stopExternalClock();
this.robotRuntime?.revoke(reason, this.arbiter.current().owner === 'external');
if (this.arbiter.current().owner === 'external') {
this.arbiter.claim('manual');
this.paused = true;
this.accumulator = 0;
this.lastNow = undefined;
}
this.robotRuntime?.capture(Number(this.data.time));
}
constructor(
readonly module: MainModule,
@@ -189,11 +324,16 @@ export class SimulationSession {
}
setPaused(paused: boolean): void {
if (paused && this.arbiter.current().owner === 'external')
this.stopExternalControl('仿真已暂停,请重新授权');
this.paused = paused;
this.accumulator = 0;
this.lastNow = undefined;
this.robotRuntime?.capture(Number(this.data.time));
}
setSpeed(speed: number): void {
if (this.arbiter.current().owner === 'external' && speed !== 1)
throw new RobotError('CONFLICT', '外部控制期间只支持1×速度');
this.speed = Math.min(4, Math.max(0.1, speed));
}
reset(): void {
@@ -205,16 +345,30 @@ export class SimulationSession {
this.data.ctrl.fill(0);
this.pythonController?.reset(Number(this.data.time));
this.rlPolicy?.reset(Number(this.data.time));
this.robotRuntime?.reset();
this.module.mj_forward(this.model, this.data);
this.robotRuntime?.capture(Number(this.data.time));
this.dataRecorder.simulationReset();
}
singleStep(): void {
this.runController();
if (this.arbiter.current().owner === 'external' && this.paused)
throw new RobotError('PAUSED', '外部控制不能在暂停时单步');
if (!this.runController()) return;
this.applyForce();
this.module.mj_step(this.model, this.data);
this.robotRuntime?.afterStep(Number(this.data.time));
this.dataRecorder.capture();
}
advance(now: number): FrameResult {
if (this.externalTimer !== undefined) {
const frame = this.externalFrame;
this.externalFrame = { steps: 0, stepMs: 0, overBudget: false };
return frame;
}
return this.advancePhysics(now);
}
private advancePhysics(now: number): FrameResult {
if (this.lastNow === undefined) {
this.lastNow = now;
return { steps: 0, stepMs: 0, overBudget: false };
@@ -231,9 +385,10 @@ export class SimulationSession {
steps < this.maxCatchUpSteps &&
performance.now() - started < this.frameBudgetMs
) {
this.runController();
if (!this.runController()) break;
this.applyForce();
this.module.mj_step(this.model, this.data);
this.robotRuntime?.afterStep(Number(this.data.time));
this.dataRecorder.capture();
this.accumulator -= dt;
steps++;
@@ -245,23 +400,31 @@ export class SimulationSession {
async loadPythonController(source: string, path: string): Promise<ControllerStatus> {
const generation = ++this.controllerLoadGeneration;
const runtime = await PythonControllerRuntime.load(source, path, this.controllerBindings());
const scope: ControllerWriteScope = {};
const runtime = await PythonControllerRuntime.load(
source,
path,
this.controllerBindings(scope),
);
if (this.disposed || generation !== this.controllerLoadGeneration) {
runtime.dispose();
throw new Error('模型已切换,控制器加载已取消');
}
if (this.arbiter.current().owner === 'python') this.selectControl('manual');
this.pythonController?.dispose();
this.pythonScope = scope;
this.pythonController = runtime;
return runtime.status();
}
setControllerEnabled(enabled: boolean): void {
if (enabled && this.pythonController) {
this.data.ctrl.fill(0);
this.rlPolicy?.setEnabled(false, Number(this.data.time));
if (enabled && this.pythonController && this.pythonScope) {
this.pythonScope.ticket = this.selectControl('python');
this.pythonController.setEnabled(true, Number(this.data.time));
} else {
this.pythonController?.setEnabled(false, Number(this.data.time));
if (this.arbiter.current().owner === 'python') this.selectControl('manual');
}
this.pythonController?.setEnabled(enabled, Number(this.data.time));
if (!enabled) this.data.ctrl.fill(0);
}
navigationStatus() {
@@ -372,8 +535,15 @@ export class SimulationSession {
)
throw new Error('策略配套地图尚未加载或配置不匹配');
let cancelled = false;
const scope: ControllerWriteScope = {};
const writeActuator = (id: number, value: number) => {
if (!this.disposed && !cancelled) this.setActuator(id, value);
if (
!this.disposed &&
!cancelled &&
this.policyScope === scope &&
this.arbiter.owns(scope.ticket)
)
this.writeActuator(id, value);
};
const bindings =
deployment?.taskId === OBSTACLE_TASK_ID
@@ -386,20 +556,22 @@ export class SimulationSession {
runtime.dispose();
throw new Error('模型已切换,ONNX 策略加载已取消');
}
this.data.ctrl.fill(0);
if (this.arbiter.current().owner === 'policy') this.selectControl('manual');
this.rlPolicy?.dispose();
this.policyScope = scope;
this.rlPolicy = runtime;
this.obstacleBindings = bindings instanceof Go2ObstacleAvoidanceBindings ? bindings : undefined;
return runtime.status();
}
setRLPolicyEnabled(enabled: boolean): void {
if (enabled && this.rlPolicy) {
this.data.ctrl.fill(0);
this.pythonController?.setEnabled(false, Number(this.data.time));
if (enabled && this.rlPolicy && this.policyScope) {
this.policyScope.ticket = this.selectControl('policy');
this.rlPolicy.setEnabled(true, Number(this.data.time));
} else {
this.rlPolicy?.setEnabled(false, Number(this.data.time));
if (this.arbiter.current().owner === 'policy') this.selectControl('manual');
}
this.rlPolicy?.setEnabled(enabled, Number(this.data.time));
if (!enabled) this.data.ctrl.fill(0);
}
setRLCommand(command: RLCommand): void {
@@ -427,10 +599,11 @@ export class SimulationSession {
removeRLPolicy(): void {
this.rlPolicyLoadGeneration += 1;
if (this.arbiter.current().owner === 'policy') this.selectControl('manual');
this.policyScope = undefined;
this.rlPolicy?.dispose();
this.rlPolicy = undefined;
this.obstacleBindings = undefined;
this.data.ctrl.fill(0);
}
sendControllerCommand(command: ControllerCommand): void {
@@ -439,59 +612,28 @@ export class SimulationSession {
removeController(): void {
this.controllerLoadGeneration += 1;
if (this.arbiter.current().owner === 'python') this.selectControl('manual');
this.pythonScope = undefined;
this.pythonController?.dispose();
this.pythonController = undefined;
this.data.ctrl.fill(0);
}
private runController(): void {
private runController(): boolean {
try {
this.pythonController?.stepIfDue(Number(this.data.time));
this.rlPolicy?.step(Number(this.data.time));
const owner = this.arbiter.current().owner;
if (owner === 'external' && !this.robotRuntime?.beforeStep()) return false;
if (owner === 'python') this.pythonController?.stepIfDue(Number(this.data.time));
if (owner === 'policy') this.rlPolicy?.step(Number(this.data.time));
return true;
} catch (error) {
this.setPaused(true);
this.data.ctrl.fill(0);
this.clearControlTargets();
throw error;
}
}
private controllerBindings(): ControllerBindings {
const joints = new Map<string, number>(),
actuators = new Map<string, number>(),
sensors = new Map<string, number>(),
bodies = new Map<string, number>();
for (let id = 0; id < this.model.njnt; id += 1) {
const item = this.model.jnt(id);
try {
if (item.name) joints.set(item.name, id);
} finally {
item.delete();
}
}
for (let id = 0; id < this.model.nactuator; id += 1) {
const item = this.model.actuator(id);
try {
if (item.name) actuators.set(item.name, id);
} finally {
item.delete();
}
}
for (let id = 0; id < this.model.nsensor; id += 1) {
const item = this.model.sensor(id);
try {
if (item.name) sensors.set(item.name, id);
} finally {
item.delete();
}
}
for (let id = 0; id < this.model.nbody; id += 1) {
const item = this.model.body(id);
try {
if (item.name) bodies.set(item.name, id);
} finally {
item.delete();
}
}
private controllerBindings(scope: ControllerWriteScope): ControllerBindings {
const { joints, actuators, sensors, bodies } = new ModelBindings(this.model);
const resolve = (items: Map<string, number>, kind: string, name: string) => {
const id = items.get(name);
if (id === undefined) throw new Error(`模型中找不到${kind}:${name}`);
@@ -504,64 +646,76 @@ export class SimulationSession {
sensor: (name) => resolve(sensors, '传感器', name),
body: (name) => resolve(bodies, 'Body', name),
},
createStepApi: (time, dt) => ({
time,
dt,
qpos: (jointId) => {
const joint = this.model.jnt(jointId);
try {
const type = Number(joint.type);
if (type !== 2 && type !== 3)
throw new Error(`关节 ${jointId} 不是标量 hinge/slide 关节`);
return Number(this.data.qpos[Number(joint.qposadr)]);
} finally {
joint.delete();
}
},
qvel: (jointId) => {
const joint = this.model.jnt(jointId);
try {
return Number(this.data.qvel[Number(joint.dofadr)]);
} finally {
joint.delete();
}
},
sensor: (sensorId) => {
if (sensorId < 0 || sensorId >= this.model.nsensor)
throw new Error(`传感器 ID 无效:${sensorId}`);
const adr = Number(this.model.sensor_adr[sensorId]),
dim = Number(this.model.sensor_dim[sensorId]);
return Array.from(this.data.sensordata.subarray(adr, adr + dim), Number);
},
body_quat: (bodyId) => {
if (bodyId < 0 || bodyId >= this.model.nbody) throw new Error(`Body ID 无效:${bodyId}`);
const adr = bodyId * 4;
return [
Number(this.data.xquat[adr]),
Number(this.data.xquat[adr + 1]),
Number(this.data.xquat[adr + 2]),
Number(this.data.xquat[adr + 3]),
];
},
body_position: (bodyId) => {
if (bodyId < 0 || bodyId >= this.model.nbody) throw new Error(`Body ID 无效:${bodyId}`);
const adr = bodyId * 3;
return [
Number(this.data.xpos[adr]),
Number(this.data.xpos[adr + 1]),
Number(this.data.xpos[adr + 2]),
];
},
set_control: (actuatorId, value) => {
if (!Number.isFinite(value)) throw new Error(`控制输出不是有限数:${value}`);
this.setActuator(actuatorId, value);
},
}),
createStepApi: (time, dt) => {
const ticket = scope.ticket;
return {
time,
dt,
qpos: (jointId) => {
const joint = this.model.jnt(jointId);
try {
const type = Number(joint.type);
if (type !== 2 && type !== 3)
throw new Error(`关节 ${jointId} 不是标量 hinge/slide 关节`);
return Number(this.data.qpos[Number(joint.qposadr)]);
} finally {
joint.delete();
}
},
qvel: (jointId) => {
const joint = this.model.jnt(jointId);
try {
return Number(this.data.qvel[Number(joint.dofadr)]);
} finally {
joint.delete();
}
},
sensor: (sensorId) => {
if (sensorId < 0 || sensorId >= this.model.nsensor)
throw new Error(`传感器 ID 无效:${sensorId}`);
const adr = Number(this.model.sensor_adr[sensorId]),
dim = Number(this.model.sensor_dim[sensorId]);
return Array.from(this.data.sensordata.subarray(adr, adr + dim), Number);
},
body_quat: (bodyId) => {
if (bodyId < 0 || bodyId >= this.model.nbody)
throw new Error(`Body ID 无效:${bodyId}`);
const adr = bodyId * 4;
return [
Number(this.data.xquat[adr]),
Number(this.data.xquat[adr + 1]),
Number(this.data.xquat[adr + 2]),
Number(this.data.xquat[adr + 3]),
];
},
body_position: (bodyId) => {
if (bodyId < 0 || bodyId >= this.model.nbody)
throw new Error(`Body ID 无效:${bodyId}`);
const adr = bodyId * 3;
return [
Number(this.data.xpos[adr]),
Number(this.data.xpos[adr + 1]),
Number(this.data.xpos[adr + 2]),
];
},
set_control: (actuatorId, value) => {
if (!Number.isFinite(value)) throw new Error(`控制输出不是有限数:${value}`);
if (!this.disposed && this.pythonScope === scope && this.arbiter.owns(ticket))
this.writeActuator(actuatorId, value);
},
};
},
};
}
setActuator(id: number, value: number): void {
if (id < 0 || id >= this.model.nactuator) return;
if (this.arbiter.current().owner !== 'manual')
throw new RobotError('CONFLICT', '请先停止当前控制器,再手动控制执行器');
this.writeActuator(id, value);
}
private writeActuator(id: number, value: number): void {
if (!Number.isFinite(value)) throw new RobotError('INVALID_MESSAGE', '执行器目标必须为有限数');
if (this.disposed || id < 0 || id >= this.model.nactuator) return;
const actuator = this.model.actuator(id);
try {
const address = Number(this.model.actuator_ctrladr[id] ?? id),
@@ -580,6 +734,7 @@ export class SimulationSession {
}
setActuatorParameters(id: number, parameters: ActuatorParameters): boolean {
if (this.arbiter.current().owner !== 'manual' || this.robotAdapter) return false;
if (id < 0 || id >= this.model.nactuator) return false;
const finite = (value: number, fallback: number) => (Number.isFinite(value) ? value : fallback);
const ordered = (
@@ -654,6 +809,11 @@ export class SimulationSession {
}
setJointPosition(id: number, value: number): boolean {
if (this.arbiter.current().owner !== 'manual' || !Number.isFinite(value)) return false;
// Robot profiles use physical drives. Direct qpos writes teleport through
// colliders, even with perfect collision meshes. Keep pose editing for generic
// models; a configured robot is controlled via its associated actuators.
if (this.robotAdapter) return false;
if (id < 0 || id >= this.model.njnt) return false;
const joint = this.model.jnt(id);
try {
@@ -667,6 +827,7 @@ export class SimulationSession {
this.setPaused(true);
this.data.qpos[Number(joint.qposadr)] = next;
this.module.mj_forward(this.model, this.data);
this.robotRuntime?.capture(Number(this.data.time));
return true;
} finally {
joint.delete();
@@ -674,6 +835,10 @@ export class SimulationSession {
}
resetJoints(): void {
if (this.arbiter.current().owner !== 'manual')
throw new RobotError('CONFLICT', '请先停止当前控制器');
if (this.robotAdapter)
throw new RobotError('CONFLICT', '机器人 profile 不允许关节瞬移;请使用驱动器或完整仿真重置');
this.setPaused(true);
for (let id = 0; id < this.model.njnt; id += 1) {
const joint = this.model.jnt(id);
@@ -687,9 +852,14 @@ export class SimulationSession {
}
}
this.module.mj_forward(this.model, this.data);
this.robotRuntime?.capture(Number(this.data.time));
}
setIgnoreJointLimits(ignore: boolean): void {
if (this.arbiter.current().owner !== 'manual')
throw new RobotError('CONFLICT', '请先停止当前控制器');
if (this.robotAdapter)
throw new RobotError('CONFLICT', '机器人 profile 的关节限位不可修改;请使用物理驱动器控制');
this.ignoreJointLimits = ignore;
for (let id = 0; id < this.model.njnt; id += 1) {
const joint = this.model.jnt(id);
@@ -1005,7 +1175,10 @@ export class SimulationSession {
limitMax: original.max,
limited,
limitsIgnored: this.ignoreJointLimits,
editable: type === 2 || type === 3,
editable:
!this.robotAdapter &&
(type === 2 || type === 3) &&
this.arbiter.current().owner === 'manual',
bodyId: Number(joint.bodyid),
axis: [Number(joint.axis[0]), Number(joint.axis[1]), Number(joint.axis[2])],
};
@@ -1027,6 +1200,11 @@ export class SimulationSession {
});
return {
time: Number(this.data.time),
paused: this.paused,
speed: this.speed,
wasmHeapBytes: this.robotRuntime
? (this.data.qpos.buffer?.byteLength as number | undefined)
: undefined,
qpos: Array.from(this.data.qpos),
qvel: Array.from(this.data.qvel),
ctrl: Array.from(this.data.ctrl),
@@ -1036,6 +1214,9 @@ export class SimulationSession {
warnings: this.warnings,
controller: this.pythonController?.status(),
rlPolicy: this.rlPolicy?.status(),
controlOwner: this.arbiter.current().owner,
robot: this.describeRobot(),
externalControl: this.robotRuntime?.status(),
telemetry: this.dataRecorder.status(),
model: {
nbody: this.model.nbody,
@@ -1059,6 +1240,9 @@ export class SimulationSession {
console.warn(`[MuJoCo] 释放${label}失败`, error);
}
};
this.stopExternalClock();
this.arbiter.claim('manual');
cleanup('机器人接口', () => this.robotRuntime?.dispose());
cleanup('Python 控制器', () => this.removeController());
cleanup('RL 策略', () => this.removeRLPolicy());
cleanup('外力状态', () => this.clearExternalForce());
@@ -0,0 +1,55 @@
import { expect, test } from 'vitest';
import { BoxGeometry, Vector3 } from 'three';
import { surfaceDistance, type Surface } from '../../physics/meshDistance';
function box(x: number): Surface {
const geometry = new BoxGeometry(1, 1, 1);
const position = geometry.getAttribute('position'),
index = geometry.getIndex()!;
const result = {
vertices: Array.from({ length: position.count }, (_, i) =>
new Vector3().fromBufferAttribute(position, i).add(new Vector3(x, 0, 0)),
),
faces: Array.from({ length: index.count / 3 }, (_, i) =>
[0, 1, 2].map((c) => index.getX(i * 3 + c)),
),
};
geometry.dispose();
return result;
}
test('independent surface distance: separated, touching and intersecting boxes', () => {
for (const x of [0.2, 1, 1.001, 2]) {
const a = box(0),
b = box(x);
const distance = Math.max(0, x - 1);
expect(surfaceDistance(a, b).distance).toBeCloseTo(distance, 10);
expect(surfaceDistance(b, a).distance).toBeCloseTo(distance, 10);
}
});
test('skew edge/edge closest points and rigid-transform invariance', () => {
const a: Surface = {
vertices: [
[-1, 0, 0],
[1, 0, 0],
[-1, -1, 0],
].map((v) => new Vector3(...v)),
faces: [[0, 1, 2]],
};
const b: Surface = {
vertices: [
[0, -1, 1],
[0, 1, 1],
[1, 1, 2],
].map((v) => new Vector3(...v)),
faces: [[0, 1, 2]],
};
expect(surfaceDistance(a, b).distance).toBeCloseTo(1, 10);
for (const shape of [a, b])
for (const point of shape.vertices) {
point.applyAxisAngle(new Vector3(1, 2, 3).normalize(), 0.35).add(new Vector3(2, 3, 4));
}
expect(surfaceDistance(a, b).distance).toBeCloseTo(1, 10);
expect(() => surfaceDistance({ vertices: [], faces: [] }, b)).toThrow('有效三角形');
});

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