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.
This commit is contained in:
@@ -0,0 +1,457 @@
|
||||
# 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)。
|
||||
Reference in New Issue
Block a user