feat(lekiwi): release V0.10.1 初步集成 LeKiwi,优化碰撞模型
集成通用机器人数值接口、本机控制桥、LeRobot 插件和统一键盘遥操作。采用离线 CoACD 全臂碰撞配方 revision 4、局部装配区切分与结构自接触,限制直接关节位姿写入并保留安全看门狗。同步版本号、变更记录、来源许可证和兼容性验证。
This commit is contained in:
@@ -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/
|
||||
|
||||
@@ -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
|
||||
@@ -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 局部坐标,每帧随部件平移和旋转更新,即使鼠标静止也持续跟随;保持现有外力方向/大小控制,补充位姿跟随与释放清理测试。
|
||||
|
||||
@@ -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 官方支持的产品。
|
||||
|
||||
@@ -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] }
|
||||
]
|
||||
}
|
||||
@@ -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"
|
||||
}
|
||||
]
|
||||
}
|
||||
@@ -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" }
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -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.
|
||||
@@ -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)。
|
||||
@@ -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
|
||||
@@ -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()
|
||||
@@ -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()
|
||||
@@ -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 不能冒充确定性锁步或高吞吐训练环境。
|
||||
@@ -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)。
|
||||
@@ -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()
|
||||
@@ -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()
|
||||
@@ -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
|
||||
@@ -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()
|
||||
@@ -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()
|
||||
@@ -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.
|
||||
@@ -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`。本包不授予额外的商标或硬件认证权利。
|
||||
@@ -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)。
|
||||
@@ -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"]
|
||||
@@ -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()
|
||||
@@ -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)
|
||||
Generated
+2
-2
@@ -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
@@ -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,2 +1,3 @@
|
||||
-r training_server/requirements.txt
|
||||
ruff==0.16.5
|
||||
-e ./control_bridge
|
||||
|
||||
@@ -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.
|
||||
@@ -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
@@ -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."
|
||||
}
|
||||
}
|
||||
@@ -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
@@ -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 启动本地桥接服务:
|
||||
|
||||
@@ -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());
|
||||
}
|
||||
});
|
||||
@@ -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());
|
||||
}
|
||||
});
|
||||
@@ -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());
|
||||
}
|
||||
});
|
||||
@@ -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();
|
||||
}
|
||||
});
|
||||
@@ -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());
|
||||
});
|
||||
@@ -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();
|
||||
}
|
||||
});
|
||||
@@ -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() };
|
||||
}
|
||||
@@ -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>
|
||||
@@ -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;
|
||||
@@ -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,
|
||||
},
|
||||
],
|
||||
});
|
||||
|
||||
@@ -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,
|
||||
},
|
||||
});
|
||||
@@ -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>
|
||||
);
|
||||
}
|
||||
@@ -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();
|
||||
});
|
||||
});
|
||||
@@ -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),
|
||||
);
|
||||
}
|
||||
});
|
||||
@@ -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 },
|
||||
};
|
||||
}
|
||||
@@ -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`,
|
||||
);
|
||||
}
|
||||
@@ -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;
|
||||
}
|
||||
@@ -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);
|
||||
});
|
||||
});
|
||||
@@ -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;
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -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(
|
||||
|
||||
@@ -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)}
|
||||
/>
|
||||
|
||||
@@ -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
Reference in New Issue
Block a user