# Mujoco LinkerBot（ROS 2）

Linker Hand 的 MuJoCo + ROS 2 仿真工作区。支持：

- **O6**：经典 6 通道 0~255 控制（launch）
- **G20 左手**：PLAN 桥接、标定查表、MJCF/URDF 零位同步
- **L20 V10.1 左手**：与 G20 双窗对比
- 关节弧度监视 GUI、曲线录制与实验脚本

远程仓库：<https://gitea.robotquan.com/sunxianghui/mujoco_linkerbot.git>

---

## 环境要求

- Ubuntu + ROS 2 **Jazzy**
- 工作区路径示例：`~/linker_hand_mujoco_ros2`
- Python 依赖在本地 `venv`（含 `mujoco` 等；**不要**把 `venv` 提交进仓库）
- `ros2 launch` 使用系统 Python，仅 `source venv` 不够，请用仓库里的启动脚本（会把 `venv/site-packages` 加入 `PYTHONPATH`）

---

## 一、编译（首次或改代码后）

```bash
cd ~/linker_hand_mujoco_ros2
source /opt/ros/jazzy/setup.bash
colcon build --symlink-install --packages-select linker_hand_mujoco_ros2
source install/setup.bash
```

`venv/` 已放 `COLCON_IGNORE`，不会被 colcon 误扫。

---

## 二、O6 仿真（原有流程）

```bash
./run_sim.sh
```

- 控制话题：`/cb_left_hand_control_cmd`
- 类型：`sensor_msgs/msg/JointState`
- `position`：6 个数，约 **0~255**（**255≈张开，0≈握紧**）

| 索引 | 含义 |
|------|------|
| 0 | 拇指弯曲 |
| 1 | 拇指横摆 |
| 2 | 食指弯曲 |
| 3 | 中指弯曲 |
| 4 | 无名指弯曲 |
| 5 | 小指弯曲 |

更多 O6 / 多机 DDS / 曲线录制说明见下文「附录：O6 与多机」。

---

## 三、G20 左手仿真（推荐）

### 3.1 一键启动

```bash
./run_g20_mujoco_plan.sh
```

默认行为：

| 项 | 值 |
|----|-----|
| 模型 | `urdf/G20/linker_hand_g20_left/linker_hand_g20_left.xml` |
| 控制话题 | `/g20/cb_left_hand_control_cmd` |
| 状态话题 | `/sim/mujoco/g20/left/joint_state` |
| 指令 | 20 维 0~255（与真机 SDK 通道对齐） |
| 映射 | 标定 JSON 查表 + URDF 被动 mimic |
| 速度 | 约 1.35× 墙钟，可用 `--speed` / `--slew-rad-s` 调整 |

常用参数示例：

```bash
./run_g20_mujoco_plan.sh --speed 1.0
./run_g20_mujoco_plan.sh --cmd-topic /g20/cb_left_hand_control_cmd
./run_g20_mujoco_plan.sh --mapping g20_calibrated
```

### 3.2 关节弧度 GUI

```bash
./run_g20_joint_gui.sh
```

默认订阅 `/sim/mujoco/g20/left/joint_state`，按手指分组显示 hinge 弧度（默认 6 位小数）。

### 3.3 G20 + L20 双窗对比

```bash
./run_dual_mujoco_compare.sh
```

- 左窗：当前标定 **G20**（`g20_calibrated`）
- 右窗：**L20 V10.1** 原版 URDF 映射（`l20_urdf`，默认关 collision project，避免自碰卡住）
- 共用同一条 0~255 控制话题（默认 `/g20/cb_left_hand_control_cmd`）

---

## 四、标定与模型文件（G20）

目录：

`src/linkerhand-sim/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/G20/linker_hand_g20_left/`

| 文件 | 说明 |
|------|------|
| `linkerhand_g20_left.urdf` | 当前使用的 URDF（含投影零位修正） |
| `linker_hand_g20_left.xml` | 当前使用的 MJCF（与 URDF 原点/限位同步） |
| `g20_left_G20_LEFT_001_calibration.json` | 运行时 0~255→弧度查表（schema 3） |
| `meshes/` | 网格 |

