mujoco_linkerbot/docs/G20_Isaac_技术详解.md

G20 左手 · Isaac Sim 技术详解

资产路径、URDF→USD 改动清单见 G20_Isaac_实现说明.md。
MuJoCo 对照：G20_MuJoCo_技术详解.md。

主文件：

角色	路径
桥主程序	/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()。

┌─────────────────────────────────────────────────────────────┐
│  模块加载期（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 在顶层？

# 文件顶层执行顺序（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() 调用链

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

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：

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_* 对齐：

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 调用链

_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)

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 名义路径

_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 里）

默认位置硬写：

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

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

--pd-drive：

# 主动关节
拇指四关节: 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() — 每物理步的核心

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()

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 为何硬写后还要「再钉一次」？

主循环：

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

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

---

6. 异常恢复与状态发布

6.1 recover_if_invalid()

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 定时器）：

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。

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 路径

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。