From 6922b181870b7e2e9c23d3be283cd4d4f7892644 Mon Sep 17 00:00:00 2001 From: arm_dev Date: Thu, 24 Sep 2026 09:46:09 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BB=BF=E7=9C=9F=E5=A4=B9=E7=88=AA=E9=80=82?= =?UTF-8?q?=E9=85=8D=E5=99=A8=EF=BC=9A=E6=9C=8D=E5=8A=A1=E6=8E=A5=E5=8F=A3?= =?UTF-8?q?=E5=AF=B9=E9=BD=90=E7=9C=9F=E6=9C=BA=EF=BC=8C=E5=BC=80=E5=8F=A3?= =?UTF-8?q?=E6=8C=89=E6=AD=A3=E8=BF=90=E5=8A=A8=E5=AD=A6=E6=8D=A2=E7=AE=97?= =?UTF-8?q?=E6=88=90=E7=9C=9F=E6=AF=AB=E7=B1=B3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 为什么 ------ 应用层只认 arm_msgs/srv/GripperExecute（开口毫米），真机与仿真必须同一套接口。 但 GN01 的 URDF 里 是**线性近似**：直接按线性系数下发 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 --- devices/sim/gazebo/README.md | 95 ++- docs/FRAMEWORK_RULES.md | 47 +- docs/HANDBOOK.md | 48 +- scripts/check_sim_gripper.py | 411 ++++++++++++ scripts/lib/sim_procs.sh | 174 +++++ scripts/run_sim_gz.sh | 60 +- scripts/stop_sim_gz.sh | 127 ++-- .../arm_sim_gazebo/sim_gripper.py | 618 ++++++++++++++++++ .../arm_sim_gazebo/sim_gripper_adapter.py | 424 ++++++++++++ .../arm_sim_gazebo/sim_gripper_kin.py | 368 +++++++++++ .../launch/gazebo_sim.launch.py | 49 ++ src/arm_sim_gazebo/setup.py | 4 + src/arm_sim_gazebo/test/test_sim_gripper.py | 497 ++++++++++++++ .../test/test_sim_gripper_kin.py | 288 ++++++++ 14 files changed, 3118 insertions(+), 92 deletions(-) create mode 100644 scripts/check_sim_gripper.py create mode 100644 scripts/lib/sim_procs.sh create mode 100644 src/arm_sim_gazebo/arm_sim_gazebo/sim_gripper.py create mode 100644 src/arm_sim_gazebo/arm_sim_gazebo/sim_gripper_adapter.py create mode 100644 src/arm_sim_gazebo/arm_sim_gazebo/sim_gripper_kin.py create mode 100644 src/arm_sim_gazebo/test/test_sim_gripper.py create mode 100644 src/arm_sim_gazebo/test/test_sim_gripper_kin.py diff --git a/devices/sim/gazebo/README.md b/devices/sim/gazebo/README.md index 074e32a..d4b3b64 100644 --- a/devices/sim/gazebo/README.md +++ b/devices/sim/gazebo/README.md @@ -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 用一条**线性** `` 耦合把 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:` 顺序、 +归一化时删下来的 `` 表、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`） diff --git a/docs/FRAMEWORK_RULES.md b/docs/FRAMEWORK_RULES.md index 7d43d71..0fd8fe6 100644 --- a/docs/FRAMEWORK_RULES.md +++ b/docs/FRAMEWORK_RULES.md @@ -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，夹不住工件 | 上游 `` 是线性近似，开口必须走正运动学反解 | R4.4、坑 12 | +| 残留进程把旧相机外参按在新仿真上 | `static_transform_publisher` 发的是 latched `/tf_static` | R4.11 | --- diff --git a/docs/HANDBOOK.md b/docs/HANDBOOK.md index 390d7a3..46b6e6e 100644 --- a/docs/HANDBOOK.md +++ b/docs/HANDBOOK.md @@ -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 | diff --git a/scripts/check_sim_gripper.py b/scripts/check_sim_gripper.py new file mode 100644 index 0000000..b7e2a1b --- /dev/null +++ b/scripts/check_sim_gripper.py @@ -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()) diff --git a/scripts/lib/sim_procs.sh b/scripts/lib/sim_procs.sh new file mode 100644 index 0000000..df99322 --- /dev/null +++ b/scripts/lib/sim_procs.sh @@ -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命令行" +# 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 -` 一次性带走整组， +# 不依赖任何模式串。模式扫描退化成兜底（对付 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命令行"。排除本进程/祖先/扫描用的 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) +} diff --git a/scripts/run_sim_gz.sh b/scripts/run_sim_gz.sh index fdfee7d..178a288 100755 --- a/scripts/run_sim_gz.sh +++ b/scripts/run_sim_gz.sh @@ -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}" diff --git a/scripts/stop_sim_gz.sh b/scripts/stop_sim_gz.sh index dba967a..754285d 100755 --- a/scripts/stop_sim_gz.sh +++ b/scripts/stop_sim_gz.sh @@ -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命令行"，供两次调用（收集 / 复查）共用。 -# -# 两处必须共用同一个实现，否则"复查用的模式"和"杀的时候用的模式"会各自漂移， -# 而漂移的表现就是"说已停止、其实有残留"——正是这个脚本要防的事。 -# -# ⚠️ 模式串用**环境变量**传给 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 diff --git a/src/arm_sim_gazebo/arm_sim_gazebo/sim_gripper.py b/src/arm_sim_gazebo/arm_sim_gazebo/sim_gripper.py new file mode 100644 index 0000000..6438a7f --- /dev/null +++ b/src/arm_sim_gazebo/arm_sim_gazebo/sim_gripper.py @@ -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 的 ```` **整个丢掉**，gz-sim 也不实现 mimic。 +所以仿真里 6 个从动关节不会跟随主关节，必须由适配器算出角度后**一起下发**。 +``urdf_normalize`` 在归一化时把 ```` 删掉（避免 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: + """一条 ```` 耦合关系：从动关节 = 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 里读 ``.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 + 里真实的行程）。再配一份等于给自己一个"改了描述没改配置"的机会， + 而症状是开口悄悄不按预期走。 + """ + # 只认 那一段，避免匹配到别的关节。 + pat = re.compile( + r"]*\bname\s*=\s*[\"']%s[\"'][^>]*>(.*?)" + % 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"），不能当主关节。" + ) + lm = re.search(r"]*>", body) + if not lm: + raise GripperConfigError(f"关节 {joint_name!r} 的 标签缺失。") + + 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} 的 缺 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) diff --git a/src/arm_sim_gazebo/arm_sim_gazebo/sim_gripper_adapter.py b/src/arm_sim_gazebo/arm_sim_gazebo/sim_gripper_adapter.py new file mode 100644 index 0000000..5a77a59 --- /dev/null +++ b/src/arm_sim_gazebo/arm_sim_gazebo/sim_gripper_adapter.py @@ -0,0 +1,424 @@ +"""仿真夹爪适配器：``GripperExecute`` 服务 → ``JointGroupPositionController``。 + +============================================================================= +它在链路上的位置 +============================================================================= + 应用层（P6 状态机 / 现场脚本） + │ 服务 /open | /close | /move | /rotate + │ （arm_msgs/srv/GripperExecute，与真机**同名同类型**） + ▼ + 本节点 + │ ① 开口毫米 → 主关节值（绝对物理映射，见 sim_gripper.py 文件头） + │ ② 按 mimic 耦合算出 6 个从动关节的目标角 + │ ③ 按 controllers.yaml 的 joints 顺序拼成数组 + ▼ + 话题 /commands (std_msgs/Float64MultiArray) + ▼ + JointGroupPositionController（gz_ros2_control，在 Gazebo 进程里） + ▼ + Gazebo 里的 7 个 sim_* 夹爪关节 + + 同时订阅 /joint_states，把**反馈关节**的实际值换算回开口毫米， + 发布到 /status（arm_msgs/GripperStatus）。 + 注意不是主关节 —— 主关节在 Gazebo 里压根不动，见下面第 4 个坑。 + +============================================================================= +为什么必须由适配器驱动全部 7 个关节 +============================================================================= +sdformat 把 URDF 的 ```` 整个丢掉，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 留空：真机那边填的是 "_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() diff --git a/src/arm_sim_gazebo/arm_sim_gazebo/sim_gripper_kin.py b/src/arm_sim_gazebo/arm_sim_gazebo/sim_gripper_kin.py new file mode 100644 index 0000000..3d4c9e2 --- /dev/null +++ b/src/arm_sim_gazebo/arm_sim_gazebo/sim_gripper_kin.py @@ -0,0 +1,368 @@ +"""由 URDF 做夹爪正运动学，把"开口毫米"换算成**真的**关节角（纯逻辑，不起 ROS）。 + +============================================================================= +为什么需要它：上游的 是线性近似 +============================================================================= +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) diff --git a/src/arm_sim_gazebo/launch/gazebo_sim.launch.py b/src/arm_sim_gazebo/launch/gazebo_sim.launch.py index 3bee2c1..cbc4746 100644 --- a/src/arm_sim_gazebo/launch/gazebo_sim.launch.py +++ b/src/arm_sim_gazebo/launch/gazebo_sim.launch.py @@ -492,6 +492,55 @@ def _prepare(context, *args, **kwargs): ] actions.extend(controllers) + # --- 6b) 仿真夹爪适配器 -------------------------------------------------- + # 为什么必须有它：sdformat 把 URDF 的 整个丢掉，gz-sim 也不实现， + # 所以 6 个从动关节不会跟随主关节。适配器按耦合公式算出 7 个关节的目标值 + # 一起下发，并对外提供与真机**同名同类型**的 GripperExecute 服务 + #（/open|close|move|rotate + status），上层代码一行都不用改。 + # + # 耦合表来自 report.stripped_mimics —— 也就是 urdf_normalize 删掉 + # 时记下来的那几张表。让 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=( diff --git a/src/arm_sim_gazebo/setup.py b/src/arm_sim_gazebo/setup.py index f78f972..28e282b 100644 --- a/src/arm_sim_gazebo/setup.py +++ b/src/arm_sim_gazebo/setup.py @@ -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', diff --git a/src/arm_sim_gazebo/test/test_sim_gripper.py b/src/arm_sim_gazebo/test/test_sim_gripper.py new file mode 100644 index 0000000..5584e44 --- /dev/null +++ b/src/arm_sim_gazebo/test/test_sim_gripper.py @@ -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 = ( + '\n\n' + ' \n' + ' \n' + ' \n' + ' \n' + "\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 = ( + '' + "" + ) + with pytest.raises(GripperConfigError, match="upper"): + joint_limits(xml, "j") + + def test_inverted_limits_raise(self): + xml = '' + with pytest.raises(GripperConfigError, match="限位不合法"): + joint_limits(xml, "j") + + def test_mimic_joint_is_rejected_as_source(self): + """从动关节不能当主关节 —— 选错了整条链都没人驱动。""" + xml = ( + '' + ) + 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 = ( + '' + ) + 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() diff --git a/src/arm_sim_gazebo/test/test_sim_gripper_kin.py b/src/arm_sim_gazebo/test/test_sim_gripper_kin.py new file mode 100644 index 0000000..5e78130 --- /dev/null +++ b/src/arm_sim_gazebo/test/test_sim_gripper_kin.py @@ -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 = """ + + + + + + + + + + + + + + + + + + + + + +""" + +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( + '', + '', + 1, + ) + with pytest.raises(KinematicsError): + UrdfTree(bad) + + def test_zero_axis_is_rejected(self): + """axis 为零向量 → 无法确定转动轴，必须报错而不是算出 NaN。""" + bad = FINGER_URDF.replace( + ' 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)