sunxianghui/isaac_linkerbot
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Add LinkerHand Isaac Sim bridge and G20 assets.

Browse Source 
Includes G20 PLAN/cmd_u8 bridge, joint mapping, dual-hand sim scripts,
and G20/L20/O6 URDF with USD payloads for Isaac Sim integration.

Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
sunxianghui
2026-07-27 14:13:00 +08:00
commit 0b727785c6
134 changed files with 17662 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
docs/G20_Isaac_实现说明.md
+328
View File
@@ -0,0 +1,328 @@
# 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` 若 **&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 桥参数不同，联调时不要混用入口脚本。
docs/G20_Isaac_技术详解.md
+499
View File
@@ -0,0 +1,499 @@
# 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)。