# G20 左手 · Isaac Sim 技术详解

本文面向**读代码、改逻辑、排查问题**：按「程序从哪进、函数怎么调、数据怎么流」写清楚实现细节。  
资产路径、URDF→USD 改动清单见 [`G20_Isaac_实现说明.md`](./G20_Isaac_实现说明.md)。  
MuJoCo 对照：[`G20_MuJoCo_技术详解.md`](../../linker_hand_mujoco_ros2/docs/G20_MuJoCo_技术详解.md)（若在 mujoco 仓的 `docs/` 下阅读，则为同目录链接）。

主文件：

| 角色 | 路径 |
|------|------|
| 桥主程序 | `/home/lxqs/isaacsim/linkerhand/isaac_g20_plan_bridge.py` |
| 关节映射 | `/home/lxqs/isaacsim/linkerhand/joint_mapping.py` |
| 启动脚本 | `/home/lxqs/isaacsim/linkerhand/run_g20_plan.sh` |
| 默认 USD | `.../urdf/g20/left/linkerhand_g20_left/linkerhand_g20_left.usda` |

---

## 1. 总体结构：单线程循环（与 MuJoCo 不同）

Isaac 桥**没有**单独的 ROS daemon 线程，也**没有** `threading.Lock`。  
主循环里每个物理步先 `rclpy.spin_once`，再写关节、再 `SimulationManager.step()`。

```text
┌─────────────────────────────────────────────────────────────┐
│  模块加载期（import 顶层）                                     │
│    parse_args()                                             │
│    SimulationApp(...)          # 必须最早启动 Isaac           │
│    enable_extension(ros2.bridge)                            │
│    import joint_mapping / rclpy                             │
├─────────────────────────────────────────────────────────────┤
│  main() 初始化                                               │
│    create_new_stage / GroundPlane / DistantLight            │
│    _load_g20(usd) → Articulation                            │
│    _fix_base_to_world / set_dof_gains / 自碰               │
│    G20PlanBridge(...)                                       │
│    apply_targets() 初始 snap 到 open                        │
├─────────────────────────────────────────────────────────────┤
│  主循环（每 render 帧内跑 phys_per_render 次物理步）          │
│    for _ in range(phys_per_render):                         │
│      rclpy.spin_once(node)     # 处理回调 → 改 goal          │
│      bridge.apply_targets()    # slew → 写 Articulation     │
│      SimulationManager.step()  # PhysX 一步                 │
│      recover_if_invalid()                                   │
│      bridge.publish_state()                                 │
│      if 位置硬写: apply_targets() 再钉一次                   │
│    RenderingManager.render()                                │
│    simulation_app.update()                                  │
└─────────────────────────────────────────────────────────────┘
```

**核心状态变量（`G20PlanBridge`）：**

| 变量 | 类型 | 含义 |
|------|------|------|
| `goal` | `dict[str, float]` | 最新目标关节角（弧度，含 mimic） |
| `applied` | `dict[str, float]` | 本步实际写入的角（slew 后） |
| `targets` | 同 `applied` | 别名，历史兼容 |
| `track_nominal` | `dict[str, float] \| None` | 主动关节目标，供 `tracking_error()` |
| `dof_index` | `dict[str, int]` | 关节名 → Articulation DOF 下标 |
| `use_physics_drive` | `bool` | True=`--pd-drive`；False=位置硬写 |

**与 MuJoCo 的关键差异：**

| 项 | Isaac | MuJoCo |
|----|-------|--------|
| 线程模型 | 单线程 `spin_once` + 物理循环 | ROS daemon + 主仿真线程 + Lock |
| 防穿模 | USD 选择性 collider + 自碰 | `collision_project` + `mj_forward` |
| slew 单位 | **rad / 物理步**，默认 **0**（瞬达） | **rad / 秒**，默认 **5.0** |
| 写关节 API | `set_dof_positions` / `set_dof_position_targets` | `data.qpos` + `mj_forward` |
| 物理步进 | 每 render 默认 16 次 `SimulationManager.step` | 硬写模式不 `mj_step` |

---

## 2. 程序入口与加载顺序（很重要）

入口：`./run_g20_plan.sh` → `$ISAAC_SIM_DIR/python.sh isaac_g20_plan_bridge.py`。

### 2.1 为何 `parse_args` + `SimulationApp` 在顶层？

