双臂操作沙盒(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 <noreply@anthropic.com>
Claude-Session: https://claude.ai/code/session_016tgfYxR5mWNRKQ67PKga4T
This commit is contained in:
@@ -0,0 +1,6 @@
|
||||
__pycache__/
|
||||
*.pyc
|
||||
*.bak
|
||||
MUJOCO_LOG.TXT
|
||||
.DS_Store
|
||||
release/
|
||||
@@ -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`，本包只是双臂抓取这一条线。
|
||||
+885
@@ -0,0 +1,885 @@
|
||||
<meta charset="utf-8">
|
||||
<meta name="viewport" content="width=device-width,initial-scale=1">
|
||||
<title>双臂操作沙盒</title>
|
||||
|
||||
<script type="importmap">
|
||||
{
|
||||
"imports": {
|
||||
"three": "https://cdn.jsdelivr.net/npm/three@0.170.0/build/three.module.js",
|
||||
"three/examples/jsm/": "https://cdn.jsdelivr.net/npm/three@0.170.0/examples/jsm/"
|
||||
}
|
||||
}
|
||||
</script>
|
||||
|
||||
<style>
|
||||
/* ── Mission Control 令牌 ────────────────────────────────────────────
|
||||
深空藏青底 + 琥珀遥测 + 等宽数字。数据色只表状态,不做装饰;
|
||||
圆角一律 ≤4px。字体走系统栈而非 Google Fonts —— 这页要能离线跑。 */
|
||||
:root {
|
||||
--bg:#0B1120; --surface:#111827; --surface-2:#1A2535; --surface-on:#1E3A5F;
|
||||
--line:#1E3A5F; --line-soft:#162035;
|
||||
--data:#FFB800; --data-2:#00D4FF;
|
||||
--good:#26DE81; --warn:#FF9F43; --err:#FF4757;
|
||||
--fg:#E8F0FE; --fg-2:#8BA3C7; --fg-3:#4A6080;
|
||||
--dim:#4A6080; /* pushLog 里内联 var(--dim) 画时间戳,别改名 */
|
||||
--mono:ui-monospace,SFMono-Regular,"SF Mono",Menlo,Consolas,"Liberation Mono",monospace;
|
||||
--sans:Inter,-apple-system,BlinkMacSystemFont,"Segoe UI","PingFang SC","Microsoft YaHei",sans-serif;
|
||||
--r:3px;
|
||||
--t:100ms ease-in;
|
||||
}
|
||||
* { box-sizing:border-box; }
|
||||
html,body { height:100%; margin:0; }
|
||||
|
||||
/* 三栏控制台:左轨 / 视口 / 右轨。轨道用固定列宽,面板 display:none 时
|
||||
列依然存在,所以启动时画面不会跳。 */
|
||||
body { background:var(--bg); color:var(--fg); overflow:hidden;
|
||||
font:14px/1.5 var(--sans);
|
||||
display:grid; grid-template-columns:292px 1fr 308px; }
|
||||
#view { grid-column:2; width:100%; height:100%; min-width:0; min-height:0;
|
||||
display:block; background:var(--bg); }
|
||||
|
||||
.panel { background:var(--surface); overflow-y:auto; overflow-x:hidden;
|
||||
display:flex; flex-direction:column; padding:0 0 20px; min-height:0; }
|
||||
#left { grid-column:1; border-right:1px solid var(--line); }
|
||||
#right { grid-column:3; border-left:1px solid var(--line); }
|
||||
|
||||
/* 轨头:标题 + 状态徽章,吸顶 */
|
||||
.rail-hd { position:sticky; top:0; z-index:2; background:var(--surface);
|
||||
border-bottom:1px solid var(--line); padding:12px 14px 10px;
|
||||
display:flex; align-items:center; justify-content:space-between; gap:8px; }
|
||||
.rail-hd h3 { margin:0; font-size:13px; font-weight:600; letter-spacing:.1em;
|
||||
text-transform:uppercase; color:var(--fg); }
|
||||
.badge { font-size:10px; font-weight:600; letter-spacing:.05em; text-transform:uppercase;
|
||||
padding:2px 8px; border-radius:2px; font-family:var(--mono);
|
||||
background:rgba(0,212,255,.14); color:var(--data-2);
|
||||
border:1px solid rgba(0,212,255,.3); }
|
||||
.badge.right { background:rgba(255,184,0,.14); color:var(--data);
|
||||
border-color:rgba(255,184,0,.3); }
|
||||
|
||||
section { padding:14px 14px 0; }
|
||||
h4 { margin:0 0 8px; font-size:11px; font-weight:600; letter-spacing:.1em;
|
||||
text-transform:uppercase; color:var(--fg-2);
|
||||
display:flex; align-items:center; gap:7px; }
|
||||
h4::before { content:""; width:2px; height:11px; background:var(--data); flex:none; }
|
||||
h4 .unit { margin-left:auto; font-family:var(--mono); font-size:10px;
|
||||
letter-spacing:0; text-transform:none; color:var(--fg-3); }
|
||||
|
||||
/* ── 控件 ── */
|
||||
button { font:inherit; font-size:12px; color:var(--fg-2); background:var(--surface-2);
|
||||
border:1px solid var(--line); border-radius:var(--r); padding:6px 9px;
|
||||
cursor:pointer; transition:border-color var(--t), color var(--t),
|
||||
background var(--t); }
|
||||
button:hover { border-color:var(--data-2); color:var(--fg); }
|
||||
button:active { background:var(--surface-on); }
|
||||
button.on { background:var(--surface-on); color:var(--fg); font-weight:600;
|
||||
border-color:var(--surface-on); }
|
||||
button:focus-visible { outline:none; box-shadow:0 0 0 3px rgba(0,212,255,.28); }
|
||||
|
||||
/* 分段选择器:1px 缝隙拼成一条,像仪表面板上的连排按键 */
|
||||
.seg { display:grid; grid-auto-flow:column; grid-auto-columns:1fr; gap:1px;
|
||||
background:var(--line); border:1px solid var(--line);
|
||||
border-radius:var(--r); overflow:hidden; }
|
||||
.seg button { border:0; border-radius:0; background:var(--surface-2); }
|
||||
/* 左右臂各带自己的颜色,和场上那两个末端小球一一对应 */
|
||||
#side-left.on { color:var(--data-2); box-shadow:inset 0 -2px 0 var(--data-2); }
|
||||
#side-right.on { color:var(--data); box-shadow:inset 0 -2px 0 var(--data); }
|
||||
|
||||
input[type=number], input[type=text] {
|
||||
font:inherit; font-family:var(--mono); font-size:12px; width:100%;
|
||||
color:var(--fg); background:var(--bg); border:1px solid var(--line);
|
||||
border-radius:var(--r); padding:5px 6px; }
|
||||
input[type=number]:focus, input[type=text]:focus {
|
||||
outline:none; border-color:var(--data-2); box-shadow:0 0 0 3px rgba(0,212,255,.22); }
|
||||
input[type=range] { width:100%; accent-color:var(--data-2); margin:2px 0 0; }
|
||||
|
||||
.row { display:flex; gap:6px; align-items:center; }
|
||||
.row > * { flex:1; }
|
||||
.grid3 { display:grid; grid-template-columns:repeat(3,1fr); gap:4px; }
|
||||
|
||||
/* ── 坐标读数(Coordinate Display)── */
|
||||
.coords { border:1px solid var(--line); border-radius:var(--r);
|
||||
background:var(--bg); overflow:hidden; }
|
||||
.coord { display:flex; justify-content:space-between; align-items:baseline;
|
||||
padding:5px 9px; border-bottom:1px solid var(--line-soft); }
|
||||
.coord:last-child { border-bottom:0; }
|
||||
.coord > span { font-size:10px; font-weight:600; letter-spacing:.08em;
|
||||
text-transform:uppercase; color:var(--fg-3); }
|
||||
.coord > b { font-family:var(--mono); font-size:13px; font-weight:500;
|
||||
color:var(--data-2); font-variant-numeric:tabular-nums; }
|
||||
|
||||
/* ── 数据瓦片(Data Tile)—— IK 残差是这个界面最要紧的一个数 ── */
|
||||
.tile { margin-top:7px; border:1px solid var(--line); border-radius:var(--r);
|
||||
background:var(--bg); padding:8px 10px 9px;
|
||||
display:flex; align-items:baseline; justify-content:space-between; }
|
||||
.tile-l { font-size:10px; font-weight:600; letter-spacing:.08em;
|
||||
text-transform:uppercase; color:var(--fg-2); }
|
||||
/* 注意:refreshHud() 会整个替换 #t-err 的 className,所以字号字族只能挂 id,
|
||||
.ok/.warn/.bad 只负责上色。而 id 选择器(1-0-0)会压过单类(0-1-0),
|
||||
所以上色规则必须写成 #t-err.ok 这种同权形式,否则残差永远是默认色、不会报警。 */
|
||||
#t-err { font-family:var(--mono); font-size:22px; font-weight:700; line-height:1;
|
||||
color:var(--data); font-variant-numeric:tabular-nums; }
|
||||
#t-err.ok { color:var(--good); } #t-err.warn { color:var(--warn); }
|
||||
#t-err.bad { color:var(--err); }
|
||||
|
||||
.ok { color:var(--good); } .bad { color:var(--err); } .warn { color:var(--warn); }
|
||||
|
||||
/* ── 点动键盘 ── */
|
||||
.grid3 button { font-family:var(--mono); font-size:12px; padding:8px 4px;
|
||||
letter-spacing:.02em; }
|
||||
.slider-hd { display:flex; justify-content:space-between; align-items:baseline;
|
||||
font-size:10px; font-weight:600; letter-spacing:.08em;
|
||||
text-transform:uppercase; color:var(--fg-2); margin:9px 0 1px; }
|
||||
.slider-hd b { font-family:var(--mono); font-size:12px; font-weight:600;
|
||||
color:var(--data); letter-spacing:0; }
|
||||
|
||||
/* ── 物体卡 ── */
|
||||
.obj { border:1px solid var(--line); border-radius:var(--r);
|
||||
background:var(--bg); padding:9px; margin-bottom:8px; }
|
||||
.obj-hd { display:flex; align-items:center; gap:7px; margin-bottom:8px; }
|
||||
.obj-name { flex:1; font-size:12.5px; font-weight:600; color:var(--fg);
|
||||
overflow:hidden; text-overflow:ellipsis; white-space:nowrap; }
|
||||
.obj-hd button { flex:none; font-size:11px; padding:3px 8px;
|
||||
background:transparent; color:var(--fg-3); }
|
||||
.obj .fields { display:grid; grid-template-columns:1fr 1fr 1.25fr; gap:5px; }
|
||||
.obj .fields label { display:block; }
|
||||
.obj .fields label span { display:block; font-size:9.5px; font-weight:600;
|
||||
letter-spacing:.07em; text-transform:uppercase;
|
||||
color:var(--fg-3); margin-bottom:2px; }
|
||||
.obj .apply { width:100%; margin-top:7px; font-size:11.5px; padding:5px; }
|
||||
.obj-tel { font-family:var(--mono); font-size:10.5px; color:var(--fg-3);
|
||||
margin-top:6px; font-variant-numeric:tabular-nums; }
|
||||
|
||||
/* 状态指示灯:方形而非圆点 —— 和 Status Badge 同一套语言。
|
||||
className 会被 JS 整个替换成 dot / dot held,样式只能挂这两个类上。 */
|
||||
.dot { width:7px; height:7px; border-radius:1px; flex:none;
|
||||
background:var(--fg-3); display:inline-block;
|
||||
transition:background var(--t), box-shadow var(--t); }
|
||||
.dot.held { background:var(--good); box-shadow:0 0 7px rgba(38,222,129,.75); }
|
||||
|
||||
/* ── 自动抓取判定卡 ── */
|
||||
.plan { border:1px solid var(--line); border-radius:var(--r); background:var(--bg);
|
||||
padding:9px 10px; }
|
||||
.plan-empty { font-size:11px; color:var(--fg-3); line-height:1.5; }
|
||||
.plan-hd { display:flex; align-items:center; gap:7px; margin-bottom:7px; }
|
||||
.plan-obj { font-size:12.5px; font-weight:600; color:var(--fg); }
|
||||
.verdict { margin-left:auto; font-family:var(--mono); font-size:10px; font-weight:600;
|
||||
letter-spacing:.05em; text-transform:uppercase; padding:2px 7px;
|
||||
border-radius:2px; white-space:nowrap; }
|
||||
.verdict.go { background:rgba(38,222,129,.15); color:var(--good);
|
||||
border:1px solid rgba(38,222,129,.3); }
|
||||
.verdict.no { background:rgba(255,71,87,.15); color:var(--err);
|
||||
border:1px solid rgba(255,71,87,.3); }
|
||||
.verdict.busy { background:rgba(255,184,0,.15); color:var(--data);
|
||||
border:1px solid rgba(255,184,0,.3); }
|
||||
.plan-kv { display:flex; justify-content:space-between; gap:8px; padding:2.5px 0;
|
||||
font-size:11px; border-bottom:1px solid var(--line-soft); }
|
||||
.plan-kv:last-of-type { border-bottom:0; }
|
||||
.plan-kv > span { color:var(--fg-3); }
|
||||
.plan-kv > b { font-family:var(--mono); font-weight:500; color:var(--fg-2);
|
||||
font-variant-numeric:tabular-nums; }
|
||||
.plan-kv > b.hit { color:var(--data-2); }
|
||||
.plan-why { font-size:11px; line-height:1.5; margin-top:7px;
|
||||
padding-top:7px; border-top:1px solid var(--line-soft); }
|
||||
/* 爪口方向小罗盘:一条线表示两爪会从哪个方向合上 */
|
||||
.rose { width:44px; height:44px; flex:none; }
|
||||
.rose .ring { fill:none; stroke:var(--line); stroke-width:1; }
|
||||
.rose .obj { fill:rgba(96,165,250,.18); stroke:var(--fg-3); stroke-width:1; }
|
||||
.rose .jaw { stroke:var(--data); stroke-width:2.5; stroke-linecap:round; }
|
||||
button:disabled { opacity:.4; cursor:not-allowed; }
|
||||
button:disabled:hover { border-color:var(--line); color:var(--fg-2); }
|
||||
|
||||
/* ── 计数条 ── */
|
||||
.counters { display:grid; grid-template-columns:repeat(3,1fr); gap:1px;
|
||||
background:var(--line); border:1px solid var(--line);
|
||||
border-radius:var(--r); overflow:hidden; margin-bottom:8px; }
|
||||
.cnt { background:var(--bg); padding:6px 8px 7px; }
|
||||
.cnt span { display:block; font-size:9.5px; font-weight:600; letter-spacing:.07em;
|
||||
text-transform:uppercase; color:var(--fg-3); }
|
||||
.cnt b { font-family:var(--mono); font-size:17px; font-weight:700; line-height:1.15;
|
||||
font-variant-numeric:tabular-nums; }
|
||||
|
||||
/* ── 事件日志 ── */
|
||||
#log { max-height:210px; overflow:auto; font-size:11.5px;
|
||||
font-family:var(--mono); font-variant-numeric:tabular-nums;
|
||||
border:1px solid var(--line); border-radius:var(--r);
|
||||
background:var(--bg); padding:2px 8px; }
|
||||
#log div { padding:4px 0; border-bottom:1px solid var(--line-soft); }
|
||||
#log div:last-child { border-bottom:0; }
|
||||
|
||||
.hint { font-size:11px; color:var(--fg-3); margin-top:9px; line-height:1.5; }
|
||||
.hint b { color:var(--fg-2); font-weight:600; }
|
||||
kbd { font-family:var(--mono); font-size:10px; color:var(--fg-2);
|
||||
background:var(--surface-2); border:1px solid var(--line);
|
||||
border-radius:2px; padding:0 4px; }
|
||||
|
||||
/* ── 启动屏 ── */
|
||||
#boot { position:fixed; inset:0; z-index:9; background:var(--bg);
|
||||
display:flex; flex-direction:column; gap:10px;
|
||||
align-items:center; justify-content:center; text-align:center; }
|
||||
#boot b { font-size:13px; font-weight:600; letter-spacing:.22em;
|
||||
text-transform:uppercase; color:var(--fg); }
|
||||
#boot span { font-family:var(--mono); font-size:12px; color:var(--data); }
|
||||
#boot i { display:block; width:132px; height:2px; background:var(--line);
|
||||
position:relative; overflow:hidden; }
|
||||
#boot i::after { content:""; position:absolute; inset:0; width:42px;
|
||||
background:var(--data); animation:scan 1.15s ease-in-out infinite; }
|
||||
@keyframes scan { 0%{transform:translateX(-42px)} 100%{transform:translateX(132px)} }
|
||||
|
||||
/* 细滚动条,别抢数据的注意力 */
|
||||
.panel::-webkit-scrollbar, #log::-webkit-scrollbar { width:8px; }
|
||||
.panel::-webkit-scrollbar-thumb, #log::-webkit-scrollbar-thumb {
|
||||
background:var(--line); border-radius:4px; }
|
||||
.panel::-webkit-scrollbar-track, #log::-webkit-scrollbar-track { background:transparent; }
|
||||
|
||||
@media (max-width:1180px) {
|
||||
body { grid-template-columns:258px 1fr 272px; }
|
||||
}
|
||||
@media (prefers-reduced-motion:reduce) {
|
||||
*, *::before, *::after { animation-duration:.01ms !important;
|
||||
animation-iteration-count:1 !important; transition-duration:.01ms !important; }
|
||||
}
|
||||
</style>
|
||||
|
||||
<div class="panel" id="left" style="display:none">
|
||||
<div class="rail-hd"><h3>机械臂控制台</h3><span class="badge" id="arm-badge">LEFT</span></div>
|
||||
|
||||
<section>
|
||||
<h4>手臂</h4>
|
||||
<div class="seg">
|
||||
<button id="side-left" class="on">左臂</button>
|
||||
<button id="side-right">右臂</button>
|
||||
</div>
|
||||
</section>
|
||||
|
||||
<section>
|
||||
<h4>抓取点目标 <span class="unit">世界系 · m</span></h4>
|
||||
<div class="coords">
|
||||
<div class="coord"><span>X</span><b id="t-x">—</b></div>
|
||||
<div class="coord"><span>Y</span><b id="t-y">—</b></div>
|
||||
<div class="coord"><span>Z</span><b id="t-z">—</b></div>
|
||||
</div>
|
||||
<div class="tile"><span class="tile-l">IK 残差</span><b id="t-err">—</b></div>
|
||||
</section>
|
||||
|
||||
<section>
|
||||
<h4>点动</h4>
|
||||
<div class="grid3">
|
||||
<button data-jog="-1,0,0">X−</button><button data-jog="0,0,1">Z+ 抬</button><button data-jog="1,0,0">X+</button>
|
||||
<button data-jog="0,1,0">Y+ 左</button><button data-jog="0,0,-1">Z− 降</button><button data-jog="0,-1,0">Y− 右</button>
|
||||
</div>
|
||||
<div class="slider-hd"><span>步长</span><b><span id="stepv">2.0</span> cm</b></div>
|
||||
<input type="range" id="step" min="0.5" max="5" step="0.5" value="2">
|
||||
</section>
|
||||
|
||||
<section>
|
||||
<h4>爪口朝向</h4>
|
||||
<div class="seg">
|
||||
<button class="ap on" data-ap="down">朝下</button>
|
||||
<button class="ap" data-ap="front">朝前</button>
|
||||
<button class="ap" data-ap="free">不约束</button>
|
||||
</div>
|
||||
</section>
|
||||
|
||||
<section>
|
||||
<h4>夹爪</h4>
|
||||
<div class="slider-hd"><span>开合</span><b id="gripv">0%</b></div>
|
||||
<input type="range" id="grip" min="0" max="100" step="1" value="0">
|
||||
<div class="row" style="margin-top:7px">
|
||||
<button id="open">张开</button><button id="close">夹紧</button>
|
||||
</div>
|
||||
</section>
|
||||
|
||||
<section>
|
||||
<h4>复位</h4>
|
||||
<div class="row">
|
||||
<button id="reset-arm">复位手臂</button><button id="reset-all">全部复位</button>
|
||||
</div>
|
||||
<div class="hint"><kbd>←→↑↓</kbd> XY · <kbd>PgUp</kbd><kbd>PgDn</kbd> Z ·
|
||||
<kbd>空格</kbd> 夹紧/张开 · <kbd>Tab</kbd> 换手<br>
|
||||
爪口朝下时可达高度约台面上方 10 cm 以内,抓取先悬停 6~8 cm 再下降。</div>
|
||||
</section>
|
||||
</div>
|
||||
|
||||
<canvas id="view"></canvas>
|
||||
|
||||
<div class="panel" id="right" style="display:none">
|
||||
<div class="rail-hd"><h3>载荷与遥测</h3><span class="badge right" id="obj-badge">—</span></div>
|
||||
|
||||
<section>
|
||||
<h4>测试物体 <span class="unit">位置 · 质量</span></h4>
|
||||
<div id="objs"></div>
|
||||
<div class="hint">画面里可以直接<b>拖动物体</b>换位置;拖<b>末端小球</b>可直接带动机械臂
|
||||
(按住 <kbd>Shift</kbd> 拖高度)。</div>
|
||||
</section>
|
||||
|
||||
<section>
|
||||
<h4>自动抓取 <span class="unit">拖动物体后自动判定</span></h4>
|
||||
<div id="plan" class="plan">
|
||||
<div class="plan-empty">把物体拖到一个位置,或点「应用」/「归位」,<br>这里会判断哪只手能抓、怎么下爪。</div>
|
||||
</div>
|
||||
<div class="row" style="margin-top:7px">
|
||||
<button id="go-grasp" disabled>自动抓取</button>
|
||||
<button id="stop-grasp" style="flex:0 0 68px" disabled>中止</button>
|
||||
</div>
|
||||
</section>
|
||||
|
||||
<section>
|
||||
<h4>抓放记录</h4>
|
||||
<div class="counters" id="score">
|
||||
<div class="cnt"><span>抓取</span><b id="n-pick" class="ok">0</b></div>
|
||||
<div class="cnt"><span>放置</span><b id="n-place" class="ok">0</b></div>
|
||||
<div class="cnt"><span>掉落</span><b id="n-drop" class="bad">0</b></div>
|
||||
</div>
|
||||
<!-- 占位 div 必须和 #log 同行无空白:pushLog 用 box.firstChild.style 判断它在不在,
|
||||
一换行 firstChild 就成了空白文本节点,占位符永远清不掉。 -->
|
||||
<div id="log"><div style="color:var(--dim)">还没有记录</div></div>
|
||||
<div class="hint">判定:物体离抓取点 <9 cm 且高于原位 5 cm 并保持 0.5 秒 = 抓取成功;
|
||||
松手后静止落在台面 = 放置成功;掉到台下 = 失败。</div>
|
||||
</section>
|
||||
</div>
|
||||
|
||||
<div id="boot"><b>双臂操作沙盒</b><i></i><span id="boot-msg">加载中…</span></div>
|
||||
|
||||
<script type="module">
|
||||
import * as THREE from 'three';
|
||||
import { OrbitControls } from 'three/examples/jsm/controls/OrbitControls.js';
|
||||
import { buildGeomMeshes, applyPose } from './src/scene-bridge.js';
|
||||
import { matToQuat } from './src/math.js';
|
||||
|
||||
// 本地模块带时间戳动态导入,强制绕开浏览器的模块缓存。
|
||||
// ES 模块缓存得很死:改完 src/*.js 普通刷新照样跑旧代码,而且**不报错**,
|
||||
// 只会在调用新方法时抛 "xxx is not a function" —— 极难往缓存上想。
|
||||
const V = `?v=${Date.now()}`;
|
||||
const [manipMod, agMod] = await Promise.all([
|
||||
import(`./src/manip.js${V}`),
|
||||
import(`./src/autograsp.js${V}`),
|
||||
]);
|
||||
const { ArmController, GraspMonitor, APPROACH } = manipMod;
|
||||
const { GraspPlanner } = agMod;
|
||||
|
||||
const $ = (id) => document.getElementById(id);
|
||||
const $$ = (s) => document.querySelectorAll(s);
|
||||
const SCENE = new URLSearchParams(location.search).get('scene') || 'so101_pair';
|
||||
|
||||
$('boot-msg').textContent = '载入 MuJoCo WASM…';
|
||||
const loadMujoco = (await import('https://cdn.jsdelivr.net/npm/@mujoco/mujoco/+esm')).default;
|
||||
const mujoco = await loadMujoco();
|
||||
|
||||
$('boot-msg').textContent = '下载场景…';
|
||||
const [meta, xml] = await Promise.all([
|
||||
fetch(`./scenes/${SCENE}/meta.json`).then((r) => r.json()),
|
||||
fetch(`./scenes/${SCENE}/model.xml`).then((r) => r.text()),
|
||||
]);
|
||||
|
||||
$('boot-msg').textContent = `编译 MJCF(${meta.xmlMB} MB,网格已内联,会花几秒)…`;
|
||||
await new Promise((r) => setTimeout(r, 30));
|
||||
const model = mujoco.MjModel.from_xml_string(xml);
|
||||
const data = new mujoco.MjData(model);
|
||||
mujoco.mj_resetDataKeyframe(model, data, 0);
|
||||
mujoco.mj_forward(model, data);
|
||||
|
||||
const arms = {
|
||||
left: new ArmController(mujoco, model, data, meta, 'left'),
|
||||
right: new ArmController(mujoco, model, data, meta, 'right'),
|
||||
};
|
||||
const monitor = new GraspMonitor(model, data, meta, arms);
|
||||
const planner = new GraspPlanner(model, data, meta, arms, monitor);
|
||||
let side = 'left';
|
||||
let running = true;
|
||||
let curPlan = null; // 最近一次判定结果
|
||||
let busy = false; // 自动抓取执行中
|
||||
|
||||
// ─────────── 渲染 ───────────
|
||||
function extractDesc(m) {
|
||||
const meshes = [];
|
||||
for (let i = 0; i < m.nmesh; i++) {
|
||||
const va = m.mesh_vertadr[i], vn = m.mesh_vertnum[i];
|
||||
const fa = m.mesh_faceadr[i], fn = m.mesh_facenum[i];
|
||||
const vertices = new Float32Array(vn * 3);
|
||||
for (let k = 0; k < vn * 3; k++) vertices[k] = m.mesh_vert[va * 3 + k];
|
||||
const indices = new Uint32Array(fn * 3);
|
||||
for (let k = 0; k < fn * 3; k++) indices[k] = m.mesh_face[fa * 3 + k];
|
||||
meshes.push({ vertices, indices });
|
||||
}
|
||||
const geoms = [];
|
||||
for (let g = 0; g < m.ngeom; g++) {
|
||||
const matid = m.geom_matid ? m.geom_matid[g] : -1;
|
||||
const rgba = matid >= 0 && m.mat_rgba
|
||||
? [m.mat_rgba[matid * 4], m.mat_rgba[matid * 4 + 1],
|
||||
m.mat_rgba[matid * 4 + 2], m.mat_rgba[matid * 4 + 3]]
|
||||
: [m.geom_rgba[g * 4], m.geom_rgba[g * 4 + 1],
|
||||
m.geom_rgba[g * 4 + 2], m.geom_rgba[g * 4 + 3]];
|
||||
geoms.push({
|
||||
type: m.geom_type[g],
|
||||
size: [m.geom_size[g * 3], m.geom_size[g * 3 + 1], m.geom_size[g * 3 + 2]],
|
||||
rgba, group: m.geom_group ? m.geom_group[g] : 0,
|
||||
contype: m.geom_contype ? m.geom_contype[g] : 1,
|
||||
conaffinity: m.geom_conaffinity ? m.geom_conaffinity[g] : 1,
|
||||
body: m.geom_bodyid ? m.geom_bodyid[g] : 0,
|
||||
meshId: m.geom_dataid ? m.geom_dataid[g] : -1,
|
||||
});
|
||||
}
|
||||
return { geoms, meshes };
|
||||
}
|
||||
|
||||
const BG = 0x0b1120; // 和面板底色同一个值,视口才不会像贴上去的
|
||||
const canvas = $('view');
|
||||
const renderer = new THREE.WebGLRenderer({ canvas, antialias: true });
|
||||
renderer.setClearColor(BG);
|
||||
renderer.shadowMap.enabled = true;
|
||||
const scene = new THREE.Scene();
|
||||
scene.fog = new THREE.Fog(BG, 6, 22);
|
||||
const camera = new THREE.PerspectiveCamera(42, 1, 0.03, 60);
|
||||
camera.up.set(0, 0, 1);
|
||||
camera.position.set(1.55, -1.15, 1.45);
|
||||
const controls = new OrbitControls(camera, canvas);
|
||||
controls.target.set(0.40, 0, 0.88);
|
||||
controls.enableDamping = true; controls.dampingFactor = 0.08;
|
||||
scene.add(new THREE.AmbientLight(0xffffff, 0.62));
|
||||
const sun = new THREE.DirectionalLight(0xffffff, 1.5);
|
||||
sun.position.set(2.5, -2.5, 4); sun.castShadow = true;
|
||||
scene.add(sun);
|
||||
|
||||
const rig = buildGeomMeshes(extractDesc(model), 0, { filter: 'visual' });
|
||||
scene.add(rig.group);
|
||||
const pose = new Float32Array(model.ngeom * 7);
|
||||
|
||||
// 目标点的小标记。颜色和左右臂按钮一致:左=青(data-2),右=琥珀(data)。
|
||||
const MARK_COLOR = { left: 0x00d4ff, right: 0xffb800 };
|
||||
const marks = {};
|
||||
for (const s of ['left', 'right']) {
|
||||
const mk = new THREE.Mesh(
|
||||
new THREE.SphereGeometry(0.022, 20, 14),
|
||||
new THREE.MeshBasicMaterial({ color: MARK_COLOR[s],
|
||||
transparent: true, opacity: 0.6 }));
|
||||
mk.userData.side = s;
|
||||
scene.add(mk); marks[s] = mk;
|
||||
}
|
||||
|
||||
// ─────────── 拖拽移动物体 ───────────
|
||||
// 射线打到物体的视觉网格就开始拖,拖动时把射线和"台面高度"的水平面求交,
|
||||
// 直接改自由关节的 qpos —— 比在面板里敲坐标直观得多。
|
||||
const ray = new THREE.Raycaster();
|
||||
const ndc = new THREE.Vector2();
|
||||
const hitPt = new THREE.Vector3();
|
||||
const pickMap = new Map(); // THREE.Mesh -> 物体
|
||||
for (const o of monitor.objs) {
|
||||
const mesh = rig.meshes[o.geom_id];
|
||||
if (mesh) { pickMap.set(mesh, o); mesh.userData.obj = o; }
|
||||
}
|
||||
const pickables = [...pickMap.keys()];
|
||||
const handles = ['left', 'right'].map((s) => marks[s]); // 末端目标小球
|
||||
let dragging = null; // 正在拖的物体
|
||||
let dragArm = null; // 正在拖的末端小球所属手臂
|
||||
let lastPtrY = 0; // 上一次指针的屏幕 y,Shift 拖高度用
|
||||
const tmpV = new THREE.Vector3();
|
||||
const plane = new THREE.Plane();
|
||||
|
||||
/** 屏幕 1 像素在目标处对应多少米(按相机距离换算,拖起来手感和缩放无关)。 */
|
||||
function metersPerPixel(at) {
|
||||
const dist = camera.position.distanceTo(tmpV.set(at[0], at[1], at[2]));
|
||||
return (2 * Math.tan((camera.fov * Math.PI / 180) / 2) * dist) / canvas.clientHeight;
|
||||
}
|
||||
|
||||
function setNdc(e) {
|
||||
const r = canvas.getBoundingClientRect();
|
||||
ndc.x = ((e.clientX - r.left) / r.width) * 2 - 1;
|
||||
ndc.y = -((e.clientY - r.top) / r.height) * 2 + 1;
|
||||
ray.setFromCamera(ndc, camera);
|
||||
}
|
||||
|
||||
canvas.addEventListener('pointerdown', (e) => {
|
||||
if (e.button !== 0) return;
|
||||
setNdc(e);
|
||||
// 末端小球优先:它比物体小,挡在前面时应该先抓到它
|
||||
const hh = ray.intersectObjects(handles, false)[0];
|
||||
const ho = ray.intersectObjects(pickables, false)[0];
|
||||
if (hh && (!ho || hh.distance <= ho.distance + 0.02)) {
|
||||
dragArm = hh.object.userData.side;
|
||||
setSide(dragArm);
|
||||
} else if (ho) {
|
||||
dragging = pickMap.get(ho.object);
|
||||
} else return;
|
||||
lastPtrY = e.clientY;
|
||||
controls.enabled = false; // 拖拽时别转相机
|
||||
try { canvas.setPointerCapture(e.pointerId); } catch { /* 合成事件没有真实指针 */ }
|
||||
canvas.style.cursor = 'grabbing';
|
||||
});
|
||||
|
||||
canvas.addEventListener('pointermove', (e) => {
|
||||
if (!dragging && !dragArm) {
|
||||
setNdc(e);
|
||||
const over = ray.intersectObjects(handles, false).length
|
||||
|| ray.intersectObjects(pickables, false).length;
|
||||
canvas.style.cursor = over ? 'grab' : '';
|
||||
return;
|
||||
}
|
||||
setNdc(e);
|
||||
|
||||
// 拖末端小球 = 直接拖 IK 目标。平着拖改 XY,按住 Shift 改高度。
|
||||
if (dragArm) {
|
||||
const t = arms[dragArm].target;
|
||||
if (e.shiftKey) {
|
||||
// 竖直方向不用平面求交(相机接近俯视时那个竖直平面几乎和视线平行,
|
||||
// 交点会飞很远甚至求不出来),直接把屏幕上移多少像素换算成抬高多少米。
|
||||
arms[dragArm].jog(0, 0, (lastPtrY - e.clientY) * metersPerPixel(t));
|
||||
} else {
|
||||
plane.setFromNormalAndCoplanarPoint(new THREE.Vector3(0, 0, 1),
|
||||
new THREE.Vector3(t[0], t[1], t[2]));
|
||||
if (ray.ray.intersectPlane(plane, hitPt)) {
|
||||
arms[dragArm].jog(hitPt.x - t[0], hitPt.y - t[1], 0);
|
||||
}
|
||||
}
|
||||
lastPtrY = e.clientY;
|
||||
refreshHud();
|
||||
return;
|
||||
}
|
||||
|
||||
// 注意复用上面的 plane —— 这里再 const 一个同名的会遮蔽它,
|
||||
// 让前面 dragArm 分支里的引用落进暂时性死区直接抛异常(拖小球没反应就是这个)。
|
||||
const z = meta.scene.table.top + dragging.half_h + 0.002;
|
||||
plane.setFromNormalAndCoplanarPoint(new THREE.Vector3(0, 0, 1),
|
||||
new THREE.Vector3(0, 0, z));
|
||||
if (!ray.ray.intersectPlane(plane, hitPt)) return;
|
||||
const T = meta.scene.table, m = 0.04; // 别拖到台面外
|
||||
const x = Math.min(Math.max(hitPt.x, T.x - T.half[0] + m), T.x + T.half[0] - m);
|
||||
const y = Math.min(Math.max(hitPt.y, -T.half[1] + m), T.half[1] - m);
|
||||
placeObj(dragging.name, x, y);
|
||||
});
|
||||
|
||||
for (const ev of ['pointerup', 'pointercancel']) {
|
||||
canvas.addEventListener(ev, (e) => {
|
||||
if (!dragging && !dragArm) return;
|
||||
// 松手立刻出完整判定;再排一次落稳后的复判(物体可能翻倒,姿态会变)。
|
||||
if (dragging) replan(dragging, { fast: false, settle: 900 });
|
||||
dragging = null;
|
||||
dragArm = null;
|
||||
controls.enabled = true;
|
||||
try { canvas.releasePointerCapture?.(e.pointerId); } catch { /* 同上 */ }
|
||||
canvas.style.cursor = '';
|
||||
});
|
||||
}
|
||||
|
||||
function packPose() {
|
||||
const xpos = data.geom_xpos, xmat = data.geom_xmat;
|
||||
for (let g = 0; g < model.ngeom; g++) {
|
||||
const o = g * 7;
|
||||
pose[o] = xpos[g * 3]; pose[o + 1] = xpos[g * 3 + 1]; pose[o + 2] = xpos[g * 3 + 2];
|
||||
matToQuat(xmat, g * 9, pose, o + 3);
|
||||
}
|
||||
}
|
||||
|
||||
function resize() {
|
||||
const w = canvas.clientWidth, h = canvas.clientHeight;
|
||||
if (!w || !h) return;
|
||||
const pr = Math.min(devicePixelRatio || 1, 2);
|
||||
if (canvas.width === Math.floor(w * pr) && canvas.height === Math.floor(h * pr)) return;
|
||||
renderer.setPixelRatio(pr); renderer.setSize(w, h, false);
|
||||
camera.aspect = w / h; camera.updateProjectionMatrix();
|
||||
}
|
||||
|
||||
// ─────────── 主循环 ───────────
|
||||
const DT = meta.sim.timestep;
|
||||
const SUB = meta.sim.substeps; // 每个控制步的物理子步
|
||||
let last = performance.now();
|
||||
function tick() {
|
||||
requestAnimationFrame(tick);
|
||||
const now = performance.now();
|
||||
const el = Math.min((now - last) / 1000, 0.2); last = now;
|
||||
|
||||
if (running) {
|
||||
const ctrlSteps = Math.max(1, Math.round(el / (DT * SUB)));
|
||||
for (let c = 0; c < ctrlSteps; c++) {
|
||||
arms.left.step();
|
||||
arms.right.step();
|
||||
for (let i = 0; i < SUB; i++) mujoco.mj_step(model, data);
|
||||
const ev = monitor.step();
|
||||
if (ev) pushLog(ev);
|
||||
}
|
||||
refreshHud();
|
||||
}
|
||||
packPose(); applyPose(rig.meshes, pose);
|
||||
for (const s of ['left', 'right']) marks[s].position.set(...arms[s].target);
|
||||
resize(); controls.update();
|
||||
renderer.render(scene, camera);
|
||||
}
|
||||
|
||||
// ─────────── 界面 ───────────
|
||||
function refreshHud() {
|
||||
const a = arms[side];
|
||||
$('t-x').textContent = a.target[0].toFixed(3);
|
||||
$('t-y').textContent = a.target[1].toFixed(3);
|
||||
$('t-z').textContent = a.target[2].toFixed(3);
|
||||
$('t-err').textContent = `${(a.err * 1000).toFixed(0)} mm`;
|
||||
$('t-err').className = a.err < 0.01 ? 'ok' : a.err < 0.04 ? 'warn' : 'bad';
|
||||
let nHeld = 0;
|
||||
for (const o of monitor.objs) {
|
||||
const el = document.getElementById(`st-${o.name}`);
|
||||
if (el) {
|
||||
el.className = `dot${o.state === 'held' ? ' held' : ''}`;
|
||||
el.title = o.state === 'held' ? '已抓起' : '在台面';
|
||||
}
|
||||
if (o.state === 'held') nHeld++;
|
||||
const dv = document.getElementById(`d-${o.name}`);
|
||||
if (dv) dv.textContent = `离手 ${(o.dist ?? 0).toFixed(2)}m 抬升 ${((o.lift ?? 0) * 100).toFixed(1)}cm`;
|
||||
}
|
||||
$('obj-badge').textContent = nHeld ? `持有 ${nHeld}` : `${monitor.objs.length} 件在台`;
|
||||
}
|
||||
|
||||
// ─────────── 自动抓取:判定 + 执行 ───────────
|
||||
const R2D = 180 / Math.PI;
|
||||
|
||||
/** 爪口方向小罗盘:外圈是物体的水平轮廓,琥珀线是两爪合上的方向。 */
|
||||
function rose(pr, theta) {
|
||||
const S = 22, r = 17;
|
||||
const pts = pr.widths.map((w) => {
|
||||
// 半径按该方向的宽度缩放,画出来就是物体的粗细分布
|
||||
const a = w.deg / R2D, k = (w.w / pr.maxW) * r;
|
||||
return [S + Math.cos(a) * k, S - Math.sin(a) * k];
|
||||
});
|
||||
const poly = [...pts, ...pts.map(([x, y]) => [2 * S - x, 2 * S - y])]
|
||||
.map(([x, y]) => `${x.toFixed(1)},${y.toFixed(1)}`).join(' ');
|
||||
const jx = Math.cos(theta) * r, jy = Math.sin(theta) * r;
|
||||
return `<svg class="rose" viewBox="0 0 44 44">
|
||||
<circle class="ring" cx="22" cy="22" r="20"/>
|
||||
<polygon class="obj" points="${poly}"/>
|
||||
<line class="jaw" x1="${(S - jx).toFixed(1)}" y1="${(S + jy).toFixed(1)}"
|
||||
x2="${(S + jx).toFixed(1)}" y2="${(S - jy).toFixed(1)}"/>
|
||||
</svg>`;
|
||||
}
|
||||
|
||||
function renderPlan(p, phase) {
|
||||
const box = $('plan');
|
||||
if (!p) {
|
||||
box.innerHTML = '<div class="plan-empty">把物体拖到一个位置,或点「应用」/「归位」,'
|
||||
+ '<br>这里会判断哪只手能抓、怎么下爪。</div>';
|
||||
$('go-grasp').disabled = true;
|
||||
return;
|
||||
}
|
||||
const nm = (s) => (s === 'left' ? '左臂' : '右臂');
|
||||
const vc = phase ? 'busy' : p.ok ? 'go' : 'no';
|
||||
const vt = phase ? phase : p.ok ? nm(p.side) : '不可抓';
|
||||
const armRows = (p.arms || []).map((c) => {
|
||||
const hit = p.ok && c.side === p.side;
|
||||
const bad = !c.fits && c.reach < 0.012;
|
||||
return `<div class="plan-kv"><span>${nm(c.side)}${hit ? ' ←' : ''}</span>`
|
||||
+ `<b class="${hit ? 'hit' : ''}">${(c.reach * 1000).toFixed(0)} mm`
|
||||
+ ` · 夹 <span class="${bad ? 'bad' : ''}">${(c.gripW * 100).toFixed(1)}</span> cm`
|
||||
+ `${c.rolled ? ' · 转腕' : ''}</b></div>`;
|
||||
}).join('');
|
||||
// 罗盘画**实际**下爪方向:可抓时用选中臂真正解出来的爪口角,
|
||||
// 不可抓时退回物体的最窄方向(至少告诉你"理想是往哪夹")
|
||||
const shown = p.ok ? (p.arms.find((c) => c.side === p.side)?.yaw ?? p.theta) : p.theta;
|
||||
|
||||
box.innerHTML = `
|
||||
<div class="plan-hd">
|
||||
${rose(p.pr, shown)}
|
||||
<div>
|
||||
<div class="plan-obj">${p.obj.label || p.obj.name}</div>
|
||||
<div style="font-size:10.5px;color:var(--fg-3)">${p.post.label}
|
||||
· 各向异性 ${p.aniso.toFixed(2)}</div>
|
||||
</div>
|
||||
<span class="verdict ${vc}">${vt}</span>
|
||||
</div>
|
||||
<div class="plan-kv"><span>下爪方向</span><b>${
|
||||
(((shown * R2D) % 180 + 180) % 180).toFixed(0)}°${
|
||||
p.ok && p.rolled ? ' (转腕)' : p.ok ? ' (IK 自选)' : ''}</b></div>
|
||||
<div class="plan-kv"><span>该方向宽度</span><b>${p.ok
|
||||
? `${(p.gripW * 100).toFixed(1)} cm` : '—'}</b></div>
|
||||
<div class="plan-kv"><span>最窄 / 爪口</span><b>${(p.width * 100).toFixed(1)}
|
||||
/ ${(p.span * 100).toFixed(1)} cm</b></div>
|
||||
${armRows}
|
||||
<div class="plan-why" style="color:var(--${p.ok ? 'good' : 'err'})">${p.reason}</div>`;
|
||||
$('go-grasp').disabled = !p.ok || busy;
|
||||
}
|
||||
|
||||
/**
|
||||
* 重新判定。分两档:
|
||||
* · 拖动中走 fast(少起点、跳过悬停点,约 8 ms)—— 边拖边出结论;
|
||||
* · 松手/点按钮后跑完整版(约 40 ms),并在物体落稳后再补一次
|
||||
* —— 刚放下还在弹,这时量到的是半空中的姿态,立着/倒了会判反。
|
||||
*/
|
||||
let planTimer = null, settleTimer = null;
|
||||
function doPlan(obj, fast) {
|
||||
if (busy) return;
|
||||
try {
|
||||
curPlan = planner.plan(obj, { fast });
|
||||
} catch (e) {
|
||||
curPlan = { ok: false, obj, reason: `判定出错:${e.message}`,
|
||||
pr: { widths: [], maxW: 1, minDeg: 0 }, post: { label: '—' },
|
||||
aniso: 0, width: 0, span: 0, theta: 0, arms: [] };
|
||||
}
|
||||
renderPlan(curPlan);
|
||||
}
|
||||
|
||||
function replan(obj, { fast = true, settle = 0 } = {}) {
|
||||
clearTimeout(planTimer);
|
||||
// 30 ms 只为把一帧里的多次调用合并掉,不是"等一等" —— 拖动时每帧都会进来。
|
||||
planTimer = setTimeout(() => doPlan(obj, fast), fast ? 30 : 0);
|
||||
clearTimeout(settleTimer);
|
||||
if (settle) settleTimer = setTimeout(() => doPlan(obj, false), settle);
|
||||
}
|
||||
|
||||
async function runGrasp() {
|
||||
if (!curPlan || !curPlan.ok || busy) return;
|
||||
busy = true;
|
||||
$('go-grasp').disabled = true;
|
||||
$('stop-grasp').disabled = false;
|
||||
setSide(curPlan.side); // 让面板跟着切到执行的那只手
|
||||
const r = await planner.run(curPlan, {
|
||||
phase: (ph) => renderPlan(curPlan, ph),
|
||||
});
|
||||
busy = false;
|
||||
$('stop-grasp').disabled = true;
|
||||
curPlan = { ...curPlan, ok: r.ok, reason: r.reason };
|
||||
renderPlan(curPlan);
|
||||
$('go-grasp').disabled = false;
|
||||
}
|
||||
|
||||
$('go-grasp').onclick = runGrasp;
|
||||
$('stop-grasp').onclick = () => planner.cancel();
|
||||
|
||||
function buildObjPanel() {
|
||||
$('objs').innerHTML = monitor.objs.map((o) => `
|
||||
<div class="obj">
|
||||
<div class="obj-hd">
|
||||
<span class="dot" id="st-${o.name}"></span>
|
||||
<span class="obj-name">${o.label || o.name}</span>
|
||||
<button data-home="${o.name}">归位</button>
|
||||
</div>
|
||||
<div class="fields">
|
||||
<label><span>X m</span><input type="number" step="0.01" id="x-${o.name}" value="${o.home[0].toFixed(2)}"></label>
|
||||
<label><span>Y m</span><input type="number" step="0.01" id="y-${o.name}" value="${o.home[1].toFixed(2)}"></label>
|
||||
<label><span>质量 kg</span><input type="number" step="0.05" min="0.01" id="m-${o.name}" value="${o.mass}"></label>
|
||||
</div>
|
||||
<button class="apply" data-apply="${o.name}">应用</button>
|
||||
<div class="obj-tel" id="d-${o.name}"></div>
|
||||
</div>`).join('');
|
||||
|
||||
$$('[data-apply]').forEach((b) => { b.onclick = () => applyObj(b.dataset.apply); });
|
||||
$$('[data-home]').forEach((b) => { b.onclick = () => placeObj(b.dataset.home); });
|
||||
}
|
||||
|
||||
/** 改物体的位置和质量。质量要连惯量一起按比例缩,否则转起来不对。 */
|
||||
function applyObj(name) {
|
||||
const o = monitor.objs.find((x) => x.name === name);
|
||||
const x = +$(`x-${name}`).value, y = +$(`y-${name}`).value;
|
||||
const mNew = Math.max(0.01, +$(`m-${name}`).value);
|
||||
const b = o.body_id;
|
||||
const scale = mNew / model.body_mass[b];
|
||||
model.body_mass[b] = mNew;
|
||||
for (let k = 0; k < 3; k++) model.body_inertia[3 * b + k] *= scale;
|
||||
o.mass = mNew;
|
||||
placeObj(name, x, y);
|
||||
}
|
||||
|
||||
function placeObj(name, x, y) {
|
||||
const o = monitor.objs.find((n) => n.name === name);
|
||||
const px = x ?? o.home[0], py = y ?? o.home[1];
|
||||
const q = data.qpos, i = o.qadr;
|
||||
q[i] = px; q[i + 1] = py; q[i + 2] = meta.scene.table.top + o.half_h + 0.002;
|
||||
q[i + 3] = 1; q[i + 4] = 0; q[i + 5] = 0; q[i + 6] = 0;
|
||||
const dof = model.jnt_dofadr[model.body_jntadr[o.body_id]];
|
||||
for (let k = 0; k < 6; k++) data.qvel[dof + k] = 0;
|
||||
o.home = [px, py, q[i + 2]];
|
||||
o.state = 'idle'; o.hold = 0; o.rest = 0; o.peak = 0;
|
||||
mujoco.mj_forward(model, data);
|
||||
$(`x-${name}`).value = px.toFixed(2);
|
||||
$(`y-${name}`).value = py.toFixed(2);
|
||||
// 位置一变就重新判定。拖动时这函数每帧都会被调,所以走快速档。
|
||||
replan(o, { fast: true });
|
||||
}
|
||||
|
||||
let nPick = 0, nPlace = 0, nDrop = 0;
|
||||
function pushLog(ev) {
|
||||
const t = (ev.t * DT * SUB).toFixed(1);
|
||||
const obj = (monitor.objs.find((o) => o.name === ev.obj) || {}).label || ev.obj;
|
||||
const s = ev.side === 'left' ? '左手' : '右手';
|
||||
let html;
|
||||
if (ev.kind === 'pick') {
|
||||
nPick++;
|
||||
html = `<span class="ok">✅ 抓取成功</span> ${s} · ${obj} · 抬升 ${(ev.lift * 100).toFixed(1)} cm`;
|
||||
} else if (ev.kind === 'place') {
|
||||
nPlace++;
|
||||
html = `<span class="ok">📦 放置成功</span> ${s} · ${obj} · 落点 (${ev.pos[0].toFixed(2)}, ${ev.pos[1].toFixed(2)})`;
|
||||
} else {
|
||||
nDrop++;
|
||||
html = `<span class="bad">❌ 掉落</span> ${s} · ${obj}`;
|
||||
}
|
||||
const box = $('log');
|
||||
if (box.firstChild && box.firstChild.style) box.innerHTML = '';
|
||||
const div = document.createElement('div');
|
||||
div.innerHTML = `<span style="color:var(--dim)">${t}s</span> ${html}`;
|
||||
box.prepend(div);
|
||||
$('n-pick').textContent = nPick;
|
||||
$('n-place').textContent = nPlace;
|
||||
$('n-drop').textContent = nDrop;
|
||||
}
|
||||
|
||||
function setSide(s) {
|
||||
side = s;
|
||||
$('side-left').classList.toggle('on', s === 'left');
|
||||
$('side-right').classList.toggle('on', s === 'right');
|
||||
$('arm-badge').textContent = s === 'left' ? 'LEFT' : 'RIGHT';
|
||||
$('arm-badge').classList.toggle('right', s === 'right');
|
||||
$('grip').value = Math.round(arms[s].grip * 100);
|
||||
$('gripv').textContent = `${Math.round(arms[s].grip * 100)}%`;
|
||||
$$('.ap').forEach((b) => b.classList.toggle('on', b.dataset.ap === arms[s].approach));
|
||||
refreshHud();
|
||||
}
|
||||
|
||||
$('side-left').onclick = () => setSide('left');
|
||||
$('side-right').onclick = () => setSide('right');
|
||||
$('step').oninput = (e) => { $('stepv').textContent = (+e.target.value).toFixed(1); };
|
||||
$$('[data-jog]').forEach((b) => {
|
||||
b.onclick = () => {
|
||||
const s = +$('step').value / 100;
|
||||
const [dx, dy, dz] = b.dataset.jog.split(',').map(Number);
|
||||
arms[side].jog(dx * s, dy * s, dz * s);
|
||||
refreshHud();
|
||||
};
|
||||
});
|
||||
$$('.ap').forEach((b) => {
|
||||
b.onclick = () => {
|
||||
arms[side].approach = b.dataset.ap;
|
||||
$$('.ap').forEach((x) => x.classList.toggle('on', x === b));
|
||||
};
|
||||
});
|
||||
$('grip').oninput = (e) => {
|
||||
arms[side].grip = +e.target.value / 100;
|
||||
$('gripv').textContent = `${e.target.value}%`;
|
||||
};
|
||||
$('open').onclick = () => { $('grip').value = 0; $('grip').oninput({ target: $('grip') }); };
|
||||
$('close').onclick = () => { $('grip').value = 100; $('grip').oninput({ target: $('grip') }); };
|
||||
$('reset-arm').onclick = () => { arms[side].reset(); setSide(side); };
|
||||
$('reset-all').onclick = () => {
|
||||
mujoco.mj_resetDataKeyframe(model, data, 0);
|
||||
mujoco.mj_forward(model, data);
|
||||
arms.left.reset(); arms.right.reset();
|
||||
for (const o of monitor.objs) placeObj(o.name);
|
||||
setSide(side);
|
||||
};
|
||||
|
||||
addEventListener('keydown', (e) => {
|
||||
if (e.target.tagName === 'INPUT') return;
|
||||
const s = +$('step').value / 100;
|
||||
const map = { ArrowUp: [s, 0, 0], ArrowDown: [-s, 0, 0], ArrowLeft: [0, s, 0],
|
||||
ArrowRight: [0, -s, 0], PageUp: [0, 0, s], PageDown: [0, 0, -s] };
|
||||
if (map[e.key]) { e.preventDefault(); arms[side].jog(...map[e.key]); refreshHud(); }
|
||||
else if (e.key === ' ') {
|
||||
e.preventDefault();
|
||||
const g = arms[side].grip > 0.5 ? 0 : 100;
|
||||
$('grip').value = g; $('grip').oninput({ target: $('grip') });
|
||||
} else if (e.key === 'Tab') { e.preventDefault(); setSide(side === 'left' ? 'right' : 'left'); }
|
||||
});
|
||||
|
||||
buildObjPanel();
|
||||
setSide('left');
|
||||
$('boot').style.display = 'none';
|
||||
$('left').style.display = '';
|
||||
$('right').style.display = '';
|
||||
window.__dbg = { mujoco, model, data, meta, arms, monitor, camera, canvas, THREE,
|
||||
marks, handles, pickables, scene, planner,
|
||||
plan: () => curPlan, replan, runGrasp, placeObj };
|
||||
tick();
|
||||
</script>
|
||||
@@ -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}
|
||||
File diff suppressed because one or more lines are too long
@@ -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}
|
||||
File diff suppressed because one or more lines are too long
@@ -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)
|
||||
@@ -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`,
|
||||
};
|
||||
}
|
||||
}
|
||||
+644
@@ -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;
|
||||
}
|
||||
}
|
||||
+58
@@ -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,
|
||||
];
|
||||
}
|
||||
@@ -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<THREE.Mesh|null>, 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<THREE.Mesh|null>} 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;
|
||||
}
|
||||
}
|
||||
@@ -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"
|
||||
@@ -0,0 +1,670 @@
|
||||
"""把双臂工作站 + 操作台 + 测试物体导出成浏览器能直接跑的自包含 MJCF。
|
||||
|
||||
~/unitree_rl_mjlab/.venv/bin/python tools/export_manip_scene.py
|
||||
|
||||
产出 scenes/<name>/:
|
||||
model.xml 自包含 MJCF(网格内联 + 视觉网格抽稀,不依赖任何外部文件)
|
||||
meta.json 给前端用的索引:臂/手的执行器、工具 site、指尖 geom、
|
||||
物体和桌面的几何参数、就绪位形
|
||||
|
||||
这里没有策略,是纯手动操作的沙盒 —— 前端用 IK 驱动腕部、用一个握力滑条驱动手指。
|
||||
|
||||
网格处理照搬 unitree_rl_mjlab/scripts/export_web_dog.py 里踩出来的三条:
|
||||
· 写回顶点前要用 mesh_pos/mesh_quat 变换回原坐标系,否则重新编译会再对齐一次,
|
||||
几何整体偏移;
|
||||
· MJCF 的 <mesh normal> 是**每顶点**一条,不是每面;
|
||||
· 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 反正只用凸包,行为完全不变)
|
||||
· 视觉用抽稀版 `<name>_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()
|
||||
Reference in New Issue
Block a user