映射实现：`utils/g20_joints.py`

要点：

1. **主动关节**：用标定 JSON 的 `angle_rad[0..255]` 查表。
2. **零位**：用 `zero_angles.table_projected_zero_rad` 相对夹具参考修正 URDF/MJCF `origin`（相机投影零位，非完整外参优化）。
3. **被动关节**：`thumb_ip`、四指 DIP 走 URDF mimic；JSON 里 `thumb_ip` 是累计投影角，**不能**直接当相对关节角。
4. **四指侧摆**：运行时符号与真机约定见 `G20_FLIP_JOINT_SIGN`。

当前已知：张开大姿态已基本对齐；拇指 CMC 零位仍可能有小量前探/拧向残差。更稳妥的做法是解耦重测三个 CMC 投影零位，或导出 TAG 全位姿后做原点优化，而不是肉眼加减角度。

---

## 五、主要源码入口

| 路径 | 说明 |
|------|------|
| `linker_hand_mujoco_ros2/g20_plan_bridge.py` | G20/L20 MuJoCo PLAN 桥 |
| `linker_hand_mujoco_ros2/g20_joint_monitor_gui.py` | 关节弧度监视 GUI |
| `linker_hand_mujoco_ros2/utils/g20_joints.py` | G20 标定查表与映射 |
| `launch/linker_hand_mujoco_ros2.launch.py` | O6 launch |
| `urdf/O6/` | O6 MJCF |
| `urdf/G20/` | G20 URDF/MJCF + 标定 |
| `urdf/L20_V10.1/` | L20 V10.1 URDF/MJCF |
| `tools/` | 曲线录制、实验脚本 |

Console 入口（`setup.py`）：`g20_plan_bridge`、`g20_joint_monitor_gui` 等。

---

## 六、克隆与推送

```bash
git clone https://gitea.robotquan.com/sunxianghui/mujoco_linkerbot.git
cd mujoco_linkerbot
# 自建 venv 并安装 mujoco 等依赖后，再 colcon build
```

本仓库 `origin` 指向上述 Gitea 地址。提交后：

```bash
git push -u origin main
```

---

## 附录：O6 与多机

### 本机发一帧（O6 握拳示例）

```bash
source /opt/ros/jazzy/setup.bash
export ROS_DOMAIN_ID=30
export ROS_LOCALHOST_ONLY=0

ros2 topic pub --once /cb_left_hand_control_cmd sensor_msgs/msg/JointState "{
  header: {stamp: {sec: 0, nanosec: 0}, frame_id: ''},
  name: [],
  position: [0.0, 100.0, 0.0, 0.0, 0.0, 0.0],
  velocity: [],
  effort: []
}"
```

### 两台电脑共用 ROS 2

两边约定同一 Domain：

```bash
export ROS_DOMAIN_ID=30
export ROS_LOCALHOST_ONLY=0
```

仿真机跑 `./run_sim.sh` 或 `./run_g20_mujoco_plan.sh`；控制机 `ros2 topic list` / `hz` / `echo` 检查。

### 通用曲线录制

```bash
source /opt/ros/jazzy/setup.bash
export ROS_DOMAIN_ID=30
export ROS_LOCALHOST_ONLY=0
python3 tools/hand_curve_recorder_standalone.py --hand left --channel 3 --label sim
```

### 常见问题

**`ModuleNotFoundError: No module named 'mujoco'`**  
用 `./run_sim.sh` / `./run_g20_mujoco_plan.sh`，或手动把 `venv/lib/python3.12/site-packages` 加入 `PYTHONPATH`。

**L20 双窗对比时手不动**  
自碰投影会把目标打回；`run_dual_mujoco_compare.sh` 已对 L20 默认 `--no-collision-project`。

---

## 许可与归属

包内模型与标定数据来自 Linker Hand / 本机标定流程；请按公司内部仓库规范使用与分发。