```text
# 文件顶层执行顺序（main 之前）
parse_args()                    # ① 先解析，因为 SimulationApp 要 headless
SimulationApp({"headless": ...}) # ② 启动 Omniverse / PhysX 运行时
... import isaacsim.* ...
from joint_mapping import ...
enable_extension("isaacsim.ros2.bridge")
simulation_app.update()
import rclpy ...
```

Isaac 要求：**先有 `SimulationApp`，才能 import 多数 isaacsim 模块**。  
因此参数解析不能放在 `main()` 里才做；顶层 `args = parse_args()` 是刻意的。

`_normalize_argv`：把 `-pd-drive` 这类单横杠改成 `--pd-drive`，避免被 argparse 拆成 `-p -d ...`。

### 2.2 `main()` 调用链

```text
main()
├── stage_utils.create_new_stage()
├── GroundPlane / DistantLight
├── _compare_camera_eye_target() → set_camera_view(...)
├── hand_path = _load_g20(args.g20_usd)
│     ├── define_prim("/World/G20Hand")
│     ├── XformPrim(..., positions=[[0, 0.45, 0.70]])
│     ├── add_reference_to_stage(usd → .../hand)
│     ├── set_prim_variants(Physics=physx)
│     └── _patch_joint_max_forces(hand_path)
├── SimulationManager.set_physics_dt(1/120)
├── play → update → _fix_base_to_world → stop → play
├── art = Articulation(hand_path)
├── art.set_enabled_self_collisions(...)
├── art.set_link_enabled_gravities(...)
├── set_dof_gains / set_dof_max_efforts     # 见 §5
├── rclpy.init(); Node("isaac_g20_plan_bridge")
├── G20PlanBridge(...)
├── slew=0; apply_targets(); 恢复 slew      # 初始 snap open
└── while simulation_app.is_running():
        for _ in range(phys_per_render):  # 默认 16
            spin_once → apply_targets → step → publish → [再钉]
        render + update
```

---

## 3. 场景加载相关函数

### 3.1 `_load_g20(usd_path) → hand_path`

```python
mount = "/World/G20Hand"
stage_utils.define_prim(mount, "Xform")
XformPrim(mount, positions=[[0.0, HAND_MOUNT_Y, HAND_MOUNT_Z]], ...)
hand_path = f"{mount}/hand"
stage_utils.add_reference_to_stage(usd_path=..., path=hand_path)
prim_utils.set_prim_variants(hand_path, variants=[("Physics", "physx")])
_patch_joint_max_forces(hand_path)
return hand_path
```

要点：

1. **运行时不读 URDF**，只挂预烘焙 USD。  
2. 手装在世界坐标 `(0, 0.45, 0.70)`，离地，避免和地面穿插。  
3. Physics variant 强制 `physx`（USD 里还有 mujoco/newton 等层）。

### 3.2 `_patch_joint_max_forces(hand_path, max_force=8.0)`

遍历手下面所有 `UsdPhysics.RevoluteJoint`，取 angular DriveAPI：

```python
if maxForce < 8.0:
    attr.Set(8.0)
```

防止某些关节 drive 力矩上限太小，PD 跟不上。

### 3.3 `_fix_base_to_world(hand_path)`

钉住掌心，手不会掉下去：

1. 在手 prim 树里找 `hand_base_link`（或 `base_link` / `palm`）RigidBody。  
2. 若已有合法 `root_joint`：改 `LocalPos0/Rot0` 对齐当前世界位姿，Body1=base，Body0=世界。  
3. 否则新建 `{hand_path}/WorldFixedJoint`（`UsdPhysics.FixedJoint`）。

调用时机：`play → update → fix → stop → play`，让 PhysX 在重建 articulation 前把固定关节写进 stage。

### 3.4 `_compare_camera_eye_target()`

与 MuJoCo `COMPARE_*` 对齐：

```python
target = hand_origin + COMPARE_LOOKAT_LOCAL   # (0.04,-0.04,0.12) 相对手
eye.x = target.x + distance * cos(elev)      # 从 +X 看
eye.z = target.z - distance * sin(elev)      # elev=-12° 略俯视
```

`set_camera_view(eye, target, "/OmniverseKit_Persp")`；play/stop 后会再设一次，防止 viewport 被重置。

---

