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.

docs/G20_MuJoCo_实现说明.md

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