# G20 左手 · Isaac Sim 仿真实现说明

本文记录 **Isaac G20 PLAN 名义弧度桥** 从 URDF 到 USD、再到可跑仿真的完整实现过程，以及相对原始 URDF / 自动转换结果的全部具体改动。

- **函数调用与逐步逻辑**：见 [`G20_Isaac_技术详解.md`](./G20_Isaac_技术详解.md)（推荐先读此文理解代码）
- **MuJoCo 对照**：`/home/lxqs/linker_hand_mujoco_ros2/docs/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` 若 **< 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 桥参数不同，联调时不要混用入口脚本。