## 4. 映射层：`joint_mapping.py` 函数说明

与 MuJoCo `g20_joints.py` **数值一致**，但 API 多了 `hand`/`side` 参数（同一文件还服务 O6/L20）。

### 4.1 Legacy 0~255 调用链

```text
_on_cmd_u8(msg)
  vals = [255]*20
  优先按 msg.name → L20_SDK_TO_URDF 反查 SDK 索引填 vals
  否则按下标填 vals[:20]
  sdk_range_to_full_urdf_positions(vals, "G20", "left")
    ├── sdk_range_to_urdf_positions(...)
    │     ├── range_to_arc(vals, "G20", "left")
    │     └── SDK15: thumb_mcp = arc/1.02
    │         SDK16..19: *_pip = arc/0.89
    └── 补全 dip/ip（函数内部直接写，不等价于再调 complete_*，结果相同）
  set_nominal_targets(full)
    └── goal = complete_g20_mimic_positions(positions)
```

### 4.2 `range_to_arc(values, hand, side)`

```python
j_min, j_max, j_dir = _tables("G20", "left")  # → G20_L_MIN/MAX/DIR
for i in 0..19:
    if 11 <= i <= 14: continue
    if j_dir[i] == -1:
        arcs[i] = lerp(val, 0, 255, j_max[i], j_min[i])
    else:
        arcs[i] = lerp(val, 0, 255, j_min[i], j_max[i])  # 侧摆 6..9
```

### 4.3 `sdk_range_to_urdf_positions` vs `sdk_range_to_full_urdf_positions`

| 函数 | 输出 |
|------|------|
| `sdk_range_to_urdf_positions` | 仅主动 16 关节（已做 SDK15/16~19 除 mimic） |
| `sdk_range_to_full_urdf_positions` | 主动 + 软件解析的 `thumb_ip` / `*_dip` |

Isaac **硬写模式**写全关节（含 mimic）；**PD 模式**只写主动关节，mimic 交给 PhysX USD mimic。

### 4.4 `complete_g20_mimic_positions`

与 MuJoCo 相同：`thumb_ip = thumb_mcp * 1.02`，`*_dip = *_pip * 0.89`；已有 mimic 值不覆盖。

### 4.5 `open_hand_command("G20")`

返回 `G20_OPEN_CMD`（float 列表），与 MuJoCo `G20_OPEN_CMD` 同值。

### 4.6 PLAN 名义路径

```text
_on_nominal(msg)
  incoming = {name: position}
  缺主动关节 → 与旧 goal 合并
  goal = complete_g20_mimic_positions(merged)
  track_nominal = {主动关节: goal[...]}
```

注意：Isaac 回调里**直接改 `self.goal`**，没有 MuJoCo 的 `_set_goal_only` + Lock；因为主循环同线程 `spin_once`，不存在并发写 Articulation。

---

## 5. 驱动模式：`set_dof_gains` 与 `apply_targets`

### 5.1 初始化时增益怎么设（`main` 里）

**默认位置硬写：**

```python
art.set_dof_gains(stiffnesses=zeros(n), dampings=zeros(n))
```

零增益，避免 PD 与 `set_dof_positions` 抢控制。

**`--pd-drive`：**

```python
# 主动关节
拇指四关节: stiff/damp/effort = 10 / 3 / 6
其余主动:   stiff/damp/effort = 15 / 2.5 / 8
art.set_dof_gains(...); art.set_dof_max_efforts(...)

# mimic 关节强制 0 增益 —— 否则与 PhysX mimic 冲突 → Invalid PhysX transform
art.set_dof_gains(0, 0, dof_indices=mimic_idx)
```

`--hand-gravity` 仅在 PD 下生效：`set_link_enabled_gravities(True)`。

### 5.2 `apply_targets()` — 每物理步的核心

```python
def apply_targets(self):
    self._slew_applied_toward_goal()   # goal → applied

    if use_physics_drive:
        name_iter = (n for n in applied if n in actuated)  # 只主动
    else:
        name_iter = applied.keys()                         # 含 mimic

    positions = [...]
    if use_physics_drive:
        art.set_dof_position_targets(positions, dof_indices=indices)
    else:
        art.set_dof_positions(positions, dof_indices=indices)
        art.set_dof_velocities(zeros(...), dof_indices=indices)
```

