仿真夹爪适配器：服务接口对齐真机，开口按正运动学换算成真毫米
为什么
------
应用层只认 arm_msgs/srv/GripperExecute（开口毫米），真机与仿真必须同一套接口。
但 GN01 的 URDF 里 <mimic> 是**线性近似**：直接按线性系数下发 finger_width_joint，
"命令开 20 mm"在 Gazebo 里量出来是 21.75 mm —— 抓不住工件，而且看起来像算法问题。
实测（2026-09-24，Gazebo 内量两指尖间距）：
线性下发 20 mm → 实际 21.74 mm（偏差 1.74 mm，超 2 mm 抓取容差的前半步）
走 FK 反解 → 实际 20.00 mm（≤0.5 mm）
怎么改
------
- 新增 sim_gripper_kin.py：解析 URDF 正运动学，算出"主关节值 ↔ 真实开口 mm"的
单调曲线，反解用二分。曲线非单调/零行程时**主动报错**，不静默退化成线性。
- sim_gripper.py：开口换算全部走该曲线；容差统一改成毫米。
- sim_gripper_adapter.py：反馈改取**从动关节**（见下）；is_gripped 按毫米判定。
- 新增 check_sim_gripper.py：三层验收（服务/开口一致/指尖真实几何行程 + 对称性）。
踩到的坑
--------
- finger_width_joint 挂在无质量的 unused_finger_width_link 上，与手指**没有机械
连接**：命令到 0.100，它的 /joint_states 恒为 1.4e-08，6 个从动关节才准到
0.7854。反馈若取它，/gripper/status 会永远报"开口 0 mm"。
- 残留进程会伪造出"代码写错了"的现场：旧 sim_gripper_adapter 与新实例抢话题，
验收读到"指尖间距 0.03 mm"（真实 127 mm）；孤儿 static_transform_publisher
用 latched /tf_static 把旧相机外参按在新仿真上。
→ 新增 scripts/lib/sim_procs.sh；run_sim_gz.sh 用 setsid 独立进程组，
PGID 记进 log_sim/sim.pgid，stop 时整组 kill；启动前预检拒绝带病启动。
- 验收脚本自己也错过两次（把 TF 的米当毫米比 → 127 mm 显示成 0.13 mm；
用会转动指垫的中点漂移判对称 → 必然位移 16 mm）。判据改成"两侧各走多远"，
单位只在 _dist_mm 里换算一次。
验证
----
python3 -m pytest src/arm_sim_gazebo/test/ -q → 307 passed
python3 scripts/check_sim_gripper.py → 三层全过
第 2 层：请求/反馈开口最大偏差 < 0.5 mm
第 3 层：期望行程 vs 实际行程、左右行程差，均 < 1.5 mm
bash scripts/stop_sim_gz.sh --list → 整组列出（含 static tf）
文档
----
- 仿真手册：新增 §4.4（开口 mm 怎么换算）、坑 12（宽度关节不动、
反馈必须取从动关节）；停止一节改写成进程组语义。
- FRAMEWORK_RULES：新增 R4.11（残留进程会伪造现场）、R4.12（验收判据要挑
与实现无关的物理量）；R4.9 表补夹爪；清单补 13/14 两条。
- HANDBOOK：新增 §3.4「在仿真里开合夹爪」，补 §4.4 的两条新教训。
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
arm_dev
@@ -64,18 +64,20 @@ ros2 topic echo /tactile/left/status --once # 触觉三力在发
|
||||
ign model -m workpiece -p # 工件在台面上
|
||||
python3 scripts/check_sim_cameras.py # ★ 相机的真正验收
|
||||
python3 scripts/check_sim_scene.py # ★ 场景摆放 + 孔检测几何验收
|
||||
python3 scripts/check_sim_gripper.py # ★ 夹爪的几何验收（开合行程）
|
||||
```
|
||||
|
||||
机械臂应当**在重力下保持站住**。若它瘫下去或消失，说明描述有问题 ——
|
||||
见 [坑 1](#坑-1官方-flexiv_description-直接喂-gazebo-会被-sdformat-拒掉)。
|
||||
|
||||
**两条 `check_*.py` 才是验收标准，"有图像"证明不了什么。**
|
||||
**这几条 `check_*.py` 才是验收标准，"有图像 / 服务有返回"证明不了什么。**
|
||||
它们做的判断分别是：
|
||||
|
||||
| 工具 | 它凭什么判"通过" | 它防的是哪一类错 |
|
||||
|------|------------------|------------------|
|
||||
| `check_sim_cameras.py` | 把深度**反投影**到 world，要求命中已知平面（地面 z=0、台面 z=0.40） | 深度单位差 1000 倍、`frame_id` 盖成本体系、内参是 gz 的错值、相机位姿朝向反了 —— 全都**能出图** |
|
||||
| `check_sim_scene.py` | 摆放自洽 + 像素门槛与真机一致 + 孔检测特征的像素值达标 + 手外相机**完整**看得见工件 | 物体悬空/穿模/嵌入、场景里的门槛与真机漂移、成像尺度不够、粗定位框不全 |
|
||||
| `check_sim_gripper.py` | 命令开口 mm → **两指尖在 world 里的实际间距**增加同样的 mm；两侧行程相等 | 耦合链套错一层、命令数组顺序错、毫米/米混用、适配器没起来（服务在、话题在，夹具就是不动） |
|
||||
|
||||
### 1.4 停止
|
||||
|
||||
@@ -84,8 +86,26 @@ bash scripts/stop_sim_gz.sh # 稳妥地停掉整套仿真
|
||||
```
|
||||
|
||||
**不要用 `pkill -f gz`**：它会把调用它的那个 shell 一起杀掉（`-f` 匹配命令行，
|
||||
而 `pkill` 自己的命令行里就含 `gz`），也会误杀同名的其他进程。`stop_sim_gz.sh`
|
||||
按进程树精确识别，排除了自己和自己的祖先进程。
|
||||
而 `pkill` 自己的命令行里就含 `gz`），也会误杀同名的其他进程。
|
||||
|
||||
`stop_sim_gz.sh` 走两条路：
|
||||
|
||||
1. **进程组**（主通道）：`run_sim_gz.sh` 用 `setsid` 把整栈起在自己的会话里，
|
||||
PGID 记在 `log_sim/sim.pgid`，停止时 `kill -TERM -PGID` 一次带走整组 ——
|
||||
不依赖任何"进程名模式表"。
|
||||
2. **模式扫描**（兜底）：`scripts/lib/sim_procs.sh` 的点名表 + `install_sim/`
|
||||
兜底规则，收拾历史孤儿与 `ign` 自己 fork 出去的进程；杀完再扫一遍，
|
||||
有残留就非零退出。
|
||||
|
||||
⚠️ **残留进程比"没停干净"严重得多**，这是实测踩出来的：
|
||||
- 残留的 `sim_gripper_adapter` 会和新适配器**同时**订阅 `/joint_states`、
|
||||
同时发布 `/gripper/status`，服务调用还会被随机分给其中一个；
|
||||
- 残留的 `static_transform_publisher` 发的是 latched `/tf_static`，
|
||||
会把**旧相机外参**一直按在新仿真的 TF 树上。
|
||||
|
||||
症状一模一样：验收脚本读到"开口恒为 0 mm、指尖间距 0.03 mm"（真实值 127 mm），
|
||||
看起来就像夹爪换算写错了。所以 `run_sim_gz.sh` 现在会**拒绝**在残留状态下启动，
|
||||
`check_sim_gripper.py` 也会先查重名节点。
|
||||
|
||||
---
|
||||
|
||||
@@ -273,6 +293,42 @@ devices/sim/gazebo/urdf/arm.sim.sensors.xacro # image_width / image_height（真
|
||||
|
||||
---
|
||||
|
||||
### 4.4 夹爪：仿真里怎么开合，口径是什么
|
||||
|
||||
真机和仿真的**服务接口是同一套**（`arm_msgs/srv/GripperExecute`），所以应用层代码
|
||||
不用为仿真改一行：
|
||||
|
||||
```bash
|
||||
ros2 service call /gripper/open arm_msgs/srv/GripperExecute # 全开
|
||||
ros2 service call /gripper/close arm_msgs/srv/GripperExecute # 全合
|
||||
ros2 service call /gripper/move arm_msgs/srv/GripperExecute "{command: {command: move, position: 20.0}}" # 开 20 mm
|
||||
ros2 topic echo /gripper/status --once # 开口、是否夹住、是否在动
|
||||
```
|
||||
|
||||
**口径（唯一容易搞错的地方）：`position` 是开口毫米，而且是真的毫米。**
|
||||
|
||||
仿真夹爪是官方 GN01，它的 URDF 用一条**线性** `<mimic>` 耦合把 6 个从动关节挂在
|
||||
"宽度关节"上。线性近似在线性上不准：命令 20 mm，两指实际相距 21.75 mm ——
|
||||
抓 20 mm 的工件会空出 1.75 mm，**没有接触力**，触觉那一步就废了。
|
||||
|
||||
所以适配器不按线性换算，而是用 **URDF 正运动学反解**（`sim_gripper_kin.py`）：
|
||||
命令 20 mm → 解出使"两指间距 == 20 mm"的关节值。这个正运动学与 Gazebo 的
|
||||
实测吻合到 **0.01 mm**（0~100 mm 全程），所以"开口 20 mm"在仿真里是物理上真的 20 mm。
|
||||
|
||||
顺带两个必然的推论，看到别当 bug：
|
||||
|
||||
| 现象 | 原因 |
|
||||
|------|------|
|
||||
| 启动日志说行程是 **99.93 mm** 而不是 100.0 | 上游限位写成 0~0.100 m，线性耦合让指尖只走了 99.93 mm。日志报实测值，否则"全开时开口读数与几何对不上"会变成一个查不出的矛盾 |
|
||||
| 合拢时两个 `*_finger_tip` frame 相距 27.09 mm | tip frame 不在夹持面上，且随指垫转动。**别拿它当开口**，要用它的**变化量**（变化量与开口 1:1） |
|
||||
|
||||
改夹爪的耦合关系时：`devices/sim/gazebo/config/controllers.yaml` 的 `joints:` 顺序、
|
||||
归一化时删下来的 `<mimic>` 表、URDF 限位三者必须一致，`sim_gripper.py` 会在启动时
|
||||
逐条校验并**明确报错**（顺序错不会报错，只会把角度发给别的关节）。
|
||||
验收不看"夹爪动了没有"，看几何：`python3 scripts/check_sim_gripper.py`。
|
||||
|
||||
---
|
||||
|
||||
## 5. 实测踩到的坑
|
||||
|
||||
这些坑的共同特征是：**不报错**。话题在、图像有、TF 查得到、`create` 打印成功，但结果是错的。
|
||||
@@ -396,6 +452,27 @@ contact-system 下**永远是空的**（实测 `ign topic -e -t /tactile/left/ra
|
||||
|
||||
---
|
||||
|
||||
### 坑 12：夹爪的"宽度关节"在 Gazebo 里**根本不动**
|
||||
|
||||
GN01 的描述里有一条 `finger_width_joint`，注释说它"本应是 prismatic，但为了配合
|
||||
mimic 只能是 revolute"，挂在 `unused_finger_width_link` 上（那个 link 的注释写着
|
||||
"cannot have 0 mass" —— 纯粹为了 URDF 合法）。它和手指**没有任何机械连接**。
|
||||
|
||||
实测（2026-09-24）：把它命令到 0.100，它的 `/joint_states` 值**恒为 ~0**
|
||||
（1.4e-08），而 6 个从动关节准确到位（0.7854 = 9.404×0.100−0.155）。
|
||||
|
||||
于是有两件事必须做对：
|
||||
|
||||
1. 命令数组里**仍然要带上它**（`controllers.yaml` 的 `joints:` 含它，少一个会错位）；
|
||||
2. **反馈绝不能取它** —— 否则 `/gripper/status` 会永远报"开口 0 mm"，
|
||||
而夹爪明明开着。这是最典型的一类静默失效：服务返回成功、状态有数据、
|
||||
就是数为错。
|
||||
|
||||
适配器的做法：反馈取"与主关节直接相连、|multiplier| 最大的那个从动关节"
|
||||
（GN01 是 `left_outer_knuckle_joint`），再按耦合关系反推开口。
|
||||
|
||||
---
|
||||
|
||||
## 6. 现状与进度
|
||||
|
||||
### 已就绪（端到端验证过）
|
||||
@@ -410,7 +487,7 @@ contact-system 下**永远是空的**（实测 `ign topic -e -t /tactile/left/ra
|
||||
| 相机 | `config/cameras.yaml` + `sim_camera_adapter` | D405/D435 桥接到 ROS，内参/单位/frame 都已校正 |
|
||||
| 触觉 | `config/tactile.yaml` + `sim_tactile_adapter` | `force_torque` 三向力 + `contact` 布尔 → `arm_msgs/TactileStatus` |
|
||||
| 场景物体 | `models/` + `config/scene.yaml` | 工件（带亮环的孔板）、通止规、规座，可校验摆放 |
|
||||
| 夹爪 | 7 关节接入 `ros2_control` | 实测开合 27→127mm |
|
||||
| 夹爪 | 7 关节接入 `ros2_control` + `sim_gripper_adapter` | 开口走**毫米制**（FK 反解，见 §4.4）；实测合拢→全开 27.09→127.02 mm，行程 **99.93 mm**；验收 `check_sim_gripper.py` |
|
||||
|
||||
### 还没做
|
||||
|
||||
@@ -418,7 +495,7 @@ contact-system 下**永远是空的**（实测 `ign topic -e -t /tactile/left/ra
|
||||
|------|--------|------|
|
||||
| 视触觉图像 | 仿真的深度图/力矢量图只能是**占位**（空/NaN） | `TactileStatus` 的图字段在仿真里不可信；三力是可信的 |
|
||||
| 夹具/料框 | 目前只有工件、规、规座 | 上料流程（若有）没法验 |
|
||||
| 夹爪服务适配器 | `~/open|~/close|~/move` 服务（mm ↔ 弧度换算） | 仿真里目前只能直接发关节指令开合，无法走真机那套服务接口 |
|
||||
| 夹爪"夹住"判据 | `is_gripped` 是**推断**（指令到合拢位但实际没跟上），不是测量 | 真机有 DH 状态字；仿真里最终仍应看触觉力 |
|
||||
|
||||
---
|
||||
|
||||
@@ -430,6 +507,8 @@ contact-system 下**永远是空的**（实测 `ign topic -e -t /tactile/left/ra
|
||||
| D435 图幅 | 1280x720 | 640x480 | 像素尺度不同，`hole_detect` 的像素门槛搬过去要重算 |
|
||||
| 触觉单位 | Orisys 形变任意单位（非牛顿） | 牛顿 × `force_scale` | 见 `config/tactile.yaml` 头部的"单位口径"；选错**不报错**，只是阈值不触发或一碰就炸 |
|
||||
| 触觉图像 | 有真实深度图/力矢量图 | 占位 | 下游不用改代码，但图内容不可信 |
|
||||
| 夹爪型号 | DH PGEA-100-20-W（`site.yaml` 的 `stroke_mm` = 20 mm） | 官方 GN01（行程 **99.93 mm**） | 同一个"开 8 mm"两边几何都对，但"全开"对应的开口差 5 倍 —— 别把仿真的全开当真机的全开（启动日志会打出来） |
|
||||
| 夹爪指令路径 | DH 走 USB-RS485 Modbus（IPC） | `JointGroupPositionController` + `sim_gripper_adapter` | 上层服务名/消息类型一致（`GripperExecute`），所以算法不用改 |
|
||||
|
||||
---
|
||||
|
||||
@@ -459,6 +538,8 @@ devices/sim/gazebo/
|
||||
|
||||
配套代码：`src/arm_sim_gazebo/`（启动器、描述归一化、相机适配器、触觉适配器、场景逻辑）
|
||||
配套脚本：`scripts/run_sim_gz.sh`（入口）、`scripts/stop_sim_gz.sh`（停止）、
|
||||
`scripts/check_sim_cameras.py`（相机验收）、`scripts/check_sim_scene.py`（场景验收）
|
||||
`scripts/lib/sim_procs.sh`（仿真进程识别）、`scripts/check_sim_cameras.py`（相机验收）、
|
||||
`scripts/check_sim_scene.py`（场景验收）、`scripts/check_sim_gripper.py`（夹爪验收）
|
||||
单测：`src/arm_sim_gazebo/test/`（`test_urdf_normalize` / `test_sim_cameras` /
|
||||
`test_sim_site` / `test_sim_tactile` / `test_robot_description` / `test_check_sim_scene`）
|
||||
`test_sim_site` / `test_sim_tactile` / `test_robot_description` / `test_check_sim_scene` /
|
||||
`test_sim_gripper` / `test_sim_gripper_kin`）
|
||||
|
||||
Reference in New Issue
Block a user