mujoco_linkerbot/docs/G20_MuJoCo_实现说明.md

# 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（每物理步瞬达） |