| 模式 | API | 写哪些关节 |
|------|-----|------------|
| 位置硬写（默认） | `set_dof_positions` + `set_dof_velocities(0)` | 全部（含软件算好的 mimic） |
| PD | `set_dof_position_targets` | 仅主动 16 个 |

### 5.3 `_slew_applied_toward_goal()`

```python
max_step = self.slew_rad          # 默认 0 → 每步直达 goal
margin = LIMIT_MARGIN_RAD         # 0.05，离限位留边
for name, g in goal.items():
    g = clip(g, lo+margin, hi-margin)
    if max_step <= 0:
        applied[name] = g
    else:
        applied[name] = a ± min(|g-a|, max_step)
applied = complete_g20_mimic_positions(applied)
```

单位是 **每物理步弧度**，不是每秒。  
物理 dt=1/120，默认每 render 16 步 → 墙钟约 16/120 ≈ 0.133 s 一帧渲染。

### 5.4 为何硬写后还要「再钉一次」？

主循环：

```python
bridge.apply_targets()
SimulationManager.step()          # PhysX 可能轻微挪动 DOF
bridge.publish_state()
if not use_pd:
    bridge.apply_targets()        # 再钉回 applied
```

接触/积分会让硬写姿态漂移；步进后再写一遍，保证跟指令贴。

---

## 6. 异常恢复与状态发布

### 6.1 `recover_if_invalid()`

```python
pos = art.get_dof_positions()
if not all finite:
    goal = applied = open_full
    set_dof_positions(open)
    return True
```

PD 爆炸或数值坏掉时复位张开姿态。

### 6.2 `publish_state()`

每物理步调用（不是独立 50Hz 定时器）：

```python
pos = art.get_dof_positions()
vel = art.get_dof_velocities()  # 失败则填 0
pub → /sim/isaac/g20/left/joint_state
```

`legacy_pub` 恒为 `None`，不会发 `/cb_left_hand_state`；  
`urdf_positions_to_sdk_range` 已实现，预留反算 0~255。

### 6.3 `tracking_error()`

对 `track_nominal` 里每个主动关节：`|q_sim - q_nominal|`，返回 `(max, mean)`。

---

## 7. 自碰与防穿模（Isaac 怎么做）

**没有** MuJoCo 的 `collision_project`。手段是：

1. **USD `instances.usda`**：选择性开碰撞（掌心 + 指尖 ON，近端/中节多数 OFF）。  
2. **`physx.usda`**：`enabledSelfCollisions = 1`。  
3. **运行时**：`art.set_enabled_self_collisions([True])`（`--no-self-collision` 关）。

因此：

- 硬写时 PhysX **推不开** 被写死的 DOF → 仍可能视觉穿模。  
- PD 模式下接触力可挡一部分，但跟手变差。  
- 这与 MuJoCo「硬写前投影」是不同哲学。

---

## 8. 完整时序例子：收到半握命令后的一物理步

假设默认硬写、`slew_rad=0`。

```text
1. rclpy.spin_once
     → _on_cmd_u8 → sdk_range_to_full_urdf_positions
     → set_nominal_targets → goal = 半握全关节弧度

2. apply_targets()
     → _slew_applied_toward_goal: applied = goal（瞬达）
     → set_dof_positions(全部关节含 mimic)
     → set_dof_velocities(0)

3. SimulationManager.step()
     → PhysX 积分（硬写下姿态应贴近刚写的值）

4. recover_if_invalid()  # 通常 False

5. publish_state()
     → 读 get_dof_positions → 发 /sim/isaac/...

6. apply_targets() 再钉一次（硬写模式）
```

若 `slew_rad=0.02`：每物理步每个关节最多走 0.02 rad，需多步才到 goal。

---

## 9. `--track-test` 路径

```text
max_steps 默认 480（若未指定）
每 160 物理步切换一次目标：
  phase 偶: open_hand_command("G20")
  phase 奇: [80.0]*20
  set_nominal_targets(sdk_range_to_full_urdf_positions(...))

循环结束后再 settle 240 步:
  apply_targets + SimulationManager.step

tracking_error() → max <= 0.02 → PASS，否则 exit 2
```

与 MuJoCo track-test（open→半握、关投影、slew=0）流程类似，但步进计数和切换方式不同。

---

## 10. 常用 Isaac / PhysX API 速查（本工程用到的）

