仿真夹爪适配器：服务接口对齐真机，开口按正运动学换算成真毫米
为什么
------
应用层只认 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`）
|
||||
|
||||
+46
-1
@@ -318,11 +318,49 @@ conda 激活不会往 `LD_LIBRARY_PATH` 加东西，而 gz 的插件搜索路径
|
||||
|------|------|
|
||||
| `scripts/check_sim_cameras.py` | 深度反投影必须命中已知平面（地面 z=0 / 台面 z=0.40） |
|
||||
| `scripts/check_sim_scene.py` | 摆放自洽 + 像素门槛与真机一致 + 成像几何达标 + 手外相机**完整**看得见工件 |
|
||||
| `scripts/check_sim_gripper.py` | 命令开口 mm → 两指尖在 `world` 里的**实际间距**变化同样的 mm；两侧行程相等；指尖坐标在 `--verbose` 里打出来（判错时可以回看它到底取了哪两个点） |
|
||||
|
||||
### R4.10 停仿真用 `scripts/stop_sim_gz.sh`，不要 `pkill -f`
|
||||
|
||||
`pkill -f` 会先匹配到**调用它的那个 shell**（它的命令行里含同样的字符串），
|
||||
表现是"命令秒返回、什么都没停"。脚本按进程树精确识别，并在杀完之后**复查残留**。
|
||||
表现是"命令秒返回、什么都没停"。
|
||||
|
||||
**更重要的是：残留进程比"没停干净"严重得多**（R4.11 解释为什么）。
|
||||
脚本走两条路：进程组（主）+ 模式扫描（兜底），杀完**复查残留**并非零退出。
|
||||
|
||||
### R4.11 仿真栈必须能"整组停掉"：残留会伪造出"代码写错了"的现场
|
||||
|
||||
实测（2026-09-24）踩到的两个残留，症状与"夹爪换算写错了"**一模一样**：
|
||||
|
||||
| 残留的进程 | 它干了什么 | 现象 |
|
||||
|-----------|-----------|------|
|
||||
| `sim_gripper_adapter`（旧会话的） | 与新适配器**同时**订阅 `/joint_states`、同时发布 `/gripper/status`；服务调用被随机分给其中一个 | 验收读到"开口恒为 0 mm、指尖间距 0.03 mm"（真实 127 mm） |
|
||||
| `static_transform_publisher`（5 个孤儿） | 发 **latched** 的 `/tf_static`，把旧相机外参一直按在新仿真的 TF 树上 | TF 查询返回互相矛盾的数据 |
|
||||
|
||||
所以定下三条：
|
||||
|
||||
1. 仿真必须整栈起在**自己的会话/进程组**里（`setsid`，PGID 记在 `log_sim/sim.pgid`），
|
||||
停止时 `kill -TERM -PGID` 一次带走 —— 不依赖任何"进程名模式表"；
|
||||
2. 模式扫描只当兜底，且除点名表外要有**兜底规则**（命令行含 `<仓库>/install_sim/`
|
||||
就算仿真进程），这样"以后新增节点忘了改表"不会再留孤儿；
|
||||
3. **启动前预检 + 验收第一步查重名节点**：宁可拒绝启动，也不给一个
|
||||
"看起来能跑、数据是错的"仿真。
|
||||
|
||||
### R4.12 验收工具自己也会说谎：判据要挑"与实现无关的物理量"
|
||||
|
||||
`check_sim_gripper.py` 的第 3 层先后给过**两个错误结论**，都是脚本自己的问题：
|
||||
|
||||
| 写出来的判据 | 为什么会错 |
|
||||
|-------------|-----------|
|
||||
| 指尖间距差（米）与期望（毫米）比较 | 漏了 ×1000：127 mm 显示成 0.13 mm，**恰好看起来像夹爪没张开** |
|
||||
| "两指中点漂移"判对称 | tip frame 固连在**会转动**的指垫上、走的是弧线，中点必然位移 16 mm；而左右行程其实相等（52.56 / 52.56 mm） |
|
||||
|
||||
推论：
|
||||
|
||||
- 单位换算**只在一个函数里做**（`_dist_mm`），别处不许再乘 1000；
|
||||
- 判据要挑**与机构无关**的量（"两侧各走了多远"），别挑带方向的量再投影；
|
||||
- `--verbose` 要把**原始物理量**（指尖坐标）打出来，判错时能回看；
|
||||
- 结论不对时，先怀疑判据，再怀疑被测对象 —— 上面两条都是先怀疑了代码。
|
||||
|
||||
---
|
||||
|
||||
@@ -493,6 +531,8 @@ grep -l ros_gz_sim install/*/lib/*/* | wc -l # 应当变成 0
|
||||
| 10 | 我写进文档的命令，自己跑过一遍吗？ | R6.7 |
|
||||
| 11 | 这次构建是在**目标环境**里做的吗？（`head -1 install/*/lib/*/*`） | R6.8、R5.7 |
|
||||
| 12 | 我碰的文件属于"跟随当前臂"还是"按臂各留一份"？改完跑 `check_arm_prefixes.py` 了吗？ | R1.1.1 |
|
||||
| 13 | 我验收用的判据，是"与被测实现无关的物理量"吗？单位只在一处换算了吗？ | R4.12 |
|
||||
| 14 | 我起过仿真/长跑进程吗？停干净了吗（`stop_sim_gz.sh --list` 应为空）？ | R4.10、R4.11 |
|
||||
|
||||
---
|
||||
|
||||
@@ -532,6 +572,11 @@ grep -l ros_gz_sim install/*/lib/*/* | wc -l # 应当变成 0
|
||||
| 代码里一半英文 docstring 一半中文注释 | 没有统一口径，谁写谁定 | R6.5 |
|
||||
| 真机栈哪天突然报"模块找不到"，明明装过 | `install/` 的入口脚本 shebang 指向**仿真环境**的 python（哑弹） | R6.8 |
|
||||
| 照着文档敲命令，命令根本不存在 | 写文档时没真跑过 | R6.7 |
|
||||
| 夹爪明明开着，`status` 一直报"开口 0 mm" | 反馈取了"虚拟宽度关节"，它在 Gazebo 里根本不动 | R4.11、坑 12 |
|
||||
| 验收说指尖只张开 0.13 mm（其实 127 mm） | 验收脚本自己漏了 ×1000（米 vs 毫米） | R4.12 |
|
||||
| 服务/话题/TF 全在，但读数全是乱的 | 上一轮的 `sim_gripper_adapter` / `static_transform_publisher` 还活着 | R4.11 |
|
||||
| 命令"开 20 mm"，两指实际 21.75 mm，夹不住工件 | 上游 `<mimic>` 是线性近似，开口必须走正运动学反解 | R4.4、坑 12 |
|
||||
| 残留进程把旧相机外参按在新仿真上 | `static_transform_publisher` 发的是 latched `/tf_static` | R4.11 |
|
||||
|
||||
---
|
||||
|
||||
|
||||
+39
-9
@@ -170,6 +170,7 @@ D405 YOLO 手眼 T 伺服 ros2_cont
|
||||
| 触觉灵敏度 | `site.yaml` → `hole_detect.stage3:` 的 `*_delta` | 看启动日志打印的派生阈值 | [`TACTILE.md`](TACTILE.md) |
|
||||
| 相机曝光 / 图幅 / 视场 | `site.yaml` → `cameras:`（真机） | `list_realsense.sh` | [`SITE_CONFIG.md`](SITE_CONFIG.md) |
|
||||
| 仿真场景里放什么、放哪 | `devices/sim/gazebo/config/scene.yaml` | `python3 scripts/check_sim_scene.py` | [仿真手册 §4.1](../devices/sim/gazebo/README.md) |
|
||||
| 仿真里开合夹爪（开口 mm） | 服务 `/gripper/{open,close,move}`，与真机同一套 | `python3 scripts/check_sim_gripper.py` | [仿真手册 §4.4](../devices/sim/gazebo/README.md) |
|
||||
| 仿真相机视场/图幅 | 仿真手册 §4.3，**三处要同时改** | `python3 scripts/check_sim_cameras.py` | 同上 §4.3 |
|
||||
| 算法权重（换模型） | `models/<分组>/` + `site.yaml` 的路径 | `scripts/weights.sh list` | [`models/README.md`](../models/README.md) |
|
||||
| 接一个新品牌设备 | `devices/<类别>/<品牌>/` + `registry.yaml` 一行 | `ros2 run arm_interfaces device_check` | [`devices/README.md`](../devices/README.md) |
|
||||
@@ -212,7 +213,25 @@ source scripts/sim_env.sh
|
||||
python3 scripts/check_sim_scene.py # 悬空 / 穿模 / 出界 / 互相嵌入 / 像素几何
|
||||
```
|
||||
|
||||
### 3.4 加一个算法权重
|
||||
### 3.4 在仿真里开合夹爪
|
||||
|
||||
服务名与真机一致，所以脚本两边通用：
|
||||
|
||||
```bash
|
||||
source scripts/sim_env.sh
|
||||
ros2 service call /gripper/move arm_msgs/srv/GripperExecute \
|
||||
"{command: {command: move, position: 20.0}}" # 开口 20 mm（真的 20 mm）
|
||||
ros2 topic echo /gripper/status --once # 开口 / 是否夹住 / 是否在动
|
||||
python3 scripts/check_sim_gripper.py # ★ 验收：拿两指尖的实际间距说话
|
||||
```
|
||||
|
||||
`position` 是**开口毫米**，仿真里由 URDF 正运动学反解保证是真的毫米
|
||||
（上游的线性 mimic 近似会让"开 20 mm"变成 21.75 mm，抓不住工件 ——
|
||||
见[仿真手册 §4.4](../devices/sim/gazebo/README.md)）。
|
||||
|
||||
如果启动日志说行程是 **99.93 mm**：那是对的，是实测值，不是名义的 100.0。
|
||||
|
||||
### 3.5 加一个算法权重
|
||||
|
||||
放进去 → 打标签 → 改路径 → 体检。TensorRT 转换也在这条链上：
|
||||
|
||||
@@ -225,7 +244,7 @@ bash scripts/weights.sh doctor --check-engines # 查版本不匹配这
|
||||
布局是 `models/<分组>/<模型名>.{pt,onnx,engine}` + 旁挂 `<模型名>.labels.yaml`。
|
||||
**二进制不进 git**，标签文件进。规则见 [`models/README.md`](../models/README.md)。
|
||||
|
||||
### 3.5 接一个新品牌设备
|
||||
### 3.6 接一个新品牌设备
|
||||
|
||||
四步（完整说明在 [`devices/README.md`](../devices/README.md)，这里只给顺序）：
|
||||
|
||||
@@ -248,13 +267,13 @@ ros2 run arm_interfaces device_check --list-brands # 看每类现在有哪
|
||||
> `required: true` 的配置键在 `site.yaml` 里有没有值。
|
||||
> 任何一项不满足**立即报错并指出该去哪个文件补什么**，不静默降级。
|
||||
|
||||
### 3.6 改相机分辨率（最容易漏改）
|
||||
### 3.7 改相机分辨率（最容易漏改）
|
||||
|
||||
真机改 `site.yaml` 就行。**仿真要改三处**，因为像素门槛是绑在图幅上的 ——
|
||||
改完不算清楚，孔检测在仿真里会"根本不可能通过"，而看起来像算法不行。
|
||||
详见[仿真手册 §4.3](../devices/sim/gazebo/README.md)。
|
||||
|
||||
### 3.6.1 换一台机械臂（最容易漏改，而且很难查）
|
||||
### 3.7.1 换一台机械臂（最容易漏改，而且很难查）
|
||||
|
||||
`site.yaml` 里写的是**哪台臂**，但有一批一级配置文件里写死了**臂的关节名**
|
||||
（Flexiv 官方把 SN 编进关节名：`Rizon4-063989_joint1`）。这些文件必须跟着改：
|
||||
@@ -284,7 +303,7 @@ bash scripts/build.sh # 4. 让 install/ 同步
|
||||
> 排查提示：`ROBOT_SN` / `ROBOT_ARM` 环境变量会**覆盖** `site.yaml` 的 `robot.sn`。
|
||||
> "明明改了配置却没生效"时先 `echo $ROBOT_SN`，或看 `--list` 打印的"来源"。
|
||||
|
||||
### 3.7 改完必须做的一件事
|
||||
### 3.8 改完必须做的一件事
|
||||
|
||||
**问自己：我故意弄坏它，哪个检查会红？**
|
||||
|
||||
@@ -324,6 +343,7 @@ python3 scripts/check_arm_prefixes.py # 含臂名的配置是否
|
||||
# —— 仿真验收（★ 必做，别靠肉眼）——
|
||||
python3 scripts/check_sim_cameras.py # 深度反投影命中已知平面才算过
|
||||
python3 scripts/check_sim_scene.py # 摆放 / 像素门槛漂移 / 成像几何 / 可见性
|
||||
python3 scripts/check_sim_gripper.py # 夹爪：命令开口 mm == 两指实际张开 mm
|
||||
|
||||
# —— 只改文字时自证 ——
|
||||
python3 scripts/check_text_only.py
|
||||
@@ -372,9 +392,19 @@ python3 -m pytest src/arm_sim_gazebo/test -q
|
||||
于是"没检查"被伪装成"通过"（R5.3、R5.4）。
|
||||
- `bash -n` 说脚本语法 OK，脚本却**跑不起来** —— CRLF 行尾会漏报，
|
||||
只有真跑 `--help` 才暴露（R6.6）。
|
||||
- 验收脚本自己**算错了单位** —— `check_sim_gripper.py` 把 TF 的"米"当成"毫米"
|
||||
去比阈值，于是 127 mm 被显示成 0.13 mm，**恰好看起来像夹爪没张开**，
|
||||
让人跑去查夹爪代码（R4.12）。
|
||||
- 上一轮的**残留进程**伪造出"代码写错了"的现场 —— 两个同名适配器抢话题/服务，
|
||||
残留的 `static_transform_publisher` 把旧外参按在新仿真上，读数全是乱的（R4.11）。
|
||||
|
||||
**所以**：写完校验，先故意弄坏被校验的对象，确认检查真的会红；
|
||||
负向测试和正向测试一样重要（R5.2）。
|
||||
**所以**：
|
||||
|
||||
1. 写完校验，先故意弄坏被校验的对象，确认检查真的会红（负向测试和正向测试
|
||||
一样重要，R5.2）；
|
||||
2. 判据要挑**与被测实现无关的物理量**，单位只在一处换算；
|
||||
3. 结论不对时，先怀疑**判据和环境**，再怀疑被测对象 —— 上面后两条都是
|
||||
"先怀疑了代码，结果是自己错"（R4.12）。
|
||||
|
||||
---
|
||||
|
||||
@@ -407,8 +437,8 @@ python3 -m pytest src/arm_sim_gazebo/test -q
|
||||
| 校验代码永远通过 | `sdformat` 用 `__default__` / `__default_topic__` 假值填了缺失字段 | 仿真手册坑 9 |
|
||||
| 脚本报 `$'\r': 未找到命令` | 脚本是 CRLF 行尾 | R6.6 |
|
||||
| 改了参数不生效 | 改的是包内 yaml，被 `site.yaml` 覆盖了 | 3.2 |
|
||||
| MoveIt 说规划成功、机械臂不动 | SRDF / controllers 里还是**旧臂**的关节名 | 3.6.1、R1.1.1 |
|
||||
| 控制器报 `Joint 'Rizon4-xxxxxx_joint1' not found` | 换了臂但一级配置没跟着改（或改了两处漏一处） | 3.6.1 |
|
||||
| MoveIt 说规划成功、机械臂不动 | SRDF / controllers 里还是**旧臂**的关节名 | 3.7.1、R1.1.1 |
|
||||
| 控制器报 `Joint 'Rizon4-xxxxxx_joint1' not found` | 换了臂但一级配置没跟着改（或改了两处漏一处） | 3.7.1 |
|
||||
| 仿真镜头位置不对、只看得见地面 | 相机 `rpy` 复合顺序 | 仿真手册坑 5 |
|
||||
| 脚本说"已停止"，进程还在 | 进程模式表漏项 | R4.10 |
|
||||
| 孔检测在仿真里怎么都不过 | 图幅与真机不同，像素门槛失效 | 仿真手册 §4.3 |
|
||||
|
||||
@@ -0,0 +1,411 @@
|
||||
#!/usr/bin/env python3
|
||||
"""仿真夹爪的验收：**几何**行程对不对，不是"服务有返回"。
|
||||
|
||||
为什么需要它
|
||||
------------
|
||||
夹爪出问题的方式几乎全是静默的：
|
||||
|
||||
* 耦合链只套了一层 multiplier —— 夹爪会动、角度也是那个量级，就是夹不准；
|
||||
* 命令数组顺序与控制器不一致 —— 角度发给了别的关节，夹爪"动得很奇怪"；
|
||||
* 毫米与米混用（差 1000 倍） —— 夹爪开到别的地方；
|
||||
* 夹爪适配器根本没起来 —— 服务不存在，但**话题照旧**、控制器照旧 active。
|
||||
|
||||
前三条都能通过"调服务有 success=true、status 里有数"这种验收。
|
||||
所以这里分三层，只有第二、三层算数：
|
||||
|
||||
第 1 层 /gripper/* 服务在、/gripper/status 有数据 （不算验收）
|
||||
第 2 层 请求开口 mm == 关节反馈换回来的开口 mm （证明下发链路）
|
||||
第 3 层 两个指尖在 world 系里的**实际间距**随请求变化，
|
||||
且"合拢→张开"的行程 == 请求的开口 （证明耦合与几何）
|
||||
|
||||
第 3 层是关键：第 2 层只说明"关节值到位了"。如果 mimic 的 multiplier 写错，
|
||||
手指根本不会按预期张开，而第 2 层完全看不出来。也正因为这一层是**独立**的，
|
||||
它才逼出了下面两个真实故障 —— 都是"验收脚本自己错"，比被测对象错更常见：
|
||||
|
||||
⚠️ 三个踩过的坑（都让本脚本先给出过错误结论）
|
||||
--------------------------------------------
|
||||
1. **米 / 毫米**。TF 查出来的坐标是米，而本脚本的阈值与期望值全是毫米。
|
||||
漏掉 ×1000 时，127 mm 会显示成 0.13 mm —— 恰好"看起来像夹爪没张开"，
|
||||
于是跑去查耦合公式，而真正的问题就在本文件里。（判据全部走 _dist_mm。）
|
||||
|
||||
2. **残留进程**。旧会话没停干净时，会有两个 sim_gripper_adapter 同时订阅
|
||||
/joint_states、同时发布 /gripper/status，服务调用还会被随机分给其中一个；
|
||||
残留的 static_transform_publisher 更阴 —— 它发 latched 的 /tf_static，
|
||||
会把**旧相机外参**按在新仿真的 TF 树上。症状一样是"读数全乱"。
|
||||
→ 所以本脚本第一步查重名节点（`:meth:`GripperCheck._duplicate_nodes`），
|
||||
启动侧也加了闸门（scripts/run_sim_gz.sh 的预检 + setsid 进程组）。
|
||||
|
||||
3. **判据要与机构无关**。两侧"对称"不能用"两指中点漂移"来判：tip frame 是
|
||||
刚性固连在**会转动**的指垫上的（指垫既平移又转，指面才保持平行），所以
|
||||
frame 走的是弧线，中点必然位移 16 mm 左右。真正与方向无关的判据是
|
||||
"两侧各自走了多远"（实测 52.56 / 52.56 mm）。
|
||||
|
||||
⚠️ 指尖坐标系原点的固有间距
|
||||
实测两个 `*_finger_tip` frame 在完全合拢时仍然相距 27.09 mm（tip frame 不在
|
||||
夹持面上，而且会随手指转动）。所以**不要**拿"指尖间距"直接当开口看；
|
||||
本脚本用它的**变化量**（行程）判据 —— 那个变化量与开口是 1:1 的
|
||||
（实测：请求 8 / 20 / 50 mm → 间距增加 8.00 / 20.00 / 50.00 mm）。
|
||||
|
||||
用法
|
||||
----
|
||||
source scripts/sim_env.sh
|
||||
python3 scripts/check_sim_gripper.py # 需要 Gazebo 仿真已在跑
|
||||
python3 scripts/check_sim_gripper.py --verbose # 另打印每档的指尖原始坐标
|
||||
|
||||
退出码：0 = 通过；1 = 未通过；2 = 环境不可用（服务不在 / 有重名节点）。
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import math
|
||||
import sys
|
||||
import time
|
||||
from typing import Dict, List, Optional, Tuple
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from tf2_ros import Buffer, TransformListener
|
||||
|
||||
try:
|
||||
from arm_msgs.msg import GripperStatus
|
||||
from arm_msgs.srv import GripperExecute
|
||||
except ModuleNotFoundError as exc: # pragma: no cover - 只在环境没 source 时触发
|
||||
# 直接让 Python 抛"找不到模块"很难定位：真正的原因通常是**忘了 source 环境**，
|
||||
# 而不是 arm_msgs 没编译。所以这里把原因和修法一次说清。
|
||||
sys.exit(
|
||||
f"找不到 {exc.name}：请先 source 仿真环境（它把 install_sim/ 加进 PYTHONPATH）：\n"
|
||||
f" source scripts/sim_env.sh\n"
|
||||
f"再执行本脚本。若仍失败，说明 install_sim/ 还没构建：\n"
|
||||
f" bash scripts/build_sim.sh"
|
||||
)
|
||||
|
||||
#: 指尖坐标系的**固有**间距不影响行程判据，但打印出来省得有人误读。
|
||||
TIP_LEFT = "sim_left_finger_tip"
|
||||
TIP_RIGHT = "sim_right_finger_tip"
|
||||
WORLD = "world"
|
||||
|
||||
#: 每个开口请求之后等多久再量（位置控制器 + 物理稳定）。
|
||||
SETTLE_S = 2.5
|
||||
|
||||
#: 第 2 层容差（毫米）：请求开口 vs 关节反馈换回的开口。
|
||||
POSITION_TOL_MM = 0.5
|
||||
#: 第 3 层容差（毫米）：指尖行程 vs 期望行程。
|
||||
TRAVEL_TOL_MM = 1.5
|
||||
#: 左右对称性容差（毫米）：两侧位移量之差。
|
||||
SYMMETRY_TOL_MM = 1.5
|
||||
|
||||
|
||||
def _spin(node: Node, seconds: float) -> None:
|
||||
deadline = time.monotonic() + seconds
|
||||
while time.monotonic() < deadline:
|
||||
rclpy.spin_once(node, timeout_sec=0.05)
|
||||
|
||||
|
||||
def _tip_positions(buf: Buffer, node: Node, attempts: int = 40):
|
||||
"""取两个指尖在 world 系里的位置；查不到返回 None。"""
|
||||
for _ in range(attempts):
|
||||
try:
|
||||
left = buf.lookup_transform(WORLD, TIP_LEFT, rclpy.time.Time())
|
||||
right = buf.lookup_transform(WORLD, TIP_RIGHT, rclpy.time.Time())
|
||||
return (
|
||||
(left.transform.translation.x, left.transform.translation.y, left.transform.translation.z),
|
||||
(right.transform.translation.x, right.transform.translation.y, right.transform.translation.z),
|
||||
)
|
||||
except Exception:
|
||||
_spin(node, 0.2)
|
||||
return None
|
||||
|
||||
|
||||
def _dist_pts(a, b) -> float:
|
||||
"""两点距离，**米**（原始量纲，调用方自己决定怎么用）。"""
|
||||
return math.sqrt(sum((x - y) ** 2 for x, y in zip(a, b)))
|
||||
|
||||
|
||||
def _dist_mm(a, b) -> float:
|
||||
"""两个位置之间的距离，**毫米**。
|
||||
|
||||
⚠️ 这个换算漏过一次，症状很典型：TF 查出来的坐标是**米**，而本脚本里所有
|
||||
阈值/期望值都是**毫米**（POSITION_TOL_MM、TRAVEL_TOL_MM、请求开口…）。
|
||||
漏掉 *1000 的话，127 mm 会显示成 0.13 —— 恰好"看起来像夹爪没张开"，
|
||||
于是跑去看耦合公式，而真正的问题只在本文件这一行。
|
||||
（实测：同一份数据下，指尖坐标是对的，只是判据全被缩小了 1000 倍。）
|
||||
"""
|
||||
return 1000.0 * _dist_pts(a, b)
|
||||
|
||||
|
||||
# ⚠️ 属性名都要避开 rclpy.Node 自己的内部属性：`self._clients` 是 Node 的私有
|
||||
# 成员（一个 list），覆盖它会直接在 create_client 里崩（实测踩过）。
|
||||
# 所以这里统一用 `_grip_clients` / `_last_status` / `_tf_buf`。
|
||||
class GripperCheck(Node):
|
||||
def __init__(self) -> None:
|
||||
super().__init__("sim_gripper_check")
|
||||
self._tf_buf = Buffer()
|
||||
TransformListener(self._tf_buf, self)
|
||||
self._last_status: Optional[GripperStatus] = None
|
||||
self.create_subscription(GripperStatus, "/gripper/status", self._on_status, 10)
|
||||
self._grip_clients: Dict[str, object] = {}
|
||||
for verb in ("open", "close", "move"):
|
||||
self._grip_clients[verb] = self.create_client(GripperExecute, f"/gripper/{verb}")
|
||||
|
||||
def _on_status(self, msg: GripperStatus) -> None:
|
||||
self._last_status = msg
|
||||
|
||||
# --- 基础设施 ---------------------------------------------------------
|
||||
def wait_ready(self, timeout: float = 30.0) -> List[str]:
|
||||
"""等服务和状态都就绪。返回缺失项（空列表=就绪）。"""
|
||||
missing = []
|
||||
for verb, cli in self._grip_clients.items():
|
||||
if not cli.wait_for_service(timeout_sec=timeout): # type: ignore[attr-defined]
|
||||
missing.append(f"/gripper/{verb}")
|
||||
if self._last_status is None:
|
||||
_spin(self, 3.0)
|
||||
if self._last_status is None:
|
||||
missing.append("/gripper/status（话题无数据）")
|
||||
return missing
|
||||
|
||||
def call(self, verb: str, position: float = 0.0) -> Tuple[bool, str]:
|
||||
cli = self._grip_clients[verb]
|
||||
req = GripperExecute.Request()
|
||||
req.command.command = verb
|
||||
req.command.position = float(position)
|
||||
fut = cli.call_async(req) # type: ignore[attr-defined]
|
||||
while not fut.done():
|
||||
_spin(self, 0.05)
|
||||
res = fut.result()
|
||||
if res is None:
|
||||
return False, "服务没有返回"
|
||||
return bool(res.success), str(res.message)
|
||||
|
||||
def movement(self, verb: str, position: float = 0.0):
|
||||
"""下发一条命令，等稳定，返回 (请求说明, 状态开口, 两指尖, 是否成功)。"""
|
||||
ok, msg = self.call(verb, position)
|
||||
_spin(self, SETTLE_S)
|
||||
tips = _tip_positions(self._tf_buf, self)
|
||||
aperture = float(self._last_status.position) if self._last_status else None
|
||||
return ok, msg, aperture, tips
|
||||
|
||||
# --- 三层验收 ---------------------------------------------------------
|
||||
def _duplicate_nodes(self) -> List[str]:
|
||||
"""查出"同名节点有两个"的情况。
|
||||
|
||||
⚠️ 这是踩出来的：残留的旧会话与新会话同时活着时，两个
|
||||
``sim_gripper_adapter`` 会同时订阅 /joint_states、同时发布
|
||||
/gripper/status，服务调用还会被随机分给其中一个 ——
|
||||
验收脚本读到的开口恒为 0 mm、指尖间距 0.03 mm（真实值 127 mm），
|
||||
看起来完全像夹爪换算写错了。
|
||||
|
||||
所以验收第一步就是确认"图里只有一个我"。启动侧的那道闸门在
|
||||
scripts/run_sim_gz.sh 的预检里（拒绝起第二个仿真），这里是第二道。
|
||||
"""
|
||||
names = self.get_node_names()
|
||||
dupes = []
|
||||
for known in (
|
||||
"sim_gripper_adapter",
|
||||
"sim_camera_adapter",
|
||||
"sim_tactile_adapter",
|
||||
"robot_state_publisher",
|
||||
"joint_state_broadcaster",
|
||||
"gripper_controller",
|
||||
):
|
||||
n = sum(1 for name in names if name.strip("/") == known)
|
||||
if n > 1:
|
||||
dupes.append(f"{known} × {n}")
|
||||
return dupes
|
||||
|
||||
def check(self, verbose: bool) -> int:
|
||||
dupes = self._duplicate_nodes()
|
||||
if dupes:
|
||||
print("环境不干净：仿真节点有重名（说明有残留的旧会话在跑）：", file=sys.stderr)
|
||||
for d in dupes:
|
||||
print(f" - {d}", file=sys.stderr)
|
||||
print(
|
||||
"\n两个同名节点会抢话题/服务/TF，读数全是乱的。先停干净：\n"
|
||||
" bash scripts/stop_sim_gz.sh\n"
|
||||
"再重启（run_sim_gz.sh 自己也会拒绝在残留状态下启动）。",
|
||||
file=sys.stderr,
|
||||
)
|
||||
return 2
|
||||
|
||||
missing = self.wait_ready()
|
||||
if missing:
|
||||
print("环境不可用，缺：", file=sys.stderr)
|
||||
for m in missing:
|
||||
print(f" - {m}", file=sys.stderr)
|
||||
print(
|
||||
"\n先确认仿真在跑（bash scripts/run_sim_gz.sh），"
|
||||
"且夹爪适配器起来了（ros2 node list | grep sim_gripper_adapter）。",
|
||||
file=sys.stderr,
|
||||
)
|
||||
return 2
|
||||
|
||||
print("第 1 层：服务与状态")
|
||||
print(f" /gripper/{{open,close,move}} 可用；/gripper/status 有数据（开口 {self._last_status.position:.2f} mm）")
|
||||
|
||||
# --- 第 2、3 层：走一遍开口序列 -----------------------------------
|
||||
requests: List[Tuple[str, float]] = [
|
||||
("close", 0.0),
|
||||
("move", 8.0),
|
||||
("move", 20.0),
|
||||
("move", 50.0),
|
||||
("open", 0.0),
|
||||
]
|
||||
rows = []
|
||||
for verb, pos in requests:
|
||||
ok, msg, aperture, tips = self.movement(verb, pos)
|
||||
label = "全开" if verb == "open" else ("全闭合" if verb == "close" else f"{pos:.0f} mm")
|
||||
rows.append((label, float(pos) if verb == "move" else None, ok, msg, aperture, tips))
|
||||
|
||||
print("\n第 2 层：请求开口 vs 关节反馈换回的开口（下发链路）")
|
||||
print(f" {'命令':<10}{'期望开口':>10}{'状态开口':>10}{'差(mm)':>10} ")
|
||||
l2_fail = []
|
||||
# "期望开口"对 open 来说是仿真行程，从状态里推出来（open 时的读数）。
|
||||
open_row = rows[-1]
|
||||
stroke = open_row[4]
|
||||
for label, want, ok, msg, aperture, tips in rows:
|
||||
expect = stroke if label == "全开" else (0.0 if label == "全闭合" else want)
|
||||
if aperture is None:
|
||||
l2_fail.append(f"{label}: 拿不到状态开口")
|
||||
print(f" {label:<10}{expect:>10.2f}{'--':>10}{'--':>10} 状态缺失")
|
||||
continue
|
||||
diff = aperture - (expect or 0.0)
|
||||
bad = abs(diff) > POSITION_TOL_MM
|
||||
if bad:
|
||||
l2_fail.append(f"{label}: 状态开口 {aperture:.2f} != 期望 {expect:.2f}")
|
||||
if not ok:
|
||||
l2_fail.append(f"{label}: 服务返回失败（{msg}）")
|
||||
print(f" {label:<10}{expect:>10.2f}{aperture:>10.2f}{diff:>10.2f} {'✗' if bad else '✓'} {msg}")
|
||||
|
||||
print("\n第 3 层：指尖的**实际**位移（真几何，这一层才证明耦合是对的）")
|
||||
tips_ok = all(r[5] is not None for r in rows)
|
||||
if not tips_ok:
|
||||
print(" 查不到指尖 TF，无法验证几何 —— 这不算通过。", file=sys.stderr)
|
||||
return 1
|
||||
|
||||
# ⚠️ 这一层**故意不用 status 的开口读数**当基准：开口反馈取自从动关节，
|
||||
# 而第 2 层已经在验那条链路。这里只认"我请求了多少毫米"，
|
||||
# 几何摆在那里，跟任何软件读数无关 —— 这才叫独立验证。
|
||||
closed_tips = rows[0][5]
|
||||
open_tips = rows[-1][5]
|
||||
span_closed = _dist_mm(*closed_tips)
|
||||
span_open = _dist_mm(*open_tips)
|
||||
spans = [_dist_mm(*r[5]) for r in rows]
|
||||
|
||||
print(f" 合拢：指尖间距 {span_closed:7.2f} mm（tip frame 固有间距，不是开口）")
|
||||
print(f" 全开：指尖间距 {span_open:7.2f} mm")
|
||||
|
||||
# 逐档对比"请求开口"与"实测间距增量"：仿真里 1 mm 开口 ≙ 1 mm 间距，
|
||||
# 所以两者必须相等（探针实测 27.02→127.02 mm，正好 1.000）。
|
||||
travel_bad = []
|
||||
for label, want, ok, msg, aperture, tips in rows:
|
||||
if want is None:
|
||||
continue # close / open 的 request 不是毫米数，单独判
|
||||
actual = _dist_mm(*tips) - span_closed
|
||||
err = actual - want
|
||||
bad = abs(err) > TRAVEL_TOL_MM
|
||||
if bad:
|
||||
travel_bad.append(
|
||||
f"请求 {want:.0f} mm，指尖实际张开 {actual:.2f} mm（差 {err:+.2f}）"
|
||||
)
|
||||
print(
|
||||
f" 请求 {want:5.1f} mm：指尖张开 {actual:7.2f} mm "
|
||||
f"差 {err:+6.2f} mm {'✗' if bad else '✓'}"
|
||||
)
|
||||
|
||||
# 全开那一档用**几何自洽**来判：必须比 50 mm 那档还大，且与第 2 层的
|
||||
# 开口读数一致（两者都错成一样是不可能的）。
|
||||
if stroke:
|
||||
err = (span_open - span_closed) - stroke
|
||||
if abs(err) > TRAVEL_TOL_MM:
|
||||
travel_bad.append(
|
||||
f"全开行程 {span_open - span_closed:.2f} mm != 开口读数 {stroke:.2f} mm"
|
||||
)
|
||||
print(
|
||||
f" 全开行程 {span_open - span_closed:7.2f} mm 期望 {stroke:.2f} mm "
|
||||
f"差 {err:+.2f} mm {'✓' if abs(err) <= TRAVEL_TOL_MM else '✗'}"
|
||||
)
|
||||
|
||||
# 单调性：请求越大，间距必须越大（方向错了会在这里露出来）。
|
||||
monotonic = all(b > a for a, b in zip(spans, spans[1:]))
|
||||
print(f" 单调递增：{'✓' if monotonic else '✗ ' + ' → '.join(f'{s:.2f}' for s in spans)}")
|
||||
|
||||
# 对称性：两侧**行程相等**才算这条耦合链是对的。
|
||||
#
|
||||
# ⚠️ 这里换过两次判据，两次都是"判据本身错了"，值得记下来：
|
||||
# 1) 最初把两侧位移投影到"左指的张开方向"再比大小 → 一个完全
|
||||
# 对称的夹爪被判成不对称（左 +52.6 / 右 -42.5）。因为左指的
|
||||
# 位移方向自己带着 x 分量，拿它当基准去投影右侧必然偏。
|
||||
# 2) 改成"两指中点漂移" → 又误判（漂移 16.29 mm，而两侧各走
|
||||
# 52.56 / 52.55 mm，好得不能再好）。原因：tip frame 是刚性固连
|
||||
# 在**会旋转**的指垫上的（指垫既平移又转动，指面才保持平行），
|
||||
# 所以 frame 走的是弧线 —— 两侧弧线的"共同分量"让中点发生位移，
|
||||
# 这跟"左右不对称"是两回事。
|
||||
# 真正与方向无关、又能反映耦合好坏的量只有一个：**两侧各自走了多远**。
|
||||
left_move = 1000.0 * _dist_pts(closed_tips[0], open_tips[0])
|
||||
right_move = 1000.0 * _dist_pts(closed_tips[1], open_tips[1])
|
||||
asym = abs(left_move - right_move)
|
||||
asym_bad = asym > SYMMETRY_TOL_MM
|
||||
mid_closed = tuple((a + b) / 2.0 for a, b in zip(*closed_tips))
|
||||
mid_open = tuple((a + b) / 2.0 for a, b in zip(*open_tips))
|
||||
mid_drift = 1000.0 * _dist_pts(mid_closed, mid_open)
|
||||
print(
|
||||
f" 对称性：左右各走 {left_move:.2f} / {right_move:.2f} mm，"
|
||||
f"差 {asym:.2f} mm {'✗' if asym_bad else '✓'}"
|
||||
)
|
||||
print(
|
||||
f" （参考：两指中点整体位移 {mid_drift:.2f} mm。tip frame 固连在"
|
||||
f"会转动的指垫上，会画弧，所以这个数不为 0 是正常的，不参与判据。）"
|
||||
)
|
||||
|
||||
if verbose:
|
||||
print("\n明细：")
|
||||
for label, _want, ok, msg, aperture, tips in rows:
|
||||
d = "--" if tips is None else f"{_dist_mm(*tips):.2f}"
|
||||
print(f" {label:<10} ok={ok} 开口={aperture} 指尖间距={d} {msg}")
|
||||
# 原始坐标也打出来：上面那一列数字是"间距"，看着对/错时都需要
|
||||
# 回头核对它到底取到了哪两个点（历史上这里出过取错帧的假象）。
|
||||
print("\n 指尖坐标（world 系，米）：")
|
||||
for label, _want, _ok, _msg, _ap, tips in rows:
|
||||
if tips is None:
|
||||
print(f" {label:<10} --")
|
||||
continue
|
||||
lx, rx = tips
|
||||
print(
|
||||
f" {label:<10} 左=({lx[0]:+.4f},{lx[1]:+.4f},{lx[2]:+.4f}) "
|
||||
f"右=({rx[0]:+.4f},{rx[1]:+.4f},{rx[2]:+.4f})"
|
||||
)
|
||||
|
||||
problems = list(l2_fail)
|
||||
problems.extend(travel_bad)
|
||||
if not monotonic:
|
||||
problems.append("开口不是单调递增 —— 方向或顺序反了")
|
||||
if asym_bad:
|
||||
problems.append(
|
||||
f"左右不对称（左右行程差 {asym:.2f} mm）—— 某一侧耦合没生效"
|
||||
)
|
||||
|
||||
print()
|
||||
if problems:
|
||||
print("结论：不通过", file=sys.stderr)
|
||||
for p in problems:
|
||||
print(f" - {p}", file=sys.stderr)
|
||||
return 1
|
||||
print("结论：通过（下发链路 + 耦合几何 + 方向 + 对称性）")
|
||||
return 0
|
||||
|
||||
|
||||
def main() -> int:
|
||||
ap = argparse.ArgumentParser(description=__doc__.splitlines()[0])
|
||||
ap.add_argument("--verbose", action="store_true", help="打印每一档的明细")
|
||||
args = ap.parse_args()
|
||||
|
||||
rclpy.init()
|
||||
node = GripperCheck()
|
||||
try:
|
||||
return node.check(args.verbose)
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,174 @@
|
||||
#!/usr/bin/env bash
|
||||
# =============================================================================
|
||||
# scripts/lib/sim_procs.sh —— 仿真进程扫描（被 stop_sim_gz.sh / run_sim_gz.sh 共用）
|
||||
#
|
||||
# 为什么单独抽一层
|
||||
# ----------------
|
||||
# "哪些进程算仿真栈"这件事以前只写在 stop_sim_gz.sh 里。新增一个仿真节点
|
||||
# （sim_gripper_adapter）时**忘了同步那张模式表**，于是：
|
||||
#
|
||||
# 脚本说"仿真已停止" → 其实残留了一个夹爪适配器
|
||||
# → 下一次启动后**两个适配器同时订阅 /joint_states、同时发布 /gripper/status**
|
||||
# → 服务调用被负载均衡到任意一个；TF 也有两份来源
|
||||
# → 验收脚本读到的开口/指尖间距全是乱的（实测：开口恒为 0 mm，
|
||||
# 指尖间距 0.03 mm，而实际是 127 mm）
|
||||
#
|
||||
# 这个坑的可怕之处在于**它看起来像夹爪代码写错了**。所以这里做三件事：
|
||||
#
|
||||
# 1. 两份脚本共用同一个扫描实现 —— 不再各写一份会漂移的模式表；
|
||||
# 2. 停止的主通道改成**进程组**：仿真整栈用 setsid 起在自己的会话里，
|
||||
# PGID 记在 log_sim/sim.pgid，停止时 kill 整组，不依赖模式匹配；
|
||||
# 3. 模式扫描当兜底（对付 ign 自己 fork 的进程与历史孤儿）。扫描本身是
|
||||
# "点名表 + install_sim/ 兜底"：凡命令行含 ``<仓库>/install_sim/``
|
||||
# 的进程都算仿真进程，所以**以后再加新节点，不改这个文件也能被扫到**。
|
||||
# 点名表只用于少数"不在 install_sim 里"的进程（Gazebo 本体、桥接、
|
||||
# robot_state_publisher、static_transform_publisher）。
|
||||
#
|
||||
# 用法（在 `set -uo pipefail` 之后 source）：
|
||||
# source "$(dirname "$0")/lib/sim_procs.sh"
|
||||
# mapfile -t targets < <(sim_proc_targets) # 只输出 PID（已排除自身/祖先）
|
||||
# sim_proc_scan # 输出 "PID<TAB>命令行"
|
||||
# sim_pgid_read / sim_pgid_leader_is_ours / sim_pgid_stop
|
||||
# =============================================================================
|
||||
|
||||
# 兜底模式：仿真安装空间。用绝对路径前缀，避免误伤 install/（真机那边）。
|
||||
# ⚠️ 路径要按 awk 正则转义：路径里可能有 . + - 之类字符，不转义的话
|
||||
# "." 会匹配任意字符（匹配面变大 = 误杀无关进程）。
|
||||
_sim_install_dir() {
|
||||
printf '%s/install_sim/' "$(_sim_repo_root)"
|
||||
}
|
||||
|
||||
_sim_regex_escape() {
|
||||
printf '%s' "$1" | sed -e 's/[][\.^$*+?(){}|\\]/\\&/g'
|
||||
}
|
||||
|
||||
# 点名表：列**不在 install_sim/ 下**、但属于仿真栈的进程。
|
||||
# ign gazebo / gz sim Gazebo 服务端与 GUI（conda Fortress 里可执行名是 ign gazebo）
|
||||
# parameter_bridge 相机/触觉/clock 的 ros_gz_bridge（在 conda env 的 lib 下）
|
||||
# robot_state_publisher 发 TF 与 /robot_description（同上，不在 install_sim）
|
||||
# static_transform_publisher 相机的静态外参（同上是 conda env 里的可执行文件）
|
||||
# controller_manager.*spawner / gz_ros2_control 控制器与它的 spawner
|
||||
#
|
||||
# ⚠️ 这几个必须**点名**：它们的可执行文件来自 conda 环境或 ROS 发行版，
|
||||
# 命令行里没有 install_sim/ 字样，兜底规则扫不到。
|
||||
# 实测教训：只留兜底规则时 robot_state_publisher 会漏，而它的症状是
|
||||
# "脚本说已停止，它还在发上一份 TF 与 /robot_description"。
|
||||
# 更阴的是 static_transform_publisher：它发的是**latched**的 /tf_static，
|
||||
# 残留进程会一直发**旧的外参**，表现成"改了相机安装位置却不生效"。
|
||||
#
|
||||
# 有意**不**包含 rviz2：headless 模式不起 RViz，用户可能开着 RViz 看别的东西，
|
||||
# 写进来会误杀无关进程 —— 误杀比漏杀更糟。
|
||||
SIM_PROC_NAMED='ign gazebo|gz sim|parameter_bridge|ros_gz_bridge|robot_state_publisher|static_transform_publisher|controller_manager.*spawner|gz_ros2_control'
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 进程组：精确停止的主通道
|
||||
# ---------------------------------------------------------------------------
|
||||
# 模式匹配永远是"猜"，上面那张表就漏过好几次。更可靠的做法是让**整个仿真栈
|
||||
# 待在自己的进程组里**：启动时用 setsid 新开一个会话，PID 即 PGID，写进
|
||||
# log_sim/sim.pgid；停止时 `kill -TERM -<PGID>` 一次性带走整组，
|
||||
# 不依赖任何模式串。模式扫描退化成兜底（对付 ign 自己 fork 出去的进程、
|
||||
# 以及历史遗留的孤儿）。
|
||||
#
|
||||
# ⚠️ 文件里的 PGID 可能是**过时的**（那次仿真的进程早退出了，PID 被别人复用）。
|
||||
# 所以用之前必须确认那个进程确实是我们的仿真栈 —— 见 sim_pgid_leader_is_ours。
|
||||
sim_pgid_file() {
|
||||
printf '%s/log_sim/sim.pgid' "$(_sim_repo_root)"
|
||||
}
|
||||
|
||||
_sim_repo_root() {
|
||||
cd "$(dirname "${BASH_SOURCE[0]}")/../.." && pwd
|
||||
}
|
||||
|
||||
# 读 pgid 文件里的 PGID；没有/格式不对 → 输出空。
|
||||
sim_pgid_read() {
|
||||
local f pgid
|
||||
f="$(sim_pgid_file)"
|
||||
[ -f "$f" ] || return 0
|
||||
read -r pgid <"$f" 2>/dev/null || return 0
|
||||
case "${pgid:-}" in
|
||||
''|*[!0-9]*) return 0 ;; # 不是纯数字 → 当作无效
|
||||
esac
|
||||
printf '%s' "$pgid"
|
||||
}
|
||||
|
||||
# 该 PGID 的组长进程是否确实是仿真栈（防止 PID 复用导致误杀无关进程组）。
|
||||
sim_pgid_leader_is_ours() {
|
||||
local pgid="$1" args
|
||||
args="$(ps -o args= -p "$pgid" 2>/dev/null || true)"
|
||||
[ -n "$args" ] || return 1
|
||||
printf '%s' "$args" | grep -Eq 'arm_sim_gazebo|gazebo_sim|ign gazebo|gz sim|ros2 launch|ros2 run'
|
||||
}
|
||||
|
||||
# 组是否还活着（任一组员存活即可）。
|
||||
sim_pgid_alive() {
|
||||
local pgid="$1"
|
||||
kill -0 -- "-$pgid" 2>/dev/null
|
||||
}
|
||||
|
||||
# 停止整组：先 TERM 等一下，仍在就 KILL。成功输出 "stopped"。
|
||||
sim_pgid_stop() {
|
||||
local pgid="$1" i
|
||||
if ! sim_pgid_alive "$pgid"; then
|
||||
rm -f "$(sim_pgid_file)"
|
||||
return 0
|
||||
fi
|
||||
kill -TERM -- "-$pgid" 2>/dev/null
|
||||
for i in $(seq 1 20); do
|
||||
sim_pgid_alive "$pgid" || break
|
||||
sleep 0.5
|
||||
done
|
||||
if sim_pgid_alive "$pgid"; then
|
||||
echo "进程组 $pgid 没响应 TERM，用 KILL" >&2
|
||||
kill -KILL -- "-$pgid" 2>/dev/null
|
||||
sleep 0.5
|
||||
fi
|
||||
rm -f "$(sim_pgid_file)"
|
||||
}
|
||||
|
||||
sim_proc_patterns() {
|
||||
printf '%s|%s' "$SIM_PROC_NAMED" "$(_sim_regex_escape "$(_sim_install_dir)")"
|
||||
}
|
||||
|
||||
# 扫描：输出 "PID<TAB>命令行"。排除本进程/祖先/扫描用的 awk 自身。
|
||||
#
|
||||
# ⚠️ 两个必须踩过的坑（原 stop_sim_gz.sh 的注释，这里保留）：
|
||||
# 1. `pkill -f <模式>` 会匹配到自己所在的 shell（命令行里含那个模式），
|
||||
# 表现成"命令立刻返回、什么都没杀"。
|
||||
# 2. 模式串用**环境变量**传给 awk，不用 `-v pat=...`：用了 -v 的话 awk 自己的
|
||||
# 命令行里就含整串模式，于是它匹配到自己，复查时多报一个幽灵 PID。
|
||||
sim_proc_scan() {
|
||||
export PAT="$(sim_proc_patterns)"
|
||||
export ME="${SIM_PROC_SELF:-$$}"
|
||||
ps -eo pid=,ppid=,args= | awk '
|
||||
BEGIN { pat = ENVIRON["PAT"]; me = ENVIRON["ME"] }
|
||||
$0 ~ pat {
|
||||
if (NF < 3) next # 没有 args 字段（僵尸/内核线程）
|
||||
pid = $1; ppid = $2
|
||||
args = substr($0, index($0, $3))
|
||||
if (ppid == me) next # 本脚本的直接子进程（如 $(...) 的 fork）
|
||||
if (args ~ /sim_procs\.sh/) next # 本库自己（含 grep/sed 的读数）
|
||||
if (args ~ /stop_sim_gz\.sh/) next
|
||||
if (args ~ /run_sim_gz\.sh/) next
|
||||
if (args ~ /^\s*awk/) next
|
||||
print pid "\t" args
|
||||
}'
|
||||
}
|
||||
|
||||
# 排除自己与全部祖先，只输出 PID。
|
||||
sim_proc_targets() {
|
||||
local exclude=() p=$$ pid _rest e skip
|
||||
while [ "$p" -gt 1 ] 2>/dev/null; do
|
||||
exclude+=("$p")
|
||||
p=$(ps -o ppid= -p "$p" 2>/dev/null | tr -d ' ')
|
||||
[ -z "$p" ] && break
|
||||
done
|
||||
while read -r pid _rest; do
|
||||
[ -z "${pid:-}" ] && continue
|
||||
skip=0
|
||||
for e in "${exclude[@]}"; do
|
||||
[ "$pid" = "$e" ] && skip=1 && break
|
||||
done
|
||||
[ "$skip" = "1" ] && continue
|
||||
echo "$pid"
|
||||
done < <(sim_proc_scan)
|
||||
}
|
||||
+59
-1
@@ -67,9 +67,37 @@ fi
|
||||
|
||||
if [[ "${PASS_ONLY}" -eq 1 ]]; then
|
||||
# 这些选项不启动任何东西，只准备描述，直接转发。
|
||||
# （所以放在"残留进程"预检**之前**：手上有仿真在跑时，照样应该能 --print-urdf。）
|
||||
exec ros2 run arm_sim_gazebo gazebo_sim_launcher "${LAUNCH_ARGS[@]}"
|
||||
fi
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 预检：不许在"上一轮仿真还有残留"的情况下启动
|
||||
# ---------------------------------------------------------------------------
|
||||
# ⚠️ 这不是洁癖。实测过的最坏情况：残留了一个 sim_gripper_adapter，新起一个之后
|
||||
# 两个适配器**同时**订阅 /joint_states、同时发布 /gripper/status，
|
||||
# 服务调用还会被随机分给其中一个 —— 验收脚本读到的开口恒为 0 mm、
|
||||
# 指尖间距 0.03 mm（真实值 127 mm），看起来完全像夹爪换算写错了。
|
||||
# 残留的 static_transform_publisher 更阴：它发 latched 的 /tf_static，
|
||||
# 会一直把**旧外参**按在新仿真的 TF 树上。
|
||||
# 所以这里宁可拒绝启动，也不给你一个"看起来能跑、数据是错的"仿真。
|
||||
# shellcheck source=lib/sim_procs.sh
|
||||
source "${ROOT}/scripts/lib/sim_procs.sh"
|
||||
|
||||
mapfile -t running < <(sim_proc_targets)
|
||||
if [[ "${#running[@]}" != "0" ]]; then
|
||||
{
|
||||
echo "已有仿真相关进程在跑，拒绝启动第二个："
|
||||
for pid in "${running[@]}"; do
|
||||
printf ' %-7s %s\n' "$pid" "$(ps -o args= -p "$pid" 2>/dev/null | cut -c1-120)"
|
||||
done
|
||||
echo
|
||||
echo "先停掉它们： bash scripts/stop_sim_gz.sh"
|
||||
echo "只想看看残留是谁、不动手：bash scripts/stop_sim_gz.sh --list"
|
||||
} >&2
|
||||
exit 3
|
||||
fi
|
||||
|
||||
echo "=== 启动 Gazebo 仿真 ==="
|
||||
echo " 环境 : ${CONDA_DEFAULT_ENV:-?} ROS_DOMAIN_ID=${ROS_DOMAIN_ID:-?}"
|
||||
echo " 退出方式 : Ctrl-C"
|
||||
@@ -78,4 +106,34 @@ echo " 首次启动约需 10~20 秒（Gazebo 初始化 + spawn + 拉控制器
|
||||
echo " 验收: ros2 control list_controllers 应看到两个 active"
|
||||
echo
|
||||
|
||||
exec ros2 run arm_sim_gazebo gazebo_sim_launcher "${LAUNCH_ARGS[@]}"
|
||||
# ---------------------------------------------------------------------------
|
||||
# 起在自己的**会话/进程组**里，并把 PGID 记下来
|
||||
# ---------------------------------------------------------------------------
|
||||
# 这样 stop_sim_gz.sh 可以用 `kill -TERM -PGID` 一次性带走整栈（含
|
||||
# robot_state_publisher / static_transform_publisher 这些"不在 install_sim 下"
|
||||
# 的节点），不必依赖模式匹配 —— 模式表漏一个就留一个孤儿的教训见脚本注释。
|
||||
#
|
||||
# 代价：进程组不在前台，终端里的 Ctrl-C **不会**传到仿真。所以这里自己接
|
||||
# SIGINT/SIGTERM，转发成组信号，行为与以前"Ctrl-C 停仿真"一致。
|
||||
PGID_FILE="$(sim_pgid_file)"
|
||||
mkdir -p "$(dirname "${PGID_FILE}")"
|
||||
|
||||
setsid bash -c 'echo $$ > "$1"; shift; exec "$@"' _ "${PGID_FILE}" \
|
||||
ros2 run arm_sim_gazebo gazebo_sim_launcher "${LAUNCH_ARGS[@]}" &
|
||||
SIM_PID=$!
|
||||
|
||||
_forward_signal() {
|
||||
local pgid
|
||||
pgid="$(sim_pgid_read)"
|
||||
if [[ -n "${pgid}" ]]; then
|
||||
echo
|
||||
echo "收到中断信号，正在停止整栈（进程组 ${pgid}）…" >&2
|
||||
sim_pgid_stop "${pgid}"
|
||||
fi
|
||||
}
|
||||
trap _forward_signal INT TERM
|
||||
|
||||
wait "${SIM_PID}"
|
||||
status=$?
|
||||
rm -f "${PGID_FILE}"
|
||||
exit "${status}"
|
||||
|
||||
+53
-74
@@ -22,83 +22,61 @@ set -uo pipefail
|
||||
LIST_ONLY=0
|
||||
[ "${1:-}" = "--list" ] && LIST_ONLY=1
|
||||
|
||||
# 这些模式覆盖仿真栈的全部进程：
|
||||
# gz sim / ign gazebo 服务端与 GUI（conda 的 Fortress 里可执行名是 ign gazebo）
|
||||
# gazebo_sim.launch.py ros2 launch 的壳
|
||||
# gazebo_sim_launcher 入口可执行文件（展开描述后转交 launch）
|
||||
# parameter_bridge 相机/触觉/clock 的桥接
|
||||
# sim_camera_adapter / sim_tactile_adapter 两个适配器
|
||||
# robot_state_publisher 发 TF 与 /robot_description
|
||||
# controller_manager.*spawner / gz_ros2_control 控制器
|
||||
# \"哪些进程算仿真栈\"这张表**不在本文件里**，在 scripts/lib/sim_procs.sh：
|
||||
# 它同时被 run_sim_gz.sh（启动前的预检）使用，两处共用一份实现，不会漂移。
|
||||
#
|
||||
# ⚠️ 这里漏过一个（robot_state_publisher），症状是"脚本说已停止，但它还在跑"：
|
||||
# 下一次启动时它继续发上一份 /robot_description 与 TF，而两个进程的
|
||||
# 参数可能不同 —— 表现成"改了描述却不生效"，极难往"有个残留进程"上想。
|
||||
# 所以脚本末尾加了一次**复查**：杀完再扫一遍，有残留就报出来并非零退出。
|
||||
# 加进新进程时，务必确认复查也能扫到它。
|
||||
# 历史教训（都真实发生过，所以规则才写这么细）：
|
||||
# - 漏过 robot_state_publisher：脚本说"已停止"，它还在跑；下一次启动时
|
||||
# 继续发上一份 /robot_description 与 TF，表现成"改了描述却不生效"。
|
||||
# - 漏过 static_transform_publisher：更阴 —— 它发的是 latched 的 /tf_static，
|
||||
# 残留进程会一直把**旧相机外参**按在新仿真的 TF 树上。
|
||||
# - 漏过 sim_gripper_adapter：残留的适配器与新适配器**同时**订阅 /joint_states、
|
||||
# 同时发布 /gripper/status，服务调用还会被随机分给其中一个。
|
||||
# 验收脚本读到的开口恒为 0 mm、指尖间距 0.03 mm（实际 127 mm），
|
||||
# 看起来就像夹爪代码算错了 —— 这是最难查的一类残留。
|
||||
#
|
||||
# 有意**不**包含 rviz2：本仿真栈的 headless 模式不起 RViz，而用户可能同时
|
||||
# 开着 RViz 看别的东西。把它写进来会误杀无关进程，比漏杀更糟。
|
||||
PATTERNS='ign gazebo|gz sim|gazebo_sim\.launch\.py|gazebo_sim_launcher|parameter_bridge|sim_camera_adapter|sim_tactile_adapter|robot_state_publisher|controller_manager.*spawner|gz_ros2_control'
|
||||
# 所以停止分两步：
|
||||
# ① 主通道：run_sim_gz.sh 用 setsid 起仿真，PGID 记在 log_sim/sim.pgid，
|
||||
# `kill -TERM -PGID` 一次带走整组（**不依赖任何模式串**）；
|
||||
# ② 兜底：模式扫描（点名表 + install_sim/ 兜底），收拾历史孤儿与
|
||||
# ign 自己 fork 出去的进程；杀完再扫一遍，有残留就报出来并非零退出。
|
||||
# shellcheck source=lib/sim_procs.sh
|
||||
source "$(dirname "${BASH_SOURCE[0]}")/lib/sim_procs.sh"
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 扫描：输出 "PID<TAB>命令行"，供两次调用（收集 / 复查）共用。
|
||||
#
|
||||
# 两处必须共用同一个实现，否则"复查用的模式"和"杀的时候用的模式"会各自漂移，
|
||||
# 而漂移的表现就是"说已停止、其实有残留"——正是这个脚本要防的事。
|
||||
#
|
||||
# ⚠️ 模式串用**环境变量**传给 awk，不用 `-v pat=...`：
|
||||
# 用了 -v 的话，awk 自己的命令行里就含整串模式，于是它匹配到自己。
|
||||
# 这是 pkill -f 那个坑的同款，只是换了个位置 —— 实测表现为每次复查都会
|
||||
# 多报出一个"没有命令行"的幽灵 PID（awk 已经退出，ps 查不到它了）。
|
||||
#
|
||||
# 另外用 ps 而不是 pgrep -f：ps 的输出里可以用 awk 精确控制字段与排除规则。
|
||||
scan_pids() {
|
||||
# ⚠️ 必须 export：`VAR=x ps ... | awk` 只会把 VAR 给 ps，awk 看不到。
|
||||
export PAT="$PATTERNS"
|
||||
ps -eo pid=,ppid=,args= | awk '
|
||||
BEGIN { pat = ENVIRON["PAT"]; me = ENVIRON["ME"] }
|
||||
$0 ~ pat {
|
||||
if (NF < 3) next # 没有 args 字段（僵尸/内核线程）：跳过
|
||||
pid = $1; ppid = $2
|
||||
args = substr($0, index($0, $3))
|
||||
if (ppid == me) next # 本脚本的直接子进程（进程替换 fork 出的子 shell）
|
||||
if (args ~ /stop_sim_gz\.sh/) next # 本脚本自己 / 调用它的壳的文本
|
||||
if (args ~ /^\s*awk/) next # 扫描用的 awk 本身
|
||||
print pid "\t" args
|
||||
}'
|
||||
}
|
||||
export ME=$$
|
||||
PGID="$(sim_pgid_read)"
|
||||
if [ -n "$PGID" ] && sim_pgid_alive "$PGID"; then
|
||||
if ! sim_pgid_leader_is_ours "$PGID"; then
|
||||
echo "⚠ log_sim/sim.pgid 里的进程组 $PGID 看起来不是仿真栈（PID 可能被复用），忽略它。"
|
||||
PGID=""
|
||||
fi
|
||||
else
|
||||
PGID=""
|
||||
fi
|
||||
|
||||
# 排除自己与全部祖先 —— 这是本脚本存在的核心理由（见文件头）。
|
||||
exclude=()
|
||||
p=$$
|
||||
while [ "$p" -gt 1 ] 2>/dev/null; do
|
||||
exclude+=("$p")
|
||||
p=$(ps -o ppid= -p "$p" 2>/dev/null | tr -d ' ')
|
||||
[ -z "$p" ] && break
|
||||
done
|
||||
if [ "$LIST_ONLY" = "1" ]; then
|
||||
if [ -n "$PGID" ]; then
|
||||
echo "仿真的进程组：$PGID（kill -TERM -$PGID 可整组停掉）"
|
||||
ps -o pid=,args= -g "$PGID" 2>/dev/null | cut -c1-150
|
||||
fi
|
||||
fi
|
||||
|
||||
# 从扫描结果里剔掉自己/祖先，只输出 PID。
|
||||
# 用 `cut -f1` 而不是 read 到数组再拼接，避免命令行里再出现长的进程描述。
|
||||
filter_targets() {
|
||||
local pid
|
||||
while read -r pid _rest; do
|
||||
[ -z "$pid" ] && continue
|
||||
local skip=0
|
||||
for e in "${exclude[@]}"; do
|
||||
[ "$pid" = "$e" ] && skip=1 && break
|
||||
done
|
||||
[ "$skip" = "1" ] && continue
|
||||
echo "$pid"
|
||||
done
|
||||
}
|
||||
# 模式扫描的候选（--list 时先打印，再决定动不动手）。
|
||||
mapfile -t targets < <(sim_proc_targets)
|
||||
|
||||
mapfile -t SCAN < <(scan_pids)
|
||||
mapfile -t targets < <(printf '%s\n' "${SCAN[@]:-}" | filter_targets)
|
||||
if [ "${#targets[@]}" = "0" ] && [ -z "$PGID" ]; then
|
||||
echo "没有正在运行的仿真进程（已排除本脚本自身与调用它的 shell）。"
|
||||
exit 0
|
||||
fi
|
||||
|
||||
if [ -n "$PGID" ] && [ "$LIST_ONLY" != "1" ]; then
|
||||
echo "停止仿真进程组 $PGID（整组，含不在 install_sim/ 下的节点）…"
|
||||
sim_pgid_stop "$PGID"
|
||||
# 组信号发完后重新扫一遍：可能还有孤儿。
|
||||
mapfile -t targets < <(sim_proc_targets)
|
||||
fi
|
||||
|
||||
if [ "${#targets[@]}" = "0" ]; then
|
||||
echo "没有正在运行的仿真进程（已排除本脚本自身与调用它的 shell）。"
|
||||
echo "仿真已停止。"
|
||||
exit 0
|
||||
fi
|
||||
|
||||
@@ -136,15 +114,16 @@ done
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 复查：杀完再扫一遍。
|
||||
# 这一步是为了让"模式漏了某个进程"变成**看得见的告警**，而不是
|
||||
# "脚本说已停止、实际有残留"。robot_state_publisher 就这么漏过一次。
|
||||
# 这一步是为了让"表漏了某个进程"变成**看得见的告警**，而不是
|
||||
# "脚本说已停止、实际有残留"。robot_state_publisher 与 sim_gripper_adapter
|
||||
# 都这么漏过，而后者导致的假象是"夹爪算错了"。
|
||||
# ---------------------------------------------------------------------------
|
||||
sleep 0.5
|
||||
mapfile -t SCAN2 < <(scan_pids)
|
||||
mapfile -t leftover < <(printf '%s\n' "${SCAN2[@]:-}" | filter_targets)
|
||||
mapfile -t leftover < <(sim_proc_targets)
|
||||
|
||||
if [ "${#leftover[@]}" != "0" ]; then
|
||||
echo "⚠ 仍有仿真相关进程存活（说明上面的模式没覆盖它，请把它加进 PATTERNS）："
|
||||
echo "⚠ 仍有仿真相关进程存活（说明它既不在 lib/sim_procs.sh 的点名表里，"
|
||||
echo " 也不在 install_sim/ 下 —— 请把它加进点名表，或让它从 install_sim/ 启动）："
|
||||
for pid in "${leftover[@]}"; do
|
||||
printf ' %-7s %s\n' "$pid" "$(ps -o args= -p "$pid" 2>/dev/null | cut -c1-140)"
|
||||
done
|
||||
|
||||
@@ -0,0 +1,618 @@
|
||||
"""仿真夹爪的耦合关系、行程换算与命令向量拼装（纯逻辑，不起 ROS）。
|
||||
|
||||
=============================================================================
|
||||
为什么单独一个模块
|
||||
=============================================================================
|
||||
把"夹爪服务（mm）"变成"7 个关节的目标角（弧度/米）"，真正有内容的是四件事：
|
||||
|
||||
1. 耦合关系 从动关节 = multiplier × 主关节 + offset
|
||||
（上游 GN01 描述里主关节是 ``finger_width_joint``，
|
||||
6 个从动关节按各自的 multiplier 跟随）
|
||||
2. 行程换算 应用层说的是**开口毫米**，Gazebo 要的是**关节值**（米）
|
||||
3. 命令顺序 JointGroupPositionController 收到的数组顺序必须与
|
||||
controllers.yaml 里 ``joints:`` 完全一致
|
||||
4. 夹住判据 真机有状态字，仿真只能靠"指令到了下限、实际值没跟上"推断
|
||||
|
||||
四件都是纯数据变换，不需要 ROS 跑起来就能验。所以实现放这里、用普通函数表达，
|
||||
节点（sim_gripper_adapter.py）只做订阅/发布这层壳。这样"multiplier 用错"
|
||||
这种错误才能被单测钉住，而不是靠"看起来夹住了"。
|
||||
|
||||
=============================================================================
|
||||
为什么 Gazebo 里必须由适配器驱动 7 个关节
|
||||
=============================================================================
|
||||
sdformat 把 URDF 的 ``<mimic>`` **整个丢掉**，gz-sim 也不实现 mimic。
|
||||
所以仿真里 6 个从动关节不会跟随主关节，必须由适配器算出角度后**一起下发**。
|
||||
``urdf_normalize`` 在归一化时把 ``<mimic>`` 删掉（避免 robot_state_publisher
|
||||
和 Gazebo 各说各话），删掉的信息记在 ``NormalizeReport.stripped_mimics`` 里，
|
||||
launch 再把那张表作为参数交给本适配器 —— 这样"耦合关系"只有一个来源。
|
||||
|
||||
=============================================================================
|
||||
毫米 ↔ 关节值的口径（最容易搞错的一处）
|
||||
=============================================================================
|
||||
``finger_width_joint`` 是上游描述里的**虚拟宽度关节**（注释说它"本应是
|
||||
prismatic，但为了配合 mimic 只能是 revolute"），单位是**米**，限位 **0~0.100**，
|
||||
语义就是开口宽度：0 = 两指合拢，0.100 = 开口 100 mm。
|
||||
|
||||
本适配器采用**绝对（物理）映射**：命令"开口 8 mm"，仿真里两指就真的相距 8 mm。
|
||||
（而不是归一化映射 —— 那种把"100% 行程"当成全开，会让中间开口全部失真，
|
||||
仿真就失去"拿真几何试算法"的意义。）
|
||||
|
||||
⚠️ **但这里有个坑：不能简单地写成 `关节值 = 开口_mm / 1000`。**
|
||||
上游的耦合是**线性近似**，而机构本身是非线性的（手指是"转 + 再转"，指垫
|
||||
既平移又旋转）。按线性口径命令 20 mm，Gazebo 实测两指相距 **21.75 mm** ——
|
||||
抓 20 mm 的工件会空出 1.75 mm，没有接触力，触觉那一步在仿真里直接废掉。
|
||||
|
||||
所以开口↔关节值走**URDF 正运动学反解**（见 sim_gripper_kin.py）：
|
||||
|
||||
aperture_to_source(20.0) → 令两指间距 − 合拢间距 == 20.0 的关节值
|
||||
|
||||
这个正运动学与 Gazebo 的实测吻合到 0.01 mm（0~100 mm 全程），两边完全独立
|
||||
（一个是矩阵乘法，一个是物理引擎），所以"开口 20 mm"在仿真里是物理上真的 20 mm。
|
||||
顺带一个推论：GN01 的**实测**行程是 99.93 mm，不是名义的 100.0 ——
|
||||
启动日志里报的是实测值。
|
||||
|
||||
万一某个夹爪的指尖 link 命名不符合约定，正运动学建不出来，会**退回线性近似**
|
||||
并在启动日志里明确写出口径变了（中段偏差可达 4 mm/100mm 行程）。
|
||||
别在那种情况下拿仿真去标触觉阈值。
|
||||
|
||||
⚠️ 还要注意**仿真夹爪和真机夹爪不是同一个夹爪**：仿真是官方 GN01（行程
|
||||
100 mm），现场是 DH PGEA-100-20-W（``site.yaml`` 的 ``gripper.stroke_mm``
|
||||
= 20 mm）。同一个"8 mm 开口"在两者上都物理正确，但"全开"对应的开口
|
||||
完全不同。适配器启动时会把两者的行程打出来，避免有人默认它们一样。
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
import re
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
from typing import Dict, List, Mapping, Optional, Sequence, Tuple
|
||||
|
||||
from .sim_gripper_kin import ApertureCurve, KinematicsError, UrdfTree, find_tip_pair
|
||||
|
||||
import yaml
|
||||
|
||||
# 默认控制器配置位置（相对仓库根）。与真机那份同名不同目录，别搞混。
|
||||
DEFAULT_CONTROLLERS_REL = "devices/sim/gazebo/config/controllers.yaml"
|
||||
|
||||
#: 适配器负责的控制器名。改这里要同步改 launch 与 controllers.yaml。
|
||||
DEFAULT_CONTROLLER = "gripper_controller"
|
||||
|
||||
#: 关节值（米）与开口（毫米）的换算系数。
|
||||
MM_PER_M = 1000.0
|
||||
|
||||
#: 参数里 mimic 表的序列化分隔符。关节名里不会有 ':'，所以安全。
|
||||
_MIMIC_SEP = ":"
|
||||
|
||||
|
||||
class GripperConfigError(RuntimeError):
|
||||
"""夹爪配置/描述有问题时抛出，消息面向使用者。"""
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class MimicRelation:
|
||||
"""一条 ``<mimic>`` 耦合关系：从动关节 = multiplier × 主关节 + offset。"""
|
||||
|
||||
joint: str
|
||||
source: str
|
||||
multiplier: float
|
||||
offset: float
|
||||
|
||||
def apply(self, source_value: float) -> float:
|
||||
"""按这条关系算出从动关节的值。"""
|
||||
return self.multiplier * source_value + self.offset
|
||||
|
||||
def to_param(self) -> str:
|
||||
"""序列化成 ROS 参数用的字符串（launch 与本模块必须用同一格式）。"""
|
||||
return format_mimic_param(self.joint, self.source, self.multiplier, self.offset)
|
||||
|
||||
|
||||
def format_mimic_param(joint: str, source: str, multiplier: object, offset: object) -> str:
|
||||
"""把一条耦合关系序列化成 ROS 参数用的字符串。
|
||||
|
||||
launch 从 ``NormalizeReport.stripped_mimics`` 拿到的是**四个字符串**，
|
||||
本模块解析的也是这个格式，所以格式化只留这一个实现 ——
|
||||
两边各写一份的话，改了格式就会一边发一边收不到（而且不报错）。
|
||||
"""
|
||||
return _MIMIC_SEP.join(
|
||||
(str(joint).strip(), str(source).strip(), repr(float(multiplier)), repr(float(offset)))
|
||||
)
|
||||
|
||||
|
||||
class GripperLayout:
|
||||
"""夹爪在仿真里的完整描述：关节顺序 + 耦合关系 + 主关节限位。
|
||||
|
||||
由 :func:`layout_from_parts` 构造（它做全部校验）；不要直接 new。
|
||||
"""
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
order: Sequence[str],
|
||||
relations: Mapping[str, MimicRelation],
|
||||
source: str,
|
||||
lower: float,
|
||||
upper: float,
|
||||
curve: Optional[ApertureCurve] = None,
|
||||
kin_note: str = "",
|
||||
) -> None:
|
||||
self.order: Tuple[str, ...] = tuple(order)
|
||||
self.relations: Dict[str, MimicRelation] = dict(relations)
|
||||
self.source = source
|
||||
self.lower = float(lower)
|
||||
self.upper = float(upper)
|
||||
self.feedback = self._pick_feedback()
|
||||
# 由 URDF 正运动学给出的"真实开口曲线"；None = 退回线性近似。
|
||||
self.curve: Optional[ApertureCurve] = curve
|
||||
#: 口径说明（给日志用）：是 FK 精确口径还是线性近似。
|
||||
self.kin_note: str = kin_note
|
||||
|
||||
# --- 反馈关节 ---------------------------------------------------------
|
||||
def _pick_feedback(self) -> str:
|
||||
"""挑一个**真的会动**的关节来当开口反馈源。
|
||||
|
||||
⚠️ 主关节 ``finger_width_joint`` 修不出来：上游描述里它挂在一个叫
|
||||
``unused_finger_width_link`` 的**孤立假 link** 上（注释原话：
|
||||
"The unused link cannot have 0 mass"），和手指没有任何机械连接。
|
||||
实测（2026-09-24，Gazebo Fortress + gz_ros2_control）：命令它到 0.100，
|
||||
它的状态**始终停在 0**，而 6 个从动关节准确到位。
|
||||
所以拿主关节当反馈源的话，``status`` 会永远报"开口 0 mm"，
|
||||
而夹爪明明开着 —— 这正是"看起来在工作"的静默失效。
|
||||
|
||||
于是改从**与主关节直接相连的那个从动关节**反推开口：
|
||||
state = m × source + o → source = (state - o) / m
|
||||
选 |m| 最大的那个（分辨率最好；m 为 0 的不能用）。
|
||||
|
||||
单关节夹爪（没有耦合关系）时退回到主关节自己。
|
||||
"""
|
||||
candidates = [
|
||||
(abs(rel.multiplier), joint)
|
||||
for joint, rel in self.relations.items()
|
||||
if rel.source == self.source and abs(rel.multiplier) > 1e-9
|
||||
]
|
||||
if not candidates:
|
||||
return self.source
|
||||
candidates.sort(reverse=True)
|
||||
return candidates[0][1]
|
||||
|
||||
def feedback_value(self, source_value: float) -> float:
|
||||
"""反馈关节在"主关节 = source_value"时应有的值（用于判"没跟上"）。"""
|
||||
if self.feedback == self.source:
|
||||
return float(source_value)
|
||||
return self.value_for(self.feedback, source_value)
|
||||
|
||||
def source_from_feedback(self, feedback_state: float) -> float:
|
||||
"""反馈关节实测值 → 等价的主关节值（米）。"""
|
||||
if self.feedback == self.source:
|
||||
return float(feedback_state)
|
||||
rel = self.relations[self.feedback]
|
||||
if abs(rel.multiplier) < 1e-9:
|
||||
raise GripperConfigError(
|
||||
f"反馈关节 {self.feedback!r} 的 multiplier 为 0，无法反推主关节值。"
|
||||
)
|
||||
return (float(feedback_state) - rel.offset) / rel.multiplier
|
||||
|
||||
def aperture_from_feedback(self, feedback_state: float) -> float:
|
||||
"""反馈关节的实测值 → 开口（毫米）。"""
|
||||
return self.source_to_aperture(self.source_from_feedback(feedback_state))
|
||||
|
||||
# --- 行程口径 ---------------------------------------------------------
|
||||
@property
|
||||
def stroke_mm(self) -> float:
|
||||
"""这个夹爪在仿真里**实际**能开的宽度（毫米）。
|
||||
|
||||
有 FK 曲线时用实测行程（GN01 ≈ 99.93 mm，不是名义的 100.0 ——
|
||||
上游把宽度关节限位定在 0~0.100 m，而线性耦合让指尖只走了 99.93，
|
||||
报名义值会让"全开时开口读数与几何对不上"变成一个查不出的矛盾）。
|
||||
没有 FK 曲线时只能按限位换算，并在 :attr:`kin_note` 里说明是近似。
|
||||
|
||||
真机那份行程在 ``site.yaml`` 的 ``gripper.stroke_mm``；两者不同是正常的
|
||||
（不同夹爪），但要显式打出来，见 :func:`stroke_note`。
|
||||
"""
|
||||
if self.curve is not None:
|
||||
return self.curve.stroke_mm
|
||||
return (self.upper - self.lower) * MM_PER_M
|
||||
|
||||
@property
|
||||
def closed_separation_mm(self) -> Optional[float]:
|
||||
"""合拢时两侧 tip frame 的固有间距（GN01 ≈ 27 mm）；无 FK 时为 None。
|
||||
|
||||
验收脚本用它把"指尖间距"折成"开口"（指垫合拢时间距不为 0，
|
||||
因为 tip frame 不在指垫表面）。见 scripts/check_sim_gripper.py。
|
||||
"""
|
||||
return None if self.curve is None else self.curve.closed_separation_mm
|
||||
|
||||
def aperture_to_source(self, aperture_mm: float) -> float:
|
||||
"""开口（毫米）→ 主关节值（米），并按限位截断。
|
||||
|
||||
**绝对物理映射**：夹爪真的开这么宽。越界不报错而是截断（撞到限位是
|
||||
正常操作），但调用方可以从 :meth:`is_clamped` 知道发生了截断。
|
||||
|
||||
⚠️ 有 FK 曲线时走**反解**，不是线性换算。因为上游的耦合是线性近似、
|
||||
而机构是非线性的：线性口径下"命令 20 mm"实际是 21.75 mm 的指垫间距，
|
||||
抓 20 mm 的工件会空出 1.75 mm（没有接触力），
|
||||
触觉那一步在仿真里就废了。细节见 sim_gripper_kin.py 文件头。
|
||||
"""
|
||||
if self.curve is not None:
|
||||
return self.curve.joint_value(aperture_mm)
|
||||
raw = self.lower + float(aperture_mm) / MM_PER_M
|
||||
return min(max(raw, self.lower), self.upper)
|
||||
|
||||
def source_to_aperture(self, source_value: float) -> float:
|
||||
"""主关节值（米）→ 开口（毫米）。与 :meth:`aperture_to_source` 互逆。"""
|
||||
if self.curve is not None:
|
||||
return self.curve.aperture_mm(source_value)
|
||||
return (float(source_value) - self.lower) * MM_PER_M
|
||||
|
||||
def is_clamped(self, aperture_mm: float) -> bool:
|
||||
"""这个开口请求是否会被限位截断（用来给调用方一句实话）。"""
|
||||
if self.curve is not None:
|
||||
return self.curve.is_clamped(aperture_mm)
|
||||
raw = self.lower + float(aperture_mm) / MM_PER_M
|
||||
return not (self.lower <= raw <= self.upper)
|
||||
|
||||
# --- 命令向量 ---------------------------------------------------------
|
||||
def command_vector(self, aperture_mm: float) -> List[float]:
|
||||
"""按 ``order`` 拼出一整组关节目标（主关节 + 从动关节）。
|
||||
|
||||
顺序必须与 controllers.yaml 的 ``joints:`` 一致 —— 顺序错了，
|
||||
JointGroupPositionController 不会报错，只会把角度发给**别的关节**，
|
||||
表现为"夹爪动得很奇怪"。
|
||||
"""
|
||||
return self.command_vector_from_source(self.aperture_to_source(aperture_mm))
|
||||
|
||||
def command_vector_from_source(self, source_value: float) -> List[float]:
|
||||
"""同上，但直接给主关节值（米），不经过"毫米"这一层来回换算。
|
||||
|
||||
``stop``（保持当前位置）这类命令手里只有关节状态值，走毫米会在
|
||||
两次换算里引入舍入误差，所以单独留这个入口。
|
||||
"""
|
||||
return [self.value_for(name, source_value) for name in self.order]
|
||||
|
||||
def value_for(self, joint: str, source_value: float) -> float:
|
||||
"""某个关节在"主关节 = source_value"时应有的值。
|
||||
|
||||
主关节自己就是 ``source_value``；从动关节沿**耦合链**逐级算出来。
|
||||
|
||||
⚠️ GN01 的耦合不是"6 个手指关节全部直挂主关节"，而是一条链：
|
||||
|
||||
finger_width_joint ─9.404,-0.155→ left_outer_knuckle_joint
|
||||
├─ 1, 0 → left_inner_knuckle
|
||||
├─ -1,0 → left_inner_finger
|
||||
├─ 1, 0 → right_outer_knuckle
|
||||
├─ 1, 0 → right_inner_knuckle
|
||||
└─ -1,0 → right_inner_finger
|
||||
|
||||
所以必须**逐级**套用 multiplier/offset（先套靠近根的、再套靠近叶的），
|
||||
不能只套一层。只套一层的话，指尖角度会错得不多但一直不对 ——
|
||||
表现为"夹爪动作看着像那么回事，就是夹不准"。
|
||||
"""
|
||||
if joint == self.source:
|
||||
return float(source_value)
|
||||
|
||||
# 从 joint 往上爬到根，把沿途的关系按"从根到叶"的顺序套用。
|
||||
chain: List[MimicRelation] = []
|
||||
seen = set()
|
||||
cur = joint
|
||||
while True:
|
||||
rel = self.relations.get(cur)
|
||||
if rel is None:
|
||||
break
|
||||
if cur in seen:
|
||||
raise GripperConfigError(
|
||||
f"耦合链成环：{cur!r} 又回到了自己（{' → '.join(sorted(seen))}）"
|
||||
)
|
||||
seen.add(cur)
|
||||
chain.append(rel)
|
||||
cur = rel.source
|
||||
if cur != self.source:
|
||||
raise GripperConfigError(
|
||||
f"关节 {joint!r} 的耦合链终止在 {cur!r}，而不是主关节 "
|
||||
f"{self.source!r} —— 描述里的耦合表不完整。"
|
||||
)
|
||||
value = float(source_value)
|
||||
for rel in reversed(chain):
|
||||
value = rel.apply(value)
|
||||
return value
|
||||
|
||||
def source_index(self) -> int:
|
||||
"""主关节在命令数组里的下标。"""
|
||||
return self.order.index(self.source)
|
||||
|
||||
def _source_limits_note(self) -> str:
|
||||
return f"限位 [{self.lower:.4f}, {self.upper:.4f}] m"
|
||||
|
||||
def describe(self) -> str:
|
||||
"""启动时打给操作者看的一行摘要（含必须知道的口径）。"""
|
||||
note = f"；{self.kin_note}" if self.kin_note else ""
|
||||
return (
|
||||
f"主关节={self.source} {self._source_limits_note()} "
|
||||
f"→ 仿真行程 {self.stroke_mm:.2f} mm；"
|
||||
f"共 {len(self.order)} 个关节，{len(self.relations)} 条耦合{note}"
|
||||
)
|
||||
|
||||
def stroke_note(self, real_stroke_mm: Optional[float]) -> str:
|
||||
"""仿真行程与真机行程的对比说明（两者不同是正常的，但必须说出来）。"""
|
||||
if real_stroke_mm is None or real_stroke_mm <= 0:
|
||||
return (
|
||||
f"仿真夹爪行程 {self.stroke_mm:.1f} mm（site.yaml 里没读到真机行程，"
|
||||
f"无法对比）"
|
||||
)
|
||||
same = abs(real_stroke_mm - self.stroke_mm) < 0.5
|
||||
head = f"仿真行程 {self.stroke_mm:.1f} mm vs 真机 {real_stroke_mm:.1f} mm"
|
||||
if same:
|
||||
return head + "：一致。"
|
||||
return (
|
||||
head + "：**不一致**（不同型号的夹爪，这是正常的）。\n"
|
||||
" 开口毫米采用绝对物理映射，所以同一个开口在两边几何上都对；\n"
|
||||
" 但\"全开\"对应的开口不同，别把仿真里的全开当成真机的全开。\n"
|
||||
" 力/触觉阈值也要按各自夹爪重新标定（见 R3.5）。"
|
||||
)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 解析与校验
|
||||
# ---------------------------------------------------------------------------
|
||||
def parse_mimics(entries: Sequence[str]) -> Dict[str, MimicRelation]:
|
||||
"""解析参数里的 mimic 表（每条 ``joint:source:multiplier:offset``）。
|
||||
|
||||
格式错就报错，不做"跳过这一条"的宽容处理：静默丢一条耦合关系，
|
||||
结果是某个手指关节永远不动，而 Gazebo 不会抱怨。
|
||||
"""
|
||||
out: Dict[str, MimicRelation] = {}
|
||||
for raw in entries:
|
||||
parts = str(raw).split(_MIMIC_SEP)
|
||||
if len(parts) != 4:
|
||||
raise GripperConfigError(
|
||||
f"mimic 参数格式不对：{raw!r}\n"
|
||||
f" 应为 '从动关节:主关节:multiplier:offset' 四段。"
|
||||
)
|
||||
joint, source, mult_s, off_s = (p.strip() for p in parts)
|
||||
if not joint or not source:
|
||||
raise GripperConfigError(f"mimic 参数里关节名为空：{raw!r}")
|
||||
try:
|
||||
mult, off = float(mult_s), float(off_s)
|
||||
except ValueError as exc:
|
||||
raise GripperConfigError(
|
||||
f"mimic 参数里的 multiplier/offset 不是数字：{raw!r}"
|
||||
) from exc
|
||||
if joint == source:
|
||||
raise GripperConfigError(
|
||||
f"mimic 参数里从动关节与主关节同名（{joint}）—— 描述有问题。"
|
||||
)
|
||||
if joint in out:
|
||||
raise GripperConfigError(
|
||||
f"mimic 参数里关节 {joint!r} 出现了两次，无法确定用哪条。"
|
||||
)
|
||||
out[joint] = MimicRelation(joint, source, mult, off)
|
||||
return out
|
||||
|
||||
|
||||
def gripper_joint_order(
|
||||
controllers_file: Path, controller: str = DEFAULT_CONTROLLER
|
||||
) -> List[str]:
|
||||
"""从 controllers.yaml 里读 ``<controller>.ros__parameters.joints``。
|
||||
|
||||
为什么解析而不是让调用方再配一份顺序：顺序是
|
||||
``JointGroupPositionController`` 的硬契约，配两份必然漂移，
|
||||
而漂移的症状是"角度发给了别的关节"、不报错。
|
||||
"""
|
||||
path = Path(controllers_file)
|
||||
try:
|
||||
data = yaml.safe_load(path.read_text(encoding="utf-8")) or {}
|
||||
except OSError as exc:
|
||||
raise GripperConfigError(f"读不到控制器配置 {path}：{exc}") from exc
|
||||
except yaml.YAMLError as exc:
|
||||
raise GripperConfigError(f"控制器配置 {path} 不是合法 YAML：{exc}") from exc
|
||||
|
||||
section = (data.get(controller) or {}).get("ros__parameters")
|
||||
if not isinstance(section, dict):
|
||||
raise GripperConfigError(
|
||||
f"{path} 里没有 {controller}.ros__parameters 段。\n"
|
||||
f" 仿真夹爪要靠这个控制器的 joints 顺序下发命令，缺了就无法工作。"
|
||||
)
|
||||
joints = section.get("joints")
|
||||
if not isinstance(joints, list) or not joints:
|
||||
raise GripperConfigError(
|
||||
f"{path} 的 {controller}.ros__parameters 里 joints 为空。"
|
||||
)
|
||||
names = [str(j).strip() for j in joints]
|
||||
if len(set(names)) != len(names):
|
||||
raise GripperConfigError(
|
||||
f"{path} 的 {controller} joints 里有重复项：{names}"
|
||||
)
|
||||
return names
|
||||
|
||||
|
||||
def joint_limits(urdf_xml: str, joint_name: str) -> Tuple[float, float]:
|
||||
"""从 URDF 文本里取某个关节的 (lower, upper)。取不到就报错。
|
||||
|
||||
为什么不从参数再传一遍限位：限位是描述里的**唯一真相**（也就是 Gazebo
|
||||
里真实的行程）。再配一份等于给自己一个"改了描述没改配置"的机会，
|
||||
而症状是开口悄悄不按预期走。
|
||||
"""
|
||||
# 只认 <joint name="..."> 那一段，避免匹配到别的关节。
|
||||
pat = re.compile(
|
||||
r"<joint\b[^>]*\bname\s*=\s*[\"']%s[\"'][^>]*>(.*?)</joint>"
|
||||
% re.escape(joint_name),
|
||||
re.DOTALL,
|
||||
)
|
||||
m = pat.search(urdf_xml or "")
|
||||
if not m:
|
||||
raise GripperConfigError(
|
||||
f"在 robot_description 里找不到关节 {joint_name!r}。\n"
|
||||
f" 常见原因：描述没带夹爪（load_gripper:=false），或关节名前缀不对"
|
||||
f"（仿真是 sim_ 前缀）。"
|
||||
)
|
||||
body = m.group(1)
|
||||
if re.search(r"<mimic\b", body):
|
||||
raise GripperConfigError(
|
||||
f"关节 {joint_name!r} 自己是从动关节（带 <mimic>），不能当主关节。"
|
||||
)
|
||||
lm = re.search(r"<limit\b[^>]*>", body)
|
||||
if not lm:
|
||||
raise GripperConfigError(f"关节 {joint_name!r} 的 <limit> 标签缺失。")
|
||||
|
||||
def _attr(name: str) -> Optional[float]:
|
||||
am = re.search(r"\b%s\s*=\s*[\"']([^\"']+)[\"']" % name, lm.group(0))
|
||||
if not am:
|
||||
return None
|
||||
try:
|
||||
return float(am.group(1))
|
||||
except ValueError:
|
||||
return None
|
||||
|
||||
lower, upper = _attr("lower"), _attr("upper")
|
||||
if lower is None or upper is None:
|
||||
# URDF 允许省略 limit 的上下限（continuous 关节就没有）。夹爪的宽度
|
||||
# 关节必须有，否则我们无从知道"全开"是多少 —— 这时报错比猜好。
|
||||
raise GripperConfigError(
|
||||
f"关节 {joint_name!r} 的 <limit> 缺 lower/upper，"
|
||||
f"无法确定开口行程。原始标签：{lm.group(0)}"
|
||||
)
|
||||
if upper <= lower:
|
||||
raise GripperConfigError(
|
||||
f"关节 {joint_name!r} 的限位不合法：lower={lower} upper={upper}"
|
||||
)
|
||||
return lower, upper
|
||||
|
||||
|
||||
def layout_from_parts(
|
||||
order: Sequence[str],
|
||||
mimics: Mapping[str, MimicRelation],
|
||||
urdf_xml: str,
|
||||
) -> GripperLayout:
|
||||
"""把三份数据拼成 :class:`GripperLayout`，并做全部一致性校验。
|
||||
|
||||
校验点（每一条都对应一种"不报错但行为错"的故障）：
|
||||
|
||||
* ``mimics`` 里的从动关节必须在 ``order`` 里 —— 否则控制器根本没声明它，
|
||||
我们发出去的这一路会被忽略；
|
||||
* 从动关节引用的 ``source`` 也必须在 ``order`` 里 —— 链上有个关节没被
|
||||
控制器声明的话，那一级的角度就没人驱动；
|
||||
* 主关节（不带 mimic 的那个）必须**唯一** —— 多个候选说明推断逻辑不成立，
|
||||
这时静默挑第一个会挑错；
|
||||
* 每条耦合链都必须能走到主关节（不终止、成环都报错）—— 只校验开头几层
|
||||
的话，链尾错一处只表现为"某个指尖角度一直不对"。
|
||||
"""
|
||||
order = [str(j) for j in order]
|
||||
if not order:
|
||||
raise GripperConfigError("夹爪控制器没有声明任何关节。")
|
||||
|
||||
followers = dict(mimics)
|
||||
unknown = sorted(set(followers) - set(order))
|
||||
if unknown:
|
||||
raise GripperConfigError(
|
||||
f"mimic 表里的关节不在控制器 joints 列表里：{unknown}\n"
|
||||
f" 控制器声明的是：{order}\n"
|
||||
f" 两边必须一致，否则我们算出来的角度会被控制器忽略。"
|
||||
)
|
||||
missing_src = sorted({r.source for r in followers.values()} - set(order))
|
||||
if missing_src:
|
||||
raise GripperConfigError(
|
||||
f"mimic 表里有关节的**主关节**不在控制器 joints 列表里：{missing_src}\n"
|
||||
f" 控制器声明的是：{order}\n"
|
||||
f" 耦合链上少一环，下面所有关节的角度都会算错。"
|
||||
)
|
||||
|
||||
candidates = [j for j in order if j not in followers]
|
||||
if len(candidates) != 1:
|
||||
raise GripperConfigError(
|
||||
f"无法唯一确定主关节：控制器里有 {len(candidates)} 个关节不带 mimic "
|
||||
f"（{candidates}），期望恰好 1 个。\n"
|
||||
f" 通常是描述里夹爪的 mimic 表没传进来（load_gripper 关了？），"
|
||||
f"或者上游描述换了耦合方式。"
|
||||
)
|
||||
source = candidates[0]
|
||||
|
||||
lower, upper = joint_limits(urdf_xml, source)
|
||||
layout = GripperLayout(order, followers, source, lower, upper)
|
||||
|
||||
# 逐条把耦合链走一遍：成环 / 不终止在这里就报出来，别等到运行期。
|
||||
for joint in followers:
|
||||
layout.value_for(joint, 0.0)
|
||||
|
||||
# 开口曲线要在 layout 之后建：它调 layout.value_for 拿每个关节的角度，
|
||||
# 而 value_for 是耦合链的**唯一**权威实现 —— 曲线里再抄一份迟早会漂移。
|
||||
# （所以是"先建后回填"，不是构造参数。）
|
||||
layout.curve, layout.kin_note = _build_aperture_curve(layout, urdf_xml)
|
||||
return layout
|
||||
|
||||
|
||||
def _build_aperture_curve(
|
||||
layout: GripperLayout, urdf_xml: str
|
||||
) -> Tuple[Optional[ApertureCurve], str]:
|
||||
"""建"开口毫米 ↔ 主关节值"的精确曲线；建不出来就退回线性并**说明白**。
|
||||
|
||||
返回 ``(曲线或 None, 给日志的一句话)``。
|
||||
|
||||
为什么允许退回：换一个夹爪时，如果它的指尖 link 命名不符合约定，
|
||||
硬报错会让整个仿真起不来；而线性近似虽然中段有几毫米偏差，但
|
||||
"能不能动、方向对不对"仍然可验。所以退回，但**必须在日志里说清楚
|
||||
口径变成了近似** —— 否则有人会拿它当毫米精度的夹爪去标定触觉阈值。
|
||||
"""
|
||||
try:
|
||||
tree = UrdfTree(urdf_xml)
|
||||
tips = find_tip_pair(tree)
|
||||
if tips is None:
|
||||
return None, (
|
||||
"⚠ 没在 URDF 里认出左右指尖 link（按 *left*finger*tip* / "
|
||||
"*right*finger*tip* 约定找）→ 开口退回**线性近似**"
|
||||
"（中段偏差可达 4 mm/100mm 行程，别拿它标触觉阈值）"
|
||||
)
|
||||
curve = ApertureCurve(
|
||||
tree,
|
||||
tips,
|
||||
angles_of=lambda v: {j: layout.value_for(j, v) for j in layout.order},
|
||||
lower=layout.lower,
|
||||
upper=layout.upper,
|
||||
)
|
||||
return curve, (
|
||||
f"开口由 URDF 正运动学反解（{tips[0]} / {tips[1]}；"
|
||||
f"合拢时间距 {curve.closed_separation_mm:.2f} mm）"
|
||||
)
|
||||
except KinematicsError as exc:
|
||||
return None, (
|
||||
f"⚠ 正运动学不可用（{exc}）→ 开口退回**线性近似**"
|
||||
f"（中段偏差可达 4 mm/100mm 行程）"
|
||||
)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 夹住判据
|
||||
# ---------------------------------------------------------------------------
|
||||
#: 判"夹住了"的默认容差（**毫米开口**）。GN01 全行程 100 mm，2 mm 的开口差
|
||||
#: 只可能是被挡住了，不可能是控制精度问题。
|
||||
DEFAULT_GRASP_TOLERANCE_MM = 2.0
|
||||
|
||||
#: 判"还在动"的默认容差（毫米开口）。比上面小：它只是用来区分 MOVING / IDLE。
|
||||
DEFAULT_MOVING_TOLERANCE_MM = 0.5
|
||||
|
||||
|
||||
def is_grasping(
|
||||
target_aperture_mm: float,
|
||||
actual_aperture_mm: float,
|
||||
tolerance_mm: float = DEFAULT_GRASP_TOLERANCE_MM,
|
||||
) -> bool:
|
||||
"""由"指令到了合拢位、实际开口却更大"推断夹住了东西。
|
||||
|
||||
真机（DH）有状态字可以问 "object caught"，仿真没有。但仿真里有一个等价
|
||||
且更本质的信号：位置控制器把目标压到合拢位，若两指之间夹着东西，
|
||||
关节就**到不了**那个位置，稳态开口会停在工件宽度附近。
|
||||
|
||||
两个条件同时成立才算夹住：
|
||||
|
||||
1. 目标在合拢侧（``target_aperture_mm`` 小于容差）—— 否则"有误差"只是没走到；
|
||||
2. 实际开口比目标大出容差 —— 被挡住了。
|
||||
|
||||
⚠️ 口径统一用**毫米开口**，不是主关节值（米）。因为开口↔关节值在
|
||||
FK 口径下是非线性的（见 sim_gripper_kin.py），拿米当口径的话"2 mm 容差"
|
||||
在不同开度下含义不同，标定出来的阈值换台夹爪就废了。
|
||||
|
||||
这是**推断**，不是测量：工件被夹在两指之间、但关节仍走到位的特殊情况
|
||||
（例如工件很薄且被压进夹爪根部）会漏判。所以 P6 的最终判据仍然用触觉
|
||||
力（见 R3.1），这里只提供 ``GripperStatus.is_gripped`` 这个粗信号。
|
||||
"""
|
||||
near_closed = float(target_aperture_mm) <= float(tolerance_mm)
|
||||
blocked = (
|
||||
float(actual_aperture_mm) - float(target_aperture_mm)
|
||||
) > float(tolerance_mm)
|
||||
return bool(near_closed and blocked)
|
||||
@@ -0,0 +1,424 @@
|
||||
"""仿真夹爪适配器：``GripperExecute`` 服务 → ``JointGroupPositionController``。
|
||||
|
||||
=============================================================================
|
||||
它在链路上的位置
|
||||
=============================================================================
|
||||
应用层（P6 状态机 / 现场脚本）
|
||||
│ 服务 <ns>/open | <ns>/close | <ns>/move | <ns>/rotate
|
||||
│ （arm_msgs/srv/GripperExecute，与真机**同名同类型**）
|
||||
▼
|
||||
本节点
|
||||
│ ① 开口毫米 → 主关节值（绝对物理映射，见 sim_gripper.py 文件头）
|
||||
│ ② 按 mimic 耦合算出 6 个从动关节的目标角
|
||||
│ ③ 按 controllers.yaml 的 joints 顺序拼成数组
|
||||
▼
|
||||
话题 <controller>/commands (std_msgs/Float64MultiArray)
|
||||
▼
|
||||
JointGroupPositionController（gz_ros2_control，在 Gazebo 进程里）
|
||||
▼
|
||||
Gazebo 里的 7 个 sim_* 夹爪关节
|
||||
|
||||
同时订阅 /joint_states，把**反馈关节**的实际值换算回开口毫米，
|
||||
发布到 <ns>/status（arm_msgs/GripperStatus）。
|
||||
注意不是主关节 —— 主关节在 Gazebo 里压根不动，见下面第 4 个坑。
|
||||
|
||||
=============================================================================
|
||||
为什么必须由适配器驱动全部 7 个关节
|
||||
=============================================================================
|
||||
sdformat 把 URDF 的 ``<mimic>`` 整个丢掉，gz-sim 也不实现 mimic ——
|
||||
6 个从动关节在仿真里不会跟随主关节。所以适配器必须自己算。
|
||||
这件事同时解决了一个 TF 隐患：``robot_state_publisher`` 是按 /joint_states
|
||||
里的**真实值**发布 TF 的，只要 7 个关节都被驱动、且值符合耦合公式，
|
||||
RViz/TF 与 Gazebo 就一致（详见 urdf_normalize.py 里那段说明）。
|
||||
|
||||
=============================================================================
|
||||
⚠️ 仿真夹爪与真机夹爪不是同一个夹爪
|
||||
=============================================================================
|
||||
仿真是官方 Flexiv-GN01（行程 100 mm），现场是 DH PGEA-100-20-W
|
||||
（``site.yaml`` 的 ``gripper.stroke_mm`` = 20 mm）。开口用绝对物理映射，
|
||||
所以同一句"开 8 mm"两边都对；但"全开"对应的开口不同。启动时会把两者
|
||||
都打出来，差异大时会明确提示（见 :meth:`GripperLayout.stroke_note`）。
|
||||
|
||||
=============================================================================
|
||||
三个必须知道的坑
|
||||
=============================================================================
|
||||
1) **命令数组的顺序**取自 controllers.yaml，不是本节点里的常量。
|
||||
顺序错不会报错，只会把角度发给别的关节 —— 表现成"夹爪动得很奇怪"。
|
||||
所以本节点启动时先把顺序读出来并校验（从动关节必须在列表里）。
|
||||
|
||||
2) **``rotate`` 在平行夹爪上没有对应自由度**。真机那边 RGI 才有。这里明确
|
||||
返回 ``success=false`` 并说明原因，而不是假装成功 —— 假装成功会让上层
|
||||
的"拧入"流程在仿真里走到一个永远不动的分支上。
|
||||
|
||||
3) **``status`` 的 ``is_gripped`` 是推断值**（指令到合拢位但实际值没跟上），
|
||||
不是测量值。真机有 DH 状态字，仿真没有。最终夹持判据仍应看触觉力，
|
||||
见 sim_gripper.is_grasping 的说明。
|
||||
|
||||
4) **主关节 ``finger_width_joint`` 在 Gazebo 里不会动**。上游描述把它挂在
|
||||
孤立的 ``unused_finger_width_link`` 上（那个 link 的注释写着 "cannot have
|
||||
0 mass"，就是为了让 URDF 合法），它和手指没有任何机械连接。
|
||||
实测：命令到 0.100，它的 /joint_states 值恒为 ~0，而 6 个从动关节准确到位。
|
||||
所以：
|
||||
- 命令数组里**仍然要带上它**（controllers.yaml 的 joints 含它，少一个会错位）；
|
||||
- 但**反馈绝不能取它**，否则 status 永远报"开口 0 mm"而夹爪明明开着。
|
||||
适配器改用"与主关节直接相连、|multiplier| 最大的那个从动关节"反推开口
|
||||
（见 GripperLayout._pick_feedback / aperture_from_feedback）。
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
from pathlib import Path
|
||||
from typing import List, Optional
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from std_msgs.msg import Float64MultiArray
|
||||
|
||||
from arm_msgs.msg import GripperStatus
|
||||
from arm_msgs.srv import GripperExecute
|
||||
from sensor_msgs.msg import JointState
|
||||
|
||||
from .sim_gripper import (
|
||||
DEFAULT_CONTROLLER,
|
||||
DEFAULT_CONTROLLERS_REL,
|
||||
DEFAULT_GRASP_TOLERANCE_MM,
|
||||
DEFAULT_MOVING_TOLERANCE_MM,
|
||||
GripperConfigError,
|
||||
GripperLayout,
|
||||
gripper_joint_order,
|
||||
is_grasping,
|
||||
layout_from_parts,
|
||||
parse_mimics,
|
||||
)
|
||||
|
||||
#: 主关节状态多少秒没更新就算"数据不新鲜"（仿真时间，不是墙钟）。
|
||||
#: 控制器 100Hz、JSB 也在这个量级，1 秒足够宽松。
|
||||
STATE_STALE_S = 1.0
|
||||
|
||||
#: 服务后缀 → GripperCommand.command 的映射。前四个就是 capabilities.py
|
||||
#: 里声明的接口名；execute 是通用入口（与 dual_gripper_driver 对齐）。
|
||||
_SERVICE_COMMANDS = ("open", "close", "move", "rotate")
|
||||
|
||||
|
||||
def _site_stroke_mm() -> Optional[float]:
|
||||
"""从 site.yaml 读真机夹爪行程（只为启动时打一句对比，读不到返回 None）。
|
||||
|
||||
读不到不影响工作 —— 它只是提示信息，不该让仿真起不来。
|
||||
"""
|
||||
try:
|
||||
from arm_interfaces import load_site # 延迟导入：只需要它一处
|
||||
except Exception:
|
||||
return None
|
||||
try:
|
||||
data = load_site()
|
||||
except Exception:
|
||||
return None
|
||||
grip = data.get("gripper") if isinstance(data, dict) else None
|
||||
if not isinstance(grip, dict):
|
||||
return None
|
||||
for key in ("stroke_mm", "stroke"):
|
||||
try:
|
||||
val = float(grip.get(key))
|
||||
except (TypeError, ValueError):
|
||||
continue
|
||||
if val > 0:
|
||||
return val
|
||||
return None
|
||||
|
||||
|
||||
class SimGripperAdapter(Node):
|
||||
"""把 ``GripperExecute`` 服务翻译成 Gazebo 里 7 个夹爪关节的目标位置。"""
|
||||
|
||||
def __init__(self) -> None:
|
||||
super().__init__("sim_gripper_adapter")
|
||||
|
||||
# 参数：全部由 launch 从**同一份** prepare_urdf 结果与 controllers.yaml
|
||||
# 派生，避免这里自己再展开一次 xacro 造成两边不一致。
|
||||
self.declare_parameter("controllers_file", "")
|
||||
self.declare_parameter("controller", DEFAULT_CONTROLLER)
|
||||
self.declare_parameter("mimics", [""])
|
||||
# ⚠️ 不要 declare_parameter("use_sim_time", ...)：launch 会作为参数文件
|
||||
# 传进来，rclpy 那时已经自动声明过，再声明会抛异常让节点起不来。
|
||||
self.declare_parameter("namespace", "/gripper")
|
||||
self.declare_parameter("tolerance_mm", DEFAULT_GRASP_TOLERANCE_MM) # 夹住判据容差（毫米开口）
|
||||
self.declare_parameter("default_speed_pct", 50.0)
|
||||
self.declare_parameter("default_force_pct", 20.0)
|
||||
|
||||
controller = str(self.get_parameter("controller").value or DEFAULT_CONTROLLER)
|
||||
raw_ctrl = str(self.get_parameter("controllers_file").value or "")
|
||||
controllers_file = Path(raw_ctrl) if raw_ctrl else Path.cwd() / DEFAULT_CONTROLLERS_REL
|
||||
|
||||
# mimics 参数传进来的是字符串数组；空字符串要滤掉（ROS 对空数组不友好）。
|
||||
raw_mimics = [
|
||||
str(m).strip()
|
||||
for m in (self.get_parameter("mimics").value or [])
|
||||
if str(m).strip()
|
||||
]
|
||||
|
||||
urdf_xml = self._robot_description()
|
||||
try:
|
||||
order = gripper_joint_order(controllers_file, controller)
|
||||
relations = parse_mimics(raw_mimics)
|
||||
self.layout: GripperLayout = layout_from_parts(order, relations, urdf_xml)
|
||||
except GripperConfigError as exc:
|
||||
# 配置错就让节点直接失败：一个"什么都不动"的夹爪适配器比没有更糟 ——
|
||||
# 它看起来在工作，而上层会一直等一个永远不来的状态变化。
|
||||
self.get_logger().error(f"仿真夹爪配置有问题：\n{exc}")
|
||||
raise
|
||||
|
||||
self._controller = controller
|
||||
self._ns = str(self.get_parameter("namespace").value or "/gripper").rstrip("/")
|
||||
self._tolerance_mm = float(self.get_parameter("tolerance_mm").value)
|
||||
self._speed = float(self.get_parameter("default_speed_pct").value)
|
||||
self._force = float(self.get_parameter("default_force_pct").value)
|
||||
|
||||
# 当前目标（主关节值，米）与"最近一次是否有状态"。
|
||||
self._target = self.layout.aperture_to_source(self.layout.stroke_mm) # 默认全开
|
||||
self._feedback_state: Optional[float] = None
|
||||
self._state_stamp: Optional[float] = None
|
||||
# ⚠️ 反馈取 layout.feedback，**不是**主关节：上游的 finger_width_joint
|
||||
# 挂在孤立的 unused_finger_width_link 上，在 Gazebo 里根本不动
|
||||
# （实测：命令 0.100，状态恒为 0）。拿它当反馈会永远报"开口 0 mm"。
|
||||
# 详见 sim_gripper.GripperLayout._pick_feedback 的说明。
|
||||
self._state_name = f"{self.layout.feedback}"
|
||||
|
||||
self._pub_cmd = self.create_publisher(
|
||||
Float64MultiArray, f"/{controller}/commands", 10
|
||||
)
|
||||
self._pub_status = self.create_publisher(GripperStatus, f"{self._ns}/status", 10)
|
||||
self.create_subscription(JointState, "/joint_states", self._on_joint_state, 10)
|
||||
|
||||
self._srv: List[object] = []
|
||||
for name in _SERVICE_COMMANDS:
|
||||
self._srv.append(
|
||||
self.create_service(
|
||||
GripperExecute, f"{self._ns}/{name}", self._make_handler(name)
|
||||
)
|
||||
)
|
||||
self._srv.append(
|
||||
self.create_service(GripperExecute, f"{self._ns}/execute", self._on_execute)
|
||||
)
|
||||
|
||||
self.get_logger().info(f"夹爪：{self.layout.describe()}")
|
||||
if self.layout.feedback != self.layout.source:
|
||||
self.get_logger().info(
|
||||
f"夹爪开口反馈取自 {self.layout.feedback}（反推主关节 "
|
||||
f"{self.layout.source}；主关节本身在 Gazebo 里不跟随命令）"
|
||||
)
|
||||
self.get_logger().info(
|
||||
f"夹爪行程口径：{self.layout.stroke_note(_site_stroke_mm())}"
|
||||
)
|
||||
self.get_logger().info(
|
||||
f"服务 {self._ns}/{{{'|'.join(_SERVICE_COMMANDS)}|execute}} → "
|
||||
f"话题 /{controller}/commands"
|
||||
)
|
||||
|
||||
# 先把夹爪摆到默认全开，免得机器人一出场就是捏着的状态。
|
||||
self._publish_target(self._target)
|
||||
self.create_timer(0.1, self._tick_status)
|
||||
|
||||
# --- 参数与描述 -------------------------------------------------------
|
||||
def _robot_description(self) -> str:
|
||||
"""取归一化后的 robot_description（launch 与 RSP 用的是同一份）。"""
|
||||
if not self.has_parameter("robot_description"):
|
||||
self.declare_parameter("robot_description", "")
|
||||
xml = str(self.get_parameter("robot_description").value or "")
|
||||
if not xml.strip():
|
||||
raise GripperConfigError(
|
||||
"没拿到 robot_description 参数，无法确定夹爪关节的限位。\n"
|
||||
" 正常由 gazebo_sim.launch.py 传入（它与发给 robot_state_publisher "
|
||||
"的是同一份归一化描述）。单独手跑本节点时要显式给：\n"
|
||||
" ros2 run arm_sim_gazebo sim_gripper_adapter "
|
||||
"--ros-args -p robot_description:=\"$(xacro ...)\""
|
||||
)
|
||||
return xml
|
||||
|
||||
# --- 下发 -------------------------------------------------------------
|
||||
def _publish_target(self, source_value: float) -> None:
|
||||
"""把一整组关节目标（主 + 从动）按顺序发出去。"""
|
||||
self._target = source_value
|
||||
msg = Float64MultiArray()
|
||||
msg.data = [float(v) for v in self.layout.command_vector_from_source(source_value)]
|
||||
self._pub_cmd.publish(msg)
|
||||
|
||||
def _move_to_aperture(self, aperture_mm: float) -> None:
|
||||
self._publish_target(self.layout.aperture_to_source(aperture_mm))
|
||||
|
||||
# --- 状态 -------------------------------------------------------------
|
||||
def _on_joint_state(self, msg: JointState) -> None:
|
||||
for name, pos in zip(msg.name, msg.position):
|
||||
if name == self._state_name:
|
||||
self._feedback_state = float(pos)
|
||||
self._state_stamp = self._now()
|
||||
return
|
||||
|
||||
def _now(self) -> float:
|
||||
return self.get_clock().now().nanoseconds * 1e-9
|
||||
|
||||
def _fresh_state(self) -> Optional[float]:
|
||||
"""反馈关节的当前值；太久没更新或从没收到过就返回 None。"""
|
||||
if self._feedback_state is None or self._state_stamp is None:
|
||||
return None
|
||||
if (self._now() - self._state_stamp) > STATE_STALE_S:
|
||||
return None
|
||||
return self._feedback_state
|
||||
|
||||
def _status_msg(self) -> GripperStatus:
|
||||
state = self._fresh_state()
|
||||
msg = GripperStatus()
|
||||
msg.header.stamp = self.get_clock().now().to_msg()
|
||||
# frame_id 留空：真机那边填的是 "<side>_gripper"，而仿真里没有任何东西
|
||||
# 发布那个坐标系（TF 只来自 robot_state_publisher）。
|
||||
# 填一个查不到的 frame 比留空更糟 —— tf2 会一直报 frame 不存在。
|
||||
msg.header.frame_id = ""
|
||||
msg.name = "gn01_sim"
|
||||
msg.gripper_type = "parallel"
|
||||
msg.connected = state is not None
|
||||
msg.initialized = state is not None
|
||||
msg.velocity = self._speed
|
||||
msg.force = self._force
|
||||
|
||||
if state is None:
|
||||
# 没状态就别编：position 保持 0 会让上层以为夹爪是合拢的。
|
||||
msg.state = "ERROR"
|
||||
msg.error_msg = (
|
||||
f"{STATE_STALE_S:.1f}s 内没收到 {self._state_name} 的 /joint_states，"
|
||||
f"无法反馈开口。检查 joint_state_broadcaster 是否 active。"
|
||||
)
|
||||
return msg
|
||||
|
||||
aperture = self.layout.aperture_from_feedback(state)
|
||||
target_aperture = self.layout.source_to_aperture(self._target)
|
||||
# 判"还在动"用**开口口径**（毫米），不用主关节值：开口↔关节值在 FK
|
||||
# 口径下是非线性的，拿米当口径的话同一个容差在不同开度下含义不同。
|
||||
moving = abs(aperture - target_aperture) > DEFAULT_MOVING_TOLERANCE_MM
|
||||
msg.position = float(aperture)
|
||||
# 真机这里填 DH 的千分比；仿真的"宽度关节"本身就是千分比语义
|
||||
#（0.100 m ≙ 1000‰），所以直接换算，口径与真机一致。
|
||||
msg.finger_position_per_mille = float(
|
||||
max(0.0, min(1000.0, aperture / max(1e-9, self.layout.stroke_mm) * 1000.0))
|
||||
)
|
||||
msg.is_moving = bool(moving)
|
||||
# 判据：指令在合拢侧 + 实际开口比指令大出容差（被工件挡住）。
|
||||
msg.is_gripped = is_grasping(
|
||||
target_aperture, aperture, self._tolerance_mm
|
||||
)
|
||||
if msg.is_gripped:
|
||||
msg.state = "GRIPPING"
|
||||
elif moving:
|
||||
msg.state = "MOVING"
|
||||
else:
|
||||
msg.state = "IDLE"
|
||||
return msg
|
||||
|
||||
def _tick_status(self) -> None:
|
||||
self._pub_status.publish(self._status_msg())
|
||||
|
||||
# --- 服务 -------------------------------------------------------------
|
||||
def _make_handler(self, verb: str):
|
||||
def handler(request: GripperExecute.Request, response: GripperExecute.Response):
|
||||
# 服务名本身就是命令；但请求里也可能带 command（通用入口风格）。
|
||||
# 若两者冲突，以**请求里的**为准并记一条 info —— 免得上层以为
|
||||
# 服务名能覆盖请求内容。
|
||||
req_verb = str(getattr(request.command, "command", "") or "").strip()
|
||||
if req_verb and req_verb != verb:
|
||||
self.get_logger().info(
|
||||
f"{self._ns}/{verb} 收到 command={req_verb!r}，按请求执行。"
|
||||
)
|
||||
return self._execute_verb(req_verb, request, response)
|
||||
return self._execute_verb(verb, request, response)
|
||||
|
||||
return handler
|
||||
|
||||
def _on_execute(
|
||||
self, request: GripperExecute.Request, response: GripperExecute.Response
|
||||
):
|
||||
verb = str(getattr(request.command, "command", "") or "").strip()
|
||||
if not verb:
|
||||
return self._fail(response, "execute 需要 request.command.command（如 open/close/move）")
|
||||
return self._execute_verb(verb, request, response)
|
||||
|
||||
def _execute_verb(
|
||||
self,
|
||||
verb: str,
|
||||
request: GripperExecute.Request,
|
||||
response: GripperExecute.Response,
|
||||
):
|
||||
cmd = request.command
|
||||
try:
|
||||
if verb == "open":
|
||||
self._move_to_aperture(self.layout.stroke_mm)
|
||||
note = f"全开（{self.layout.stroke_mm:.1f} mm）"
|
||||
elif verb == "close":
|
||||
self._move_to_aperture(0.0)
|
||||
note = "全闭合（开口 0 mm）"
|
||||
elif verb == "move":
|
||||
want = float(cmd.position)
|
||||
clamped = self.layout.is_clamped(want)
|
||||
self._move_to_aperture(want)
|
||||
note = f"开口 → {want:.2f} mm"
|
||||
if clamped:
|
||||
note += (
|
||||
f"（**被限位截断**到 "
|
||||
f"{min(max(want, 0.0), self.layout.stroke_mm):.2f} mm）"
|
||||
)
|
||||
elif verb == "rotate":
|
||||
return self._fail(
|
||||
response,
|
||||
"仿真夹爪（GN01）是平行夹爪，没有旋转自由度；rotate 只在真机 RGI 上有效。",
|
||||
)
|
||||
elif verb == "stop":
|
||||
# 停在当前位置：把"当前实际值"当目标发回去。
|
||||
state = self._fresh_state()
|
||||
if state is None:
|
||||
return self._fail(response, "没有关节状态，stop 无法保持位置")
|
||||
# state 是**反馈关节**的值，得先换算回主关节口径再下发。
|
||||
self._publish_target(self.layout.source_from_feedback(state))
|
||||
note = (
|
||||
"保持当前位置（"
|
||||
f"{self.layout.aperture_from_feedback(state):.2f} mm）"
|
||||
)
|
||||
else:
|
||||
return self._fail(
|
||||
response,
|
||||
f"仿真夹爪不支持命令 {verb!r}；支持 "
|
||||
f"{list(_SERVICE_COMMANDS) + ['stop']}。",
|
||||
)
|
||||
except GripperConfigError as exc: # pragma: no cover - 构造期已挡住
|
||||
return self._fail(response, str(exc))
|
||||
|
||||
response.success = True
|
||||
response.message = note
|
||||
response.status = self._status_msg()
|
||||
self.get_logger().info(f"{self._ns}/{verb}：{note}")
|
||||
return response
|
||||
|
||||
def _fail(self, response: GripperExecute.Response, why: str):
|
||||
response.success = False
|
||||
response.message = why
|
||||
# status 仍然填：调用方拿到 success=false 时也需要知道当前开口，
|
||||
# 否则只能靠猜（真机的返回结构也是这个语义）。
|
||||
response.status = self._status_msg()
|
||||
self.get_logger().warn(f"{self._ns}：{why}")
|
||||
return response
|
||||
|
||||
|
||||
def main(args=None) -> None:
|
||||
rclpy.init(args=args)
|
||||
node = None
|
||||
try:
|
||||
node = SimGripperAdapter()
|
||||
rclpy.spin(node)
|
||||
except GripperConfigError:
|
||||
rclpy.shutdown()
|
||||
raise SystemExit(2)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
if node is not None:
|
||||
node.destroy_node()
|
||||
if rclpy.ok():
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,368 @@
|
||||
"""由 URDF 做夹爪正运动学，把"开口毫米"换算成**真的**关节角（纯逻辑，不起 ROS）。
|
||||
|
||||
=============================================================================
|
||||
为什么需要它：上游的 <mimic> 是线性近似
|
||||
=============================================================================
|
||||
GN01 的描述用一条线性耦合把 6 个从动关节挂在宽度关节上：
|
||||
|
||||
从动关节 = 9.404 × 主关节 − 0.155
|
||||
|
||||
系数 9.404 是按**两端**拟合出来的，所以两端准、中段不准 —— 因为真实的
|
||||
手指不是"平移"，而是"绕着 knuckle 转 + 指掌再转"，指垫的间距
|
||||
``|tip_L(θ) − tip_R(θ)|`` 是 θ 的非线性函数。
|
||||
|
||||
实测（2026-09-24，Gazebo Fortress，见 test_sim_gripper_kin.py 里的黄金表）：
|
||||
|
||||
命令开口 指尖间距实测 与命令之差
|
||||
0 mm 27.09 mm （固有间距，指垫此时接触）
|
||||
8 mm 35.76 mm +0.67 mm
|
||||
20 mm 48.84 mm +1.75 mm
|
||||
50 mm 80.96 mm +3.87 mm
|
||||
100 mm 127.02 mm −0.07 mm
|
||||
|
||||
也就是说：**命令"开 20 mm"，两指实际相距 21.75 mm**。抓一个 20 mm 宽的
|
||||
通止规时，指垫之间会空出 1.75 mm —— 没有接触、没有接触力，触觉那一步
|
||||
在仿真里直接失效。这和真机（DH，毫米制、开多少就是多少）的口径不一致，
|
||||
仿真就失去了"拿真几何试算法"的意义。
|
||||
|
||||
所以本模块用 URDF 自己算正运动学，把 mm 反解成关节角：
|
||||
|
||||
aperture_to_source(20.0) → 令 |tip_L − tip_R| − 闭合间距 == 20.0 的关节值
|
||||
|
||||
这样"开口 20 mm"在仿真里就是**物理上真的 20 mm**，与真机口径一致。
|
||||
|
||||
=============================================================================
|
||||
为什么可靠
|
||||
=============================================================================
|
||||
本文件的正运动学与 Gazebo（DART 物理 + sdformat 转换后的模型）在
|
||||
0~100 mm 全程 **吻合到 0.01 mm**（上面那张表就是两边对比的结果）。
|
||||
两者完全独立：一个是我们的矩阵乘法，一个是物理引擎。这个一致性同时证明：
|
||||
|
||||
- URDF → SDF 的转换没有丢掉这些 link/joint 的偏移；
|
||||
- 我们对耦合公式的理解与 Gazebo 实际执行的一致。
|
||||
|
||||
=============================================================================
|
||||
通用性（换夹爪时）
|
||||
=============================================================================
|
||||
本模块不写死 GN01 的名字：
|
||||
- 树结构完全从 URDF 里读；
|
||||
- 两个"指尖"link 按命名约定找（``*left*finger*tip*`` / ``*right*...``），
|
||||
找不到就让调用方退回线性近似（并在日志里**明确说出口径变了**）；
|
||||
- 计算指尖间距用的参考系取两者的**最近公共祖先**，不需要知道夹爪 base
|
||||
叫什么名字（GN01 是 ``grav_base_link``，别的夹爪叫别的）。
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
import re
|
||||
import xml.etree.ElementTree as ET
|
||||
from dataclasses import dataclass
|
||||
from typing import Callable, Dict, List, Mapping, Optional, Sequence, Tuple
|
||||
|
||||
Mat3 = Tuple[Tuple[float, float, float], ...]
|
||||
Vec3 = Tuple[float, float, float]
|
||||
|
||||
|
||||
class KinematicsError(ValueError):
|
||||
"""URDF 结构与预期不符（缺 link、链断开、不是树等）。"""
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class JointNode:
|
||||
"""URDF 里的一条 joint（父 → 子），带固定偏移与转动轴。"""
|
||||
|
||||
name: str
|
||||
type: str
|
||||
parent: str
|
||||
child: str
|
||||
xyz: Vec3
|
||||
rpy: Vec3
|
||||
axis: Vec3
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 3×3 矩阵小工具（只有旋转，位置单独带着走 —— 比 4×4 好读）
|
||||
# ---------------------------------------------------------------------------
|
||||
def _rot_rpy(rpy: Vec3) -> Mat3:
|
||||
r, p, y = rpy
|
||||
cr, sr = math.cos(r), math.sin(r)
|
||||
cp, sp = math.cos(p), math.sin(p)
|
||||
cy, sy = math.cos(y), math.sin(y)
|
||||
return (
|
||||
(cy * cp, cy * sp * sr - sy * cr, cy * sp * cr + sy * sr),
|
||||
(sy * cp, sy * sp * sr + cy * cr, sy * sp * cr - cy * sr),
|
||||
(-sp, cp * sr, cp * cr),
|
||||
)
|
||||
|
||||
|
||||
def _rot_axis(axis: Vec3, angle: float) -> Mat3:
|
||||
x, y, z = axis
|
||||
norm = math.sqrt(x * x + y * y + z * z)
|
||||
if norm < 1e-12:
|
||||
raise KinematicsError("关节 axis 是零向量，无法确定转动轴")
|
||||
x, y, z = x / norm, y / norm, z / norm
|
||||
c, s, k = math.cos(angle), math.sin(angle), 1.0 - math.cos(angle)
|
||||
return (
|
||||
(x * x * k + c, x * y * k - z * s, x * z * k + y * s),
|
||||
(y * x * k + z * s, y * y * k + c, y * z * k - x * s),
|
||||
(z * x * k - y * s, z * y * k + x * s, z * z * k + c),
|
||||
)
|
||||
|
||||
|
||||
def _mat_mul(a: Mat3, b: Mat3) -> Mat3:
|
||||
return tuple(
|
||||
tuple(sum(a[i][k] * b[k][j] for k in range(3)) for j in range(3))
|
||||
for i in range(3)
|
||||
)
|
||||
|
||||
|
||||
def _mat_vec(a: Mat3, v: Vec3) -> Vec3:
|
||||
return tuple(sum(a[i][k] * v[k] for k in range(3)) for i in range(3)) # type: ignore[return-value]
|
||||
|
||||
|
||||
def _dist(a: Vec3, b: Vec3) -> float:
|
||||
return math.sqrt(sum((x - y) ** 2 for x, y in zip(a, b)))
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# URDF 树
|
||||
# ---------------------------------------------------------------------------
|
||||
def _floats(text: Optional[str], default: Sequence[float]) -> Vec3:
|
||||
if not text:
|
||||
return tuple(default) # type: ignore[return-value]
|
||||
parts = text.split()
|
||||
if len(parts) != 3:
|
||||
raise KinematicsError(f"URDF 里的三元组长度不对: {text!r}")
|
||||
return (float(parts[0]), float(parts[1]), float(parts[2]))
|
||||
|
||||
|
||||
class UrdfTree:
|
||||
"""URDF 的 joint 树；只关心运动学（不碰 inertial / visual / collision）。"""
|
||||
|
||||
def __init__(self, xml: str) -> None:
|
||||
root = ET.fromstring(xml)
|
||||
self.by_child: Dict[str, JointNode] = {}
|
||||
self.parent_of: Dict[str, str] = {}
|
||||
for j in root.findall("joint"):
|
||||
origin = j.find("origin")
|
||||
axis_el = j.find("axis")
|
||||
parent_el, child_el = j.find("parent"), j.find("child")
|
||||
if parent_el is None or child_el is None or j.get("name") is None:
|
||||
raise KinematicsError("URDF 里有缺 name/parent/child 的 joint")
|
||||
node = JointNode(
|
||||
name=j.get("name", ""),
|
||||
type=j.get("type", "fixed"),
|
||||
parent=parent_el.get("link", ""),
|
||||
child=child_el.get("link", ""),
|
||||
xyz=_floats(origin.get("xyz") if origin is not None else None, (0, 0, 0)),
|
||||
rpy=_floats(origin.get("rpy") if origin is not None else None, (0, 0, 0)),
|
||||
axis=_floats(axis_el.get("xyz") if axis_el is not None else None, (1, 0, 0)),
|
||||
)
|
||||
if node.child in self.by_child:
|
||||
raise KinematicsError(f"link {node.child!r} 有两条父关节，不是树")
|
||||
self.by_child[node.child] = node
|
||||
self.parent_of[node.child] = node.parent
|
||||
|
||||
# --- 结构查询 ---------------------------------------------------------
|
||||
def ancestors(self, link: str) -> List[str]:
|
||||
"""从 link 一路向上到根（含自身）。"""
|
||||
out: List[str] = []
|
||||
seen = set()
|
||||
cur = link
|
||||
while True:
|
||||
out.append(cur)
|
||||
if cur not in self.parent_of:
|
||||
return out
|
||||
if cur in seen:
|
||||
raise KinematicsError(f"URDF 的 link 链里有环（{cur}）")
|
||||
seen.add(cur)
|
||||
cur = self.parent_of[cur]
|
||||
|
||||
def common_ancestor(self, a: str, b: str) -> str:
|
||||
"""最近公共祖先 link（用 A 的祖先链去查 B 的，第一个命中的就是）。"""
|
||||
b_chain = set(self.ancestors(b))
|
||||
for link in self.ancestors(a):
|
||||
if link in b_chain:
|
||||
return link
|
||||
raise KinematicsError(f"{a!r} 与 {b!r} 没有公共祖先（URDF 断了？）")
|
||||
|
||||
def chain(self, link: str, root: str) -> List[JointNode]:
|
||||
"""root → link 的关节序列。"""
|
||||
out: List[JointNode] = []
|
||||
cur = link
|
||||
while cur != root:
|
||||
if cur not in self.by_child:
|
||||
raise KinematicsError(f"从 {link!r} 上溯不到 {root!r}")
|
||||
node = self.by_child[cur]
|
||||
out.append(node)
|
||||
cur = node.parent
|
||||
return list(reversed(out))
|
||||
|
||||
# --- 正运动学 ---------------------------------------------------------
|
||||
def position(self, link: str, root: str, angles: Mapping[str, float]) -> Vec3:
|
||||
"""link 原点在 root 坐标系里的位置。angles 缺省视为 0。"""
|
||||
rot: Mat3 = ((1.0, 0, 0), (0, 1.0, 0), (0, 0, 1.0))
|
||||
pos: Vec3 = (0.0, 0.0, 0.0)
|
||||
for node in self.chain(link, root):
|
||||
offset = _mat_vec(rot, node.xyz)
|
||||
pos = (pos[0] + offset[0], pos[1] + offset[1], pos[2] + offset[2])
|
||||
rot = _mat_mul(rot, _rot_rpy(node.rpy))
|
||||
if node.type != "fixed":
|
||||
rot = _mat_mul(rot, _rot_axis(node.axis, float(angles.get(node.name, 0.0))))
|
||||
return pos
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 找指尖 link（按命名约定，不写死 GN01）
|
||||
# ---------------------------------------------------------------------------
|
||||
_LEFT_TIP_PATTERNS = (
|
||||
r".*left.*finger.*tip$",
|
||||
r".*left.*tip$",
|
||||
r".*left.*tcp$",
|
||||
)
|
||||
_RIGHT_TIP_PATTERNS = (
|
||||
r".*right.*finger.*tip$",
|
||||
r".*right.*tip$",
|
||||
r".*right.*tcp$",
|
||||
)
|
||||
|
||||
|
||||
def _pick(links: Sequence[str], patterns: Sequence[str]) -> Optional[str]:
|
||||
for pat in patterns:
|
||||
rx = re.compile(pat) # 大小写敏感：URDF 里普遍是小写
|
||||
hits = sorted(link for link in links if rx.match(link))
|
||||
if hits:
|
||||
# 多个命中时取名字最长的（``sim_left_finger_tip`` 比 ``left_tip``
|
||||
# 更可能是真正的指垫），并保持确定性（已排序）。
|
||||
return max(hits, key=len)
|
||||
return None
|
||||
|
||||
|
||||
def find_tip_pair(tree: UrdfTree) -> Optional[Tuple[str, str]]:
|
||||
"""按命名约定找左右指垫 link；找不到返回 None（调用方退回线性近似）。"""
|
||||
links = sorted(set(tree.by_child))
|
||||
left = _pick(links, _LEFT_TIP_PATTERNS)
|
||||
right = _pick(links, _RIGHT_TIP_PATTERNS)
|
||||
if left is None or right is None or left == right:
|
||||
return None
|
||||
return (left, right)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 开口曲线：mm ↔ 关节值
|
||||
# ---------------------------------------------------------------------------
|
||||
class ApertureCurve:
|
||||
"""由正运动学给出的"关节值 → 两指间距(mm)"曲线，以及它的反解。
|
||||
|
||||
``joint_value`` 是**主关节**的值（GN01 就是宽度关节，单位米）。
|
||||
调用方通过 ``angles_of(joint_value)`` 告诉本类"主关节取这个值时，
|
||||
每个关节分别是多少" —— 耦合关系归调用方（sim_gripper.py）管，
|
||||
本类只管几何，这样换夹爪/换耦合方式都不用动几何代码。
|
||||
"""
|
||||
|
||||
#: 反解用的二分次数（40 次足够把 100 mm 行程收敛到 1e-9 mm 以内）。
|
||||
BISECT_ITERS = 60
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
tree: UrdfTree,
|
||||
tips: Tuple[str, str],
|
||||
angles_of: Callable[[float], Mapping[str, float]],
|
||||
lower: float,
|
||||
upper: float,
|
||||
) -> None:
|
||||
self.tree = tree
|
||||
self.tips = tips
|
||||
self.angles_of = angles_of
|
||||
self.lower = float(lower)
|
||||
self.upper = float(upper)
|
||||
self.root = tree.common_ancestor(tips[0], tips[1])
|
||||
# 闭合位置（主关节到下限）的指尖间距 —— 指垫此时贴在一起，
|
||||
# 所以"开口"一律相对它算，这样开口 0 就是"真的合拢"。
|
||||
self._closed = self._raw_separation(self.lower)
|
||||
self._stroke = self._raw_separation(self.upper) - self._closed
|
||||
# ⚠️ 这里不能只判 `stroke <= 0`。实测过一次：把右侧 knuckle 的符号写反时，
|
||||
# 两指**同步**摆动，间距始终 45.0000000000 mm —— 差值是 4e-13，
|
||||
# 一个裸的 `<= 0` 会放过它。而放过的后果是**静默失效**：开口曲线是
|
||||
# 水平的，`joint_value()` 的二分会一路收敛到下限，
|
||||
# 于是"开 20 mm"变成"不动"，且没有任何报错。
|
||||
# 所以阈值取"名义行程的 1%"：真实夹爪不可能只有这么点行程，
|
||||
# 而浮点噪声远小于它。
|
||||
nominal_mm = abs(self.upper - self.lower) * 1000.0
|
||||
min_stroke = max(1e-6, 0.01 * nominal_mm)
|
||||
if self._stroke <= min_stroke:
|
||||
raise KinematicsError(
|
||||
f"正运动学算出两指行程只有 {self._stroke:.6f} mm"
|
||||
f"（名义 {nominal_mm:.1f} mm）—— 耦合方向或 tip link 选错了："
|
||||
f"关节在动，但两指间距几乎不变"
|
||||
)
|
||||
self._check_monotonic()
|
||||
|
||||
# --- 基础量 -----------------------------------------------------------
|
||||
def _raw_separation(self, joint_value: float) -> float:
|
||||
angles = self.angles_of(joint_value)
|
||||
a = self.tree.position(self.tips[0], self.root, angles)
|
||||
b = self.tree.position(self.tips[1], self.root, angles)
|
||||
return 1000.0 * _dist(a, b)
|
||||
|
||||
def _check_monotonic(self, samples: int = 24) -> None:
|
||||
"""间距必须随主关节单调增 —— 否则反解会给出"看起来对但跳变"的结果。
|
||||
|
||||
真实夹爪（含 GN01）都满足；不满足说明耦合表填错了（比如某条
|
||||
multiplier 正负号写反），这时**必须报错**而不是继续，
|
||||
否则抓取时开口会忽大忽小。
|
||||
"""
|
||||
prev = None
|
||||
for i in range(samples + 1):
|
||||
v = self.lower + (self.upper - self.lower) * i / samples
|
||||
cur = self._raw_separation(v)
|
||||
if prev is not None and cur < prev - 1e-6:
|
||||
raise KinematicsError(
|
||||
f"指尖间距不随主关节单调变化（{v:.4f} 处 {cur:.3f} mm < "
|
||||
f"前一点 {prev:.3f} mm）—— 检查耦合表的正负号"
|
||||
)
|
||||
prev = cur
|
||||
|
||||
# --- 对外 -------------------------------------------------------------
|
||||
@property
|
||||
def closed_separation_mm(self) -> float:
|
||||
"""合拢时两侧 tip frame 之间的固有间距（GN01 约 27 mm）。
|
||||
|
||||
注意它不是开口：指垫此时贴在一起。验收脚本要拿它把"间距"换成
|
||||
"开口"，见 scripts/check_sim_gripper.py。
|
||||
"""
|
||||
return self._closed
|
||||
|
||||
@property
|
||||
def stroke_mm(self) -> float:
|
||||
"""两指的**实测**行程（毫米）。GN01 ≈ 99.93，而不是名义的 100.0。
|
||||
|
||||
这个差不是误差：上游把宽度关节的限位定在 0~0.100 m，而线性耦合
|
||||
让指尖实际只走了 99.93 mm。启动日志里报的是这个实测值 ——
|
||||
报名义值会让"全开时开口读数与几何对不上"变成一个查不出的矛盾。
|
||||
"""
|
||||
return self._stroke
|
||||
|
||||
def aperture_mm(self, joint_value: float) -> float:
|
||||
"""主关节值 → 开口（毫米）。
|
||||
|
||||
超出限位时按限位截断（位置控制器本来也会截断，这里提前对齐口径）。
|
||||
"""
|
||||
v = min(max(float(joint_value), self.lower), self.upper)
|
||||
return self._raw_separation(v) - self._closed
|
||||
|
||||
def joint_value(self, aperture_mm: float) -> float:
|
||||
"""开口（毫米）→ 主关节值。二分反解，超出行程按端点截断。"""
|
||||
want = min(max(float(aperture_mm), 0.0), self._stroke)
|
||||
lo, hi = self.lower, self.upper
|
||||
for _ in range(self.BISECT_ITERS):
|
||||
mid = 0.5 * (lo + hi)
|
||||
if self.aperture_mm(mid) < want:
|
||||
lo = mid
|
||||
else:
|
||||
hi = mid
|
||||
return 0.5 * (lo + hi)
|
||||
|
||||
def is_clamped(self, aperture_mm: float) -> bool:
|
||||
"""请求的开口是否超出这个夹爪能开的范围（用来提醒上层）。"""
|
||||
return not (0.0 <= float(aperture_mm) <= self._stroke)
|
||||
@@ -492,6 +492,55 @@ def _prepare(context, *args, **kwargs):
|
||||
]
|
||||
actions.extend(controllers)
|
||||
|
||||
# --- 6b) 仿真夹爪适配器 --------------------------------------------------
|
||||
# 为什么必须有它：sdformat 把 URDF 的 <mimic> 整个丢掉，gz-sim 也不实现，
|
||||
# 所以 6 个从动关节不会跟随主关节。适配器按耦合公式算出 7 个关节的目标值
|
||||
# 一起下发，并对外提供与真机**同名同类型**的 GripperExecute 服务
|
||||
#（<ns>/open|close|move|rotate + status），上层代码一行都不用改。
|
||||
#
|
||||
# 耦合表来自 report.stripped_mimics —— 也就是 urdf_normalize 删掉 <mimic>
|
||||
# 时记下来的那几张表。让 launch 传给适配器，而不是让适配器自己再展开一次
|
||||
# xacro：两次展开在参数不同时会静默不一致，而症状是"手指角度不对"。
|
||||
if load_gripper != "false":
|
||||
from arm_sim_gazebo.sim_gripper import format_mimic_param
|
||||
|
||||
mimics = [format_mimic_param(*t) for t in report.stripped_mimics]
|
||||
if not mimics:
|
||||
# 这里必须出声：拿不到耦合表说明夹爪没被加载（或上游改了耦合方式），
|
||||
# 而适配器会因此拒绝启动 —— 早知道比晚知道好。
|
||||
actions.append(
|
||||
LogInfo(
|
||||
msg="[gazebo_sim] ⚠ 归一化报告里没有 mimic 关系，"
|
||||
"仿真夹爪适配器将无法启动（它会明确报错）。"
|
||||
"检查 load_gripper 与 flexiv_description 的 grav 宏。"
|
||||
)
|
||||
)
|
||||
actions.append(
|
||||
TimerAction(
|
||||
# 等 gripper_controller 起来再发命令：控制器还没 active 时
|
||||
# 发出去的 /commands 会被丢弃（不报错），夹爪就一直停在初始位。
|
||||
period=CONTROLLER_DELAY + len(CONTROLLER_NAMES) * 2.0 + 1.0,
|
||||
actions=[
|
||||
Node(
|
||||
package="arm_sim_gazebo",
|
||||
executable="sim_gripper_adapter",
|
||||
name="sim_gripper_adapter",
|
||||
output="screen",
|
||||
parameters=[
|
||||
{
|
||||
"controllers_file": str(default_controllers(root)),
|
||||
"controller": "gripper_controller",
|
||||
"mimics": mimics,
|
||||
"robot_description": urdf,
|
||||
"namespace": "/gripper",
|
||||
},
|
||||
{"use_sim_time": True},
|
||||
],
|
||||
)
|
||||
],
|
||||
)
|
||||
)
|
||||
|
||||
actions.append(
|
||||
LogInfo(
|
||||
msg=(
|
||||
|
||||
@@ -46,6 +46,10 @@ setup(
|
||||
# arm_msgs/TactileStatus（轴向投影 + 基线相减 + 恒定发布）。
|
||||
# 由 gazebo_sim.launch.py 拉起；单独调试时可以用。
|
||||
'sim_tactile_adapter = arm_sim_gazebo.sim_tactile_adapter:main',
|
||||
# 仿真夹爪适配器：把 GripperExecute 服务（开口 mm）变成
|
||||
# 7 个夹爪关节的目标位置（含 mimic 耦合换算），让仿真能走
|
||||
# 真机那一套夹爪接口。由 gazebo_sim.launch.py 拉起。
|
||||
'sim_gripper_adapter = arm_sim_gazebo.sim_gripper_adapter:main',
|
||||
# 独立小工具：只看归一化做了什么，不启动 Gazebo。
|
||||
# 排查"为什么 Gazebo 说我的 link 不存在"时非常省时间。
|
||||
'urdf_normalize = arm_sim_gazebo.urdf_normalize:main',
|
||||
|
||||
@@ -0,0 +1,497 @@
|
||||
"""sim_gripper 的单测：耦合链、毫米↔关节值、命令向量、夹住判据。
|
||||
|
||||
这些测试锁定的都是**这个适配器真正会出错的地方**，不是凭空设想的边界情况：
|
||||
|
||||
1. GN01 的耦合是**一条链**，不是"6 个手指关节直挂主关节"。
|
||||
只套一层 multiplier 的实现看起来完全正常（夹爪会动、角度也是那个量级），
|
||||
只是指尖角度一直不对 —— 这种错误只能靠测试钉住。
|
||||
本文件第一个版本就踩了这个坑，所以下面有专门的链式测试。
|
||||
2. 命令数组的顺序取自 controllers.yaml，顺序错了 Gazebo 不报错，
|
||||
只是把角度发给别的关节。
|
||||
3. 米 / 毫米 差 1000 倍。仿真里 100 mm 行程写成 0.100，真机是 20 mm；
|
||||
混用不会报错，只会让夹爪"开到别的地方"。
|
||||
"""
|
||||
import os
|
||||
import sys
|
||||
|
||||
import pytest
|
||||
|
||||
sys.path.insert(0, os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
|
||||
|
||||
from arm_sim_gazebo.sim_gripper import ( # noqa: E402
|
||||
DEFAULT_CONTROLLER,
|
||||
DEFAULT_GRASP_TOLERANCE_MM,
|
||||
GripperConfigError,
|
||||
GripperLayout,
|
||||
MimicRelation,
|
||||
format_mimic_param,
|
||||
gripper_joint_order,
|
||||
is_grasping,
|
||||
joint_limits,
|
||||
layout_from_parts,
|
||||
parse_mimics,
|
||||
)
|
||||
|
||||
# 真实的 GN01 耦合表（从 flexiv_description 的 grav_macro.xacro 里读出来的）。
|
||||
# 注意它是链：finger_width → left_outer_knuckle → 其余 5 个。
|
||||
GN01_MIMICS = [
|
||||
("sim_left_outer_knuckle_joint", "sim_finger_width_joint", "9.404", "-0.155"),
|
||||
("sim_left_inner_knuckle_joint", "sim_left_outer_knuckle_joint", "1", "0"),
|
||||
("sim_left_inner_finger_joint", "sim_left_outer_knuckle_joint", "-1", "0"),
|
||||
("sim_right_outer_knuckle_joint", "sim_left_outer_knuckle_joint", "1", "0"),
|
||||
("sim_right_inner_knuckle_joint", "sim_left_outer_knuckle_joint", "1", "0"),
|
||||
("sim_right_inner_finger_joint", "sim_left_outer_knuckle_joint", "-1", "0"),
|
||||
]
|
||||
|
||||
GN01_ORDER = [
|
||||
"sim_finger_width_joint",
|
||||
"sim_left_outer_knuckle_joint",
|
||||
"sim_left_inner_knuckle_joint",
|
||||
"sim_left_inner_finger_joint",
|
||||
"sim_right_outer_knuckle_joint",
|
||||
"sim_right_inner_knuckle_joint",
|
||||
"sim_right_inner_finger_joint",
|
||||
]
|
||||
|
||||
# 与真实描述一致的最小 URDF 片段：主关节限位 0~0.100（米），语义是开口。
|
||||
URDF = (
|
||||
'<?xml version="1.0"?>\n<robot name="t">\n'
|
||||
' <joint name="sim_finger_width_joint" type="revolute">\n'
|
||||
' <parent link="a"/><child link="b"/>\n'
|
||||
' <limit lower="0" upper="0.100" effort="80" velocity="0.2"/>\n'
|
||||
' </joint>\n'
|
||||
"</robot>\n"
|
||||
)
|
||||
|
||||
|
||||
def _gn01_layout():
|
||||
return layout_from_parts(
|
||||
GN01_ORDER,
|
||||
parse_mimics([format_mimic_param(*t) for t in GN01_MIMICS]),
|
||||
URDF,
|
||||
)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# mimic 参数解析
|
||||
# ---------------------------------------------------------------------------
|
||||
class TestParseMimics:
|
||||
def test_round_trip(self):
|
||||
rel = parse_mimics([format_mimic_param(*GN01_MIMICS[0])])
|
||||
assert rel["sim_left_outer_knuckle_joint"].source == "sim_finger_width_joint"
|
||||
assert rel["sim_left_outer_knuckle_joint"].multiplier == pytest.approx(9.404)
|
||||
assert rel["sim_left_outer_knuckle_joint"].offset == pytest.approx(-0.155)
|
||||
|
||||
def test_negative_offset_survives_the_split(self):
|
||||
"""offset 是负数（-0.155）—— 分隔符与负号不能被搞混。"""
|
||||
rel = parse_mimics(["a:b:1:-0.5"])["a"]
|
||||
assert rel.offset == pytest.approx(-0.5)
|
||||
|
||||
@pytest.mark.parametrize(
|
||||
"bad",
|
||||
[
|
||||
"a:b:1", # 段数不够
|
||||
"a:b:1:0:0", # 段数太多
|
||||
"a::1:0", # 主关节名为空
|
||||
":b:1:0", # 从动关节名为空
|
||||
"a:b:x:0", # multiplier 不是数字
|
||||
"a:b:1:y", # offset 不是数字
|
||||
],
|
||||
)
|
||||
def test_malformed_entry_raises(self, bad):
|
||||
with pytest.raises(GripperConfigError):
|
||||
parse_mimics([bad])
|
||||
|
||||
def test_self_reference_raises(self):
|
||||
with pytest.raises(GripperConfigError, match="同名"):
|
||||
parse_mimics(["a:a:1:0"])
|
||||
|
||||
def test_duplicate_joint_raises(self):
|
||||
with pytest.raises(GripperConfigError, match="两次"):
|
||||
parse_mimics(["a:b:1:0", "a:b:2:0"])
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 控制器顺序
|
||||
# ---------------------------------------------------------------------------
|
||||
class TestGripperJointOrder:
|
||||
def test_reads_real_controllers_yaml(self):
|
||||
"""真配置里必须读到 7 个关节，且第一个是主关节。"""
|
||||
root = os.environ.get("ROS2_WS") or os.path.dirname(
|
||||
os.path.dirname(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
|
||||
)
|
||||
path = os.path.join(root, "devices/sim/gazebo/config/controllers.yaml")
|
||||
if not os.path.isfile(path):
|
||||
pytest.skip(f"找不到 {path}")
|
||||
order = gripper_joint_order(path, DEFAULT_CONTROLLER)
|
||||
assert len(order) == 7
|
||||
assert order[0] == "sim_finger_width_joint"
|
||||
assert len(set(order)) == len(order)
|
||||
|
||||
def test_missing_controller_raises(self, tmp_path):
|
||||
f = tmp_path / "c.yaml"
|
||||
f.write_text("other_controller:\n ros__parameters:\n joints: [a]\n", encoding="utf-8")
|
||||
with pytest.raises(GripperConfigError, match="ros__parameters"):
|
||||
gripper_joint_order(f, "gripper_controller")
|
||||
|
||||
def test_empty_joints_raises(self, tmp_path):
|
||||
f = tmp_path / "c.yaml"
|
||||
f.write_text("gripper_controller:\n ros__parameters:\n joints: []\n", encoding="utf-8")
|
||||
with pytest.raises(GripperConfigError, match="joints 为空"):
|
||||
gripper_joint_order(f, "gripper_controller")
|
||||
|
||||
def test_duplicate_joints_raise(self, tmp_path):
|
||||
f = tmp_path / "c.yaml"
|
||||
f.write_text(
|
||||
"gripper_controller:\n ros__parameters:\n joints: [a, a]\n", encoding="utf-8"
|
||||
)
|
||||
with pytest.raises(GripperConfigError, match="重复"):
|
||||
gripper_joint_order(f, "gripper_controller")
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 关节限位
|
||||
# ---------------------------------------------------------------------------
|
||||
class TestJointLimits:
|
||||
def test_reads_limits(self):
|
||||
assert joint_limits(URDF, "sim_finger_width_joint") == (0.0, pytest.approx(0.100))
|
||||
|
||||
def test_missing_joint_raises(self):
|
||||
with pytest.raises(GripperConfigError, match="找不到关节"):
|
||||
joint_limits(URDF, "sim_nope_joint")
|
||||
|
||||
def test_missing_limit_raises(self):
|
||||
xml = (
|
||||
'<robot><joint name="j" type="revolute"><limit effort="1" velocity="1"/>'
|
||||
"</joint></robot>"
|
||||
)
|
||||
with pytest.raises(GripperConfigError, match="upper"):
|
||||
joint_limits(xml, "j")
|
||||
|
||||
def test_inverted_limits_raise(self):
|
||||
xml = '<robot><joint name="j"><limit lower="1" upper="0"/></joint></robot>'
|
||||
with pytest.raises(GripperConfigError, match="限位不合法"):
|
||||
joint_limits(xml, "j")
|
||||
|
||||
def test_mimic_joint_is_rejected_as_source(self):
|
||||
"""从动关节不能当主关节 —— 选错了整条链都没人驱动。"""
|
||||
xml = (
|
||||
'<robot><joint name="j"><limit lower="0" upper="1"/><mimic joint="k"/></joint></robot>'
|
||||
)
|
||||
with pytest.raises(GripperConfigError, match="从动关节"):
|
||||
joint_limits(xml, "j")
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# layout 组装与校验
|
||||
# ---------------------------------------------------------------------------
|
||||
class TestLayoutFromParts:
|
||||
def test_gn01_layout(self):
|
||||
layout = _gn01_layout()
|
||||
assert layout.source == "sim_finger_width_joint"
|
||||
assert layout.lower == 0.0
|
||||
assert layout.upper == pytest.approx(0.100)
|
||||
assert layout.stroke_mm == pytest.approx(100.0)
|
||||
assert len(layout.order) == 7
|
||||
assert len(layout.relations) == 6
|
||||
|
||||
def test_follower_not_in_order_raises(self):
|
||||
rels = parse_mimics(["sim_ghost:sim_finger_width_joint:1:0"])
|
||||
with pytest.raises(GripperConfigError, match="不在控制器 joints"):
|
||||
layout_from_parts(GN01_ORDER, rels, URDF)
|
||||
|
||||
def test_source_not_in_order_raises(self):
|
||||
"""链上某一环的主关节没被控制器声明 —— 那一级角度没人驱动。"""
|
||||
rels = parse_mimics(["sim_left_outer_knuckle_joint:sim_missing_root:1:0"])
|
||||
with pytest.raises(GripperConfigError, match="主关节.*不在控制器"):
|
||||
layout_from_parts(GN01_ORDER, rels, URDF)
|
||||
|
||||
def test_multiple_roots_raise(self):
|
||||
"""两个关节都不带 mimic → 推断不出谁是被命令的那个，必须报错。"""
|
||||
with pytest.raises(GripperConfigError, match="唯一确定主关节"):
|
||||
layout_from_parts(GN01_ORDER, {}, URDF)
|
||||
|
||||
def test_cycle_raises(self):
|
||||
"""成环必须报错。
|
||||
|
||||
构造要小心：``layout_from_parts`` 先检查"不带 mimic 的关节是否唯一"，
|
||||
所以随手写两条互为 source 的关系通常会先撞上那个检查。这里用一组
|
||||
"除 root 外全都有 mimic" 的数据，才能真的走到成环检测。
|
||||
"""
|
||||
order = ["root", "a", "b"]
|
||||
urdf = (
|
||||
'<robot><joint name="root"><limit lower="0" upper="1"/></joint></robot>'
|
||||
)
|
||||
rels = parse_mimics(["a:b:1:0", "b:a:1:0"])
|
||||
with pytest.raises(GripperConfigError, match="成环"):
|
||||
layout_from_parts(order, rels, urdf)
|
||||
|
||||
def test_chain_not_reaching_root_raises(self):
|
||||
"""链走到一个"既不是主关节、也没有 mimic"的关节 → 描述不完整。
|
||||
|
||||
⚠️ 这条在 ``layout_from_parts`` 里**到不了**：那个"不带 mimic 的关节
|
||||
必须唯一"的检查已经把它挡住了（走到一个无 mimic 的非主关节，就说明
|
||||
存在第二个不带 mimic 的关节）。所以这里直接构造 ``GripperLayout``
|
||||
来钉住这个防御性分支 —— 免得将来有人调整校验顺序时把它变成真空洞。
|
||||
"""
|
||||
layout = GripperLayout(
|
||||
["root", "a"],
|
||||
{"a": MimicRelation("a", "elsewhere", 1.0, 0.0)},
|
||||
"root",
|
||||
0.0,
|
||||
1.0,
|
||||
)
|
||||
with pytest.raises(GripperConfigError, match="终止在"):
|
||||
layout.value_for("a", 0.5)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 耦合链：这是本模块最容易写错的地方
|
||||
# ---------------------------------------------------------------------------
|
||||
class TestCouplingChain:
|
||||
def test_chain_is_applied_level_by_level(self):
|
||||
"""两级链：先套靠近根的，再套靠近叶的。
|
||||
|
||||
left_inner_finger = -1 × left_outer_knuckle
|
||||
left_outer_knuckle = 9.404 × width - 0.155
|
||||
|
||||
只套一层（漏掉第二级）会得到 -1×(9.404w-0.155) 之外的值，
|
||||
这正是"夹爪会动但夹不准"的成因。
|
||||
"""
|
||||
layout = _gn01_layout()
|
||||
s = 0.04 # 40 mm 开口
|
||||
knuckle = 9.404 * s - 0.155
|
||||
assert layout.value_for("sim_left_inner_finger_joint", s) == pytest.approx(-knuckle)
|
||||
assert layout.value_for("sim_left_outer_knuckle_joint", s) == pytest.approx(knuckle)
|
||||
|
||||
def test_root_maps_written_limit_to_knuckle_limits(self):
|
||||
"""开口全开/全闭应当正好撞上 from-knuckle 关节的上下限。
|
||||
|
||||
这条把"绝对物理映射"这个口径钉住：如果哪天改成归一化映射，
|
||||
这里立刻会红。
|
||||
"""
|
||||
layout = _gn01_layout()
|
||||
assert layout.value_for("sim_left_outer_knuckle_joint", layout.upper) == pytest.approx(
|
||||
0.7854, abs=1e-4
|
||||
)
|
||||
assert layout.value_for("sim_left_outer_knuckle_joint", layout.lower) == pytest.approx(
|
||||
-0.155, abs=1e-4
|
||||
)
|
||||
|
||||
@pytest.mark.parametrize("aperture", [0.0, 5.0, 8.0, 20.0, 50.0, 100.0])
|
||||
def test_command_vector_matches_the_urdf_formulas(self, aperture):
|
||||
"""命令向量里的每个值都用**独立实现**重算一遍。
|
||||
|
||||
独立实现直接按原始耦合表递归求值（不依赖 GripperLayout），
|
||||
所以它能抓住"链的顺序搞反""只套一层""用错 source"这类错误。
|
||||
"""
|
||||
layout = _gn01_layout()
|
||||
raw = {
|
||||
j: (s, float(m), float(o))
|
||||
for j, s, m, o in GN01_MIMICS
|
||||
}
|
||||
|
||||
def expected(joint: str) -> float:
|
||||
value = layout.aperture_to_source(aperture)
|
||||
chain = []
|
||||
cur = joint
|
||||
while cur in raw:
|
||||
src, mult, off = raw[cur]
|
||||
chain.append((mult, off))
|
||||
cur = src
|
||||
for mult, off in reversed(chain):
|
||||
value = mult * value + off
|
||||
return value
|
||||
|
||||
vec = layout.command_vector(aperture)
|
||||
assert len(vec) == len(GN01_ORDER)
|
||||
for name, got in zip(GN01_ORDER, vec):
|
||||
assert got == pytest.approx(expected(name), abs=1e-12), f"{name} 对不上"
|
||||
|
||||
def test_vector_order_follows_controllers_yaml(self):
|
||||
"""数组顺序必须与传入的 joints 顺序一致（控制器按顺序取）。"""
|
||||
layout = _gn01_layout()
|
||||
vec = layout.command_vector(50.0)
|
||||
# 主关节在下标 0；把它改成另一个位置，向量里的值也必须跟着换位。
|
||||
assert vec[layout.source_index()] == pytest.approx(0.050)
|
||||
shuffled = [GN01_ORDER[3]] + [j for j in GN01_ORDER if j != GN01_ORDER[3]]
|
||||
other = layout_from_parts(
|
||||
shuffled,
|
||||
parse_mimics([format_mimic_param(*t) for t in GN01_MIMICS]),
|
||||
URDF,
|
||||
)
|
||||
assert other.command_vector(50.0)[0] == pytest.approx(vec[3])
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 毫米 ↔ 关节值
|
||||
# ---------------------------------------------------------------------------
|
||||
class TestApertureConversion:
|
||||
def test_round_trip(self):
|
||||
layout = _gn01_layout()
|
||||
for mm in (0.0, 7.5, 20.0, 100.0):
|
||||
joint = layout.aperture_to_source(mm)
|
||||
assert layout.source_to_aperture(joint) == pytest.approx(mm)
|
||||
|
||||
def test_clamped_at_both_ends(self):
|
||||
layout = _gn01_layout()
|
||||
assert layout.aperture_to_source(-5.0) == pytest.approx(0.0)
|
||||
assert layout.aperture_to_source(250.0) == pytest.approx(0.100)
|
||||
assert layout.is_clamped(-5.0)
|
||||
assert layout.is_clamped(250.0)
|
||||
assert not layout.is_clamped(50.0)
|
||||
|
||||
def test_mm_is_not_confused_with_m(self):
|
||||
"""100 mm 必须是 0.100 m，不是 100 m。差 1000 倍是这里唯一的量纲。"""
|
||||
layout = _gn01_layout()
|
||||
assert layout.aperture_to_source(100.0) == pytest.approx(0.100)
|
||||
assert layout.source_to_aperture(0.100) == pytest.approx(100.0)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 行程口径提示
|
||||
# ---------------------------------------------------------------------------
|
||||
class TestStrokeNote:
|
||||
def test_reports_the_real_difference(self):
|
||||
"""仿真 100 mm vs 真机 20 mm 必须**说出来**，不能默认一致。"""
|
||||
note = _gn01_layout().stroke_note(20.0)
|
||||
assert "不一致" in note
|
||||
assert "100.0" in note and "20.0" in note
|
||||
|
||||
def test_says_so_when_equal(self):
|
||||
note = _gn01_layout().stroke_note(100.0)
|
||||
assert "一致" in note and "不一致" not in note
|
||||
|
||||
def test_handles_missing_real_stroke(self):
|
||||
assert "无法对比" in _gn01_layout().stroke_note(None)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 夹住判据
|
||||
# ---------------------------------------------------------------------------
|
||||
class TestIsGrasping:
|
||||
"""夹住判据走**毫米开口**口径（不是主关节值）。
|
||||
|
||||
为什么强调口径：开口↔关节值在 FK 口径下是非线性的。用米当口径的话，
|
||||
"2 mm 的容差"在不同开度下含义不同 —— 标定出来的阈值换台夹爪就废了。
|
||||
"""
|
||||
|
||||
def test_blocked_at_lower_is_grasping(self):
|
||||
"""指令到合拢位，实际开口停在 20 mm → 夹住了东西。"""
|
||||
assert is_grasping(0.0, 20.0)
|
||||
|
||||
def test_reached_lower_is_not_grasping(self):
|
||||
assert not is_grasping(0.0, 0.0)
|
||||
|
||||
def test_far_from_lower_is_not_grasping(self):
|
||||
"""目标是全开（100 mm）—— 实际只走了 80 mm 只是还没走到，不是夹住。"""
|
||||
assert not is_grasping(100.0, 80.0)
|
||||
|
||||
def test_within_tolerance_is_not_grasping(self):
|
||||
"""开口差在容差内不算夹住（否则控制精度差一点就误报夹住）。"""
|
||||
assert not is_grasping(0.0, DEFAULT_GRASP_TOLERANCE_MM * 0.5)
|
||||
|
||||
def test_respects_custom_tolerance(self):
|
||||
assert not is_grasping(0.0, 3.0, tolerance_mm=5.0)
|
||||
assert is_grasping(0.0, 3.0, tolerance_mm=1.0)
|
||||
|
||||
def test_realistic_case(self):
|
||||
"""抓 20 mm 通止规：指令合拢，实际停在 20 mm 附近 → 夹住。"""
|
||||
assert is_grasping(0.0, 20.0)
|
||||
assert not is_grasping(0.0, 1.0) # 只是没走到
|
||||
assert not is_grasping(50.0, 20.0) # 目标是 50 mm，谈不上"夹"
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 开口反馈源：为什么不能取主关节
|
||||
# ---------------------------------------------------------------------------
|
||||
class TestFeedbackJoint:
|
||||
"""锁住一个**实测**出来的坑，不是设想。
|
||||
|
||||
2026-09-24 实测（Gazebo Fortress + gz_ros2_control）：把
|
||||
``finger_width_joint`` 命令到 0.100，它的 /joint_states 值恒为 ~0
|
||||
（1.4e-08），而 6 个从动关节准确到位（0.7854 = 9.404×0.100-0.155）。
|
||||
|
||||
原因是上游描述里它挂在一个孤立的 ``unused_finger_width_link`` 上
|
||||
（那个 link 的注释写着 "cannot have 0 mass"，纯粹为了 URDF 合法），
|
||||
和手指没有机械连接，Gazebo 里推不动也测不着。
|
||||
|
||||
拿它当反馈的后果是**静默失效**：夹爪明明开着，status 永远报"开口 0 mm"。
|
||||
"""
|
||||
|
||||
def test_feedback_is_not_the_source_joint(self):
|
||||
layout = _gn01_layout()
|
||||
assert layout.feedback != layout.source
|
||||
|
||||
def test_picks_the_direct_follower_with_biggest_gain(self):
|
||||
"""与主关节直接相连的只有 left_outer_knuckle（增益 9.404），选它。"""
|
||||
assert _gn01_layout().feedback == "sim_left_outer_knuckle_joint"
|
||||
|
||||
def test_feedback_value_is_the_driven_command(self):
|
||||
"""反馈关节的目标值就是命令向量里发给它的那个值。"""
|
||||
layout = _gn01_layout()
|
||||
source = layout.aperture_to_source(30.0)
|
||||
assert layout.feedback_value(source) == pytest.approx(
|
||||
layout.value_for(layout.feedback, source)
|
||||
)
|
||||
|
||||
def test_aperture_can_be_recovered_from_the_follower(self):
|
||||
"""从从动关节反推开口，和直接换算必须一致（否则开口读数会整体偏）。"""
|
||||
layout = _gn01_layout()
|
||||
for mm in (0.0, 8.0, 33.0, 100.0):
|
||||
source = layout.aperture_to_source(mm)
|
||||
follower = layout.feedback_value(source)
|
||||
assert layout.aperture_from_feedback(follower) == pytest.approx(mm)
|
||||
|
||||
def test_source_from_feedback_inverts_the_relation(self):
|
||||
layout = _gn01_layout()
|
||||
rel = layout.relations[layout.feedback]
|
||||
assert layout.source_from_feedback(rel.apply(0.037)) == pytest.approx(0.037)
|
||||
|
||||
def test_moving_detection_uses_the_follower_not_the_source(self):
|
||||
"""端到端复现那个坑：用主关节值（恒 0）判"在动"会永远说"在动"。
|
||||
|
||||
生产路径在适配器里：开口 = aperture_from_feedback(反馈关节状态)，
|
||||
再和 target 的开口比。这里把那条链路走一遍。
|
||||
"""
|
||||
layout = _gn01_layout()
|
||||
target = layout.aperture_to_source(100.0) # 全开
|
||||
target_mm = layout.source_to_aperture(target)
|
||||
# 主关节实测恒为 0 → 若拿它当反馈，开口会被算成 0 mm，与 100 mm 差一大截
|
||||
assert abs(layout.source_to_aperture(0.0) - target_mm) > DEFAULT_GRASP_TOLERANCE_MM
|
||||
# 用反馈关节：已经在全开位，开口差为 0
|
||||
follower = layout.feedback_value(target)
|
||||
assert abs(layout.aperture_from_feedback(follower) - target_mm) == pytest.approx(0.0)
|
||||
|
||||
def test_single_joint_gripper_falls_back_to_the_source(self):
|
||||
"""没有耦合关系的单关节夹爪：反馈只能是主关节自己。"""
|
||||
layout = layout_from_parts(
|
||||
["sim_only_joint"], {}, URDF.replace("sim_finger_width_joint", "sim_only_joint")
|
||||
)
|
||||
assert layout.feedback == layout.source
|
||||
assert layout.feedback_value(0.02) == pytest.approx(0.02)
|
||||
assert layout.aperture_from_feedback(0.02) == pytest.approx(20.0)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 口径：线性近似 vs 正运动学
|
||||
# ---------------------------------------------------------------------------
|
||||
class TestApertureCurveFallback:
|
||||
"""测试用的最小 URDF 里没有指尖 link，所以走**线性近似**这条路。
|
||||
|
||||
这不是缺陷而是刻意的：真实 GN01 的 FK 曲线与 Gazebo 的对照表在
|
||||
test_sim_gripper_kin.py 里；这里只保证"没有 FK 时行为不变、且口径
|
||||
在日志里说明白了"。
|
||||
"""
|
||||
|
||||
def test_no_tip_links_means_linear_fallback(self):
|
||||
layout = _gn01_layout()
|
||||
assert layout.curve is None
|
||||
assert layout.closed_separation_mm is None
|
||||
assert "线性近似" in layout.kin_note
|
||||
# 线性口径下行程 = 限位区间
|
||||
assert layout.stroke_mm == pytest.approx(100.0)
|
||||
assert layout.aperture_to_source(20.0) == pytest.approx(0.020)
|
||||
|
||||
def test_describe_mentions_the_口径(self):
|
||||
assert "线性近似" in _gn01_layout().describe()
|
||||
@@ -0,0 +1,288 @@
|
||||
"""sim_gripper_kin 的单测：URDF 正运动学 vs Gazebo 实测（黄金表）。
|
||||
|
||||
=============================================================================
|
||||
这个文件里最值钱的东西是 GOLDEN 那张表
|
||||
=============================================================================
|
||||
它是 2026-09-24 在 Gazebo Fortress + gz_ros2_control 上**实测**出来的：
|
||||
启动仿真、用 GripperExecute 依次开合、再查 /joint_states 与 TF 量两指间距。
|
||||
|
||||
命令开口(主关节值) 请求开口 Gazebo 实测指尖间距
|
||||
0.000 m 0 mm 27.09 mm
|
||||
0.008 m 8 mm 35.76 mm
|
||||
0.020 m 20 mm 48.84 mm
|
||||
0.050 m 50 mm 80.96 mm
|
||||
0.100 m 100 mm 127.02 mm
|
||||
|
||||
本文件用**自己的矩阵乘法**算同一批数字，要求吻合到 0.05 mm。
|
||||
两者完全独立（一个是物理引擎 + sdformat 转换，一个是我们的 FK），
|
||||
所以这个对照同时钉住了三件事：
|
||||
|
||||
1. URDF → SDF 的转换没有丢掉手指链上的偏移；
|
||||
2. 我们对耦合公式（含 9.404/-0.155 那条链）的理解与 Gazebo 实际执行一致；
|
||||
3. 开口↔关节值必须走反解而不是线性换算 —— 表里 20 mm 那行差 1.75 mm，
|
||||
线性口径会让"抓 20 mm 工件"在仿真里抓空。
|
||||
|
||||
以后改耦合表、换 Gazebo 版本、换夹爪描述后，如果这张表开始红，
|
||||
说明仿真里的手指几何真的变了 —— 那时必须重新实测并更新表，
|
||||
而不是把容差放大。
|
||||
"""
|
||||
import os
|
||||
import sys
|
||||
|
||||
import pytest
|
||||
|
||||
sys.path.insert(0, os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
|
||||
|
||||
from arm_sim_gazebo.sim_gripper_kin import ( # noqa: E402
|
||||
ApertureCurve,
|
||||
KinematicsError,
|
||||
UrdfTree,
|
||||
find_tip_pair,
|
||||
)
|
||||
|
||||
# (主关节值 m, 请求开口 mm, Gazebo 实测指尖间距 mm)
|
||||
GOLDEN = [
|
||||
(0.000, 0, 27.09),
|
||||
(0.008, 8, 35.76),
|
||||
(0.020, 20, 48.84),
|
||||
(0.050, 50, 80.96),
|
||||
(0.100, 100, 127.02),
|
||||
]
|
||||
|
||||
#: 与 Gazebo 的允许差（毫米）。实测最高吻合到 0.01 mm，留 0.05 的余量。
|
||||
TOL_MM = 0.05
|
||||
|
||||
# GN01 的耦合链（与 flexiv_description 的 grav_macro.xacro 一致）
|
||||
MIMICS = {
|
||||
"sim_left_outer_knuckle_joint": ("sim_finger_width_joint", 9.404, -0.155),
|
||||
"sim_left_inner_knuckle_joint": ("sim_left_outer_knuckle_joint", 1.0, 0.0),
|
||||
"sim_left_inner_finger_joint": ("sim_left_outer_knuckle_joint", -1.0, 0.0),
|
||||
"sim_right_outer_knuckle_joint": ("sim_left_outer_knuckle_joint", 1.0, 0.0),
|
||||
"sim_right_inner_knuckle_joint": ("sim_left_outer_knuckle_joint", 1.0, 0.0),
|
||||
"sim_right_inner_finger_joint": ("sim_left_outer_knuckle_joint", -1.0, 0.0),
|
||||
}
|
||||
|
||||
ORDER = [
|
||||
"sim_finger_width_joint",
|
||||
"sim_left_outer_knuckle_joint",
|
||||
"sim_left_inner_knuckle_joint",
|
||||
"sim_left_inner_finger_joint",
|
||||
"sim_right_outer_knuckle_joint",
|
||||
"sim_right_inner_knuckle_joint",
|
||||
"sim_right_inner_finger_joint",
|
||||
]
|
||||
|
||||
# 一个"够像 GN01"的最小 URDF：只保留手指链上的 offset，
|
||||
# 数字全部取自归一化后的真实描述（/tmp/norm.urdf 同源）。
|
||||
FINGER_URDF = """<?xml version="1.0"?>
|
||||
<robot name="gn01_finger_chain">
|
||||
<!-- 下面 6 条 joint 是**从真实归一化描述里逐字抄下来**的手指链
|
||||
（devices/sim 的 GN01 夹爪，flexiv_description 的 grav 宏）。
|
||||
|
||||
为什么抄而不是自己写一个"像 GN01"的：这一版测试最初是手写几何，
|
||||
结果和 Gazebo 实测差了 78 mm —— 手写时把 knuckle 的安装偏移
|
||||
(0 -0.0325 0.0825) 和右侧那条 rpy="0 0 π" 的镜像都猜错了。
|
||||
黄金表要能证明"我们的 FK == 物理引擎"，夹具就必须是真几何。 -->
|
||||
<link name="sim_grav_base_link"/>
|
||||
<link name="sim_left_outer_bar"/>
|
||||
<link name="sim_left_finger_mount"/>
|
||||
<link name="sim_left_finger_tip"/>
|
||||
<link name="sim_right_outer_bar"/>
|
||||
<link name="sim_right_finger_mount"/>
|
||||
<link name="sim_right_finger_tip"/>
|
||||
<joint name="sim_left_outer_knuckle_joint" type="revolute"><origin rpy="0 0 0" xyz="0 -0.0325 0.0825" /><parent link="sim_grav_base_link" /><child link="sim_left_outer_bar" /><axis xyz="1 0 0" /><limit effort="80" lower="-0.155" upper="0.7854" velocity="2.0" /></joint>
|
||||
<joint name="sim_left_inner_finger_joint" type="revolute"><origin rpy="0 0 0" xyz="0 0 0.058" /><parent link="sim_left_outer_bar" /><child link="sim_left_finger_mount" /><axis xyz="1 0 0" /><limit effort="80" lower="-0.7854" upper="0.155" velocity="2.0" /></joint>
|
||||
<joint name="sim_left_inner_finger_pad_joint" type="fixed"><origin rpy="0 0 0" xyz="0 0.010 0.0235" /><parent link="sim_left_finger_mount" /><child link="sim_left_finger_tip" /><axis xyz="0 0 1" /></joint>
|
||||
<joint name="sim_right_outer_knuckle_joint" type="revolute"><origin rpy="0 0 3.141592653589793" xyz="0 0.0325 0.0825" /><parent link="sim_grav_base_link" /><child link="sim_right_outer_bar" /><axis xyz="1 0 0" /><limit effort="80" lower="-0.155" upper="0.7854" velocity="2.0" /></joint>
|
||||
<joint name="sim_right_inner_finger_joint" type="revolute"><origin rpy="0 0 0" xyz="0 0 0.058" /><parent link="sim_right_outer_bar" /><child link="sim_right_finger_mount" /><axis xyz="1 0 0" /><limit effort="80" lower="-0.7854" upper="0.155" velocity="2.0" /></joint>
|
||||
<joint name="sim_right_inner_finger_pad_joint" type="fixed"><origin rpy="0 0 0" xyz="0 0.010 0.0235" /><parent link="sim_right_finger_mount" /><child link="sim_right_finger_tip" /><axis xyz="0 0 1" /></joint>
|
||||
<joint name="sim_finger_width_joint" type="revolute">
|
||||
<parent link="sim_grav_base_link"/><child link="sim_unused_finger_width_link"/>
|
||||
<axis xyz="1 0 0"/><limit lower="0" upper="0.100" effort="80" velocity="0.2"/>
|
||||
</joint>
|
||||
<link name="sim_unused_finger_width_link"/>
|
||||
</robot>
|
||||
"""
|
||||
|
||||
def _angles_of(main_value: float):
|
||||
"""按耦合链算每个关节的角度（GN01 那条链）。"""
|
||||
|
||||
def value_for(joint: str):
|
||||
if joint == "sim_finger_width_joint":
|
||||
return main_value
|
||||
src, mul, off = MIMICS[joint]
|
||||
return mul * value_for(src) + off
|
||||
|
||||
return {j: value_for(j) for j in ORDER}
|
||||
|
||||
|
||||
def _curve(angles_of=_angles_of, xml: str = FINGER_URDF, upper: float = 0.100):
|
||||
"""建一条开口曲线。``angles_of`` 可替换，用来做"耦合表写错"的负例。"""
|
||||
tree = UrdfTree(xml)
|
||||
tips = find_tip_pair(tree)
|
||||
assert tips is not None
|
||||
return ApertureCurve(tree, tips, angles_of, lower=0.0, upper=upper)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 树结构
|
||||
# ---------------------------------------------------------------------------
|
||||
class TestUrdfTree:
|
||||
def test_common_ancestor(self):
|
||||
tree = UrdfTree(FINGER_URDF)
|
||||
assert tree.common_ancestor("sim_left_finger_tip", "sim_right_finger_tip") == \
|
||||
"sim_grav_base_link"
|
||||
|
||||
def test_chain_order_is_root_first(self):
|
||||
tree = UrdfTree(FINGER_URDF)
|
||||
names = [j.name for j in tree.chain("sim_left_finger_tip", "sim_grav_base_link")]
|
||||
assert names == [
|
||||
"sim_left_outer_knuckle_joint",
|
||||
"sim_left_inner_finger_joint",
|
||||
"sim_left_inner_finger_pad_joint",
|
||||
]
|
||||
|
||||
def test_duplicate_child_is_not_a_tree(self):
|
||||
bad = FINGER_URDF.replace(
|
||||
'<child link="sim_left_finger_tip" />',
|
||||
'<child link="sim_left_finger_mount" />',
|
||||
1,
|
||||
)
|
||||
with pytest.raises(KinematicsError):
|
||||
UrdfTree(bad)
|
||||
|
||||
def test_zero_axis_is_rejected(self):
|
||||
"""axis 为零向量 → 无法确定转动轴，必须报错而不是算出 NaN。"""
|
||||
bad = FINGER_URDF.replace(
|
||||
'<axis xyz="1 0 0" /><limit effort="80" lower="-0.155"',
|
||||
'<axis xyz="0 0 0" /><limit effort="80" lower="-0.155"',
|
||||
1,
|
||||
)
|
||||
tree = UrdfTree(bad)
|
||||
with pytest.raises(KinematicsError):
|
||||
tree.position("sim_left_finger_tip", "sim_grav_base_link", {})
|
||||
|
||||
|
||||
class TestFindTipPair:
|
||||
def test_finds_left_and_right(self):
|
||||
assert find_tip_pair(UrdfTree(FINGER_URDF)) == (
|
||||
"sim_left_finger_tip",
|
||||
"sim_right_finger_tip",
|
||||
)
|
||||
|
||||
def test_returns_none_when_convention_not_met(self):
|
||||
xml = FINGER_URDF.replace("sim_left_finger_tip", "sim_a").replace(
|
||||
"sim_right_finger_tip", "sim_b"
|
||||
)
|
||||
assert find_tip_pair(UrdfTree(xml)) is None
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 黄金表：我们的 FK vs Gazebo 的物理
|
||||
# ---------------------------------------------------------------------------
|
||||
class TestGoldenAgainstGazebo:
|
||||
def test_separation_matches_gazebo_everywhere(self):
|
||||
curve = _curve()
|
||||
for source, _request, gazebo_mm in GOLDEN:
|
||||
mine = curve.aperture_mm(source) + curve.closed_separation_mm
|
||||
assert abs(mine - gazebo_mm) < TOL_MM, (
|
||||
f"主关节 {source} m：FK 算得 {mine:.3f} mm，"
|
||||
f"Gazebo 实测 {gazebo_mm:.2f} mm"
|
||||
)
|
||||
|
||||
def test_closed_separation(self):
|
||||
"""合拢时两指尖间距 27.09 mm（指垫此时贴在一起，所以它不是开口）。"""
|
||||
assert _curve().closed_separation_mm == pytest.approx(27.09, abs=TOL_MM)
|
||||
|
||||
def test_stroke_is_measured_not_nominal(self):
|
||||
"""实测行程 99.93 mm，不是名义的 100.0 —— 差在非线性上，不是误差。"""
|
||||
assert _curve().stroke_mm == pytest.approx(99.93, abs=0.05)
|
||||
|
||||
def test_linear_mapping_would_be_wrong(self):
|
||||
"""把"线性口径错在哪"钉成断言，免得以后有人又把它改回去。
|
||||
|
||||
线性口径下 20 mm 请求 → 主关节 0.020 → 实测间距 48.84 mm，
|
||||
即开口 21.75 mm，比请求多 1.75 mm。
|
||||
"""
|
||||
curve = _curve()
|
||||
linear_aperture = (0.020 - 0.0) * 1000.0 # 线性口径认为它是 20.00
|
||||
real = curve.aperture_mm(0.020) # 实际是 21.75
|
||||
assert linear_aperture == pytest.approx(20.0)
|
||||
assert real == pytest.approx(21.75, abs=TOL_MM)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 反解
|
||||
# ---------------------------------------------------------------------------
|
||||
class TestApertureInversion:
|
||||
def test_round_trip(self):
|
||||
curve = _curve()
|
||||
for mm in (0.0, 0.5, 8.0, 20.0, 33.3, 50.0, 99.0):
|
||||
joint = curve.joint_value(mm)
|
||||
assert curve.aperture_mm(joint) == pytest.approx(mm, abs=1e-6)
|
||||
|
||||
def test_inverted_values_are_smaller_than_linear(self):
|
||||
"""中段反解出来的关节值必须**小于**线性值（非线性开口更快）。"""
|
||||
curve = _curve()
|
||||
for mm in (8.0, 20.0, 50.0):
|
||||
assert curve.joint_value(mm) < mm / 1000.0
|
||||
|
||||
def test_clamps_at_both_ends(self):
|
||||
curve = _curve()
|
||||
assert curve.joint_value(-5.0) == pytest.approx(0.0)
|
||||
assert curve.aperture_mm(curve.joint_value(-5.0)) == pytest.approx(0.0)
|
||||
assert curve.joint_value(500.0) == pytest.approx(0.100)
|
||||
assert curve.is_clamped(-5.0) and curve.is_clamped(500.0)
|
||||
assert not curve.is_clamped(50.0)
|
||||
|
||||
def test_grasping_a_20mm_part_actually_touches(self):
|
||||
"""这条是"为什么要做 FK 反解"的验收：抓 20 mm 工件时指垫真的贴住。
|
||||
|
||||
判据：命令开口 20 mm → 反解出的关节值 → 正运动学算回的开口 == 20 mm。
|
||||
线性口径下这一步会是 21.75 mm（差 1.75 mm，等于没碰到工件）。
|
||||
"""
|
||||
curve = _curve()
|
||||
joint = curve.joint_value(20.0)
|
||||
assert curve.aperture_mm(joint) == pytest.approx(20.0, abs=1e-6)
|
||||
# 顺带把"线性口径会差多少"写在这里，读代码的人一眼能看到动机
|
||||
assert curve.aperture_mm(0.020) - 20.0 > 1.5
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 老实报错：不单调 / 行程为 0
|
||||
# ---------------------------------------------------------------------------
|
||||
class TestRefusesBadCoupling:
|
||||
def test_wrong_coupling_direction_is_rejected(self):
|
||||
"""耦合表方向写错 → 指尖不再相向运动 → 必须报错。
|
||||
|
||||
做法：把右侧 knuckle 的符号翻掉（镜像丢了）。实测这种情况下两指
|
||||
**同步**摆动，间距恒定在 45.00 mm —— 关节都在动、`/joint_states`
|
||||
一切正常，只有"开口"这个量不动。
|
||||
|
||||
⚠️ 这一条正是"静默失效"的典型：开口曲线水平时，
|
||||
``joint_value()`` 的二分会一路收敛到下限，"开 20 mm"变成"不动"，
|
||||
而且没有任何报错。所以曲线构造期就必须拒绝它 —— 判据不是
|
||||
``stroke > 0``（这个例子的浮点差是 4e-13，能骗过 ``> 0``），
|
||||
而是"行程至少占名义行程的 1%"。
|
||||
"""
|
||||
|
||||
def wrong(main_value: float):
|
||||
left_knuckle = 9.404 * main_value - 0.155
|
||||
return {
|
||||
"sim_finger_width_joint": main_value,
|
||||
"sim_left_outer_knuckle_joint": left_knuckle,
|
||||
"sim_left_inner_finger_joint": -left_knuckle,
|
||||
"sim_right_outer_knuckle_joint": -left_knuckle, # ← 少了镜像
|
||||
"sim_right_inner_finger_joint": left_knuckle,
|
||||
}
|
||||
|
||||
with pytest.raises(KinematicsError):
|
||||
_curve(angles_of=wrong)
|
||||
|
||||
def test_zero_stroke_is_rejected(self):
|
||||
"""主关节取任何值都不动（耦合表空了）→ 报错，而不是"所有开口都是 0"。"""
|
||||
tree = UrdfTree(FINGER_URDF)
|
||||
tips = find_tip_pair(tree)
|
||||
assert tips is not None
|
||||
with pytest.raises(KinematicsError):
|
||||
ApertureCurve(tree, tips, lambda _v: {}, 0.0, 0.100)
|
||||
Reference in New Issue
Block a user