仿真夹爪适配器：服务接口对齐真机，开口按正运动学换算成真毫米

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