| API | 用途 |
|-----|------|
| `SimulationApp` | 启动 Isaac 运行时 |
| `stage_utils.add_reference_to_stage` | 挂 USD |
| `prim_utils.set_prim_variants` | 选 Physics=physx |
| `Articulation(path)` | 包装手的 DOF |
| `set_dof_positions` / `set_dof_position_targets` | 硬写 / PD 目标 |
| `set_dof_gains` / `set_dof_max_efforts` | PD 参数 |
| `set_enabled_self_collisions` | 自碰开关 |
| `get_dof_positions` / `get_dof_velocities` / `get_dof_limits` | 读状态 |
| `SimulationManager.set_physics_dt` / `step` | 物理节拍 |
| `RenderingManager.render` | 渲染一帧 |
| `set_camera_view` | 对比视角 |

---

## 11. CLI 模式对照

| 参数 | 效果 |
|------|------|
| （默认） | 订 `/g20/cb_left_hand_control_cmd`，位置硬写，slew=0，自碰 ON |
| `--plan-nominal` | 订名义弧度话题，`use_cmd_u8=False` |
| `--legacy-sdk` | 订 `/cb_left_hand_control_cmd` |
| `--pd-drive` | `set_dof_position_targets` + 主动关节增益 |
| `--hand-gravity` | 仅 PD 下开手部重力 |
| `--no-self-collision` | 关自碰（会穿模） |
| `--slew-rad N` | 每物理步最多转 N rad |
| `--phys-per-render N` | 每渲染帧物理步数（默认 16） |
| `--track-test` | 内置跟踪验收 |

---

## 12. 改代码时最常动的位置

| 需求 | 改哪里 |
|------|--------|
| SDK→弧度、mimic、OPEN_CMD | `joint_mapping.py` |
| 默认硬写/PD 增益、拇指特殊增益 | `isaac_g20_plan_bridge.py` 顶部常量 + `main` 里 `set_dof_gains` |
| slew / 物理步 / 安装高度 | `DEFAULT_SLEW_RAD` / `DEFAULT_PHYS_PER_RENDER` / `HAND_MOUNT_*` |
| 相机 | `COMPARE_*` + `_compare_camera_eye_target` |
| 碰撞谁开谁关 | USD `payloads/instances.usda` |
| 自碰/求解器 | USD `payloads/Physics/physx.usda` |
| 话题名 | `TOPIC_*` 或 CLI |

---

## 13. 与 MuJoCo 技术详解的对照表

| 概念 | Isaac 函数/位置 | MuJoCo 函数/位置 |
|------|-----------------|------------------|
| 映射入口 | `joint_mapping.sdk_range_to_full_urdf_positions` | `g20_joints.sdk_range_to_full_urdf_positions` |
| 收命令 | `_on_cmd_u8` / `_on_nominal` | `_on_legacy_cmd` / `_on_nominal` |
| 更新目标 | `set_nominal_targets` / 直接改 `goal` | `_set_goal_only`（持锁） |
| 限速 | `_slew_applied_toward_goal`（rad/步） | `_slew_applied_unlocked`（rad/s） |
| 写入仿真 | `apply_targets` → `set_dof_*` | `_hard_write_pose_unlocked` → `mj_forward` |
| 防穿模 | USD collider + 自碰（无投影） | `_project_applied_no_penetration_unlocked` |
| 物理步进 | `SimulationManager.step` | 硬写不 step；PD 用 `mj_step` |
| 发状态 | 每物理步 `publish_state` | 50Hz timer `_publish_state` |
| 状态话题 | `/sim/isaac/g20/left/joint_state` | `/sim/mujoco/g20/left/joint_state` |

---

## 14. 读代码建议顺序

1. `run_g20_plan.sh` → 确认用的是哪份 `python.sh`  
2. 文件顶层：`parse_args` → `SimulationApp` → import  
3. `main()` 前半：`_load_g20` → `_fix_base` → gains  
4. `G20PlanBridge.__init__` + `_on_cmd_u8`  
5. `apply_targets` + 主循环里「写→step→再钉」  
6. 需要改防穿模时去看 USD `instances.usda`，而不是在桥里找 project 函数  

资产与改动清单仍见 [`G20_Isaac_实现说明.md`](./G20_Isaac_实现说明.md)。