Add G20 left-hand MuJoCo PLAN bridge, assets, and docs.

Bring in kinematic hard-write control with mesh-convex collision projection, SDK mapping aligned to Isaac, launch/run scripts, and implementation/tech writeups.
2026-07-27 13:45:33 +08:00
parent 818ba00524
commit 246b48b1fb
33 changed files with 4030 additions and 0 deletions
@@ -0,0 +1,328 @@
+# G20 左手 · Isaac Sim 仿真实现说明
+
+本文记录 **Isaac G20 PLAN 名义弧度桥** 从 URDF 到 USD、再到可跑仿真的完整实现过程，以及相对原始 URDF / 自动转换结果的全部具体改动。
+
+- **函数调用与逐步逻辑**：见 [`G20_Isaac_技术详解.md`](./G20_Isaac_技术详解.md)（推荐先读此文理解代码）
+- **MuJoCo 对照**：[`G20_MuJoCo_实现说明.md`](./G20_MuJoCo_实现说明.md) / [`G20_MuJoCo_技术详解.md`](./G20_MuJoCo_技术详解.md)
+
+---
+
+## 1. 目标与数据流
+
+### 1.1 目标
+
+- 在 Isaac Sim / PhysX 中仿真 LinkerHand G20 **左手**。
+- 默认兼容真机 SDK 话题：`/g20/cb_left_hand_control_cmd`（0~255）。
+- 可选 PLAN 名义弧度话题（按关节名）。
+- 关节名、映射、mimic、拇指上限 **1.47**、对比视角与 MuJoCo 桥对齐。
+- 默认 **位置硬写**（`set_dof_positions`），可选 `--pd-drive`。
+
+### 1.2 端到端数据流
+
+```text
+/g20/cb_left_hand_control_cmd          (默认，0~255)
+  或 /retarget/g20/left/joint_target_nominal  (--plan-nominal)
+  或 /cb_left_hand_control_cmd               (--legacy-sdk，旧无前缀)
+        │
+        ▼
+  joint_mapping.py
+    · range_to_arc / sdk_range_to_*（hand="G20", side="left"）
+    · SDK15：thumb_mcp = arc/1.02
+    · SDK16..19：*_pip = arc/0.89
+    · complete_g20_mimic_positions
+        │
+        ▼
+  isaac_g20_plan_bridge.py
+    · Articulation 硬写 set_dof_positions（默认）
+      或 set_dof_position_targets（--pd-drive）
+    · 每 render 帧内 phys_per_render 次物理步进（默认 16）
+        │
+        ▼
+  /sim/isaac/g20/left/joint_state
+  + Omniverse 透视相机（COMPARE_*）
+```
+
+---
+
+## 2. 关键文件路径
+
+| 角色 | 绝对路径 |
+|------|----------|
+| 启动脚本 | `/home/lxqs/isaacsim/linkerhand/run_g20_plan.sh` |
+| 桥主程序 | `/home/lxqs/isaacsim/linkerhand/isaac_g20_plan_bridge.py` |
+| 关节映射 | `/home/lxqs/isaacsim/linkerhand/joint_mapping.py` |
+| 源 URDF | `/home/lxqs/isaacsim/linkerhand/urdf/g20/left/linkerhand_g20_left.urdf` |
+| 网格 | `/home/lxqs/isaacsim/linkerhand/urdf/g20/left/meshes/*.STL` |
+| 默认 USD 根 | `/home/lxqs/isaacsim/linkerhand/urdf/g20/left/linkerhand_g20_left/linkerhand_g20_left.usda` |
+| Isaac Python | `$ISAAC_SIM_DIR/python.sh`（默认 `/home/lxqs/isaacsim/_build/linux-x86_64/release/python.sh`） |
+
+**旁路（非 PLAN 桥，勿混淆）：**
+
+- `linker_hand_dual_sim.py` + `run_sim.sh`：另一套双端/稳定性仿真，默认 PD 参数不同。
+
+---
+
+## 3. 从 URDF 到 USD
+
+### 3.1 URDF 来源
+
+- SolidWorks → URDF Exporter 导出的 G20 左手。
+- 与 MuJoCo 仓内 URDF/网格同源；网格与 MuJoCo 侧 **MD5 一致**。
+- 当前仅有 **左手** USD；无 G20 右手资产。
+
+### 3.2 URDF 上我们改过什么
+
+| 改动项 | 具体内容 |
+|--------|----------|
+| `thumb_cmc_roll` 上限 | `upper="1.47"`（rad）；注释/映射写明相对原 **1.4** 试调 |
+| mimic（URDF 内保留） | `thumb_ip` ← `thumb_mcp` × **1.02**；四指 `*_dip` ← `*_pip` × **0.89** |
+| 其余限位 | 与 MuJoCo 侧同一套（见 MuJoCo 文档关节表） |
+
+Isaac 侧 Physics USD 中对应角度上限：
+
+- `physics:upperLimit = 84.22480`（度）= **1.47 rad**（`payloads/Physics/physics.usda` 中 `thumb_cmc_roll`）。
+
+### 3.3 转换方式（重要）
+
+**运行时不导入 URDF。**
+
+桥内 `_load_g20()` 使用：
+
+```text
+stage_utils.add_reference_to_stage(usd_path=默认 .usda)
+```
+
+挂的是 **预烘焙 USD**。根层标注：
+
+```text
+creator = "URDF USD Converter v0.1.3"
+```
+
+即离线用 Isaac / URDF→USD 转换器生成，再经手改 Physics / 碰撞实例后定稿。
+
+### 3.4 USD 资源树（转换产物结构）
+
+```text
+linkerhand_g20_left.usda          # 根；默认 Physics variant = "physx"
+  └── payloads/
+        base.usda
+        robot.usda
+        geometries.usd / materials.usda
+        instances.usda            # 碰撞实例（convexHull + 选择性开关）
+        Physics/
+          physics.usda            # 关节限位等
+          physx.usda              # PhysX articulation / 自碰等
+          mujoco.usda             # 转换器附带的 Mjc 层（桥走 physx）
+          hand_body_limits.usda   # 刚体线/角速度与阻尼限制
+```
+
+桥启动时显式：`set_prim_variants(..., [("Physics", "physx")])`。
+
+### 3.5 相对「纯 URDF→USD」我们改过的 Physics / 碰撞
+
+#### A. 自碰与求解器（`physx.usda`）
+
+| 项 | 值 |
+|----|-----|
+| `physxArticulation:enabledSelfCollisions` | **1**（开） |
+| solver 位置/速度迭代 | **32 / 4** |
+| `sleepThreshold` | **0.005** |
+| 关节 `maxJointVelocity` | **171.887** |
+| 关节 `armature` | **0.05** |
+
+#### B. 刚体限速（`hand_body_limits.usda`）
+
+各 link 大致：
+
+- `maxLinearVelocity = 3.0`
+- `maxAngularVelocity = 360.0`
+- linear/angular damping **6.0**
+
+目的：降低 PD 握拳等场景下爆炸/乱飞。
+
+#### C. 选择性碰撞（`instances.usda`）——核心手改
+
+碰撞近似：`physics:approximation = "convexHull"`，`purpose = "guide"`。  
+注释写明策略：**掌心 + 指尖开碰撞；近端/中节多数关掉**，避免张开姿态假嵌入、以及 PD 握拳末端爆炸。
+
+| 碰撞 | 典型 link（`physics:collisionEnabled`） |
+|------|------------------------------------------|
+| **ON (=1)** | `hand_base_link`（掌心）、`thumb_distal`、`index_distal`、`middle_distal`、`pinky_distal`（`ring_distal` 复用 index 远端实例，同样 ON） |
+| **OFF (=0)** | CMC/掌骨近端、四指 `proximal`/`middle`、拇指 shaft 等 |
+
+接触偏移示例：`contactOffset = 0.001`，`restOffset = -0.0005`。
+
+说明：这与 MuJoCo 的「胶囊 + exclude + 投影」是 **不同手段**；Isaac 默认硬写时 PhysX 接触 **不能** 像 MuJoCo `collision_project` 那样在写位置前把姿态投影回去，硬写仍可能视觉穿模。
+
+#### D. 运行时补丁（桥代码，非 USD 文件）
+
+| 函数/常量 | 具体行为 |
+|-----------|----------|
+| `_patch_joint_max_forces` | RevoluteJoint angular drive `maxForce` 若 **&lt; 8.0** 则抬到 **8.0** |
+| `_fix_base_to_world` | 钉住 `hand_base_link`（改 `root_joint` 或建 `WorldFixedJoint`） |
+| 安装位姿 | `/World/G20Hand` 位置 **`[0.0, 0.45, 0.70]`**（`HAND_MOUNT_Y/Z`） |
+| 自碰 API | 默认跟随 USD 开自碰；`--no-self-collision` 时 `set_enabled_self_collisions(False)` |
+
+---
+
+## 4. 关节映射 `joint_mapping.py`
+
+与 MuJoCo `g20_joints.py` 的 G20 左手数值 **一致**（同一套 SDK 表 / MIN/MAX/DIR / OPEN / mimic / SDK15–19 规则）。
+
+### 4.1 SDK → 关节
+
+| SDK | 关节 |
+|-----|------|
+| 0 | `thumb_cmc_pitch` |
+| 1–4 | `index/middle/ring/pinky_mcp_pitch` |
+| 5 | `thumb_cmc_roll` |
+| 6–9 | `index/middle/ring/pinky_mcp_roll` |
+| 10 | `thumb_cmc_yaw` |
+| 11–14 | 预留 |
+| 15 | `thumb_mcp` ← 写入 `arc/1.02` |
+| 16–19 | `*_pip` ← 写入 `arc/0.89` |
+
+### 4.2 我们写入/试调过的表项
+
+```text
+# 通道5 注释：试调 1.47（原 1.4）
+G20_L_MAX[5] = 1.47
+
+# 左手侧摆 6..9：与 L20_L_DIR 不同，强制为 +1
+G20_L_DIR = [-1×6, 1,1,1,1, -1, 0×4, -1×5]
+
+DIP_MIMIC_MULTIPLIER = 0.89
+THUMB_IP_MIMIC_MULTIPLIER = 1.02
+```
+
+张开预设 `G20_OPEN_CMD` 与 MuJoCo 相同：
+
+```text
+[255×6, 193, 148, 105, 42, 245, 255×4, 255×5]
+```
+
+主动 16 关节 / 从动 5 关节（mimic）划分与 MuJoCo 一致。
+
+---
+
+## 5. 桥节点 `isaac_g20_plan_bridge.py` 做了哪些事
+
+### 5.1 话题
+
+| 模式 | 订阅 | 发布 |
+|------|------|------|
+| 默认 | `/g20/cb_left_hand_control_cmd` | `/sim/isaac/g20/left/joint_state` |
+| `--plan-nominal` | `/retarget/g20/left/joint_target_nominal` | 同上 |
+| `--legacy-sdk` | `/cb_left_hand_control_cmd` | 同上 |
+
+- 消息类型：`sensor_msgs/JointState`。
+- cmd：优先按 `name` 映射到 SDK 索引；否则按下标。
+- 发布：仿真 DOF 名 + 弧度（及 velocity）。
+- `TOPIC_LEGACY_STATE = /cb_left_hand_state` 已定义，但当前 **未实际发布**（`legacy_pub` 恒为 `None`）。
+
+ROS2 节点名：`isaac_g20_plan_bridge`；启用扩展：`isaacsim.ros2.bridge`。
+
+### 5.2 写关节方式
+
+| 模式 | API | 细节 |
+|------|-----|------|
+| **默认硬写** | `set_dof_positions` + `set_dof_velocities(0)` | 写 **全关节含 mimic**；gains 全 0，避免与硬写冲突；物理步进后常再钉一次 |
+| **`--pd-drive`** | `set_dof_position_targets` | **仅主动关节**；mimic gains=0；拇指四关节 stiff/damp/effort ≈ **10/3/6**，其余 ≈ **15/2.5/8** |
+
+其它：
+
+- 物理 `dt = 1/120`；默认 `phys_per_render = 16`。
+- slew：`--slew-rad`，单位是 **rad / 物理步**，默认 **`0.0`（瞬达）**（与 MuJoCo 的 rad/s 不同）。
+- 限位边距：`LIMIT_MARGIN_RAD = 0.05`。
+- `--hand-gravity`：仅 PD 模式下有意义。
+- 检测到 NaN/Inf → 复位到张开位姿。
+
+### 5.3 对比视角（与 MuJoCo 对齐）
+
+| 常量 | 值 |
+|------|-----|
+| `COMPARE_LOOKAT_LOCAL` | `(0.04, -0.04, 0.12)` |
+| `COMPARE_DISTANCE` | `0.50` |
+| `COMPARE_ELEVATION_DEG` | `-12.0` |
+| 方位 | 相机从 **+X** 看向 lookat（等价 MuJoCo `azimuth=180`） |
+| 手世界原点 | `(0, HAND_MOUNT_Y, HAND_MOUNT_Z) = (0, 0.45, 0.70)` |
+| 相机 prim | `/OmniverseKit_Persp` |
+
+### 5.4 CLI 开关一览
+
+`--headless`、`--g20-usd`、`--plan-nominal`、`--legacy-sdk`、`--pd-drive`、`--hand-gravity`、`--no-self-collision`、`--slew-rad`、`--phys-per-render`、`--track-test`、`--cmd-topic`、`--nominal-topic`、`--sim-state-topic`、`--max-steps`
+
+**没有** MuJoCo 那套 `--no-collision-project`（Isaac 侧无同款投影算法）。
+
+### 5.5 相对「只加载 URDF 默认导入」桥层额外保证
+
+1. 固定使用预烘焙 USD + PhysX variant。  
+2. 基座钉世界，手抬离地面安装。  
+3. 驱动 maxForce 下限补丁。  
+4. 默认硬写零增益；PD 模式单独设拇指/四指增益。  
+5. 自碰默认 ON，可用 CLI 关。  
+6. ROS2 话题与 MuJoCo/真机协议对齐（状态话题前缀不同：`/sim/isaac/...`）。
+
+---
+
+## 6. 如何启动
+
+```bash
+/home/lxqs/isaacsim/linkerhand/run_g20_plan.sh
+```
+
+脚本等价于：
+
+```bash
+"$ISAAC_SIM_DIR/python.sh" /home/lxqs/isaacsim/linkerhand/isaac_g20_plan_bridge.py "$@"
+```
+
+常用：
+
+```bash
+./run_g20_plan.sh --plan-nominal
+./run_g20_plan.sh --pd-drive
+./run_g20_plan.sh --no-self-collision
+./run_g20_plan.sh --headless --track-test
+./run_g20_plan.sh --slew-rad 0.02 --phys-per-render 8
+```
+
+---
+
+## 7. 与 MuJoCo 实现的差异（必须分清）
+
+| 项 | Isaac（本文） | MuJoCo |
+|----|---------------|--------|
+| 模型 | 预烘焙 **USD** | **MJCF** |
+| 运行时是否读 URDF | **否** | 否（读 MJCF；MJCF 由 URDF 转出） |
+| 状态话题 | `/sim/isaac/g20/left/joint_state` | `/sim/mujoco/g20/left/joint_state` |
+| 默认驱动 | `set_dof_positions` 硬写 | `qpos` 硬写 + `mj_forward` |
+| 可选 PD | PhysX angular drive | MJCF `<position>` + `mj_step` |
+| **collision project** | **无** | **有**（默认开） |
+| 防穿模资产手段 | `instances.usda` 选择性 collider + 自碰 | visual/capsule 分层 + 28 条 exclude |
+| slew | rad/**物理步**，默认 0 | rad/**秒**，默认 5.0；另有墙钟 `speed=1.35` |
+| 手位姿 | 世界 `(0, 0.45, 0.70)` | 世界原点 |
+| 映射 | `joint_mapping.py` | `g20_joints.py`（G20 left 同数值） |
+
+---
+
+## 8. 改动 chronologically（便于对照）
+
+1. URDF + meshes 入库；离线 **URDF USD Converter v0.1.3** → `linkerhand_g20_left.usda`。  
+2. 手改 Physics：自碰、求解器、armature、body limits。  
+3. 手改 `instances.usda`：只开掌心 + 指尖等 collider。  
+4. `thumb_cmc_roll` 上限改为 **1.47**（URDF + Physics 度制上限 + `G20_L_MAX`）。  
+5. 实现 `isaac_g20_plan_bridge.py`：硬写默认、可选 PD、钉基座、maxForce 补丁、安装偏置。  
+6. 话题对齐 `/g20/cb_left_hand_control_cmd`；状态发 `/sim/isaac/...`。  
+7. 视角 `COMPARE_*` 与 MuJoCo 对齐（+X 看掌心）。  
+8. SDK15–19 除 mimic 与 MuJoCo/真机语义对齐（写进 `joint_mapping.py`）。  
+
+---
+
+## 9. 已知限制
+
+1. 默认硬写下，PhysX 接触 **推不开** 被写死的 DOF；穿模缓解主要靠选择性碰撞几何，**没有** MuJoCo 式投影。  
+2. 近端/中节碰撞多为 OFF：邻指中段穿模可能检不出。  
+3. 凸包近似与视觉 mesh 非凸外形不一致。  
+4. 仅左手 G20 USD。  
+5. `dual_sim` 与 PLAN 桥参数不同，联调时不要混用入口脚本。