mujoco_linkerbot/docs/G20_MuJoCo_技术详解.md

G20 左手 · MuJoCo 技术详解

本文面向读代码、改逻辑、排查问题：按「程序从哪进、函数怎么调、数据怎么流」写清楚实现细节。
资产路径、URDF/MJCF 改动清单见 G20_MuJoCo_实现说明.md。

---

1. 总体结构：两个线程、三块逻辑

┌─────────────────────────────────────────────────────────────┐
│  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）。

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() 里与模式相关的赋值

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__

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

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 路径）

_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）：

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 特殊处理

# 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)

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 名义弧度路径

_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 默认模式（运动学硬写 + 碰撞投影）

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() — 限速逼近

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

对每个关节名：

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 凸包穿透，则缩小步进，且按关节处理，避免拇指撞掌心时四指一起被卡住。

穿透判定：

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：两碰撞体表面距离；负表示已经穿进去。

算法步骤：

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 分支

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 命令

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）

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：

<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 条）

排除「几何嵌套」导致的假接触，例如：

<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 设为半握弧度。

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 函数路径

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。