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

Add G20 left-hand MuJoCo PLAN bridge, assets, and docs.

Browse Source 
Bring in kinematic hard-write control with mesh-convex collision projection, SDK mapping aligned to Isaac, launch/run scripts, and implementation/tech writeups.
This commit is contained in:
sunxianghui
2026-07-27 13:45:33 +08:00
parent 818ba00524
commit 246b48b1fb
33 changed files with 4030 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 对照**：[`G20_MuJoCo_实现说明.md`](./G20_MuJoCo_实现说明.md) / [`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 桥参数不同，联调时不要混用入口脚本。
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`](./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)。
docs/G20_MuJoCo_实现说明.md
+362
View File
@@ -0,0 +1,362 @@
# G20 左手 · MuJoCo 仿真实现说明
本文记录 **MuJoCo G20 PLAN 名义弧度桥** 从 URDF 到可跑仿真的完整实现过程，以及相对原始 URDF 的全部具体改动。
- **函数调用与逐步逻辑**：见 [`G20_MuJoCo_技术详解.md`](./G20_MuJoCo_技术详解.md)（推荐先读此文理解代码）
- **Isaac 对照**：[`G20_Isaac_实现说明.md`](./G20_Isaac_实现说明.md)
---
## 1. 目标与数据流
### 1.1 目标
- 在 MuJoCo 中仿真 LinkerHand G20 **左手**。
- 默认兼容真机 SDK 话题协议：`sensor_msgs/JointState`，`position` 为 **0~255**。
- 可选订阅 PLAN 名义弧度（按关节名）。
- 关节名、0~255→弧度映射、mimic、拇指限位、对比视角与 Isaac 侧对齐。
- 默认 **运动学硬写** `qpos`（跟手紧）；用碰撞投影减轻穿模，而不是改成默认 PD。
### 1.2 端到端数据流
```text
/g20/cb_left_hand_control_cmd (默认，0~255)
或 /retarget/g20/left/joint_target_nominal (--plan-nominal，弧度+关节名)
│
▼
utils/g20_joints.py
· range_to_arc_g20_left / sdk_range_to_urdf_positions
· SDK15：thumb_mcp = arc/1.02
· SDK16..19：*_pip = arc/0.89
· complete_g20_mimic_positions（dip/ip）
│
▼
g20_plan_bridge.py
· ROS 回调只写 goal（不碰 MjData）
· 仿真线程：slew → [collision_project] → 硬写 qpos/ctrl + mj_forward
│
▼
/sim/mujoco/g20/left/joint_state @ 50 Hz
+ MuJoCo viewer（可选）
```
---
## 2. 关键文件路径
| 角色 | 绝对路径 |
|------|----------|
| 启动脚本 | `/home/lxqs/linker_hand_mujoco_ros2/run_g20_mujoco_plan.sh` |
| 桥主程序 | `.../linker_hand_mujoco_ros2/g20_plan_bridge.py` |
| 关节映射 | `.../linker_hand_mujoco_ros2/utils/g20_joints.py` |
| Launch | `.../launch/g20_plan_bridge.launch.py` |
| URDF | `.../urdf/G20/linker_hand_g20_left/linkerhand_g20_left.urdf` |
| MJCF | `.../urdf/G20/linker_hand_g20_left/linker_hand_g20_left.xml` |
| 网格 | `.../urdf/G20/linker_hand_g20_left/meshes/*.STL`（22 个） |
| entry_point | `setup.py` → `g20_plan_bridge=linker_hand_mujoco_ros2.g20_plan_bridge:main` |
包根目录：
`/home/lxqs/linker_hand_mujoco_ros2/src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/`
---
## 3. 从 URDF 到 MJCF
### 3.1 URDF 来源
1. 原始模型由 **SolidWorks → URDF Exporter** 导出（URDF 头注释：Commit `1.6.0-4-g7f85cfe`）。
2. 本仓库资产来自 retarget 工程拷贝（非运行时自动下载），与 Isaac 侧网格 **MD5 一致**（22/22）。
3. 入库后的 URDF 文件名：`linkerhand_g20_left.urdf`。
### 3.2 URDF 上我们改过什么
相对更早的 G20 URDF（历史上限曾为 `thumb_cmc_roll` **1.4**），当前仓库 URDF **已写入**：
| 改动项 | 具体内容 |
|--------|----------|
| `thumb_cmc_roll` 上限 | `upper="1.47"`（rad）；与映射表 `G20_L_MAX[5]=1.47`、Isaac 同步试调 |
| mimic（保留自原 URDF） | `thumb_ip` ← `thumb_mcp` × **1.02**；四指 `*_dip` ← `*_pip` × **0.89** |
| 其它关节限位 | 与 Isaac URDF 一致（见下表），未再改数值 |
**当前 URDF 关节限位（rad，与 MJCF `range` / actuator `ctrlrange` 一致）：**
| 关节 | lower | upper |
|------|-------|-------|
| `thumb_cmc_roll` | 0 | **1.47** |
| `thumb_cmc_yaw` | 0 | 1.57 |
| `thumb_cmc_pitch` | 0 | 0.84 |
| `thumb_mcp` | 0 | 1.26 |
| `thumb_ip` | 0 | 1.29 |
| 四指 `*_mcp_roll` | -0.23 | 0.23 |
| 四指 `*_mcp_pitch` | 0 | 1.22 |
| 四指 `*_pip` | 0 | 1.74 |
| 四指 `*_dip` | 0 | 1.55 |
说明：MuJoCo **不读取** URDF 的 `<mimic>`。mimic 在 Python 里用 `complete_g20_mimic_positions` 补全后硬写进 `qpos`。
### 3.3 转换步骤（URDF → MJCF）
仓库内 **没有** 保留可一键重跑的转换脚本；实际流程是：
1. **初转**：用 MuJoCo `MjSpec.from_file(urdf)` 编译得到骨架 MJCF（关节树、mesh、惯性）。
2. **手改定稿** 为当前 `linker_hand_g20_left.xml`（下列改动全部是相对「纯 URDF 导入结果」的人工修改）。
### 3.4 MJCF 相对 URDF / 纯导入结果的全部改动清单
#### A. 仿真选项与默认参数
| 项 | 写入值 |
|----|--------|
| `compiler` | `angle="radian" meshdir="meshes" balanceinertia="true"` |
| `option` | `timestep="0.002"`，`gravity="0 0 -9.81"`，`integrator="implicitfast"` |
| 默认 joint | `damping="0.15"`，`armature="0.01"` |
| 默认 position 执行器 | `kp="200"`，`kv="8"`，`forcelimited="true"`，`forcerange="-8 8"` |
#### B. 显示 / 碰撞分层（相对 URDF「全身 mesh 碰撞」）
| 类型 | 改动 |
|------|------|
| `class="visual"` | `type="mesh"`，`contype="0"`，`conaffinity="0"`，`group="2"` → **只显示，不参与碰撞** |
| 碰撞体 | **同视觉 STL 的凸包 mesh**（`type="mesh"` + `class="collision"`）：掌心 + 拇指三段 + 四指近/中/远节；比 AABB 更贴外形 |
| collision 默认 | `margin="0.0005"`，`friction="1 0.5 0.01"`，`solref="0.004 1"`，`solimp="0.9 0.95 0.001"` |
**当前碰撞体（与视觉共用同一 `meshes/*.STL`，MuJoCo 按凸包做碰撞）：**
| body | 类型 | 说明 |
|------|------|------|
| `hand_base` | mesh 凸包 | `hand_base_link.STL` |
| `thumb_metacarpals` / `thumb_proximal` / `thumb_distal` | mesh 凸包 | 同名 STL |
| 四指 `*_proximal` / `*_middle` / `*_distal` | mesh 凸包 | 同名 STL |
| 掌骨 metacarpals、拇指 base1/base2 | 无碰撞 geom | 仅 visual；与掌心假重叠靠 `exclude` |
说明：MuJoCo 对 mesh 碰撞使用 **凸包**，不能还原 STL 凹坑，但整体轮廓远比 AABB/手写胶囊贴近视觉；张开/半握无假碰，握拳可检出指尖↔掌心穿透。
#### C. `<contact><exclude>`（共 28 条）
用途：排除 **mesh/几何本就嵌套** 造成的假碰撞（张开姿态假重叠）。
- 掌心 `hand_base` ↔ 拇指 base1/base2/metacarpals、四指 metacarpals/proximal
- 拇指链相邻 / 隔代相邻
- 四指各自 metacarpals–proximal、proximal–middle、middle–distal
未 exclude 的对（如指尖↔掌心、邻指远端）仍可产生接触，供投影使用。
#### D. 执行器
- 为 **全部 21 个 hinge**（含 5 个 mimic 关节）各加一个 `<position name="*_pos" joint="...">`。
- `thumb_cmc_roll` 的 `ctrlrange="0.0 1.47"`（与 URDF 上限一致）。
- 默认硬写模式下主要靠写 `qpos`；`--pd-drive` 时才真正用这些执行器做 `mj_step`。
#### E. 命名差异
| URDF | MJCF |
|------|------|
| `<robot name="...">` | `<mujoco model="linker_hand_g20_left">` |
| link `hand_base_link` | body 名 `hand_base`（geom 仍引用 mesh `hand_base_link`） |
---
## 4. 关节映射 `g20_joints.py`（相对 SDK / Isaac）
文件：`utils/g20_joints.py`。声明与 Isaac `joint_mapping.py` 的 G20 左手表对齐。
### 4.1 SDK 通道 → URDF 关节名
| 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`（见 4.3） |
| 16–19 | `index/middle/ring/pinky_pip`（见 4.3） |
### 4.2 线性插值表（我们写入的数值）
```text
G20_L_MIN = [0,0,0,0,0, 0.0, -0.23,-0.23,-0.23,-0.23, 0.0, 0×4, 0×5]
G20_L_MAX = [0.84, 1.22×4, 1.47, 0.23×4, 1.57, 0×4, 1.29, 1.55×4]
G20_L_DIR = [-1×6, +1,+1,+1,+1, -1, 0×4, -1×5]
```
含义：
- `DIR == -1`：SDK `0 → MAX`，`255 → MIN`（多数弯曲通道）。
- `DIR == +1`（仅侧摆 6..9）：SDK `0 → MIN`，`255 → MAX`（左手侧摆与 retarget/`g20_mapping` 一致）。
- 通道 5 上限 **1.47**（相对历史 1.4 的试调，已写进 URDF/MJCF/Isaac）。
### 4.3 SDK 15–19 的「除 mimic」规则（与 Isaac 相同，必须写清）
SDK 通道语义不是「直接写主动关节弧度」，而是电机/指尖弧度空间：
| SDK | 先算出的弧度 `arc` | 写入主动关节 |
|-----|-------------------|--------------|
| 15 | 按 `G20_L_*` 插值得到 | `thumb_mcp = arc / 1.02` |
| 16–19 | 同上 | `*_pip = arc / 0.89` |
然后再：
- `thumb_ip = thumb_mcp * 1.02`
- `*_dip = *_pip * 0.89`
若漏掉「除 mimic」，DIP/IP 会整体偏大，表现为关节对不上 Isaac。
### 4.4 张开预设
```text
G20_OPEN_CMD = [
255,255,255,255,255,255,
193,148,105,42, # 四指侧摆
245, # thumb_cmc_yaw
255,255,255,255, # 预留
255,255,255,255,255 # 15..19
]
```
`open_hand_positions()` / `half_fist_positions()` 基于此生成测试姿态。
---
## 5. 桥节点 `g20_plan_bridge.py` 做了哪些事
### 5.1 话题
| 方向 | 默认话题 | 内容 |
|------|----------|------|
| 订阅（默认） | `/g20/cb_left_hand_control_cmd` | `position` 0~255，取前 20，不足补 255 |
| 订阅（`--plan-nominal`） | `/retarget/g20/left/joint_target_nominal` | 按关节名弧度 |
| 发布 | `/sim/mujoco/g20/left/joint_state` | 全部 hinge 的 name/position/velocity，50 Hz |
### 5.2 默认驱动：运动学硬写
- **默认**（无 `--pd-drive`）：把 `applied` 写入 `data.qpos` 与对应 `ctrl`，`qvel=0`，调用 `mj_forward`；**不调用** `mj_step`。
- **`--pd-drive`**：写 `ctrl` → `mj_step` → 用真实 `qpos` 回写 `applied`（跟手变差，接触可挡，一般不作为默认）。
### 5.3 速度相关默认值（相对「瞬移」做过的调参）
| 常量 | 值 | 含义 |
|------|-----|------|
| `DEFAULT_SPEED` | `1.35` | 相对墙钟倍率 |
| `DEFAULT_SLEW_RAD_S` | `5.0` | 关节逼近 goal 的最大角速度 (rad/s)；`0` = 瞬移 |
| `sim_dt` | `0.002`（来自 MJCF） | 每步最大转角 ≈ `5.0 * 0.002 = 0.01` rad |
### 5.4 防穿模：`collision_project`（硬写专用，默认开）
相对早期实现，做过这些关键修正：
| 版本问题 | 改动 |
|----------|------|
| 仅靠 STL 自碰 | 改为 visual（不碰）+ **同 STL 凸包 collision** |
| 「相对上一帧更深才算穿」 | 改为 **绝对** 判据：`min(contact.dist) < -5e-4` 即穿 |
| 关节空间整体二分 | 拇指撞掌心会拖死四指 → 改为 **按主动关节、Δq 从大到小** 逐关节接受或二分 |
| 仍穿模 | 回退 `_last_safe_pose` |
开关：
- 默认：开（且仅在硬写模式生效）
- `--no-collision-project`：关
- `--track-test`：强制关投影，且 `slew=0`，方便验收跟手误差 ≤ 0.02 rad
常量：`PENETRATION_TOL_M = 5e-4`，`COLLISION_PROJECT_ITERS = 12`。
### 5.5 线程安全（修过闪退）
- `threading.Lock` 保护 `MjData`。
- ROS 回调 **只** 更新 `goal` / `track_nominal`（`_set_goal_only`），**禁止**在回调里碰 `MjData`。
- 主循环持锁调用 `step_simulation_unlocked()`；发 state 时持锁读 `qpos/qvel`。
### 5.6 对比视角（与 Isaac 对齐）
| 常量 | 值 |
|------|-----|
| `COMPARE_LOOKAT` | `(0.04, -0.04, 0.12)` |
| `COMPARE_DISTANCE` | `0.50` |
| `COMPARE_AZIMUTH` | `180.0`（从 +X 看向掌心） |
| `COMPARE_ELEVATION` | `-12.0` |
手模型放在世界原点；Isaac 手有安装偏置，但 lookat 本地坐标与 elevation/distance 相同。
### 5.7 启动时校验
- 用 `G20_ALL_JOINT_NAMES` 检查 MJCF 是否缺关节；缺则 **直接退出**。
- 缺 position actuator 也会报错退出。
### 5.8 其它明确改动
- **不** import 真机包 `linker_hand_ros2_sdk`（只兼容同话题协议）。
- **不** 写死 `ROS_DOMAIN_ID`（启动脚本里曾有过，已去掉）。
- 退出用 `os._exit`，规避 rclpy/mujoco 析构 abort。
---
## 6. 如何启动
```bash
cd /home/lxqs/linker_hand_mujoco_ros2
./run_g20_mujoco_plan.sh
```
脚本会：`source /opt/ros/jazzy` → 可选 `install/setup.bash` → 把 `venv` site-packages 与包源码加入 `PYTHONPATH` →
`python3 -m linker_hand_mujoco_ros2.g20_plan_bridge`。
常用参数：
```bash
./run_g20_mujoco_plan.sh --plan-nominal
./run_g20_mujoco_plan.sh --pd-drive
./run_g20_mujoco_plan.sh --no-collision-project
./run_g20_mujoco_plan.sh --speed 0.5 --slew-rad-s 3
./run_g20_mujoco_plan.sh --headless --track-test --max-steps 800
```
Launch（无参数透传，调参请用 shell）：
```bash
ros2 launch linker_hand_mujoco_ros2 g20_plan_bridge.launch.py
```
测试半握示例：
```bash
ros2 topic pub -r 20 /g20/cb_left_hand_control_cmd sensor_msgs/msg/JointState \
"{position: [80,80,80,80,80,255,193,148,105,42,245,255,255,255,255,80,80,80,80,80]}"
```
---
## 7. 已知限制（实现层面）
1. 硬写 + 投影 **不是** 真接触动力学；碰到凸包后关节会停在接触边界，不会「挤过去」。
2. 碰撞是 **凸包**，凹进去的视觉细节不会产生接触；比 AABB 贴，但仍可能与非凸视觉有细微差别。
3. 大量 `exclude` 关掉了掌心–近端假碰；真实可碰区域依赖开启碰撞的那些 link。
4. mimic 只在软件层保证；物理引擎内无 URDF mimic 约束。
5. 仅左手 G20；右手未做。
---
## 8. 改动 chronologically（便于对照 Git / 联调）
1. 拷贝 G20 URDF + meshes，生成带 position actuator 的 MJCF。
2. 实现 `g20_joints.py`（对齐 Isaac；含 SDK15–19 除 mimic）。
3. 实现桥：名校验、订 nominal / legacy、发 `/sim/mujoco/...`。
4. 修回调碰 `MjData` 导致的闪退 → 回调只改 goal。
5. 话题改为 `/g20/cb_left_hand_control_cmd`；去掉脚本里 DOMAIN。
6. 调 `speed` / `slew` 到当前默认。
7. 拇指 `max=1.47` 写入映射 + URDF/MJCF。
8. 视角 `COMPARE_*` 与 Isaac 对齐。
9. 默认保持硬写；加碰撞投影；后改为绝对穿透 + **mesh 凸包碰撞** + 按关节投影。
---
## 9. 与 Isaac 的对照摘要
| 项 | MuJoCo（本文） | Isaac |
|----|----------------|-------|
| 模型格式 | MJCF | 预烘焙 USD（非运行时 URDF 导入） |
| 状态话题 | `/sim/mujoco/g20/left/joint_state` | `/sim/isaac/g20/left/joint_state` |
| 映射表 | `g20_joints.py` | `joint_mapping.py`（G20 左手数值一致） |
| 默认驱动 | 硬写 qpos | 硬写 `set_dof_positions` |
| 防穿模 | mesh 凸包 + `collision_project` | USD 选择性 collider + PhysX 自碰 |
| slew | 默认 5 rad/s | 默认 0（每物理步瞬达） |
docs/G20_MuJoCo_技术详解.md
+457
View File
@@ -0,0 +1,457 @@
# G20 左手 · MuJoCo 技术详解
本文面向**读代码、改逻辑、排查问题**：按「程序从哪进、函数怎么调、数据怎么流」写清楚实现细节。
资产路径、URDF/MJCF 改动清单见 [`G20_MuJoCo_实现说明.md`](./G20_MuJoCo_实现说明.md)。
---
## 1. 总体结构：两个线程、三块逻辑
```text
┌─────────────────────────────────────────────────────────────┐
│ ROS 线程 (daemon) │
│ rclpy.spin_once() │
│ → _on_legacy_cmd() / _on_nominal() │
│ → set_nominal_targets() → _set_goal_only() │
│ → 只改 goal / track_nominal（持锁） │
│ → 绝不碰 model / data │
├─────────────────────────────────────────────────────────────┤
│ 主线程（仿真循环） │
│ with lock: step_simulation_unlocked() │
│ → _slew_applied_unlocked() # goal → applied │
│ → _project_applied_...() # 防穿模（默认开） │
│ → _hard_write_pose_unlocked() # applied → qpos │
│ → mujoco.mj_forward() # 更新运动学+接触 │
│ viewer.sync() / _pace() │
├─────────────────────────────────────────────────────────────┤
│ 定时器线程 (50Hz) │
│ _publish_state() → /sim/mujoco/g20/left/joint_state │
└─────────────────────────────────────────────────────────────┘
```
**核心状态变量（`G20PlanBridge` 内）：**
| 变量 | 类型 | 含义 |
|------|------|------|
| `goal` | `dict[str, float]` | 最新目标关节角（弧度，含 mimic） |
| `applied` | `dict[str, float]` | 本步实际要写入仿真的角（slew + 投影后） |
| `_last_safe_pose` | `dict[str, float]` | 上一次「无穿透」的 applied，投影失败时回退 |
| `track_nominal` | `dict[str, float] \| None` | 仅主动关节，供 `tracking_error()` 验收 |
**为什么分 `goal` 和 `applied`？**
`goal` 可以瞬间跳到新命令；`applied` 按 `slew_rad_s` 逐步逼近，且碰撞投影可能让 `applied` 停在比 `goal` 更「开」的位置。
---
## 2. 程序入口：`main()` 调用链
入口：`python3 -m linker_hand_mujoco_ros2.g20_plan_bridge`（或 `run_g20_mujoco_plan.sh`）。
```text
main()
├── parse_args() # CLI 参数
├── mujoco.MjModel.from_xml_path(xml) # 加载 MJCF
├── mujoco.MjData(model)
├── mujoco.mj_forward(model, data) # 初始化前向
├── validate_model_joint_names(names) # 关节名必须齐全
├── rclpy.init()
├── G20PlanBridge.__init__(...) # 建桥、订/发话题、初始 open 姿态
├── threading.Thread(spin_ros) # ROS 回调线程
└── while running:
with bridge.lock:
bridge.step_simulation_unlocked()
viewer.sync() / _pace()
```
### 2.1 `main()` 里与模式相关的赋值
```python
use_legacy = args.legacy_sdk and not args.plan_nominal # 默认订 0~255
slew_rad_s = 0.0 if args.track_test else args.slew_rad_s
collision_project = (not args.no_collision_project) and (not args.track_test)
use_pd_drive = args.pd_drive and not args.track_test
```
| 模式 | `hard_kinematic` | `collision_project` | `slew_rad_s` |
|------|------------------|---------------------|--------------|
| 默认 | True | True | 5.0 |
| `--pd-drive` | False | False（仅硬写模式有效） | 5.0 |
| `--no-collision-project` | True | False | 5.0 |
| `--track-test` | True | False | 0（瞬达） |
---
## 3. 初始化：`G20PlanBridge.__init__`
```text
G20PlanBridge.__init__
├── build_joint_maps(model)
│ → joint_qpos[name] # 关节名 → qpos 下标
│ → joint_dof[name] # 关节名 → qvel 下标
│ → joint_lo/hi[name] # MJCF range
│ → act_id[name] # 关节名 → actuator 下标
├── validate_model_joint_names(...)
├── open_full = open_hand_positions() # 见 §4
├── goal = applied = open_full
├── _set_goal_only(open_full)
├── slew_rad_s 临时置 0 → _slew_applied_unlocked() → _hard_write_applied_unlocked()
├── _last_safe_pose = applied
├── create_subscription(_on_legacy_cmd 或 _on_nominal)
├── create_publisher(sim_state)
└── create_timer(50Hz, _publish_state)
```
### 3.1 `build_joint_maps(model)`
遍历 `model.njnt`，只保留 **hinge**（`mjJNT_HINGE`）：
```python
name = mujoco.mj_id2name(model, mjOBJ_JOINT, i)
joint_qpos[name] = model.jnt_qposadr[i]
joint_dof[name] = model.jnt_dofadr[i]
joint_lo/hi = model.jnt_range[i]
```
遍历 `model.nu` 找 position actuator：优先匹配 `<joint>_pos` 命名。
---
## 4. 映射层：`g20_joints.py` 函数说明
外部命令（0~255 或弧度）最终都要变成 **`dict[关节名, 弧度]`**，且 mimic 关节要补全。
### 4.1 调用关系（legacy SDK 路径）
```text
_on_legacy_cmd(msg)
vals = msg.position[:20] # 不足补 255
sdk_range_to_full_urdf_positions(vals)
├── sdk_range_to_urdf_positions(vals)
│ ├── range_to_arc_g20_left(vals) # 20 维弧度
│ └── 按 L20_SDK_TO_URDF 填主动关节
│ SDK15 → thumb_mcp = arc/1.02
│ SDK16..19 → *_pip = arc/0.89
└── complete_g20_mimic_positions(...)
thumb_ip = thumb_mcp * 1.02
index_dip = index_pip * 0.89
...
set_nominal_targets(full)
└── _set_goal_only(full)
```
### 4.2 `range_to_arc_g20_left(values)` — 单通道怎么算
对每个 SDK 索引 `i`（跳过 11~14）：
```python
val = clamp(values[i], 0, 255)
if G20_L_DIR[i] == -1:
arc = lerp(val, 0, 255, G20_L_MAX[i], G20_L_MIN[i]) # 0→MAX, 255→MIN
else:
arc = lerp(val, 0, 255, G20_L_MIN[i], G20_L_MAX[i]) # 侧摆 6..9
```
**例子：** `index_mcp_pitch` 是 SDK 1，`DIR=-1`，`MAX=1.22`，`MIN=0`
→ SDK `0` 得 `1.22` rad（弯到底），SDK `255` 得 `0`（伸直）。
### 4.3 `sdk_range_to_urdf_positions` — SDK15/16~19 特殊处理
```python
# SDK 15：电机语义是「指尖弧度空间」
out["thumb_mcp"] = arc / 1.02
# SDK 16..19：电机语义是「DIP 弧度空间」，写入的是 pip
out["index_pip"] = arc / 0.89 # 同理 middle/ring/pinky
```
漏掉除法会导致仿真关节比真机/Isaac 偏大。
### 4.4 `complete_g20_mimic_positions(positions)`
```python
for mimic, (master, ratio) in G20_MIMIC_OF.items():
if mimic not in out and master in out:
out[mimic] = out[master] * ratio
```
若调用方已给出 `index_dip`，则**不覆盖**。
### 4.5 PLAN 名义弧度路径
```text
_on_nominal(msg)
incoming = {msg.name[i]: msg.position[i]}
set_nominal_targets(merged)
└── _set_goal_only(merged)
complete_g20_mimic_positions(merged) # 补 dip/ip
```
---
## 5. 仿真一步：`step_simulation_unlocked()`
**调用方必须已持有 `self.lock`。**
### 5.1 默认模式（运动学硬写 + 碰撞投影）
```python
def step_simulation_unlocked(self):
prev = dict(self.applied) # ① 记下上一帧姿态
self._slew_applied_unlocked() # ② goal → applied（限速）
if self.hard_kinematic:
if self.collision_project:
self._project_applied_no_penetration_unlocked(prev) # ③
else:
self._hard_write_applied_unlocked() # ③'
return # ④ 不调用 mj_step
# --pd-drive 分支见 §5.3
```
### 5.2 `_slew_applied_unlocked()` — 限速逼近
```python
max_step = slew_rad_s * sim_dt # 默认 5.0 * 0.002 = 0.01 rad/步
for name, target in self.goal.items():
cur = applied[name]
delta = target - cur
if abs(delta) <= max_step:
applied[name] = target
else:
applied[name] = cur + sign(delta) * max_step
applied[name] = clip(applied[name], joint_lo, joint_hi)
```
`slew_rad_s == 0` 时 `max_step is None`，直接 `applied = goal`。
### 5.3 `_hard_write_pose_unlocked(pose)` — 写入 MuJoCo
对每个关节名：
```python
self.data.qpos[joint_qpos[name]] = val
self.data.qvel[joint_dof[name]] = 0.0
self.data.ctrl[act_id[name]] = val # 同步执行器目标
self.data.qvel[:] = 0.0
mujoco.mj_forward(self.model, self.data)
```
**`mj_forward` 做什么：** 根据当前 `qpos` 更新 body 位姿、碰撞几何位置，并填充 `data.contact`（`ncon`、`contact[i].dist`）。
**不调用 `mj_step`**：没有积分、没有力矩求解，纯运动学。
### 5.4 `_project_applied_no_penetration_unlocked(prev)` — 防穿模（重点）
**目的：** `applied` 朝 `goal` 走了一步后，若 mesh 凸包穿透，则缩小步进，且**按关节**处理，避免拇指撞掌心时四指一起被卡住。
**穿透判定：**
```python
def _worst_penetration_unlocked(self):
worst = 0.0
for i in range(data.ncon):
worst = min(worst, data.contact[i].dist)
return worst # 负值 = 重叠深度（米）
def _is_penetrating_unlocked(self):
return _worst_penetration_unlocked() < -5e-4 # PENETRATION_TOL_M
```
`contact[i].dist`：两碰撞体表面距离；**负**表示已经穿进去。
**算法步骤：**
```text
cand = complete_g20_mimic_positions(applied) # slew 后的候选
_hard_write_pose_unlocked(cand)
if not penetrating:
applied = cand; _last_safe_pose = cand; return
# 有穿透：从安全起点 base 开始
_hard_write_pose_unlocked(prev)
if penetrating(prev):
base = complete_g20_mimic_positions(_last_safe_pose)
else:
base = complete_g20_mimic_positions(prev)
# 按 |cand[name]-base[name]| 从大到小排序主动关节
for name in order:
target = cand[name]
trial = complete_g20_mimic_positions({**base, name: target})
_hard_write_pose_unlocked(trial)
if not penetrating:
base = trial # 该关节可走到位
continue
# 否则在 [cur, target] 上二分 12 次
for _ in range(12):
mid = (lo+hi)/2
trial = complete_g20_mimic_positions({**base, name: mid})
_hard_write_pose_unlocked(trial)
if penetrating: hi = mid
else: lo = mid; best = trial
base = best
applied = complete_g20_mimic_positions(base)
_hard_write_pose_unlocked(applied)
if still penetrating:
applied = _last_safe_pose; 硬写回退
else:
_last_safe_pose = applied
```
**注意：** 每次试姿态都要 `complete_g20_mimic_positions`，因为改 `thumb_mcp` 会带动 `thumb_ip`。
### 5.5 `--pd-drive` 分支
```python
self._ctrl_from_applied_unlocked() # data.ctrl[act_id] = applied
mujoco.mj_step(self.model, self.data) # PhysX 式积分+接触力
self._sync_applied_from_qpos_unlocked() # applied 跟真实 qpos
```
此时靠 MuJoCo 执行器 + 接触求解，**不用** `collision_project`。
---
## 6. ROS 回调与发布
### 6.1 Legacy 命令
```python
def _on_legacy_cmd(self, msg):
vals = [float(x) for x in msg.position[:20]]
while len(vals) < 20: vals.append(255.0)
full = sdk_range_to_full_urdf_positions(vals)
self.set_nominal_targets(full) # → _set_goal_only
```
### 6.2 状态发布（50Hz）
```python
def _publish_state(self):
with self.lock:
for name in ordered_names:
pos.append(data.qpos[joint_qpos[name]])
vel.append(data.qvel[joint_dof[name]])
pub.publish(JointState(...))
```
读 `qpos` 也持锁，避免与仿真步并发写 `MjData`。
---
## 7. MJCF 碰撞：如何配合 `mj_forward`
### 7.1 双层 geom
每个要显示的 link 上通常有两个 geom：
```xml
<geom class="visual" mesh="index_proximal"/> <!-- contype=0，不碰 -->
<geom class="collision" type="mesh" mesh="index_proximal"/> <!-- 凸包碰撞 -->
```
- **visual**：`contype=0 conaffinity=0`，只渲染。
- **collision**：`contype=1 conaffinity=1`，MuJoCo 对 STL 自动生成**凸包**做碰撞。
有碰撞的 body：**掌心** + **拇指三段** + **四指近/中/远节**（共 16 个 collision geom）。
掌骨 `*_metacarpals`、拇指 `base1/base2` **无** collision geom。
### 7.2 `contact/exclude`（28 条）
排除「几何嵌套」导致的假接触，例如：
```xml
<exclude body1="hand_base" body2="index_proximal"/>
<exclude body1="index_proximal" body2="index_middle"/>
```
张开手时这些对不应产生 `ncon`；握拳时**指尖↔掌心**、**邻指远端**仍可接触，供投影使用。
### 7.3 碰撞检测在代码里何时发生
每次 `_hard_write_pose_unlocked` 末尾调用 `mujoco.mj_forward` 后：
- `data.ncon`：接触对数量
- `data.contact[i].dist`：距离（负=穿透）
投影逻辑只读这些字段，**不**施加接触力。
---
## 8. 完整时序例子：收到半握命令后的一帧
假设上一帧 `applied` 已是张开，`goal` 刚被 `_on_legacy_cmd` 设为半握弧度。
```text
1. 主线程 step_simulation_unlocked()
2. prev = applied（张开）
3. _slew_applied_unlocked()
index_pip: 0.0 → min(0.01, 目标-0) = 0.01 … 各关节类似
4. _project_applied_no_penetration_unlocked(prev)
cand = complete(applied)
_hard_write_pose_unlocked(cand) → mj_forward → 检查 ncon
若无穿透：applied=cand，结束
若有穿透：按关节二分缩小，更新 applied
5. （③ 里已硬写）viewer 显示新姿态
6. 定时器 _publish_state 读出 qpos 发布
```
ROS 回调可能在步骤 3 与 4 之间再次更新 `goal`；因持锁，要么整步完成后再处理新消息，要么下一步再 slew。
---
## 9. 自检：`--track-test` 函数路径
```text
main()
bridge.set_nominal_targets(open_hand_positions())
loop step=0..199:
step_simulation_unlocked() # slew=0，无投影，瞬到 open
step==200:
bridge.set_nominal_targets(half_fist_positions())
step==600:
tracking_error() → max|q_sim - q_nominal| 主动关节
max <= 0.02 → PASS
```
`half_fist_positions()`：复制 `G20_OPEN_CMD`，把通道 `0..4, 15..19` 设为 `80`，再 `sdk_range_to_full_urdf_positions`。
---
## 10. 常用 MuJoCo API 速查（本工程用到的）
| API | 用途 |
|-----|------|
| `MjModel.from_xml_path(path)` | 加载 MJCF |
| `mj_forward(model, data)` | 运动学前向 + 碰撞检测 |
| `mj_step(model, data)` | 动力学步进（仅 `--pd-drive`） |
| `mj_id2name(model, mjOBJ_JOINT, i)` | 关节名 |
| `data.qpos[adr]` / `data.qvel[adr]` | 广义坐标 |
| `data.ncon` / `data.contact[i].dist` | 接触对与穿透深度 |
| `viewer.launch_passive` + `cam.lookat/distance/azimuth/elevation` | 对比视角 |
---
## 11. 改代码时最常动的位置
| 需求 | 改哪里 |
|------|--------|
| SDK→弧度映射、张开/半握预设 | `utils/g20_joints.py` |
| 限速、穿透阈值、投影迭代次数 | `g20_plan_bridge.py` 顶部常量 |
| 默认驱动/投影开关 | `G20PlanBridge.__init__` 或 CLI |
| 碰撞形状、exclude | `linker_hand_g20_left.xml` |
| 话题名 | `TOPIC_*` 常量或 CLI |
| 对比相机 | `COMPARE_*` + `main()` 里 `viewer.cam.*` |
---
## 12. 与 Isaac 桥的对应关系（读代码对照用）
| MuJoCo | Isaac (`isaac_g20_plan_bridge.py`) |
|--------|--------------------------------------|
| `sdk_range_to_full_urdf_positions` | `joint_mapping.sdk_range_to_full_urdf_positions` |
| `_hard_write_pose` / `set_dof_positions` | 默认都是位置硬写 |
| `collision_project` + `mj_forward` | **无**；靠 USD 选择性 collider |
| `slew_rad_s`（rad/s） | `slew_rad`（rad/物理步，默认 0） |
| `/sim/mujoco/g20/left/joint_state` | `/sim/isaac/g20/left/joint_state` |
Isaac 对照见 [`G20_Isaac_技术详解.md`](./G20_Isaac_技术详解.md) / [`G20_Isaac_实现说明.md`](./G20_Isaac_实现说明.md)。