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