mujoco_linkerbot/docs/G20_MuJoCo_实现说明.md

G20 左手 · MuJoCo 仿真实现说明

本文记录 MuJoCo G20 PLAN 名义弧度桥 从 URDF 到可跑仿真的完整实现过程，以及相对原始 URDF 的全部具体改动。

• 函数调用与逐步逻辑：见 G20_MuJoCo_技术详解.md（推荐先读此文理解代码）
• Isaac 对照：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 端到端数据流

/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 线性插值表（我们写入的数值）

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 张开预设

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. 如何启动

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。

常用参数：

./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）：

ros2 launch linker_hand_mujoco_ros2 g20_plan_bridge.launch.py

测试半握示例：

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