Files
mujoco_linkerbot/docs/G20_Isaac_技术详解.md
T
sunxianghui 246b48b1fb 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

500 lines
18 KiB
Markdown
Raw Blame History

This file contains ambiguous Unicode characters
This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.
# G20 左手 · Isaac Sim 技术详解
资产路径、URDF→USD 改动清单见 [`G20_Isaac_实现说明.md`](./G20_Isaac_实现说明.md)。
MuJoCo 对照:[`G20_MuJoCo_技术详解.md`](./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()`。
```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)。