commit aed6e20a908bae2116f8751ead3604ed65b505e0 Author: 陈霖 <836021389@qq.com> Date: Tue Sep 1 17:16:41 2026 +0800 双臂操作沙盒(MuJoCo WASM 浏览器版)首次入库 从 ~/mujoco-web-spike 抽出 manip.html 的依赖闭包: - manip.html + src/{manip,autograsp,scene-bridge,math}.js - scenes/so101_pair(桌面并排两台 SO-101,默认)与 scenes/p7_bimanual(P7 双臂 + SO-101 夹爪) —— 自包含 MJCF,网格已内联 - serve.py(COOP/COEP + no-store 的静态服务器)、start.sh、README - tools/export_manip_scene.py 场景生成脚本(仅供参考,跑页面不需要) 快照对应 2026-08-31 的最后一次改动(自动抓取规划 autograsp.js 落地版)。 Co-Authored-By: Claude Fable 5 Claude-Session: https://claude.ai/code/session_016tgfYxR5mWNRKQ67PKga4T diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..90f279a --- /dev/null +++ b/.gitignore @@ -0,0 +1,6 @@ +__pycache__/ +*.pyc +*.bak +MUJOCO_LOG.TXT +.DS_Store +release/ diff --git a/README.md b/README.md new file mode 100644 index 0000000..c386de5 --- /dev/null +++ b/README.md @@ -0,0 +1,77 @@ +# 双臂操作沙盒（浏览器 WASM 版） + +从 `~/mujoco-web-spike` 抽出来的**自包含**子集：只含 `manip.html` 跑起来需要的文件。 +物理引擎是 MuJoCo 的 WASM 构建，全部在浏览器里运行，服务器只负责发静态文件。 +版本快照：2026-08-31 最后一次改动（自动抓取规划 `autograsp.js` 落地那一版）。 + +## 启动 + +```bash +tar xzf wasm-bimanual-grasp-20260901.tar.gz +cd wasm-bimanual-grasp-20260901 +python3 serve.py 8000 # 或 ./start.sh 8000 +``` + +然后用 **Chrome / Edge** 打开： + +| 场景 | 地址 | +|---|---| +| 默认：桌面上并排两台 SO-101（5 自由度臂 + 二指夹爪） | http://127.0.0.1:8000/manip.html | +| P7 双臂工作站，末端换成 SO-101 夹爪（7 自由度臂） | http://127.0.0.1:8000/manip.html?scene=p7_bimanual | + +首屏会依次显示「载入 MuJoCo WASM → 下载场景 → 编译 MJCF」，编译要几秒（网格已内联， +so101_pair 1.8 MB / p7_bimanual 5.3 MB）。 + +### 环境要求 + +- **Python 3**（只用标准库 `http.server`，不需要 venv、不需要 pip 装任何东西） +- **能上网**：three.js 0.170.0 和 `@mujoco/mujoco` WASM 都从 jsDelivr CDN 拉，断网会白屏 +- **桌面版 Chrome / Edge**：页面用到 SharedArrayBuffer（多线程 WASM）， + `serve.py` 已经发好 `Cross-Origin-Opener-Policy` / `Cross-Origin-Embedder-Policy` 两个头 + +### 为什么不能直接 `python3 -m http.server` + +可以起来，但页面会**卡在「初始化 MuJoCo…」不报错也不动**—— +`@mujoco/mujoco` 是 pthread 构建，浏览器只在"跨源隔离"页面里放开 SharedArrayBuffer， +而 `http.server` 不发那两个头。`serve.py` 就是 `http.server` + 这两个头 + `Cache-Control: no-store` +（ES 模块缓存极死，改了 `src/*.js` 普通刷新照样跑旧代码，所以开发期一律不缓存）。 + +## 页面怎么用 + +- **拖动物体**（鼠标直接拖桌上的易拉罐/瓶子/罐子/玩具琴/玩偶）→ 右侧实时出**判定卡**： + 自动选哪只臂、下爪方向罗盘、左右两臂 IK 残差并排；两臂都够不着会明确给出「不可抓」 +- **自动抓取** 按钮：按判定结果执行 抵近 → 预收爪口 → 下探 → 合爪 → 抬升；**中止** 随时停 +- **手动模式**：选 左臂/右臂 → `X± Y± Z±` 步进末端 → 朝向约束（朝下/朝前/不约束）→ 张开/夹紧 +- 物体面板可改**位置和重量**，点 **应用** 生效（运行时改 `body_mass`，不重编译） +- **抓放记录**：物体离抓取点 < 9 cm 且高于原位 5 cm 并保持 0.5 s = 抓取成功； + 松手后静止落在台面 = 放置成功；掉到台下 = 失败 + +## 已知状态（08-31 实测） + +- 判定侧 6/6 准确（含正确拒绝够不着的情况） +- 执行侧 **3/5**：立着的易拉罐、罐子、玩偶稳定成功；**躺倒的瓶子和玩具琴还抓不住**。 + 根因是二指爪全开时爪尖比爪中点低 2.6 cm，下探先顶到桌面；已加「按物体宽度预收爪口」 + 但仍不够，尝试过的三种夹持点选择策略都没救回来，细节见 `src/autograsp.js` 顶部注释 +- **切到别的标签页仿真会暂停**（浏览器对后台标签的 rAF 节流），抓取过程中别切走 +- `@mujoco/mujoco` 走的是 CDN 上的 `+esm` 最新版（打包时 jsDelivr latest = 3.12.0）， + 没有钉版本；如果哪天 CDN 升级后页面报 `xxx is not a function`，先把 `manip.html` 里的 + `@mujoco/mujoco/+esm` 改成 `@mujoco/mujoco@3.12.0/+esm` 试 + +## 文件 + +``` +manip.html 页面 + 启动编排（渲染、UI、事件） +serve.py 静态服务器（COOP/COEP + no-store） +start.sh python3 serve.py 的包装，顺带打印地址 +src/manip.js ArmController（阻尼最小二乘 IK、零空间次要目标）、GraspMonitor（抓放判定） +src/autograsp.js GraspPlanner：拖动后实时判定选臂/下爪方向，自动抓取状态机 +src/scene-bridge.js MuJoCo mjModel/mjData → three.js 网格与每帧位姿 +src/math.js 旋转矩阵 → 四元数 +scenes/so101_pair/ 默认场景：model.xml（自包含 MJCF）+ meta.json（臂/爪/物体索引） +scenes/p7_bimanual/ P7 双臂场景，同上 +tools/export_manip_scene.py ★ 仅供参考：生成上面两个场景的脚本， + 依赖 ~/unitree_rl_mjlab/.venv（mujoco、numpy、tyro）+ + linker-sim 资产库 + SO-101 MJCF，跑页面不需要它 +``` + +完整项目（其它页面 arm/sort/policy/urdf 等）仍在 `~/mujoco-web-spike`，本包只是双臂抓取这一条线。 diff --git a/manip.html b/manip.html new file mode 100644 index 0000000..ce505be --- /dev/null +++ b/manip.html @@ -0,0 +1,885 @@ + + +双臂操作沙盒 + + + + + + + + + + + +
双臂操作沙盒加载中…
+ + diff --git a/scenes/p7_bimanual/meta.json b/scenes/p7_bimanual/meta.json new file mode 100644 index 0000000..92ee21c --- /dev/null +++ b/scenes/p7_bimanual/meta.json @@ -0,0 +1 @@ +{"name": "p7_bimanual", "workstation": "p7_i1_l6_bimanual", "gripper": "so101", "sim": {"timestep": 0.002, "substeps": 10}, "nq": 53, "nu": 16, "ngeom": 156, "kps": [2000.0, 2000.0, 2000.0, 2000.0, 2000.0, 2000.0, 2000.0, 2000.0, 2000.0, 2000.0, 2000.0, 2000.0, 2000.0, 2000.0, 998.22, 998.22], "arms": {"left": {"act": [0, 1, 2, 3, 4, 5, 6], "names": ["arm_left_L1_Joint_act", "arm_left_L2_Joint_act", "arm_left_L3_Joint_act", "arm_left_L4_Joint_act", "arm_left_L5_Joint_act", "arm_left_L6_Joint_act", "arm_left_L7_Joint_act"], "qadr": [0, 1, 2, 3, 4, 5, 6], "range": [[-2.91, 2.91], [-3.2, 0.07], [-2.69, 2.69], [-2.05, 2.05], [-2.69, 2.69], [-1.59, 1.59], [-1.59, 1.59]], "site": "arm_left_tool0", "site_id": 2, "ready": [0.1580918164168783, 0.07, -0.039073152159846636, 1.2304709832679805, -0.06891315610756783, 0.4398884953429942, 0.04790742363418622]}, "right": {"act": [7, 8, 9, 10, 11, 12, 13], "names": ["arm_right_R1_Joint_act", "arm_right_R2_Joint_act", "arm_right_R3_Joint_act", "arm_right_R4_Joint_act", "arm_right_R5_Joint_act", "arm_right_R6_Joint_act", "arm_right_R7_Joint_act"], "qadr": [9, 10, 11, 12, 13, 14, 15], "range": [[-2.91, 2.91], [-0.07, 3.2], [-2.69, 2.69], [-2.05, 2.05], [-2.69, 2.69], [-1.59, 1.59], [-1.59, 1.59]], "site": "arm_right_tool0", "site_id": 6, "ready": [-0.16105633538148345, -0.07, 0.03882218199725755, -1.2175915928407415, 0.06952877520509076, -0.46856305129613646, -0.045417679190592676]}}, "hands": {"left": {"kind": "so101", "act": [14], "names": ["grip_left_act"], "range": [[0.0, 0.0]], "geoms": [21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 34, 35, 37, 38, 39, 40, 41, 42, 43, 44], "jaw_a": [24, 25, 26], "jaw_b": [40, 41, 42], "palm_body": 13, "site": "grip_left_gripperframe", "site_id": 3}, "right": {"kind": "so101", "act": [15], "names": ["grip_right_act"], "range": [[0.0, 0.0]], "geoms": [63, 64, 65, 66, 67, 68, 69, 70, 71, 72, 73, 74, 76, 77, 79, 80, 81, 82, 83, 84, 85, 86], "jaw_a": [66, 67, 68], "jaw_b": [82, 83, 84], "palm_body": 26, "site": "grip_right_gripperframe", "site_id": 7}}, "scene": {"table": {"x": 0.42, "top": 0.75, "half": [0.28, 0.42, 0.02]}, "objects": [{"name": "can", "label": "易拉罐", "body": "obj_can", "geom": "obj_can", "kind": "mesh", "mass": 0.15, "half_h": 0.0285347697145599, "pieces": 20, "home": [0.39999999999999997, -0.26, 0.7805347697145599], "qadr": 18, "body_id": 30, "geom_id": 93}, {"name": "bottle", "label": "瓶子", "body": "obj_bottle", "geom": "obj_bottle", "kind": "mesh", "mass": 0.12, "half_h": 0.050020083385326444, "pieces": 3, "home": [0.39999999999999997, -0.13, 0.8020200833853265], "qadr": 25, "body_id": 31, "geom_id": 114}, {"name": "jar", "label": "罐子", "body": "obj_jar", "geom": "obj_jar", "kind": "mesh", "mass": 0.16, "half_h": 0.02482021259712402, "pieces": 3, "home": [0.39999999999999997, 0.0, 0.776820212597124], "qadr": 32, "body_id": 32, "geom_id": 118}, {"name": "piano", "label": "玩具琴", "body": "obj_piano", "geom": "obj_piano", "kind": "mesh", "mass": 0.12, "half_h": 0.014234144039705122, "pieces": 30, "home": [0.39999999999999997, 0.13, 0.7662341440397051], "qadr": 39, "body_id": 33, "geom_id": 122}, {"name": "toy", "label": "玩偶", "body": "obj_toy", "geom": "obj_toy", "kind": "mesh", "mass": 0.1, "half_h": 0.02, "pieces": 2, "home": [0.39999999999999997, 0.26, 0.772], "qadr": 46, "body_id": 34, "geom_id": 153}]}, "xmlMB": 5.31} \ No newline at end of file diff --git a/scenes/p7_bimanual/model.xml b/scenes/p7_bimanual/model.xml new file mode 100644 index 0000000..87f5926 --- /dev/null +++ b/scenes/p7_bimanual/model.xml @@ -0,0 +1,624 @@ + + + + diff --git a/scenes/so101_pair/meta.json b/scenes/so101_pair/meta.json new file mode 100644 index 0000000..2ce8ebc --- /dev/null +++ b/scenes/so101_pair/meta.json @@ -0,0 +1 @@ +{"name": "so101_pair", "workstation": "p7_i1_l6_bimanual", "gripper": "so101", "sim": {"timestep": 0.002, "substeps": 10}, "nq": 47, "nu": 12, "ngeom": 166, "kps": [998.22, 998.22, 998.22, 998.22, 998.22, 998.22, 998.22, 998.22, 998.22, 998.22, 998.22, 998.22], "arms": {"left": {"act": [0, 1, 2, 3, 4], "names": ["arm_left_shoulder_pan", "arm_left_shoulder_lift", "arm_left_elbow_flex", "arm_left_wrist_flex", "arm_left_wrist_roll"], "qadr": [0, 1, 2, 3, 4], "range": [[-1.91986, 1.91986], [-1.74533, 1.74533], [-1.69, 1.69], [-1.65806, 1.65806], [-2.74385, 2.84121]], "site": "arm_left_gripperframe", "site_id": 1, "ready": [0.2664159465395377, -0.053277895226272, 0.09506432223630396, 1.5261345315220791, -0.009229323360655772]}, "right": {"act": [6, 7, 8, 9, 10], "names": ["arm_right_shoulder_pan", "arm_right_shoulder_lift", "arm_right_elbow_flex", "arm_right_wrist_flex", "arm_right_wrist_roll"], "qadr": [6, 7, 8, 9, 10], "range": [[-1.91986, 1.91986], [-1.74533, 1.74533], [-1.69, 1.69], [-1.65806, 1.65806], [-2.74385, 2.84121]], "site": "arm_right_gripperframe", "site_id": 3, "ready": [-0.2697369618426545, -0.05331271527672901, 0.09510641744154119, 1.5261271765003104, 0.01303107961674866]}}, "hands": {"left": {"kind": "so101", "act": [5], "names": ["arm_left_gripper"], "range": [[-0.17453, 1.74533]], "geoms": [12, 14, 17, 19, 21, 23, 26, 28, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 44, 45, 47, 48, 49, 50, 51, 52, 53, 54], "jaw_a": [34, 35, 36], "jaw_b": [50, 51, 52], "palm_body": 7, "site_id": 1}, "right": {"kind": "so101", "act": [11], "names": ["arm_right_gripper"], "range": [[-0.17453, 1.74533]], "geoms": [60, 62, 65, 67, 69, 71, 74, 76, 79, 80, 81, 82, 83, 84, 85, 86, 87, 88, 89, 90, 92, 93, 95, 96, 97, 98, 99, 100, 101, 102], "jaw_a": [82, 83, 84], "jaw_b": [98, 99, 100], "palm_body": 15, "site_id": 3}}, "scene": {"table": {"x": 0.15, "top": 0.4, "half": [0.3, 0.4, 0.02]}, "objects": [{"name": "can", "label": "易拉罐", "body": "obj_can", "geom": "obj_can", "kind": "mesh", "mass": 0.15, "half_h": 0.0285347697145599, "pieces": 20, "home": [0.27, -0.2, 0.43053476971455995], "qadr": 12, "body_id": 18, "geom_id": 103}, {"name": "bottle", "label": "瓶子", "body": "obj_bottle", "geom": "obj_bottle", "kind": "mesh", "mass": 0.12, "half_h": 0.050020083385326444, "pieces": 3, "home": [0.27, -0.1, 0.45202008338532645], "qadr": 19, "body_id": 19, "geom_id": 124}, {"name": "jar", "label": "罐子", "body": "obj_jar", "geom": "obj_jar", "kind": "mesh", "mass": 0.16, "half_h": 0.02482021259712402, "pieces": 3, "home": [0.27, 0.0, 0.42682021259712405], "qadr": 26, "body_id": 20, "geom_id": 128}, {"name": "piano", "label": "玩具琴", "body": "obj_piano", "geom": "obj_piano", "kind": "mesh", "mass": 0.12, "half_h": 0.014234144039705122, "pieces": 30, "home": [0.27, 0.1, 0.41623414403970516], "qadr": 33, "body_id": 21, "geom_id": 132}, {"name": "toy", "label": "玩偶", "body": "obj_toy", "geom": "obj_toy", "kind": "mesh", "mass": 0.1, "half_h": 0.02, "pieces": 2, "home": [0.27, 0.2, 0.42200000000000004], "qadr": 40, "body_id": 22, "geom_id": 163}]}, "xmlMB": 1.78} \ No newline at end of file diff --git a/scenes/so101_pair/model.xml b/scenes/so101_pair/model.xml new file mode 100644 index 0000000..b62836f --- /dev/null +++ b/scenes/so101_pair/model.xml @@ -0,0 +1,462 @@ + + + + diff --git a/serve.py b/serve.py new file mode 100644 index 0000000..ae98998 --- /dev/null +++ b/serve.py @@ -0,0 +1,31 @@ +#!/usr/bin/env python3 +""" +跟 `python3 -m http.server` 一样零依赖，只多发两个头： +Cross-Origin-Opener-Policy: same-origin + Cross-Origin-Embedder-Policy: require-corp。 + +@mujoco/mujoco 这个 WASM 包是多线程（pthread）构建，启动时要用 SharedArrayBuffer， +而浏览器只在"跨源隔离"（cross-origin isolated）页面里才允许用 SharedArrayBuffer。 +不发这两个头的话，WASM 会卡在 wasm-instantiate 那一步一直转，既不报错也不失败， +表现就是"初始化 MuJoCo…"卡死不动——console 里唯一线索是反复打印 +"still waiting on run dependencies: dependency: wasm-instantiate"。 +jsDelivr 给这个包的资源都带 Cross-Origin-Resource-Policy: cross-origin， +所以跨源隔离页面照样能从 CDN 拉东西，不会被 COEP 卡住。 +""" +import http.server +import sys + + +class Handler(http.server.SimpleHTTPRequestHandler): + def end_headers(self): + self.send_header('Cross-Origin-Opener-Policy', 'same-origin') + self.send_header('Cross-Origin-Embedder-Policy', 'require-corp') + # 开发中的资源一律不缓存。ES 模块缓存得特别死:改完 src/*.js 普通刷新 + # 照样跑旧代码,而且**不报错**,只在调用新方法时抛 + # "xxx is not a function" —— 极难往缓存上想,已经浪费过一轮排查。 + self.send_header('Cache-Control', 'no-store, must-revalidate') + super().end_headers() + + +if __name__ == '__main__': + port = int(sys.argv[1]) if len(sys.argv) > 1 else 8000 + http.server.test(HandlerClass=Handler, port=port) diff --git a/src/autograsp.js b/src/autograsp.js new file mode 100644 index 0000000..9e3c5bc --- /dev/null +++ b/src/autograsp.js @@ -0,0 +1,411 @@ +/** + * 自动抓取:量物体形状 → 定夹持方向 → 判哪只手够得着 → 执行。 + * + * 三件事都是**量出来**的,不是猜的: + * + * 1. **夹持方向**:把物体的网格顶点变到世界系,在腰部高度扫一圈, + * 算出各个水平方向上的宽度,取最窄的那个方向下爪。 + * 站立的圆柱各向异性≈1.00(哪个方向都行),推倒后跳到 2.69 + * (最窄方向自动垂到瓶身上)。所以"站立还是摔倒"不需要单独判, + * 量宽度就自然分开了 —— 而且对本来就扁的物体(玩偶 2.47)一样管用。 + * + * 2. **腕滚转**:wrist_roll 和爪口水平角是 **1:1** 的(实测 Δroll 0.5 rad + * ↔ 爪口转 28.6° = 0.5 rad),所以需要转多少可以直接算。 + * 但**两爪中点不在滚转轴上**(SO-101 是一动一定的二指爪,偏移量随位形变), + * 所以不能"解完位置再硬掰滚转" —— 实测易拉罐那一下位置残差从 0.3 mm + * 崩到 54.6 mm。得交替:对齐 → 冻住滚转把位置找回来 → 再对齐。 + * + * 3. **不约束爪口朝向**。臂只有 5 个自由度,"位置3+朝下2+偏航1"是 6 个约束, + * 数学上就解不全。放开朝向后任务只剩位置 3 个,富余 2 个自由度, + * 偏航才有地方安放。爪口从哪个角度插进去由 IK 自己挑。 + * + * 4. **判据是宽度不是角度**:看沿**实际**爪口方向的物体宽度夹不夹得下, + * 而不是要求爪口必须对准最窄方向。轴对称的物体(立着的易拉罐 1.01) + * 哪个方向都行,硬要求对齐是白白拿位置精度去换。 + */ + +const D2R = Math.PI / 180; + +/** 字典序比较两个数值数组:a 严格小于 b 吗。 */ +function lexLess(a, b) { + for (let i = 0; i < a.length; i++) { + if (a[i] !== b[i]) return a[i] < b[i]; + } + return false; +} + +/** 物体形状探针:从网格顶点算世界系下的高度和各方向宽度。 */ +export class ShapeProbe { + constructor(model, data) { + this.model = model; + this.data = data; + this._geoms = new Map(); + } + + /** 一个 body 的全部 geom —— 凸分解后一个物体有好几片(易拉罐 20 片)。 */ + geomsOf(bodyId) { + if (!this._geoms.has(bodyId)) { + const g = []; + for (let i = 0; i < this.model.ngeom; i++) { + if (this.model.geom_bodyid[i] === bodyId) g.push(i); + } + this._geoms.set(bodyId, g); + } + return this._geoms.get(bodyId); + } + + /** 全部网格顶点变到世界系。编译期 mesh_pos/quat 已烘进顶点,所以直接用 geom 位姿。 */ + worldVerts(bodyId) { + const { model: m, data: d } = this; + const out = []; + for (const g of this.geomsOf(bodyId)) { + const mid = m.geom_dataid ? m.geom_dataid[g] : -1; + if (mid < 0) continue; + const va = m.mesh_vertadr[mid], vn = m.mesh_vertnum[mid]; + const px = d.geom_xpos[3 * g], py = d.geom_xpos[3 * g + 1], pz = d.geom_xpos[3 * g + 2]; + const R = d.geom_xmat, o = 9 * g; + for (let k = 0; k < vn; k++) { + const x = m.mesh_vert[3 * (va + k)], + y = m.mesh_vert[3 * (va + k) + 1], + z = m.mesh_vert[3 * (va + k) + 2]; + out.push([ + px + R[o] * x + R[o + 1] * y + R[o + 2] * z, + py + R[o + 3] * x + R[o + 4] * y + R[o + 5] * z, + pz + R[o + 6] * x + R[o + 7] * y + R[o + 8] * z, + ]); + } + } + return out; + } + + /** + * 形状剖面。宽度只用**腰部**那一段顶点算 —— 夹爪是夹腰不是夹顶盖, + * 拿整体 AABB 会把瓶口瓶底一起算进去,方向就偏了。 + */ + profile(bodyId, step = 5) { + const V = this.worldVerts(bodyId); + if (V.length < 8) return null; + let zmin = Infinity, zmax = -Infinity; + for (const v of V) { if (v[2] < zmin) zmin = v[2]; if (v[2] > zmax) zmax = v[2]; } + const h = zmax - zmin, zc = (zmin + zmax) / 2; + const band = V.filter((v) => Math.abs(v[2] - zc) < h * 0.3); + const use = band.length > 20 ? band : V; + let cx = 0, cy = 0; + for (const v of use) { cx += v[0]; cy += v[1]; } + cx /= use.length; cy /= use.length; + + const widths = []; + for (let deg = 0; deg < 180; deg += step) { + const a = deg * D2R, ux = Math.cos(a), uy = Math.sin(a); + let lo = Infinity, hi = -Infinity; + for (const v of use) { + const p = (v[0] - cx) * ux + (v[1] - cy) * uy; + if (p < lo) lo = p; + if (p > hi) hi = p; + } + widths.push({ deg, w: hi - lo }); + } + const min = widths.reduce((b, c) => (c.w < b.w ? c : b)); + const max = widths.reduce((b, c) => (c.w > b.w ? c : b)); + return { + n: V.length, zmin, zmax, h, zc, cx, cy, widths, band: use, + minDeg: min.deg, minW: min.w, maxW: max.w, + aniso: max.w / Math.max(min.w, 1e-6), + }; + } + + /** + * 物体沿任意**三维**方向的尺寸。 + * 不能只量水平投影:放开朝向后 IK 会解出两爪上下合拢的位形(实测爪口轴 + * [0.02,0.18,0.98],水平度只有 0.18),这时"水平宽度"根本不是夹持宽度, + * 拿它判断会给出假阳性。 + */ + extentAlong(bodyId, axis) { + const V = this.worldVerts(bodyId); + const n = Math.hypot(axis[0], axis[1], axis[2]) || 1; + const u = [axis[0] / n, axis[1] / n, axis[2] / n]; + let lo = Infinity, hi = -Infinity; + for (const v of V) { + const p = v[0] * u[0] + v[1] * u[1] + v[2] * u[2]; + if (p < lo) lo = p; + if (p > hi) hi = p; + } + return hi - lo; + } + + /** 一组顶点沿某方向的尺寸 + 中心。夹持宽度只量腰部那一段时用它。 */ + spanOf(V, axis) { + const n = Math.hypot(axis[0], axis[1], axis[2]) || 1; + const u = [axis[0] / n, axis[1] / n, axis[2] / n]; + let lo = Infinity, hi = -Infinity; + for (const v of V) { + const p = v[0] * u[0] + v[1] * u[1] + v[2] * u[2]; + if (p < lo) lo = p; + if (p > hi) hi = p; + } + return { w: hi - lo, c: (lo + hi) / 2, lo, hi }; + } + + /** + * 姿态描述。两条**互相独立**的事实,分开报: + * · 截面是不是轴对称 —— 决定爪口方向要不要对齐; + * · 是立着还是横躺 —— 只是给人看的,不影响下爪方向。 + */ + posture(pr) { + const axisym = pr.aniso < 1.15; + const lying = pr.h < pr.maxW * 0.6; + return { + axisym, lying, + label: `${axisym ? '轴对称' : '有朝向'} · ${lying ? '横卧' : '直立'}`, + }; + } +} + +/** + * 抓取规划 + 执行。 + * plan() 纯计算不动仿真;run() 才真的驱动手臂。 + */ +export class GraspPlanner { + constructor(model, data, meta, arms, monitor) { + this.arms = arms; + this.monitor = monitor; + this.shape = new ShapeProbe(model, data); + this.table = meta.scene.table; + this.cfg = { + reachErr: 0.012, // 可达判据:IK 残差 < 12 mm + approach: 'free', // 不约束爪口朝向:见 _solveFrom 里的说明 + jawMargin: 0.012, // 物体最窄处要比爪口开度小这么多才敢夹 + clearance: 0.022, // 下探时爪口比物体宽出这么多(别张到最大,见 jawGeom) + floorGap: 0.004, // 爪尖离桌面至少留这么多 + hover: 0.085, // 悬停高度(物体腰部之上) + touch: 0.002, // 下探到腰部再往下一点 + lift: 0.15, // 抬起高度 + }; + this._cancel = false; + } + + /** 爪口最大开度,量一次即可(实测 SO-101 约 10.1 cm)。 */ + jawSpan(side = 'left') { + if (this._span != null) return this._span; + const arm = this.arms[side]; + this._span = arm.jawSpan(); + return this._span; + } + + /** 沿某个水平方向(弧度)的物体宽度,查剖面表线性插值。爪口 π 对称。 */ + widthAt(pr, rad) { + let deg = ((rad / D2R) % 180 + 180) % 180; + const step = 180 / pr.widths.length; + const i = Math.floor(deg / step), f = deg / step - i; + const a = pr.widths[i % pr.widths.length].w; + const b = pr.widths[(i + 1) % pr.widths.length].w; + return a + (b - a) * f; + } + + /** + * 规划:算夹持点,再问两只手够不够得着、爪口方向合不合适。 + * + * 判据是**沿实际爪口方向的宽度夹不夹得下**,而不是"爪口角必须等于最窄方向"。 + * 这两件事差很多:易拉罐立着是轴对称的(各向异性 1.01),哪个方向下爪都行, + * 硬要求对齐纯属自找麻烦 —— 5 自由度的臂满足不了"位置+朝下+偏航"6 个约束, + * 逼它对齐就得拿位置精度去换(实测能从 0.3 mm 崩到 54.6 mm)。 + * 所以先自由解,量一下这么夹宽度够不够;不够才动腕滚转,并接受它带来的位置代价。 + * + * fast=true 走少起点、不算悬停点,给拖动时的实时反馈用(约 8 ms); + * 松手后再跑完整版(约 40 ms)。 + */ + plan(obj, { fast = false } = {}) { + const pr = this.shape.profile(obj.body_id); + if (!pr) return { ok: false, obj, reason: '取不到物体网格' }; + const post = this.shape.posture(pr); + const span = this.jawSpan(); + const usable = span - this.cfg.jawMargin; + const tries = fast ? 2 : 5; + const TOP = this.table.top; + const theta0 = pr.minDeg * D2R; + const sVerts = pr.band; + + // ① 夹持点 = 腰部顶点质心。 + // + // 试过换成"外廓中点"(两侧最外缘的正中),动机是躺倒的瓶子有细颈、 + // 质心被拽偏 1.5 cm,而爪口半开 3.1 cm、物体半宽 1.9 cm, + // 1.5+1.9=3.4 > 3.1 正好擦过去。但**实测更差**:6 例从 3/5 掉到 2/5, + // 玩偶从成功变失败。又试过"两个锚点都探、按 IK 残差和夹持宽度挑", + // 还是 2/5 —— 说明**这两个量预测不了抓取成败**,挑不出来。 + // 所以退回质心。要再动这里,得先找到一个真能预测成败的判据 + // (方向:算合爪时两爪与物体的实际接触/包络,而不是只看中心对不对齐)。 + const anchors = [{ tag: '质心', x: pr.cx, y: pr.cy }]; + + // ② 抓取高度:默认腰部,**矮物体往上抬**到爪尖刚好清桌面(上限 70% 高度, + // 再往上就夹到顶沿了)。 + // 注意:加了 preGrip(下探前预收爪口)之后爪尖只低于爪中点 0.3~1.1 cm, + // 这五个物体的腰部都已经够高,**这段抬高实测从没触发过**。 + // 留着是为了换更矮的物体时兜底,以及给 lowOk 提供 drop/room 这两个数。 + const armA = Object.values(this.arms)[0]; + const dropSeed = armA.jawGeom( + armA.gripToVal(armA.gripForSpan(Math.min(pr.minW + this.cfg.clearance, span)))).drop; + const zNeed = TOP + dropSeed + this.cfg.floorGap; + const zCap = pr.zmin + 0.70 * Math.max(pr.h, 1e-4); + const zc = Math.min(Math.max(pr.zc, zNeed), Math.max(zCap, pr.zc)); + const raised = zc > pr.zc + 1e-4; + + const base = { obj, pr, post, span, usable, fast, raised, + zRaw: pr.zc, zNeed, zCap, + theta: theta0, width: pr.minW, aniso: pr.aniso }; + + if (pr.minW > usable) { + return { ...base, ok: false, arms: [], + reason: `太宽:最窄处 ${(pr.minW * 100).toFixed(1)} cm,` + + `爪口只有 ${(span * 100).toFixed(1)} cm` }; + } + + // 夹持宽度沿**真实的三维爪口轴**量,不是量水平投影 —— 见 extentAlong 的说明。 + // 宽度只量**要夹的那一片**的顶点。拿整个物体量会把长轴那一头也算进来, + // 躺倒的瓶子会量成 10 cm 而实际只需夹住 4 cm 的那一截。 + const gripWidth = (r) => (r.jaw + ? (sVerts ? this.shape.spanOf(sVerts, r.jaw).w + : this.shape.extentAlong(obj.body_id, r.jaw)) + : this.widthAt(pr, r.yaw)); + const horizOf = (r) => (r.jaw ? Math.hypot(r.jaw[0], r.jaw[1]) : 1); + + const evalAnchor = (grasp, hover) => + Object.entries(this.arms).map(([side, arm]) => { + // ① 自由解:不管爪口朝哪,先看位置够不够得着 + const g0 = arm.probeAt(grasp, null, tries, this.cfg.approach); + let use = { ...g0, theta: null, rolled: false }; + let w = gripWidth(g0); + // ② 有朝向的物体一律再试一次"对准最窄方向",不能等到塞不下才试 —— + // 玩偶自由解夹 8.4 cm(勉强塞得下 8.9 cm 的爪口),转腕后只要 3.8 cm, + // 留的余量差一倍多,夹持稳定性完全不同。 + if (w > usable || pr.aniso > 1.2) { + const g1 = arm.probeAt(grasp, pr.minDeg * D2R, tries, this.cfg.approach); + const w1 = gripWidth(g1); + // 窄 1 cm 以上才值得转腕(转腕会牺牲一点位置精度) + if (w1 < w - 0.01 && g1.err < this.cfg.reachErr) { + use = { ...g1, theta: pr.minDeg * D2R, rolled: true }; w = w1; + } + } + const hp = fast ? use : arm.probeAt(hover, use.theta, tries, this.cfg.approach); + const hv = hp.err; + const reach = Math.max(use.err, hv); + const horiz = horizOf(use); + // 下探用的开口:比物体宽出 clearance 就够,**不要张到最大**。 + // 张最大时爪尖伸得最低(2.61 cm),下探时先撞桌面,爪中点被顶在 + // 物体上方 1.5~2 cm,合爪等于在空中夹 —— 矮物体全败在这一条上。 + const preGrip = arm.gripForSpan(Math.min(w + this.cfg.clearance, span), use.q); + const drop = arm.jawGeom(arm.gripToVal(preGrip), use.q).drop; + const room = grasp[2] - this.table.top; // 腰部离桌面多高 + return { side, err: use.err, hoverErr: hv, reach, yaw: use.yaw, + qGrasp: use.q, qHover: hp.q, preGrip, drop, room, + gripW: w, horiz, rolled: use.rolled, roll: use.roll, + fits: w <= usable, flat: horiz >= 0.85, + lowOk: room >= drop + 0.002, + ok: reach < this.cfg.reachErr && w <= usable && horiz >= 0.85 + && room >= drop + 0.002 }; + }); + + // 两个候选点各探一遍,挑"能抓 > 残差小 > 夹持更窄"的那个 + let best = null; + for (const A of anchors) { + const grasp = [A.x, A.y, zc]; + const hover = [A.x, A.y, zc + this.cfg.hover]; + const cs = evalAnchor(grasp, hover); + cs.sort((a, b) => (a.ok === b.ok ? a.reach - b.reach : a.ok ? -1 : 1)); + const p0 = cs.find((c) => c.ok); + // 排序键:能抓的排前面 → 残差小 → 夹持更窄。 + // 按字典序逐项比 —— 别用数组直接 <,JS 会退化成字符串比较。 + const key = [p0 ? 0 : 1, p0 ? p0.reach : cs[0].reach, p0 ? p0.gripW : 9]; + if (!best || lexLess(key, best.key)) { + best = { key, cands: cs, pick: p0, grasp, hover, anchor: A.tag }; + } + } + const cands = best.cands, pick = best.pick; + const grasp = best.grasp, hover = best.hover; + Object.assign(base, { grasp, hover, anchor: best.anchor }); + + if (!pick) { + const why = cands.map((c) => { + const nm = c.side === 'left' ? '左臂' : '右臂'; + if (c.reach >= this.cfg.reachErr) return `${nm} 差 ${(c.reach * 1000).toFixed(0)} mm`; + if (!c.flat) return `${nm} 只能上下夹(下爪会插到桌面里)`; + if (!c.lowOk) return `${nm} 物体太矮(抓取点离桌面 ${(c.room*100).toFixed(1)} cm,` + + `爪尖要 ${(c.drop*100).toFixed(1)} cm 净空)`; + return `${nm} 那个方向 ${(c.gripW * 100).toFixed(1)} cm 夹不下`; + }).join(','); + return { ...base, ok: false, arms: cands, reason: `抓不了:${why}` }; + } + return { ...base, ok: true, arms: cands, side: pick.side, + qGrasp: pick.qGrasp, qHover: pick.qHover, + preGrip: pick.preGrip, drop: pick.drop, room: pick.room, + err: pick.err, theta: pick.rolled ? base.theta : pick.yaw, + gripW: pick.gripW, rolled: pick.rolled, rollNeed: pick.roll, + reason: `${pick.side === 'left' ? '左臂' : '右臂'}可达` + + `(残差 ${(pick.err * 1000).toFixed(0)} mm,` + + `夹 ${(pick.gripW * 100).toFixed(1)} cm` + + `${pick.rolled ? ',需转腕' : ''})` }; + } + + cancel() { this._cancel = true; } + + _wait(ms) { + return new Promise((r) => setTimeout(r, ms)); + } + + /** + * 执行:悬停 → 下探 → 合爪 → 抬起。 + * 每一步都把 target/roll/grip 设好后让主循环自己跑(不在这里推物理), + * 这样限速、IK 热启动、抓放判定全都走原来的路径。 + */ + async run(p, hooks = {}) { + if (!p.ok) return { ok: false, reason: p.reason }; + this._cancel = false; + const arm = this.arms[p.side]; + const C = this.cfg; + const g = p.grasp; + const prevAp = arm.approach; + arm.approach = this.cfg.approach; // 'free':朝向交给 IK 自己挑 + + const theta = p.rolled ? p.theta : null; + // 走**笛卡尔跟踪**:只移动目标点,位形由 IK 每步重解。 + // 试过直接给规划好的关节位形(qGoal),更差 —— 关节空间直插会让爪子 + // 扫过物体本身,原本能抓的两例都被打掉了。笛卡尔路径是平滑的,不会。 + const go = async (z, grip, ms, phase) => { + if (this._cancel) throw new Error('已取消'); + arm.target[0] = g[0]; arm.target[1] = g[1]; arm.target[2] = z; + arm.roll = theta; + arm.grip = grip; + hooks.phase?.(phase); + await this._wait(ms); + if (this._cancel) throw new Error('已取消'); + }; + + try { + const pre = p.preGrip ?? 0; // 预收爪口,别张到最大 + await go(g[2] + C.hover, pre, 1500, '悬停'); + await go(g[2] + C.touch, pre, 1500, '下探'); + await go(g[2] + C.touch, 1, 1100, '合爪'); + await go(g[2] + C.lift, 1, 2000, '抬起'); + hooks.phase?.('确认'); + await this._wait(1400); + arm.roll = null; + } catch (e) { + arm.roll = null; + arm.approach = prevAp; + hooks.phase?.(null); + return { ok: false, reason: e.message }; + } + + const o = p.obj; + hooks.phase?.(null); + const held = o.state === 'held'; + return { + ok: held, + lift: o.lift ?? 0, + err: arm.err, + reason: held + ? `抓起来了(抬升 ${((o.lift ?? 0) * 100).toFixed(1)} cm)` + : `没夹住:离手 ${((o.dist ?? 0) * 100).toFixed(1)} cm,` + + `抬升 ${((o.lift ?? 0) * 100).toFixed(1)} cm`, + }; + } +} diff --git a/src/manip.js b/src/manip.js new file mode 100644 index 0000000..80e70bf --- /dev/null +++ b/src/manip.js @@ -0,0 +1,644 @@ +/** + * 双臂手动操作:笛卡尔点动 IK + 握力映射 + 抓放判定。 + * + * 这里没有策略,全部由人点按钮驱动: + * · 每只手臂维持一个世界系目标点,每个控制步解一次 IK 写进位置伺服; + * · 手的 6 个执行器由一个 0~1 的握力滑条统一驱动; + * · 抓放判定不依赖接触数组(浏览器版 MuJoCo 的 contact 不好取), + * 用「物体离指尖质心的距离 + 抬升高度 + 静止判据」三条组合, + * 足以把"抓起来了"和"碰掉到地上"区分开。 + * + * IK 的雅可比用有限差分:embind 绑定的 mj_jacSite 给 JS 数组不回写(实测全零), + * 而臂只有 7 个关节,每次迭代多算 7 次 mj_kinematics 完全可以接受。 + */ + +const clamp = (x, lo, hi) => (x < lo ? lo : x > hi ? hi : x); + +/** + * 把角度差折到 (-π/2, π/2]。 + * 爪口是一条**直线**不是箭头,转 180° 是同一个夹法 —— 不折的话 + * 想转 170° 会去撞限位,其实反向转 10° 就到了。 + */ +export const foldPi = (d) => Math.atan2(Math.sin(2 * d), Math.cos(2 * d)) / 2; + +/** 解 A x = b(小方阵,高斯消元 + 部分主元)。 */ +function solveDense(A, b) { + const n = b.length; + const M = A.map((row, i) => [...row, b[i]]); + for (let c = 0; c < n; c++) { + let p = c; + for (let r = c + 1; r < n; r++) if (Math.abs(M[r][c]) > Math.abs(M[p][c])) p = r; + if (Math.abs(M[p][c]) < 1e-12) continue; + [M[c], M[p]] = [M[p], M[c]]; + for (let r = 0; r < n; r++) { + if (r === c) continue; + const f = M[r][c] / M[c][c]; + for (let k = c; k <= n; k++) M[r][k] -= f * M[c][k]; + } + } + return M.map((row, i) => (Math.abs(row[i]) < 1e-12 ? 0 : row[n] / row[i])); +} + +/** + * 抓取点在掌坐标系里的位置 = **手指合拢时拇指和食指相遇的地方**。 + * 实测(掌系):张开时拇指尖 (0.041,-0.130,0.091)、食指尖 (0.034,-0.029,0.179); + * 合拢后拇指尖 (0.034,-0.028,0.131)、食指尖 (0.072,-0.029,0.117) —— 中点就是这里。 + * 用它当 IK 末端,屏幕上的标记球就是"手会夹住哪儿",瞄准物体中心即可。 + */ +export const GRASP_LOCAL = [0.053, -0.028, 0.124]; + +/** + * 进近瞄准点 = **张开时拇指与食指之间的空档中心**。 + * 用合拢后的夹持中心去对准物体是错的:那个位置在张开状态下正是手指所在处, + * 手一移过去就把物体扫飞(实测 8 组偏移全部抓空)。物体要先落在空档里, + * 合拢时才会被兜住。 + */ +export const PREGRASP_LOCAL = [0.037, -0.080, 0.135]; + +export const APPROACH = { + down: { label: '朝下', axis: [0, 0, -1] }, + front: { label: '朝前', axis: [1, 0, 0] }, + free: { label: '不约束', axis: null }, +}; + +export class ArmController { + constructor(mujoco, model, data, meta, side) { + this.mujoco = mujoco; + this.model = model; + this.data = data; + this.side = side; + const a = meta.arms[side], h = meta.hands[side]; + this.act = Int32Array.from(a.act); + this.qadr = Int32Array.from(a.qadr); + this.nj = a.act.length; // 臂关节数:P7 是 7,SO-101 是 5 + this.range = a.range; + this.siteId = a.site_id; + this.ready = Float64Array.from(a.ready); + this.handAct = Int32Array.from(h.act); + this.handRange = h.range; + this.gripKind = h.kind || 'l6'; + this.palmBody = h.palm_body; + if (this.gripKind === 'so101') { + // 二指夹爪:夹持中心 = 两爪指尖球的中点(Go2 抓放 demo 验证过的算法); + // 注意开合方向和五指手相反 —— 角度大是**张开**。 + this.jawA = Int32Array.from(h.jaw_a); + this.jawB = Int32Array.from(h.jaw_b); + this.siteGrip = h.site_id; + this.gripOpenV = [1.2]; + this.gripClosedV = [-0.05]; + this.tipBodies = new Int32Array(0); + } else { + this.tipBodies = Int32Array.from(h.tip_bodies); + this.gripOpenV = h.range.map((r) => r[0]); + this.gripClosedV = h.range.map((r) => r[1]); + } + + this.q = Float64Array.from(a.ready); // 当前命令的臂关节角 + this.grip = 0; // 0 张开 → 1 握紧(用户设定值) + this.gripCmd = 0; // 限速后实际下发的值 + // 腕滚转目标(爪口的世界系水平角)。null = 不管(手动模式的原行为)。 + // 注意**不能**解完 IK 再单独掰这个关节:两爪中点不在滚转轴上, + // 转 90° 会把抓取点甩出 5 cm(实测 0.3 mm → 54.6 mm)。 + // 它是作为 IK 的一行任务参与求解的,见 _solveFrom。 + this.roll = null; + this.qGoal = null; // 非 null 时 step() 直接跟随它,不再自己解 IK + this.rollIdx = (a.names || []).findIndex((n) => /roll/i.test(n)); + if (this.rollIdx < 0) this.rollIdx = this.nj - 1; + // 二指爪抓桌面物体必须俯抓(爪口朝下)才夹得住,默认就开; + // 五指手默认不约束(它从就绪位形摆到俯抓会扫掉桌上的东西)。 + this.approach = this.gripKind === 'so101' ? 'down' : 'free'; + this.qRate = 0.03; // 每控制步的最大关节变化 [rad] + this.graspLocal = [...PREGRASP_LOCAL]; // IK 瞄准点(掌系) + // 工作空间钳位按场景算,别写死 —— 之前是 P7 的数(z 下限 0.60), + // 换成桌面式的 SO-101(目标 z≈0.47)一点动就被夹住不动。 + const T = meta.scene.table; + this.tableTop = T.top; + this.limits = { + x: [T.x - T.half[0] - 0.20, T.x + T.half[0] + 0.20], + y: [-T.half[1] - 0.25, T.half[1] + 0.25], + z: [T.top - 0.03, T.top + 0.55], + }; + this.ik = new mujoco.MjData(model); // IK 用的影子状态 + this.target = this.graspPoint().slice(); + this.err = 0; + } + + wristPos(d = this.data) { + const p = d.site_xpos, i = this.siteId; + return [p[3 * i], p[3 * i + 1], p[3 * i + 2]]; + } + + /** 指尖质心(诊断用;会随握力变化,所以不拿它当 IK 末端)。 */ + fingerCenter(d = this.data) { + const o = [0, 0, 0]; + for (const b of this.tipBodies) { + for (let k = 0; k < 3; k++) o[k] += d.xpos[3 * b + k]; + } + return o.map((v) => v / this.tipBodies.length); + } + + _meanGeom(d, ids) { + const o = [0, 0, 0]; + for (const g of ids) for (let k = 0; k < 3; k++) o[k] += d.geom_xpos[3 * g + k]; + return o.map((v) => v / ids.length); + } + + /** 抓取点。二指爪取两爪指尖中点;五指手取掌心 + 固定偏移。 */ + graspPoint(d = this.data, L = this.graspLocal) { + if (this.gripKind === 'so101') { + const a = this._meanGeom(d, this.jawA), b2 = this._meanGeom(d, this.jawB); + return [(a[0] + b2[0]) / 2, (a[1] + b2[1]) / 2, (a[2] + b2[2]) / 2]; + } + const b = this.palmBody, o = 9 * b; + return [ + d.xpos[3 * b] + d.xmat[o] * L[0] + d.xmat[o + 1] * L[1] + d.xmat[o + 2] * L[2], + d.xpos[3 * b + 1] + d.xmat[o + 3] * L[0] + d.xmat[o + 4] * L[1] + d.xmat[o + 5] * L[2], + d.xpos[3 * b + 2] + d.xmat[o + 6] * L[0] + d.xmat[o + 7] * L[1] + d.xmat[o + 8] * L[2], + ]; + } + + /** 进近轴:二指爪是 gripperframe site 的局部 +x;五指手是掌心指向指尖(+z)。 */ + palmAxis(d = this.data) { + if (this.gripKind === 'so101') { + const o = 9 * this.siteGrip, m = d.site_xmat; + return [m[o], m[o + 3], m[o + 6]]; + } + const o = 9 * this.palmBody; + return [d.xmat[o + 2], d.xmat[o + 5], d.xmat[o + 8]]; + } + + /** 爪口张开方向(世界系单位向量)。二指爪才有。 */ + jawAxis(d = this.data) { + if (this.gripKind !== 'so101') return null; + const a = this._meanGeom(d, this.jawA), b = this._meanGeom(d, this.jawB); + const v = [b[0] - a[0], b[1] - a[1], b[2] - a[2]]; + const n = Math.hypot(v[0], v[1], v[2]) || 1; + return [v[0] / n, v[1] / n, v[2] / n]; + } + + /** 爪口在水平面内的角度。爪口是条直线,所以只在 (-π/2, π/2] 上有意义。 */ + jawAngle(d = this.data) { + const v = this.jawAxis(d); + return v ? Math.atan2(v[1], v[0]) : 0; + } + + /** + * 爪子在某个开合度下的几何:开口多大、**爪尖比爪中点低多少**。 + * + * 第二个数是抓矮物体的关键。爪子张到最大时爪尖伸得最低(SO-101 实测 2.61 cm), + * 下探时爪尖先撞桌面,爪中点就卡在物体上方合了个空爪。 + * 收到 6 cm 开口时只低 0.92 cm —— 所以**别张到最大再下去**。 + * 开口 10.1cm → 低 2.61cm 6.1cm → 0.92cm + * 8.9cm → 1.95cm 4.7cm → 0.59cm + * 7.6cm → 1.38cm + */ + jawGeom(gripVal = this.gripOpenV[0], q = this.q) { + if (this.gripKind !== 'so101') return { span: Infinity, drop: 0 }; + const { ik, model, mujoco } = this; + ik.qpos.set(this.data.qpos); + this._setQ(ik, Array.from(q)); + const jid = model.actuator_trnid[2 * this.handAct[0]]; + ik.qpos[model.jnt_qposadr[jid]] = gripVal; + mujoco.mj_kinematics(model, ik); + const a = this._meanGeom(ik, this.jawA), b = this._meanGeom(ik, this.jawB); + const midZ = (a[2] + b[2]) / 2; + let low = Infinity; + for (const g of [...this.jawA, ...this.jawB]) { + low = Math.min(low, ik.geom_xpos[3 * g + 2] - (model.geom_size[3 * g] || 0)); + } + return { span: Math.hypot(a[0] - b[0], a[1] - b[1], a[2] - b[2]), + drop: midZ - low }; + } + + jawSpan(gripVal = this.gripOpenV[0]) { return this.jawGeom(gripVal).span; } + + /** 0~1 的握力 → 执行器值。 */ + gripToVal(g) { + return this.gripOpenV[0] + g * (this.gripClosedV[0] - this.gripOpenV[0]); + } + + /** 反解:要这么大的开口,握力该给多少(二分,爪子是单调的)。 */ + gripForSpan(span, q = this.q) { + if (this.gripKind !== 'so101') return 0; + let lo = 0, hi = 1; + for (let k = 0; k < 18; k++) { + const m = (lo + hi) / 2; + if (this.jawGeom(this.gripToVal(m), q).span > span) lo = m; else hi = m; + } + return (lo + hi) / 2; + } + + /** + * 把解出来的位形再转一下腕,让爪口对准 theta(世界系水平角)。 + * 差值折到 (-π/2, π/2] —— 爪口转 180° 是同一个夹法,别绕远路撞限位。 + */ + rollTo(q, theta) { + if (theta == null || this.gripKind !== 'so101') return q; + const { ik, model, mujoco } = this; + ik.qpos.set(this.data.qpos); + this._setQ(ik, q); + mujoco.mj_kinematics(model, ik); + const cur = this.jawAngle(ik); + const d = foldPi(theta - cur); + const out = Array.from(q); + const i = this.rollIdx; + out[i] = clamp(q[i] + d, this.range[i][0], this.range[i][1]); + return out; + } + + /** 把一组关节角喂进影子状态,回报抓取点和爪口角。 */ + _evalQ(q) { + const { ik, model, mujoco } = this; + ik.qpos.set(this.data.qpos); + this._setQ(ik, q); + mujoco.mj_kinematics(model, ik); + return { p: this.graspPoint(ik), yaw: this.jawAngle(ik) }; + } + + /** + * 物理可信度惩罚(单位换算成"等效多少米的位置误差")。 + * + * 不约束爪口朝向以后,IK 会解出**运动学成立但物理上进不去**的位形 —— + * 典型是爪子从桌面以下捅上来。这种解残差是 0,判定说"可达", + * 一执行手就卡在桌沿动不了(实测玩具琴那一下离手 15.9 cm)。 + * 所以在多个解里按"残差 + 惩罚"挑,而不是只挑残差最小的。 + * + * 这是**软偏好不是硬约束**:穿桌面罚得重(纯物理),朝向只轻罚 + * (从侧面进去有时候是对的,别一刀切)。 + */ + _penalty(q) { + const { ik, model, mujoco } = this; + ik.qpos.set(this.data.qpos); + this._setQ(ik, q); + mujoco.mj_kinematics(model, ik); + let pen = 0; + if (this.gripKind === 'so101') { + const a = this._meanGeom(ik, this.jawA), b = this._meanGeom(ik, this.jawB); + const zmin = Math.min(a[2], b[2]); + const floor = this.tableTop + 0.005; + if (zmin < floor) pen += (floor - zmin) * 4; // 插进桌子:重罚 + } + // 爪口轴必须基本水平。物体是**放在桌面上**的,两爪上下合拢意味着 + // 下面那只爪得钻到桌面底下 —— 运动学上解得出来,物理上进不去。 + // 这不是在约束"从哪个方向进近",是在排除桌面挡住的夹法。 + const jv = this.jawAxis(ik); + if (jv) { + const horiz = Math.hypot(jv[0], jv[1]); // 1=水平,0=竖直 + if (horiz < 0.85) pen += (0.85 - horiz) * 0.30; + } + // 偏好从上往下进近。不是"必须朝下"(那是硬约束,5 自由度顶不住), + // 是在多个可行解里优先挑俯冲的那个:斜着插进去的解会在下探时 + // 用爪子侧面把物体扫开 —— 失败的那几例离手都≈抬起高度, + // 就是"手抬起来了但东西没跟上"。 + const ax = this.palmAxis(ik); + if (ax[2] > -0.7) pen += (ax[2] + 0.7) * 0.10; + return pen; + } + + /** + * 可达探针。纯查询 —— 只碰影子状态 this.ik,不动 this.q / this.err / 热启动缓存。 + * + * theta = null 时就是普通位置 IK。给了 theta 要注意:**两爪中点不在滚转轴上** + * (SO-101 是一动一定的二指爪,偏移随位形变化),所以"解完位置再硬掰滚转" + * 会把抓取点甩出去 —— 实测易拉罐那一下从 0.3 mm 崩到 54.6 mm。 + * 正确做法是**交替**:滚转对齐 → 冻住滚转、用剩下 4 个关节把位置找回来 → 再对齐, + * 迭代几轮收敛。5 自由度的臂满足不了"位置3+朝下2+偏航1"这 6 个约束, + * 这个交替法是在可行集里找一个两头都尽量好的折中。 + */ + probeAt(target, theta = null, tries = 5, approach = undefined) { + const O = { approach }; + // 探针必须**可复现**,否则同一个物体连点两次可能一次判"左臂"一次判"右臂"。 + // 两个不确定源都得堵: + // ① 起点用了 Math.random() → 换成固定种子的 LCG; + // ② 热启动用了手臂当前位形 this.q → 判定结果不该取决于"手臂刚才在哪", + // 改成一律从 ready 位形起步。 + let seed = 0x9e3779b1; + const rnd = () => ((seed = (seed * 1664525 + 1013904223) >>> 0) / 4294967296); + const starts = [Array.from(this.ready)]; + for (let k = 0; k < tries - 1; k++) { + starts.push(this.range.map(([lo, hi]) => lo + rnd() * (hi - lo))); + } + if (theta != null) O.yaw = theta; // 偏航直接进 IK 任务 + let best = null; + for (const s of starts) { + const c = this._solveFrom(target, s, 60, O); + // 按"残差 + 偏航偏差 + 物理可信度"挑,不是只看残差 + const e = this._evalQ(c.q); + c.yaw = e.yaw; + c.score = c.err + this._penalty(c.q) + + (theta == null ? 0 : 0.05 * Math.abs(foldPi(theta - e.yaw))); + if (!best || c.score < best.score) best = c; + } + this._evalQ(best.q); + return { q: best.q, err: best.err, yaw: best.yaw, + jaw: this.jawAxis(this.ik), + rollErr: theta == null ? 0 : Math.abs(foldPi(theta - best.yaw)), + roll: best.q[this.rollIdx] }; + } + + _setQ(d, q) { + for (let i = 0; i < this.nj; i++) { + d.qpos[this.qadr[i]] = clamp(q[i], this.range[i][0], this.range[i][1]); + } + } + + /** + * 位置 IK,可选加一条"手掌朝向"约束。 + * 冗余的第 7 个自由度用弱零空间偏置拉回就绪位形,免得手腕越转越歪。 + */ + /** + * 带随机重启的 IK。热启动单点求解会卡在坏的吸引域 —— 实测"掌心朝下"这个 + * 位形明明可达(多起点能解到 0.7mm / 0.1°),单起点却停在 46mm 下不来, + * 因为真解落在 L2 关节的限位上。残差大就换几个起点重来。 + */ + /** 关节空间位移:选解时用来挑"离当前姿态最近"的那个,避免大幅重构。 */ + _travel(q) { + let t = 0; + for (let k = 0; k < this.nj; k++) t += Math.abs(q[k] - this.q[k]); + return t; + } + + solveIK(target, iters = 20) { + const a = this._solveFrom(target, Array.from(this.q), iters); + // 多起点很贵(9 起点 × 80 迭代 × 7 次有限差分),不能每个控制步都跑, + // 否则交互循环直接卡死。只在目标点/朝向刚变过时找一次好的吸引域, + // 之后一路热启动跟踪。 + const moved = !this._lastT + || Math.hypot(target[0] - this._lastT[0], target[1] - this._lastT[1], + target[2] - this._lastT[2]) > 0.01 + || this._lastAp !== this.approach; + if (moved) { this._lastT = [...target]; this._lastAp = this.approach; this._budget = 30; } + if (a.err < 0.015 || !(this._budget > 0)) { this.err = a.err; return a.q; } + this._budget -= 1; + let best = a; + const starts = [Array.from(this.ready)]; + for (let k = 0; k < 4; k++) { + starts.push(this.range.map(([lo, hi]) => lo + Math.random() * (hi - lo))); + } + const cand = [a]; + for (const s0 of starts) { + cand.push(this._solveFrom(target, s0, 60)); + } + // 选路:残差达标的解可能有好几个(冗余臂的不同肘部构型), + // 从中挑**关节空间位移最小**的 —— 否则手臂会在两个构型之间来回大幅重构, + // 中途扫翻桌上的东西(P7 上实测过)。都不达标时才退回比残差。 + const ok = cand.filter((c) => c.err < 0.008); + best = (ok.length ? ok : cand).reduce((b, c) => { + const cost = (x) => (ok.length ? this._travel(x.q) : x.err); + return cost(c) < cost(b) ? c : b; + }); + this.err = best.err; + return best.q; + } + + _solveFrom(target, qInit, iters = 20, opts = {}) { + const { ik, model, mujoco } = this; + // 朝向约束可以按次覆盖 —— 自动抓取用 'free'(不约束): + // 那样任务只剩位置 3 个约束,5 个关节多出 2 个自由度,爪口偏航才有地方安放 + // (位置3+偏航1=4 ≤ 5)。硬钉"爪口朝下"会把约束顶到 6 个,数学上就解不全。 + const ax = APPROACH[opts.approach ?? this.approach].axis; + const W = 0.25, h = 1e-4; + // 冻结的关节不进雅可比 —— 用来"先定死腕滚转、再用剩下 4 个关节把位置找回来"。 + const frozen = opts.freeze ? new Set(opts.freeze) : null; + // 爪口偏航作为**一行任务**参与求解,而不是解完再硬掰。 + // 硬掰行不通:两爪中点不在滚转轴上,转 90° 会把抓取点甩出 5 cm; + // 而"掰完冻住滚转再解位置"的交替法实测不收敛(位置解一动 pan,偏航又跑了)。 + // 放进任务里就没这问题:不约束进近方向时任务 = 位置 3 行 + 偏航 1 行 = 4, + // 臂有 5 个关节,是欠定的,DLS 一次解完。 + const wantYaw = opts.yaw != null && this.gripKind === 'so101'; + const Wy = 0.05; // 1 rad 偏航 ≈ 5 cm 位置误差 + ik.qpos.set(this.data.qpos); + let q = qInit.map((v, i) => clamp(v, this.range[i][0], this.range[i][1])); + + for (let it = 0; it < iters; it++) { + this._setQ(ik, q); + mujoco.mj_kinematics(model, ik); + const p0 = this.graspPoint(ik); + const a0 = ax ? this.palmAxis(ik) : null; + const y0 = wantYaw ? this.jawAngle(ik) : 0; + const ep = [target[0] - p0[0], target[1] - p0[1], target[2] - p0[2]]; + const ea = ax ? ax.map((v, i) => W * (v - a0[i])) : null; + const dy = wantYaw ? foldPi(opts.yaw - y0) : 0; + const epn = Math.hypot(ep[0], ep[1], ep[2]); + if (epn < 5e-4 && (!ea || Math.hypot(ea[0], ea[1], ea[2]) / W < 0.08) + && (!wantYaw || Math.abs(dy) < 0.02)) break; + + const nPos = ea ? 6 : 3; + const rows = nPos + (wantYaw ? 1 : 0); + const J = Array.from({ length: rows }, () => new Array(this.nj).fill(0)); + for (let j = 0; j < this.nj; j++) { + if (frozen && frozen.has(j)) continue; // 这一列留 0 = 该关节不动 + const qh = [...q]; + qh[j] = clamp(qh[j] + h, this.range[j][0], this.range[j][1]); + const dq = qh[j] - q[j]; + if (Math.abs(dq) < 1e-12) continue; + this._setQ(ik, qh); + mujoco.mj_kinematics(model, ik); + const p1 = this.graspPoint(ik); + for (let r = 0; r < 3; r++) J[r][j] = (p1[r] - p0[r]) / dq; + if (ea) { + const a1 = this.palmAxis(ik); + for (let r = 0; r < 3; r++) J[3 + r][j] = W * (a1[r] - a0[r]) / dq; + } + // 偏航的差分要走 foldPi,否则跨 ±90° 时会算出一个巨大的假梯度 + if (wantYaw) J[nPos][j] = Wy * foldPi(this.jawAngle(ik) - y0) / dq; + } + const e = [...ep, ...(ea || []), ...(wantYaw ? [Wy * dy] : [])]; + const JJt = Array.from({ length: rows }, (_, r) => Array.from({ length: rows }, (_, c) => { + let sum = r === c ? 1e-4 : 0; + for (let k = 0; k < this.nj; k++) sum += J[r][k] * J[c][k]; + return sum; + })); + const y = solveDense(JJt, e); + const step = new Array(this.nj).fill(0); + for (let k = 0; k < this.nj; k++) for (let r = 0; r < rows; r++) step[k] += J[r][k] * y[r]; + // 次要目标(关节限位居中)必须**投影到零空间**再加,不能直接加在解上: + // 直接加会和主任务抢自由度 —— P7 上实测把残差锁在 26mm 下不来; + // SO-101 只有 5 个自由度、任务也是 5 个约束,根本没有零空间, + // 直接加更是纯损害(台面上方 6cm 处 0.0mm/0.0° → 15.0mm/5.1°)。 + const redundant = this.nj > rows; + if (redundant) { + // z = 关节限位居中的梯度;dq_null = (I − Jᵀ(JJᵀ)⁻¹J) z + const z = q.map((v, k) => { + const [lo, hi] = this.range[k]; + return 0.6 * ((lo + hi) / 2 - v) / Math.max(hi - lo, 1e-6); + }); + const Jz = new Array(rows).fill(0); + for (let r = 0; r < rows; r++) { + for (let k = 0; k < this.nj; k++) Jz[r] += J[r][k] * z[k]; + } + const w = solveDense(JJt, Jz); + for (let k = 0; k < this.nj; k++) { + let proj = z[k]; + for (let r = 0; r < rows; r++) proj -= J[r][k] * w[r]; + step[k] += proj; + } + } + q = q.map((v, k) => clamp(v + 0.5 * step[k], this.range[k][0], this.range[k][1])); + } + this._setQ(ik, q); + mujoco.mj_kinematics(model, ik); + const p = this.graspPoint(ik); + return { q, err: Math.hypot(target[0] - p[0], target[1] - p[1], target[2] - p[2]) }; + } + + /** 目标点移动 delta(世界系),限制在场景相关的工作空间内。 */ + jog(dx, dy, dz) { + const L = this.limits; + this.target[0] = clamp(this.target[0] + dx, L.x[0], L.x[1]); + this.target[1] = clamp(this.target[1] + dy, L.y[0], L.y[1]); + this.target[2] = clamp(this.target[2] + dz, L.z[0], L.z[1]); + } + + reset() { + this.q = Float64Array.from(this.ready); + this.grip = 0; + this.gripCmd = 0; + this.roll = null; + this.qGoal = null; + this.target = this.graspPoint().slice(); + } + + /** 每个控制步调一次:解 IK、按需对齐爪口、限速后写 ctrl。 */ + step() { + let q; + if (this.qGoal) { + // 自动抓取时直接走规划好的位形。 + // 不能让控制器自己重解:探针是多起点全局解,而 solveIK 从当前位形热启动、 + // 按"关节位移最小"挑解,两边会落进**不同的分支** —— 表现就是 + // "判定说 0 mm 可达,执行却停在 16 cm 外"。 + q = Array.from(this.qGoal); + const e = this._evalQ(q); + this.err = Math.hypot(this.target[0] - e.p[0], this.target[1] - e.p[1], + this.target[2] - e.p[2]); + } else if (this.roll != null) { + // 要对齐爪口时走带偏航任务的 IK(和 probeAt 同一条路径, + // 这样"判定说能抓"和"实际怎么动"用的是同一个解)。 + const r = this._solveFrom(this.target, Array.from(this.q), 25, + { yaw: this.roll }); + q = r.q; + this.err = r.err; + } else { + q = this.solveIK(this.target); + } + // 位置伺服 kp=2000,IK 解一步到位会让手臂猛甩 —— 实测能把桌上的方块弹到 11 米外。 + // 每步最多走 qRate 弧度(50Hz 下约 1.5 rad/s),动作自然也不会砸到东西。 + for (let i = 0; i < this.nj; i++) { + const dq = q[i] - this.q[i]; + this.q[i] += Math.abs(dq) > this.qRate ? Math.sign(dq) * this.qRate : dq; + this.data.ctrl[this.act[i]] = this.q[i]; + } + // 握力同样限速:手指瞬间合拢会把物体弹出去 + const dg = this.grip - this.gripCmd; + this.gripCmd += Math.abs(dg) > 0.02 ? Math.sign(dg) * 0.02 : dg; + for (let i = 0; i < this.handAct.length; i++) { + const op = this.gripOpenV[i], cl = this.gripClosedV[i]; + this.data.ctrl[this.handAct[i]] = op + this.gripCmd * (cl - op); + } + } +} + +/** + * 抓放判定。 + * + * 抓取成功:物体离某只手的指尖质心 < holdDist,且高于桌面 liftClear, + * 连续保持 holdSteps 个控制步。 + * 放置成功:抓起过之后松手(离手 > releaseDist 或握力 < 0.2), + * 物体基本静止且落在桌面高度附近,连续保持 restSteps 步。 + */ +export class GraspMonitor { + constructor(model, data, meta, arms) { + this.model = model; + this.data = data; + this.arms = arms; + this.table = meta.scene.table; + this.objs = meta.scene.objects.map((o) => ({ + ...o, + state: 'idle', // idle → held → placed + hold: 0, + rest: 0, + peak: 0, + by: null, + })); + this.cfg = { holdDist: 0.09, releaseDist: 0.16, liftClear: 0.05, + holdSteps: 25, restSteps: 40, restSpeed: 0.03 }; + this.log = []; + this.t = 0; + } + + objPos(o) { + const b = o.body_id, x = this.data.xpos; + return [x[3 * b], x[3 * b + 1], x[3 * b + 2]]; + } + + objSpeed(o) { + const v = this.data.qvel, a = o.qadr - 1; // 自由关节:qpos 7 个 / qvel 6 个 + const i = this.dofOf(o); + return Math.hypot(v[i], v[i + 1], v[i + 2]) + 0 * a; + } + + dofOf(o) { + if (o._dof == null) o._dof = this.model.jnt_dofadr[this.model.body_jntadr[o.body_id]]; + return o._dof; + } + + /** 每个控制步调一次,返回本步新产生的事件(没有就是 null)。 */ + step() { + this.t += 1; + const C = this.cfg; + let ev = null; + const centers = Object.fromEntries( + Object.entries(this.arms).map(([s, a]) => [s, a.graspPoint()])); + + for (const o of this.objs) { + const p = this.objPos(o); + const lift = p[2] - o.home[2]; + o.lift = lift; + let near = null, nd = 1e9; + for (const [s, c] of Object.entries(centers)) { + const d = Math.hypot(p[0] - c[0], p[1] - c[1], p[2] - c[2]); + if (d < nd) { nd = d; near = s; } + } + o.dist = nd; + + const held = nd < C.holdDist && lift > C.liftClear; + if (held) { + o.hold += 1; + o.peak = Math.max(o.peak, lift); + if (o.state === 'idle' && o.hold >= C.holdSteps) { + o.state = 'held'; + o.by = near; + ev = { kind: 'pick', obj: o.name, side: near, lift: o.peak, t: this.t }; + this.log.push(ev); + } + } else { + o.hold = 0; + } + + if (o.state === 'held') { + const released = nd > C.releaseDist || this.arms[o.by].grip < 0.2; + const still = this.objSpeed(o) < C.restSpeed; + const onTable = Math.abs(p[2] - o.home[2]) < 0.05; + if (released && still && onTable) { + o.rest += 1; + if (o.rest >= C.restSteps) { + o.state = 'idle'; + o.rest = 0; + o.peak = 0; + ev = { kind: 'place', obj: o.name, side: o.by, t: this.t, + pos: [p[0], p[1], p[2]] }; + this.log.push(ev); + } + } else { + o.rest = 0; + } + if (p[2] < this.table.top - 0.15) { // 掉到台下 = 失败 + o.state = 'idle'; o.rest = 0; o.peak = 0; + ev = { kind: 'drop', obj: o.name, side: o.by, t: this.t }; + this.log.push(ev); + } + } + } + return ev; + } +} diff --git a/src/math.js b/src/math.js new file mode 100644 index 0000000..02c51d3 --- /dev/null +++ b/src/math.js @@ -0,0 +1,58 @@ +/** + * 无依赖的小工具 —— 单独成文件是为了让 physics.worker.js 能用它 + * 而不必 import three。worker 只算物理，把整个 three.js 拉进 worker + * 是几百 KB 的白扔。 + */ + +/** + * 3x3 行主序旋转矩阵（MuJoCo 的 xmat 布局）→ 四元数 [w,x,y,z]，写进 out[po..po+3] + */ +export function matToQuat(m, o, out, po) { + const m00 = m[o], m01 = m[o + 1], m02 = m[o + 2]; + const m10 = m[o + 3], m11 = m[o + 4], m12 = m[o + 5]; + const m20 = m[o + 6], m21 = m[o + 7], m22 = m[o + 8]; + const tr = m00 + m11 + m22; + let w, x, y, z; + if (tr > 0) { + const s = Math.sqrt(tr + 1.0) * 2; + w = 0.25 * s; x = (m21 - m12) / s; y = (m02 - m20) / s; z = (m10 - m01) / s; + } else if (m00 > m11 && m00 > m22) { + const s = Math.sqrt(1.0 + m00 - m11 - m22) * 2; + w = (m21 - m12) / s; x = 0.25 * s; y = (m01 + m10) / s; z = (m02 + m20) / s; + } else if (m11 > m22) { + const s = Math.sqrt(1.0 + m11 - m00 - m22) * 2; + w = (m02 - m20) / s; x = (m01 + m10) / s; y = 0.25 * s; z = (m12 + m21) / s; + } else { + const s = Math.sqrt(1.0 + m22 - m00 - m11) * 2; + w = (m10 - m01) / s; x = (m02 + m20) / s; y = (m12 + m21) / s; z = 0.25 * s; + } + out[po] = w; out[po + 1] = x; out[po + 2] = y; out[po + 3] = z; +} + +/** 四元数 [w,x,y,z] 的共轭 = 单位四元数的逆 */ +export function quatConj([w, x, y, z]) { + return [w, -x, -y, -z]; +} + +/** Hamilton 积 a⊗b，[w,x,y,z] 记法 */ +export function quatMul([aw, ax, ay, az], [bw, bx, by, bz]) { + return [ + aw * bw - ax * bx - ay * by - az * bz, + aw * bx + ax * bw + ay * bz - az * by, + aw * by - ax * bz + ay * bw + az * bx, + aw * bz + ax * by - ay * bx + az * bw, + ]; +} + +/** + * R^T·v —— 把世界系向量 v 转到旋转矩阵 R（MuJoCo xmat，行主序，偏移 o）代表的 + * 局部系下。焊接约束的 eq_data 相对位姿是在 body1 局部系下定义的，抓取时要把 + * "方块相对手掌的世界系偏移"转到手掌局部系，这个函数就是干这个的。 + */ +export function invRotateVec(m, o, [vx, vy, vz]) { + return [ + m[o] * vx + m[o + 3] * vy + m[o + 6] * vz, + m[o + 1] * vx + m[o + 4] * vy + m[o + 7] * vz, + m[o + 2] * vx + m[o + 5] * vy + m[o + 8] * vz, + ]; +} diff --git a/src/scene-bridge.js b/src/scene-bridge.js new file mode 100644 index 0000000..f0a8521 --- /dev/null +++ b/src/scene-bridge.js @@ -0,0 +1,201 @@ +/** + * MuJoCo → three.js 渲染桥。 + * + * 设计要点：不走 mjvScene。 + * + * 官方 binding 提供了 mjv_updateScene / MjvScene，但 MjvScene 里 geom 数组 + * 的 JS 侧访问方式在当前 binding 版本里没有文档保证。这里改用两组 + * 100% 是 mjModel/mjData 核心字段的数据： + * + * 静态：model.geom_type / geom_size / geom_rgba → 建 three.js 几何体 + * 每帧：data.geom_xpos / data.geom_xmat → 更新世界位姿 + * + * 好处是这条路同时服务 L0/L1（物理实时算）和 L2（回放预录位姿）， + * 两边喂的都是同一份 {pos, quat} 流，渲染代码只有一套。 + * + * 坐标系：不做 Y-up 转换，直接用 MuJoCo 的 Z-up， + * 相机 camera.up 设成 (0,0,1) 即可。调试时坐标和 MuJoCo 一一对应。 + */ +import * as THREE from 'three'; + +export { matToQuat } from './math.js'; + +export const GEOM = { + PLANE: 0, HFIELD: 1, SPHERE: 2, CAPSULE: 3, + ELLIPSOID: 4, CYLINDER: 5, BOX: 6, MESH: 7, SDF: 8, +}; + +/** 分段数随画质档位下调 —— 弱设备省顶点 */ +const SEG = { + 0: { radial: 20, cap: 6, sphere: [20, 14] }, // L0 + 1: { radial: 12, cap: 3, sphere: [12, 8] }, // L1 + 2: { radial: 10, cap: 3, sphere: [10, 6] }, // L2 +}; + +/** + * @param {Array|{geoms:Array, meshes:Array}} desc + * 传数组 = 只有 primitive（内置代理模型）； + * 传 {geoms, meshes} = 带 mesh 资产（Menagerie 真实模型）。 + * @param {number} level 0|1|2 + * @param {{maxGroup?:number, filter?:'visual'|'collision'|'all'}} opts + * + * filter（推荐）: 按 contype/conaffinity 区分视觉与碰撞几何。 + * 这条规则跨约定通用 —— Menagerie 用 group 2/3， + * unitree_rl_gym 用 group 1/0，但两边的纯视觉几何都是 + * contype=0 且 conaffinity=0。按 group 过滤会在某一方出错， + * 典型症状是视觉和碰撞两层同时画出来重叠闪烁。 + * 模型若完全没有专门的视觉几何，自动退回全画。 + * + * maxGroup（旧行为）: 只渲染 geom_group <= 该值的几何体。 + * + * @returns {{group: THREE.Group, meshes: Array, dispose: Function}} + */ +export function buildGeomMeshes(desc, level = 0, opts = {}) { + const geoms = Array.isArray(desc) ? desc : desc.geoms; + const meshData = Array.isArray(desc) ? [] : (desc.meshes ?? []); + const maxGroup = opts.maxGroup ?? 2; + const filter = opts.filter ?? null; + + const isVisual = (g) => (g.contype ?? 1) === 0 && (g.conaffinity ?? 1) === 0; + // 模型没有任何纯视觉几何时（比如我们自己写的代理模型），visual 过滤要退回全画 + const anyVisual = filter === 'visual' && geoms.some(isVisual); + // 只有当同一个 body 另外还有纯视觉几何时，才把碰撞几何藏起来。 + // 场景道具（桌子、方块）只有一个既碰撞又要看的 geom，一刀切会把它们画没。 + const bodyHasVisual = new Set(); + for (const g of geoms) if (isVisual(g)) bodyHasVisual.add(g.body ?? 0); + + const seg = SEG[level] ?? SEG[0]; + const group = new THREE.Group(); + const meshes = []; + const owned = []; + + for (const g of geoms) { + const [sx, sy, sz] = g.size; + let geometry = null; + let isStatic = false; + + // 被过滤掉的 geom 也要在 meshes 数组里占位， + // 否则索引和 geom_xpos 就错开了 + let skip; + if (filter === 'visual') { + skip = anyVisual && !isVisual(g) && bodyHasVisual.has(g.body ?? 0); + } + else if (filter === 'collision') skip = isVisual(g); + else if (filter === 'all') skip = false; + else skip = (g.group ?? 0) > maxGroup; + if (skip) { meshes.push(null); continue; } + + switch (g.type) { + case GEOM.PLANE: { + // size 为 0 表示无限大平面，给一块够用的 + const w = (sx > 0 ? sx : 12) * 2; + const h = (sy > 0 ? sy : 12) * 2; + geometry = new THREE.PlaneGeometry(w, h); + isStatic = true; // 地面不随帧更新 + break; + } + case GEOM.SPHERE: + geometry = new THREE.SphereGeometry(sx, seg.sphere[0], seg.sphere[1]); + break; + case GEOM.ELLIPSOID: + geometry = new THREE.SphereGeometry(1, seg.sphere[0], seg.sphere[1]); + geometry.scale(sx, sy, sz); + break; + case GEOM.CAPSULE: + // MuJoCo: size[0]=半径, size[1]=圆柱段半长, 沿局部 z 轴 + // three.js CapsuleGeometry 沿 y 轴 → 绕 x 转 90° + geometry = new THREE.CapsuleGeometry(sx, sy * 2, seg.cap, seg.radial); + geometry.rotateX(Math.PI / 2); + break; + case GEOM.CYLINDER: + geometry = new THREE.CylinderGeometry(sx, sx, sy * 2, seg.radial); + geometry.rotateX(Math.PI / 2); + break; + case GEOM.BOX: + geometry = new THREE.BoxGeometry(sx * 2, sy * 2, sz * 2); + break; + case GEOM.MESH: { + // MuJoCo 编译时已经把 OBJ/STL 解析好了，顶点面就在 model.mesh_vert/mesh_face。 + // 直接读出来建 BufferGeometry —— 不需要 glTF、不需要 Draco、 + // 不需要任何离线资产管线。这是整套方案里最省事的一环。 + const md = g.meshId >= 0 ? meshData[g.meshId] : null; + if (md) { + geometry = new THREE.BufferGeometry(); + geometry.setAttribute('position', new THREE.BufferAttribute(md.vertices, 3)); + geometry.setIndex(new THREE.BufferAttribute(md.indices, 1)); + geometry.computeVertexNormals(); // Menagerie 的 OBJ 不带法线 + break; + } + // 没拿到 mesh 数据时退回包围盒，至少不会整个场景炸掉 + geometry = new THREE.BoxGeometry( + Math.max(sx, 0.02) * 2, Math.max(sy, 0.02) * 2, Math.max(sz, 0.02) * 2); + break; + } + case GEOM.HFIELD: + case GEOM.SDF: + default: + geometry = new THREE.BoxGeometry( + Math.max(sx, 0.02) * 2, Math.max(sy, 0.02) * 2, Math.max(sz, 0.02) * 2); + break; + } + + const [r, gg, b, a] = g.rgba; + // MJCF 的 rgba 是 sRGB 数值。new THREE.Color(r,g,b) 走 setRGB， + // 默认按 working color space（linear-srgb）解释 —— 而 setClearColor(0x111417) + // 那条路径是会做 sRGB→linear 转换的。不显式指定的话背景对了模型偏亮。 + const material = new THREE.MeshStandardMaterial({ + color: new THREE.Color().setRGB(r, gg, b, THREE.SRGBColorSpace), + transparent: a < 1, + opacity: a, + roughness: 0.75, + metalness: 0.05, + }); + + const mesh = new THREE.Mesh(geometry, material); + mesh.castShadow = !isStatic && level === 0; + mesh.receiveShadow = true; + mesh.matrixAutoUpdate = false; // 位姿我们自己写，省掉每帧的分解/合成 + group.add(mesh); + meshes.push(isStatic ? null : mesh); // null = 不参与每帧更新 + owned.push(geometry, material); + + if (isStatic) { + mesh.position.set(0, 0, 0); + mesh.updateMatrix(); + mesh.matrixWorldNeedsUpdate = true; + } + } + + return { + group, + meshes, + dispose() { + for (const o of owned) o.dispose?.(); + group.clear(); + }, + }; +} + +const _q = new THREE.Quaternion(); +const _v = new THREE.Vector3(); +const _s = new THREE.Vector3(1, 1, 1); + +/** + * 用 {pos, quat} 流更新位姿。L0/L1（worker 实时算）和 L2（回放）共用。 + * @param {Array} meshes + * @param {Float32Array} pose 布局 [x,y,z, qw,qx,qy,qz] × ngeom + */ +export function applyPose(meshes, pose) { + for (let i = 0, n = meshes.length; i < n; i++) { + const m = meshes[i]; + if (!m) continue; + const o = i * 7; + _v.set(pose[o], pose[o + 1], pose[o + 2]); + // three.js 四元数是 (x,y,z,w)，MuJoCo 是 (w,x,y,z) + _q.set(pose[o + 4], pose[o + 5], pose[o + 6], pose[o + 3]); + m.matrix.compose(_v, _q, _s); + // 关键：matrixAutoUpdate=false 时 three.js 不会自己重算 matrixWorld， + // 不置这个标志的话物体永远停在原地（这个 bug 的症状是场景静止不动） + m.matrixWorldNeedsUpdate = true; + } +} diff --git a/start.sh b/start.sh new file mode 100755 index 0000000..4d3c120 --- /dev/null +++ b/start.sh @@ -0,0 +1,8 @@ +#!/usr/bin/env bash +# 启动双臂操作沙盒的静态服务器并打印访问地址。用法: ./start.sh [端口,默认 8000] +cd "$(dirname "$0")" +PORT="${1:-8000}" +echo "双臂操作沙盒(默认,两台 SO-101): http://127.0.0.1:${PORT}/manip.html" +echo "P7 双臂 + SO-101 夹爪场景: http://127.0.0.1:${PORT}/manip.html?scene=p7_bimanual" +echo "Ctrl+C 停止" +exec python3 serve.py "$PORT" diff --git a/tools/export_manip_scene.py b/tools/export_manip_scene.py new file mode 100644 index 0000000..23aaa80 --- /dev/null +++ b/tools/export_manip_scene.py @@ -0,0 +1,670 @@ +"""把双臂工作站 + 操作台 + 测试物体导出成浏览器能直接跑的自包含 MJCF。 + + ~/unitree_rl_mjlab/.venv/bin/python tools/export_manip_scene.py + +产出 scenes//: + model.xml 自包含 MJCF(网格内联 + 视觉网格抽稀,不依赖任何外部文件) + meta.json 给前端用的索引:臂/手的执行器、工具 site、指尖 geom、 + 物体和桌面的几何参数、就绪位形 + +这里没有策略,是纯手动操作的沙盒 —— 前端用 IK 驱动腕部、用一个握力滑条驱动手指。 + +网格处理照搬 unitree_rl_mjlab/scripts/export_web_dog.py 里踩出来的三条: +· 写回顶点前要用 mesh_pos/mesh_quat 变换回原坐标系,否则重新编译会再对齐一次, + 几何整体偏移; +· MJCF 的 是**每顶点**一条,不是每面; +· spec 里源文件的法线不受 usernormal 覆盖,只能删掉网格重建。 +""" + +import json +from dataclasses import dataclass +from pathlib import Path + +import mujoco +import numpy as np +import tyro + +HERE = Path(__file__).resolve().parent.parent +WS_ROOT = (Path.home() / "linker-sim/packages/linker-robot-assets/src" + / "linker_robot_assets/assets/workstations") + + +@dataclass +class Cfg: + workstation: str = "p7_i1_l6_bimanual" + """linker-robot-assets 里的工作站名。""" + name: str = "p7_bimanual" + """导出目录名。""" + table_top: float = 0.75 + """操作台高度 [m]。""" + table_x: float = 0.42 + """操作台中心到机器人的距离 [m]。""" + decimate: int = 32 + """视觉网格的聚类抽稀格数;0 = 不抽稀。""" + gripper: str = "so101" + """末端:l6=原装五指手;so101=换成 SO-101 二指夹爪(仅 rig=p7 时有效)。""" + rig: str = "so101_pair" + """机器人:p7=linker 双臂工作站;so101_pair=桌面上并排两台 SO-101。""" + + +# ---------------------------------------------------------------- 网格处理 + +def decimate(v, f, cells=48): + """顶点聚类抽稀:包围盒切 cells^3 的格子,每格顶点并成一个。""" + lo, hi = v.min(0), v.max(0) + size = float((hi - lo).max()) + if size <= 0: + return v, f + key = np.floor((v - lo) / (size / cells)).astype(np.int64) + uniq, inv = np.unique(key, axis=0, return_inverse=True) + acc = np.zeros((len(uniq), 3)) + cnt = np.zeros(len(uniq)) + np.add.at(acc, inv, v) + np.add.at(cnt, inv, 1) + f2 = inv[f] + keep = (f2[:, 0] != f2[:, 1]) & (f2[:, 1] != f2[:, 2]) & (f2[:, 0] != f2[:, 2]) + f2 = f2[keep] + if len(f2) < 4 or len(uniq) < 4: + return v, f + used, f3 = np.unique(f2, return_inverse=True) + v2, f3 = (acc / cnt[:, None])[used], f3.reshape(-1, 3) + # 抽稀把薄零件压成一张片时,MuJoCo 会拒收("mesh volume is too small")。 + # 退化了就原样返回,少省几百 KB 而已。 + ext = v2.max(0) - v2.min(0) + if len(v2) < 20 or float(ext.min()) < 1e-3: + return v, f + return v2, f3 + + +def convex_hull(v): + """取凸包 —— MuJoCo 做网格碰撞时用的本来就是凸包,所以这一步零精度损失, + 但顶点数能从几万降到几百。""" + from scipy.spatial import ConvexHull + + h = ConvexHull(v) + used, faces = np.unique(h.simplices, return_inverse=True) + return v[used], faces.reshape(-1, 3) + + +def inline_meshes(spec: mujoco.MjSpec, m: mujoco.MjModel, cells: int) -> None: + """网格内联进 XML。 + + 这套资产的视觉几何和碰撞几何**引用同一批网格**(41 个全是两用), + 直接抽稀会改碰撞形状,不抽稀又有 49 万顶点 / 25MB。做法是拆成两份: + · 碰撞用凸包(MuJoCo 反正只用凸包,行为完全不变) + · 视觉用抽稀版 `_vis`,再把纯视觉 geom 的 meshname 改过去 + """ + by_name = {m.mesh(i).name: i for i in range(m.nmesh)} + collide, visual = set(), set() + for g in range(m.ngeom): + if m.geom_type[g] != mujoco.mjtGeom.mjGEOM_MESH: + continue + (collide if (m.geom_contype[g] or m.geom_conaffinity[g]) else visual).add( + int(m.geom_dataid[g])) + + todo = [] + for mesh in list(spec.meshes): + i = by_name.get(mesh.name) + if i is None: + continue + va, vn = m.mesh_vertadr[i], m.mesh_vertnum[i] + fa, fn = m.mesh_faceadr[i], m.mesh_facenum[i] + v = m.mesh_vert[va:va + vn].astype(np.float64) + rot = np.zeros(9) + mujoco.mju_quat2Mat(rot, m.mesh_quat[i]) + v = v @ rot.reshape(3, 3).T + m.mesh_pos[i] # 变换回原坐标系 + f = m.mesh_face[fa:fa + fn].astype(np.int64) + todo.append((mesh.name, i, v, f)) + + for nm, _, _, _ in todo: + spec.delete(spec.mesh(nm)) # 删掉重建,甩掉源文件的法线 + + def emit(name, v, f): + nu = spec.add_mesh() + nu.name = name + nu.uservert = np.round(v, 4).reshape(-1).tolist() + nu.userface = f.astype(np.int32).reshape(-1).tolist() + + n_v0 = n_v1 = 0 + for nm, i, v, f in todo: + n_v0 += len(v) + if i in collide: + try: + cv, cf = convex_hull(v) + except Exception: # noqa: BLE001 + cv, cf = decimate(v, f, 64) + emit(nm, cv, cf) + n_v1 += len(cv) + else: + emit(nm, *((decimate(v, f, cells)) if cells else (v, f))) + n_v1 += len(spec.mesh(nm).uservert) // 3 + if i in visual and i in collide: + vv, vf = decimate(v, f, cells) if cells else (v, f) + emit(f"{nm}_vis", vv, vf) + n_v1 += len(vv) + + # 纯视觉的 geom 指向抽稀版 + for b in spec.bodies: + for g in b.geoms: + if (g.contype == 0 and g.conaffinity == 0 and g.meshname + and by_name.get(g.meshname) in visual + and by_name.get(g.meshname) in collide): + g.meshname = f"{g.meshname}_vis" + print(f" 网格顶点 {n_v0} → {n_v1}") + + +# ---------------------------------------------------------------- 场景搭建 + +# DexGraspBench example 数据集里的五个物体(灵巧手抓取那套方案用的就是这五个)。 +# 目录名很长,这里给一组短名和中文标签。 +DGB_ROOT = Path.home() / "DexGraspBench/assets/example_object" +OBJECTS = [ + # 短名, 中文, 目录, 抓握截面 m, 质量 kg, rgba + # 抓握截面按手来定:L6 手张开 13.4cm、**合拢只剩 4cm**,截面超过 4~5cm 就夹不住 + # (6.6cm 的罐子实测怎么放都会被挤出去)。这里统一缩到 4cm 上下。 + ("can", "易拉罐", "core_can_be67418a10003cc9eae3efbc9dbeea", 0.040, 0.15, (0.85, 0.30, 0.25, 1)), + ("bottle", "瓶子", "core_bottle_523cddb320608c09a37f3fc191551700", 0.038, 0.12, (0.30, 0.65, 0.85, 1)), + ("jar", "罐子", "core_jar_58c9d6575d62fbaf12bc68f36f3bdd45", 0.042, 0.16, (0.55, 0.75, 0.35, 1)), + ("piano", "玩具琴", "sem_Piano_438f64dd9ff80bfddd5efa4c0e7932e", 0.045, 0.12, (0.85, 0.70, 0.25, 1)), + ("toy", "玩偶", "sem_ToyFigure_a3087c9105fe0878d18a6b3b9816ca14", 0.040, 0.10, (0.70, 0.45, 0.80, 1)), +] + + +def load_obj(path): + """极简 OBJ 读取:只要顶点和面(多边形按扇形三角化)。""" + vs, fs = [], [] + for ln in Path(path).read_text().splitlines(): + if ln.startswith("v "): + vs.append([float(x) for x in ln.split()[1:4]]) + elif ln.startswith("f "): + idx = [int(t.split("/")[0]) - 1 for t in ln.split()[1:]] + for k in range(1, len(idx) - 1): + fs.append([idx[0], idx[k], idx[k + 1]]) + return np.array(vs, float), np.array(fs, np.int64) + + +def add_objects(spec, wb, top_z, ox, ys): + """把五个测试物体摆成一排。ox 是它们离机器人的距离,ys 是横向位置。""" + objs = [] + for (nm, label, folder, width, mass, rgba), oy in zip(OBJECTS, ys): + root = DGB_ROOT / folder + vv, vf = load_obj(root / "mesh" / "simplified.obj") + ext = vv.max(0) - vv.min(0) + # 按"抓握截面"定缩放:取次长边缩到 width,长边就是高度 + scale = width / float(sorted(ext)[1]) + vv = (vv - (vv.max(0) + vv.min(0)) / 2) * scale # 顺便把几何中心挪到原点 + + # 立起来:原始网格的长轴是 y,转 90° 让它朝 z + rot = np.array([[1, 0, 0], [0, 0, -1], [0, 1, 0]], float) + vv = vv @ rot.T + half_h = float((vv[:, 2].max() - vv[:, 2].min()) / 2) + + mv = spec.add_mesh() + mv.name = f"obj_{nm}_vis" + mv.uservert = np.round(vv, 5).reshape(-1).tolist() + mv.userface = vf.astype(np.int32).reshape(-1).tolist() + + b = wb.add_body(name=f"obj_{nm}", pos=[ox, oy, top_z + half_h + 0.002]) + b.add_freejoint() + # 视觉网格背质量(凸块按密度算出来的质量是凸分解伪影,重叠处被重复累加, + # 实测能虚高好几倍 —— 直接显式给质量) + b.add_geom(name=f"obj_{nm}", type=mujoco.mjtGeom.mjGEOM_MESH, meshname=mv.name, + rgba=list(rgba), mass=mass, contype=0, conaffinity=0, group=2) + + pieces = sorted((root / "urdf" / "meshes").glob("convex_piece_*.obj")) + for k, pf in enumerate(pieces): + cv, cf = load_obj(pf) + cv = ((cv - (vv.max(0) + vv.min(0)) * 0) * scale) # 与视觉网格同一缩放 + cv = cv - (load_obj(root / "mesh" / "simplified.obj")[0].max(0) + + load_obj(root / "mesh" / "simplified.obj")[0].min(0)) / 2 * scale + cv = cv @ rot.T + mc = spec.add_mesh() + mc.name = f"obj_{nm}_col{k}" + mc.uservert = np.round(cv, 5).reshape(-1).tolist() + mc.userface = cf.astype(np.int32).reshape(-1).tolist() + b.add_geom(name=f"obj_{nm}_c{k}", type=mujoco.mjtGeom.mjGEOM_MESH, + meshname=mc.name, density=0, contype=1, conaffinity=1, condim=4, + friction=[1.2, 0.01, 0.0005], solref=[0.008, 1.0], + group=3, rgba=[1, 0, 0, 0.25]) + + objs.append({"name": nm, "label": label, "body": f"obj_{nm}", + "geom": f"obj_{nm}", "kind": "mesh", "mass": mass, + "half_h": half_h, "pieces": len(pieces), + "home": [ox, oy, top_z + half_h + 0.002]}) + return objs + + +def add_scene(spec: mujoco.MjSpec, cfg: Cfg): + """地面、灯光、操作台、三个测试物体。""" + wb = spec.worldbody + tex = spec.add_texture( + name="grid", type=mujoco.mjtTexture.mjTEXTURE_2D, + builtin=mujoco.mjtBuiltin.mjBUILTIN_CHECKER, + rgb1=[0.17, 0.20, 0.25], rgb2=[0.12, 0.15, 0.19], width=300, height=300) + del tex + mat = spec.add_material(name="grid", texrepeat=[8, 8], reflectance=0.05) + mat.textures[mujoco.mjtTextureRole.mjTEXROLE_RGB] = "grid" + wb.add_geom(name="floor", type=mujoco.mjtGeom.mjGEOM_PLANE, size=[0, 0, 0.05], + material="grid", contype=1, conaffinity=1, condim=3) + wb.add_light(pos=[0.4, 0, 3.0], dir=[0, 0, -1], + type=mujoco.mjtLightType.mjLIGHT_DIRECTIONAL) + + # 操作台:台面 + 四条腿(纯装饰,只有台面参与碰撞) + half = (0.28, 0.42, 0.02) + top_z = cfg.table_top + t = wb.add_body(name="table", pos=[cfg.table_x, 0, 0]) + t.add_geom(name="table_top", type=mujoco.mjtGeom.mjGEOM_BOX, size=list(half), + pos=[0, 0, top_z - half[2]], rgba=[0.55, 0.42, 0.30, 1], + contype=1, conaffinity=1, condim=3, friction=[1.0, 0.005, 0.0001]) + # 台面还要一份纯视觉的副本:渲染器的"视觉"过滤会把有视觉兄弟的碰撞几何藏起来 + # (桌腿是纯视觉的),不补这一份台面就画不出来,物体看着像飘在空中。 + t.add_geom(name="table_top_vis", type=mujoco.mjtGeom.mjGEOM_BOX, + size=[half[0] * 1.001, half[1] * 1.001, half[2] * 0.999], + pos=[0, 0, top_z - half[2]], rgba=[0.55, 0.42, 0.30, 1], + contype=0, conaffinity=0, mass=0) + for sx in (-1, 1): + for sy in (-1, 1): + t.add_geom(name=f"table_leg_{sx}_{sy}", type=mujoco.mjtGeom.mjGEOM_BOX, + size=[0.02, 0.02, (top_z - 2 * half[2]) / 2], + pos=[sx * (half[0] - 0.03), sy * (half[1] - 0.03), + (top_z - 2 * half[2]) / 2], + rgba=[0.38, 0.29, 0.21, 1], contype=0, conaffinity=0) + + objs = add_objects(spec, wb, top_z, cfg.table_x - 0.02, ys=[-0.26,-0.13,0.0,0.13,0.26]) + return {"table": {"x": cfg.table_x, "top": top_z, "half": list(half)}, + "objects": objs} + + +# ---------------------------------------------------------------- 换末端 + +SO101_XML = (Path.home() / "unitree_rl_mjlab/src/assets/robots/unitree_go2_so101" + / "xmls/so101/so101.xml") +SO101_KP, SO101_KV, SO101_FRC = 998.22, 2.731, 2.94 + + +def swap_gripper(spec: mujoco.MjSpec): + """把 L6 五指手换成 SO-101 的二指夹爪。 + + 五指手在这套场景里抓不住东西:张开 13.4cm、合拢只剩 4cm,四指从张开位置 + 扫向掌心的路径正好经过物体,合拢那一下必把物体挤飞(试过 18 组瞄准位置、 + 三种掌心朝向、拇指先合的分阶段闭合,都不行)。二指夹爪是已经在 Go2 上 + 验证过的方案,直接把它的 `gripper` 子树挂到腕部安装点。 + """ + info = {} + for side in ("left", "right"): + # 每侧读一份:attach_body 会把源 spec 里的 body 取走,复用同一份第二次就是 None + grip_src = mujoco.MjSpec.from_file(str(SO101_XML)) + for act in list(grip_src.actuators): + grip_src.delete(act) # 执行器这边自己重建 + pre = "l" if side == "left" else "r" + hand = spec.body(f"hand_{side}_{pre}h_hand_base_link") + parent, pos, quat = hand.parent, list(hand.pos), list(hand.quat) + # 先删执行器再删 body,否则会留下指向已删关节的悬空执行器 + for act in list(spec.actuators): + if act.name.startswith(f"hand_{side}_"): + spec.delete(act) + for eq in list(spec.equalities): + if (eq.name1 or "").startswith(f"hand_{side}_") or (eq.name2 or "").startswith(f"hand_{side}_"): + spec.delete(eq) + spec.delete(hand) + + frame = parent.add_frame(pos=pos, quat=quat) + frame.attach_body(grip_src.body("gripper"), f"grip_{side}_", "") + a = spec.add_actuator() + a.name = f"grip_{side}_act" + a.target = f"grip_{side}_gripper" + a.trntype = mujoco.mjtTrn.mjTRN_JOINT + a.gaintype = mujoco.mjtGain.mjGAIN_FIXED + a.biastype = mujoco.mjtBias.mjBIAS_AFFINE + a.gainprm = [SO101_KP] + [0.0] * 9 + a.biasprm = [0.0, -SO101_KP, -SO101_KV] + [0.0] * 7 + a.forcerange = [-SO101_FRC, SO101_FRC] + a.forcelimited = mujoco.mjtLimited.mjLIMITED_TRUE + info[side] = {"prefix": f"grip_{side}_"} + spec.assets = {**(spec.assets or {}), **_so101_assets()} + return info + + +def _so101_assets(): + d = SO101_XML.parent / "assets" + return {f.name: f.read_bytes() for f in d.glob("*")} if d.is_dir() else {} + + +# ---------------------------------------------------------------- 双 SO-101 + +SO101_JOINTS = ("shoulder_pan", "shoulder_lift", "elbow_flex", + "wrist_flex", "wrist_roll") +PAIR = { # 桌面式布局:两台并排装在台面上,都朝 +x + "table_top": 0.40, "table_x": 0.15, "table_half": (0.30, 0.40, 0.02), + "mount_y": 0.22, "obj_x": 0.27, + "obj_ys": (-0.20, -0.10, 0.0, 0.10, 0.20), +} + + +def build_so101_pair(cfg: Cfg): + """从零搭一个场景:地面 + 操作台 + 台面上并排两台 SO-101。 + + 选这个而不是给 P7 换爪:SO-101 只有 5 个自由度,任务约束正好是 + 位置 3 + 爪口朝向 2,IK 良定;而且这套夹爪 + 这个尺寸的物体在 Go2 抓放 + demo 里已经端到端验过(随机位置 12 次成功 10 次),数值可以直接搬。 + """ + spec = mujoco.MjSpec() + spec.option.timestep = 0.002 + spec.option.integrator = mujoco.mjtIntegrator.mjINT_IMPLICITFAST + spec.option.iterations = 20 + spec.option.ls_iterations = 20 + wb = spec.worldbody + + tex = spec.add_texture( + name="grid", type=mujoco.mjtTexture.mjTEXTURE_2D, + builtin=mujoco.mjtBuiltin.mjBUILTIN_CHECKER, + rgb1=[0.17, 0.20, 0.25], rgb2=[0.12, 0.15, 0.19], width=300, height=300) + del tex + mat = spec.add_material(name="grid", texrepeat=[8, 8], reflectance=0.05) + mat.textures[mujoco.mjtTextureRole.mjTEXROLE_RGB] = "grid" + wb.add_geom(name="floor", type=mujoco.mjtGeom.mjGEOM_PLANE, size=[0, 0, 0.05], + material="grid", contype=1, conaffinity=1, condim=3) + wb.add_light(pos=[0.3, 0, 2.5], dir=[0, 0, -1], + type=mujoco.mjtLightType.mjLIGHT_DIRECTIONAL) + + P = PAIR + half, top = P["table_half"], P["table_top"] + t = wb.add_body(name="table", pos=[P["table_x"], 0, 0]) + t.add_geom(name="table_top", type=mujoco.mjtGeom.mjGEOM_BOX, size=list(half), + pos=[0, 0, top - half[2]], rgba=[0.55, 0.42, 0.30, 1], + contype=1, conaffinity=1, condim=3, friction=[1.0, 0.005, 0.0001]) + t.add_geom(name="table_top_vis", type=mujoco.mjtGeom.mjGEOM_BOX, + size=[half[0] * 1.001, half[1] * 1.001, half[2] * 0.999], + pos=[0, 0, top - half[2]], rgba=[0.55, 0.42, 0.30, 1], + contype=0, conaffinity=0, mass=0) + for sx in (-1, 1): + for sy in (-1, 1): + t.add_geom(name=f"table_leg_{sx}_{sy}", type=mujoco.mjtGeom.mjGEOM_BOX, + size=[0.02, 0.02, (top - 2 * half[2]) / 2], + pos=[sx * (half[0] - 0.03), sy * (half[1] - 0.03), + (top - 2 * half[2]) / 2], + rgba=[0.38, 0.29, 0.21, 1], contype=0, conaffinity=0) + + for side, sy in (("left", 1), ("right", -1)): + arm = mujoco.MjSpec.from_file(str(SO101_XML)) + frame = wb.add_frame(pos=[0.0, sy * P["mount_y"], top]) + spec.attach(arm, prefix=f"arm_{side}_", frame=frame) + return spec, {"table": {"x": P["table_x"], "top": top, "half": list(half)}} + + +def ik_pos(m, d, qadr, rng, eepos, target, iters=400, axis_fn=None, axis_w=None): + """位置(+可选末端朝向)IK,有限差分雅可比。用来算就绪位形。 + + 就绪位形必须和前端默认的朝向约束**用同一套约束**解,否则页面一打开 + 就在跟自己较劲(实测残差 69mm,手臂一直在往回摆)。 + """ + W = 0.25 + q = np.zeros(len(qadr)) + rows = 6 if axis_fn is not None else 3 + + def err(dd): + e = [target - eepos(dd)] + if axis_fn is not None: + e.append(W * (np.asarray(axis_w) - axis_fn(dd))) + return np.concatenate(e) + + for _ in range(iters): + d.qpos[qadr] = np.clip(q, rng[:, 0], rng[:, 1]) + mujoco.mj_kinematics(m, d) + e = err(d) + if np.linalg.norm(e[:3]) < 5e-4 and (rows == 3 or np.linalg.norm(e[3:]) / W < 0.05): + break + J = np.zeros((rows, len(qadr))) + for j in range(len(qadr)): + qh = q.copy() + qh[j] = np.clip(qh[j] + 1e-4, rng[j, 0], rng[j, 1]) + dq = qh[j] - q[j] + if abs(dq) < 1e-12: + continue + d.qpos[qadr] = np.clip(qh, rng[:, 0], rng[:, 1]) + mujoco.mj_kinematics(m, d) + J[:, j] = (err(d) - e) / -dq + q = np.clip(q + 0.5 * (J.T @ np.linalg.solve(J @ J.T + 1e-4 * np.eye(rows), e)), + rng[:, 0], rng[:, 1]) + d.qpos[qadr] = np.clip(q, rng[:, 0], rng[:, 1]) + mujoco.mj_kinematics(m, d) + return q, float(np.linalg.norm(target - eepos(d))) + + +# ---------------------------------------------------------------- 就绪位形 + +ARM_PREFIX = {"left": "L", "right": "R"} # 左臂关节叫 L1..L7,右臂叫 R1..R7 + + +def arm_joints(side): + return [f"arm_{side}_{ARM_PREFIX[side]}{i}_Joint" for i in range(1, 8)] + + +def solve_ready(m, d, side, target): + """把某一侧的腕部 site 解到 target,返回 7 个臂关节角。""" + jn = arm_joints(side) + qadr = np.array([m.jnt_qposadr[m.joint(n).id] for n in jn]) + dadr = np.array([m.jnt_dofadr[m.joint(n).id] for n in jn]) + rng = np.array([m.jnt_range[m.joint(n).id] for n in jn]) + sid = m.site(f"arm_{side}_tool0").id + q = np.zeros(7) + jacp, jacr = np.zeros((3, m.nv)), np.zeros((3, m.nv)) + for _ in range(300): + d.qpos[qadr] = q + mujoco.mj_kinematics(m, d) + mujoco.mj_comPos(m, d) + e = target - d.site_xpos[sid] + if np.linalg.norm(e) < 5e-4: + break + mujoco.mj_jacSite(m, d, jacp, jacr, sid) + J = jacp[:, dadr] + q = np.clip(q + 0.6 * (J.T @ np.linalg.solve(J @ J.T + 1e-4 * np.eye(3), e)), + rng[:, 0], rng[:, 1]) + return q.tolist(), qadr.tolist(), float(np.linalg.norm(target - d.site_xpos[sid])) + + +def build_p7(cfg: Cfg): + """linker 双臂工作站 + 操作台 + 物体。""" + src = WS_ROOT / cfg.workstation / "workstation.mjcf" + assert src.exists(), f"找不到工作站:{src}" + # MjSpec 不认 .mjcf 后缀,也不会跟着 ../../ 去找网格。 + # 读成字符串 + 把引用到的网格按**原样的相对路径**塞进 assets 字典。 + import re as _re + text = src.read_text() + assets = {} + for ref in sorted(set(_re.findall(r'file="([^"]+)"', text))): + f = (src.parent / ref).resolve() + if f.is_file(): + assets[ref] = f.read_bytes() + else: + print(f" [警告] 网格找不到:{ref}") + print(f" 载入网格 {len(assets)} 个") + spec = mujoco.MjSpec.from_string(text, assets=assets) + if cfg.gripper == "so101": + swap_gripper(spec) + print(" 末端已换成 SO-101 二指夹爪") + info = add_scene(spec, cfg) + spec.option.timestep = 0.002 + spec.option.integrator = mujoco.mjtIntegrator.mjINT_IMPLICITFAST + spec.option.iterations = 20 + spec.option.ls_iterations = 20 + return spec, info + + +def main() -> None: + cfg = tyro.cli(Cfg) + out = HERE / "scenes" / cfg.name + out.mkdir(parents=True, exist_ok=True) + + if cfg.rig == "so101_pair": + spec, info = build_so101_pair(cfg) + info["objects"] = add_objects(spec, spec.worldbody, PAIR["table_top"], + PAIR["obj_x"], list(PAIR["obj_ys"])) + print(f" 双 SO-101:台面 z={PAIR['table_top']} 底座 y=±{PAIR['mount_y']}") + else: + spec, info = build_p7(cfg) + m = spec.compile() + d = mujoco.MjData(m) + + # 就绪位形:末端停在台面上方 + ready = {} + if cfg.rig == "so101_pair": + P = PAIR + for side, sy in (("left", 1), ("right", -1)): + jn = [f"arm_{side}_{j}" for j in SO101_JOINTS] + qadr = np.array([m.jnt_qposadr[m.joint(n).id] for n in jn]) + rng = np.array([m.jnt_range[m.joint(n).id] for n in jn]) + ga = [g for g in range(m.ngeom) + if (m.geom(g).name or "").startswith(f"arm_{side}_fixed_jaw_sph_tip")] + gb = [g for g in range(m.ngeom) + if (m.geom(g).name or "").startswith(f"arm_{side}_moving_jaw_sph_tip")] + + def jaw_mid(dd, ga=ga, gb=gb): + return (dd.geom_xpos[ga].mean(0) + dd.geom_xpos[gb].mean(0)) / 2 + + sid = m.site(f"arm_{side}_gripperframe").id + + def jaw_axis(dd, sid=sid): + return dd.site_xmat[sid].reshape(3, 3)[:, 0] # 爪口进近轴 = site 局部 +x + + # 爪口朝下的可达高度有限:离台面 6cm 残差 0.3mm、10cm 9mm、14cm 就 38mm 了 + tgt = np.array([0.22, sy * (P["mount_y"] - 0.05), P["table_top"] + 0.07]) + q, err = ik_pos(m, d, qadr, rng, jaw_mid, tgt, + axis_fn=jaw_axis, axis_w=[0, 0, -1]) + ready[side] = {"q": q.tolist(), "qadr": qadr.tolist(), "err": err} + print(f" 就绪位形 {side:5s} 残差 {err * 1000:.1f} mm") + else: + for side, ty in (("left", 0.18), ("right", -0.18)): + q, qadr, err = solve_ready(m, d, side, + np.array([cfg.table_x - 0.06, ty, cfg.table_top + 0.16])) + ready[side] = {"q": q, "qadr": qadr, "err": err} + print(f" 就绪位形 {side:5s} 残差 {err * 1000:.1f} mm") + + qpos0 = np.array(m.qpos0, dtype=float).copy() + for side in ("left", "right"): + qpos0[ready[side]["qadr"]] = ready[side]["q"] + key = spec.add_key() + key.name = "ready" + key.qpos = qpos0.tolist() + key.ctrl = [0.0] * m.nu + + inline_meshes(spec, m, cfg.decimate) + xml = spec.to_xml().replace(' class=""', "") + (out / "model.xml").write_text(xml) + mb = (out / "model.xml").stat().st_size / 1e6 + + # 重新编译一遍,确认自包含且几何没跑偏,同时取出前端要的索引 + m2 = mujoco.MjModel.from_xml_path(str(out / "model.xml")) + d2 = mujoco.MjData(m2) + mujoco.mj_resetDataKeyframe(m2, d2, 0) + mujoco.mj_forward(m2, d2) + + def act_idx(prefix): + return [i for i in range(m2.nu) if m2.actuator(i).name.startswith(prefix)] + + arms, hands = {}, {} + gname = lambda g: m2.geom(g).name or "" # noqa: E731 + for side in ("left", "right"): + if cfg.rig == "so101_pair": + pre = f"arm_{side}_" + a = [i for i in range(m2.nu) + if m2.actuator(i).name in [f"{pre}{j}" for j in SO101_JOINTS]] + arms[side] = { + "act": a, + "names": [m2.actuator(i).name for i in a], + "qadr": [int(m2.jnt_qposadr[int(m2.actuator_trnid[i][0])]) for i in a], + "range": [[float(x) for x in m2.actuator_ctrlrange[i]] for i in a], + "site": f"{pre}gripperframe", + "site_id": int(m2.site(f"{pre}gripperframe").id), + "ready": ready[side]["q"], + } + gi = [i for i in range(m2.nu) if m2.actuator(i).name == f"{pre}gripper"] + hands[side] = { + "kind": "so101", + "act": gi, + "names": [m2.actuator(i).name for i in gi], + "range": [[float(x) for x in m2.actuator_ctrlrange[i]] for i in gi], + "geoms": [g for g in range(m2.ngeom) + if (m2.body(int(m2.geom_bodyid[g])).name or "").startswith(pre) + and (m2.geom_contype[g] or m2.geom_conaffinity[g])], + # 夹持中心 = 两爪指尖球的中点(Go2 抓放 demo 验证过的算法) + "jaw_a": [g for g in range(m2.ngeom) if gname(g).startswith(f"{pre}fixed_jaw_sph_tip")], + "jaw_b": [g for g in range(m2.ngeom) if gname(g).startswith(f"{pre}moving_jaw_sph_tip")], + "palm_body": int(m2.body(f"{pre}gripper").id), + "site_id": int(m2.site(f"{pre}gripperframe").id), + } + continue + + a = act_idx(f"arm_{side}_") + h = act_idx(f"hand_{side}_") + arms[side] = { + "act": a, + "names": [m2.actuator(i).name for i in a], + "qadr": [int(m2.jnt_qposadr[int(m2.actuator_trnid[i][0])]) for i in a], + "range": [[float(x) for x in m2.actuator_ctrlrange[i]] for i in a], + "site": f"arm_{side}_tool0", + "site_id": int(m2.site(f"arm_{side}_tool0").id), + "ready": ready[side]["q"], + } + if cfg.gripper == "so101": + pre = f"grip_{side}_" + h = [i for i in range(m2.nu) if m2.actuator(i).name.startswith(pre)] + gname = lambda g: m2.geom(g).name or "" # noqa: E731 + hands[side] = { + "kind": "so101", + "act": h, + "names": [m2.actuator(i).name for i in h], + "range": [[float(x) for x in m2.actuator_ctrlrange[i]] for i in h], + "geoms": [g for g in range(m2.ngeom) + if (m2.body(int(m2.geom_bodyid[g])).name or "").startswith(pre) + and (m2.geom_contype[g] or m2.geom_conaffinity[g])], + # 夹持中心 = 两爪指尖球的中点(Go2 上验证过的算法) + "jaw_a": [g for g in range(m2.ngeom) if gname(g).startswith(f"{pre}fixed_jaw_sph_tip")], + "jaw_b": [g for g in range(m2.ngeom) if gname(g).startswith(f"{pre}moving_jaw_sph_tip")], + "palm_body": int(m2.body(f"{pre}gripper").id), + "site": f"{pre}gripperframe", + "site_id": int(m2.site(f"{pre}gripperframe").id), + } + else: + pre = f"hand_{side}_{'l' if side == 'left' else 'r'}h_" + hands[side] = { + "kind": "l6", + "act": h, + "names": [m2.actuator(i).name for i in h], + "range": [[float(x) for x in m2.actuator_ctrlrange[i]] for i in h], + # 原始资产的 geom 是靠 class 配的、大多没名字,只能按所属 body 来筛 + "geoms": [g for g in range(m2.ngeom) + if (m2.body(int(m2.geom_bodyid[g])).name or "").startswith(f"hand_{side}_") + and (m2.geom_contype[g] or m2.geom_conaffinity[g])], + "tip_bodies": [int(m2.body(f"{pre}{t}_tip").id) + for t in ("thumb", "index", "middle", "ring", "pinky")], + "palm_body": int(m2.body(f"{pre}hand_base_link").id), + } + + meta = { + "name": cfg.name, + "workstation": cfg.workstation, + "gripper": cfg.gripper, + "sim": {"timestep": float(m2.opt.timestep), "substeps": 10}, + "nq": int(m2.nq), "nu": int(m2.nu), "ngeom": int(m2.ngeom), + "kps": [float(m2.actuator_gainprm[i][0]) for i in range(m2.nu)], + "arms": arms, "hands": hands, + "scene": info, + "xmlMB": round(mb, 2), + } + # 物体的 qpos 地址(自由关节),前端要用来改位置 + for o in meta["scene"]["objects"]: + bid = m2.body(o["body"]).id + o["qadr"] = int(m2.jnt_qposadr[m2.body_jntadr[bid]]) + o["body_id"] = int(bid) + o["geom_id"] = int(m2.geom(o["geom"]).id) + (out / "meta.json").write_text(json.dumps(meta, ensure_ascii=False)) + + print(f"已导出 {cfg.name} → {out}") + print(f" model.xml {mb:.2f} MB | nq={m2.nq} nu={m2.nu} ngeom={m2.ngeom} nmesh={m2.nmesh}") + print(f" 桌面 z={info['table']['top']} 中心 x={info['table']['x']} | 物体 " + + ", ".join(o["name"] for o in meta["scene"]["objects"])) + + +if __name__ == "__main__": + main()