feat(lekiwi): release V0.10.1 初步集成 LeKiwi，优化碰撞模型
集成通用机器人数值接口、本机控制桥、LeRobot 插件和统一键盘遥操作。采用离线 CoACD 全臂碰撞配方 revision 4、局部装配区切分与结构自接触，限制直接关节位姿写入并保留安全看门狗。同步版本号、变更记录、来源许可证和兼容性验证。
This commit is contained in:
@@ -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('有效三角形');
|
||||
});
|
||||
@@ -0,0 +1,30 @@
|
||||
import { MuJoCoViewer } from './MuJoCoViewer';
|
||||
|
||||
it('toggles LeKiwi collision group 3 alongside ordinary collision group 0', () => {
|
||||
const geom = { name: 'jaw_visual', delete: vi.fn() };
|
||||
const viewer = Object.assign(Object.create(MuJoCoViewer.prototype), {
|
||||
session: { model: { ngeom: 1, geom_group: [1], geom: () => geom } },
|
||||
option: { geomgroup: new Uint8Array(6) },
|
||||
displayOptions: { showVisual: true, showCollision: false },
|
||||
visualMapLayer: { visible: true },
|
||||
showVisualMap: true,
|
||||
showMapCollision: false,
|
||||
mapHasVisuals: true,
|
||||
}) as {
|
||||
applyGeomVisibility(): void;
|
||||
option: { geomgroup: Uint8Array };
|
||||
displayOptions: { showVisual: boolean; showCollision: boolean };
|
||||
};
|
||||
viewer.applyGeomVisibility();
|
||||
expect(Array.from(viewer.option.geomgroup)).toEqual([0, 1, 0, 0, 0, 0]);
|
||||
viewer.displayOptions.showCollision = true;
|
||||
viewer.applyGeomVisibility();
|
||||
expect(Array.from(viewer.option.geomgroup)).toEqual([1, 1, 0, 1, 0, 0]);
|
||||
viewer.displayOptions.showVisual = false;
|
||||
viewer.applyGeomVisibility();
|
||||
expect(Array.from(viewer.option.geomgroup)).toEqual([1, 0, 0, 1, 0, 0]);
|
||||
viewer.displayOptions.showCollision = false;
|
||||
viewer.applyGeomVisibility();
|
||||
expect(viewer.option.geomgroup[3]).toBe(0);
|
||||
expect(geom.delete).toHaveBeenCalledTimes(4);
|
||||
});
|
||||
@@ -161,6 +161,13 @@ export class MuJoCoViewer {
|
||||
powerPreference: 'high-performance',
|
||||
});
|
||||
// 高 DPI 屏幕限制内部像素比，显著降低双视口与阴影的 GPU 填充压力。
|
||||
const gl = this.renderer.getContext(),
|
||||
debug = gl.getExtension('WEBGL_debug_renderer_info');
|
||||
this.softwareRendering = debug
|
||||
? /swiftshader|llvmpipe|softpipe|software/i.test(
|
||||
String(gl.getParameter(debug.UNMASKED_RENDERER_WEBGL)),
|
||||
)
|
||||
: false;
|
||||
this.renderer.setPixelRatio(Math.min(devicePixelRatio, 1.75));
|
||||
this.renderer.shadowMap.enabled = true;
|
||||
this.renderer.outputColorSpace = THREE.SRGBColorSpace;
|
||||
@@ -264,6 +271,9 @@ export class MuJoCoViewer {
|
||||
this.runtimeErrorReported = false;
|
||||
this.snapshotDirty = false;
|
||||
this.session = session;
|
||||
// External robot profiles prioritize control latency over shadow passes,
|
||||
// especially on software WebGL. This never changes physics/contact geometry.
|
||||
this.renderer.shadowMap.enabled = !this.softwareRendering || !session?.describeRobot();
|
||||
if (!session) return;
|
||||
for (let id = 0; id < session.model.ngeom; id++) {
|
||||
const geom = session.model.geom(id);
|
||||
@@ -474,6 +484,7 @@ export class MuJoCoViewer {
|
||||
this.option.geomgroup[1] = this.displayOptions.showVisual ? 1 : 0;
|
||||
this.option.geomgroup[2] =
|
||||
this.showMapCollision || (!this.mapHasVisuals && this.showVisualMap) ? 1 : 0;
|
||||
this.option.geomgroup[3] = this.displayOptions.showCollision ? 1 : 0;
|
||||
this.option.geomgroup[5] = 0;
|
||||
this.visualMapLayer.visible = this.showVisualMap && this.displayOptions.showVisual;
|
||||
}
|
||||
@@ -615,7 +626,19 @@ export class MuJoCoViewer {
|
||||
this.camera.updateProjectionMatrix();
|
||||
}
|
||||
|
||||
private readonly softwareRendering: boolean;
|
||||
private externalDrawAfter = 0;
|
||||
private animate = (now: number): void => {
|
||||
const external = this.session?.externalControlStatus()?.enabled ?? false;
|
||||
// Physics has its own realtime clock in external mode. Leave idle time for
|
||||
// socket/timer tasks when software WebGL blocks the main thread; rendering
|
||||
// is capped at 30fps (5fps with software rasterization). SwiftShader also
|
||||
// blocks outside renderer.render(), so its cooldown cannot use JS time alone.
|
||||
if (external && now < this.externalDrawAfter) {
|
||||
this.frame = requestAnimationFrame(this.animate);
|
||||
return;
|
||||
}
|
||||
const started = performance.now();
|
||||
try {
|
||||
const result = this.session?.advance(now) ?? { steps: 0, stepMs: 0, overBudget: false };
|
||||
this.updateThemeTransition(now);
|
||||
@@ -653,6 +676,14 @@ export class MuJoCoViewer {
|
||||
this.callbacks.onError(error instanceof Error ? error : new Error(String(error)));
|
||||
}
|
||||
}
|
||||
if (external) {
|
||||
const finished = performance.now();
|
||||
const interval = this.softwareRendering ? 200 : 1000 / 30;
|
||||
this.externalDrawAfter = Math.max(
|
||||
now + interval,
|
||||
finished + Math.min(finished - started, interval),
|
||||
);
|
||||
}
|
||||
this.frame = requestAnimationFrame(this.animate);
|
||||
};
|
||||
|
||||
|
||||
@@ -110,6 +110,9 @@ export default defineConfig({
|
||||
'src/project/urdfToMjcf.ts',
|
||||
'src/project/workspace.ts',
|
||||
'src/simulation/geometry.ts',
|
||||
'src/simulation/ModelBindings.ts',
|
||||
'src/simulation/ControlArbiter.ts',
|
||||
'src/robot/**/*.ts',
|
||||
'src/stores/useAppStore.ts',
|
||||
'src/training/LocalTrainingClient.ts',
|
||||
'src/viewer/interactionMath.ts',
|
||||
|
||||
Reference in New Issue
Block a user