真机落地前的仿真补齐：夹持/腕力/真值位姿，让仿真能验证 S07–S09

这批是 09-24 那轮仿真工作的收尾，一直没提交。内容按目的分四块：

1) 仿真真能「夹住」工件
   sim_grasp_lock：给规与夹爪之间加接触锁（默认关，run_gauge_assembly --sim 才开）。
   没有它时仿真里规只靠摩擦，S08 棘轮一转就滑出指间，于是把「仿真不真」误读成
   「力判据不对」。新增 test_sim_grasp_lock.py。

2) 腕部力传感器进仿真
   sim_wrist_ft（+ config/wrist_ft.yaml）：S07 的首选判据是腕部 FT 的轴向突变，
   而仿真里以前根本没有腕部力，S07 只能退到触觉兜底，两套判据从没在同一场景下比过。
   现在两条通道都在，接触判定可对照。

3) 真值位姿可查（不再靠猜）
   sim_live_pose / sim_pose_service：把 Gazebo 里刚体的实测位姿发布出来，
   stage2 的真值路径按**实测**位姿算孔位，而不是按场景文件里的标称位姿。
   顺带把真值孔位的推导（truth_holes）独立出来并补测。

4) 工具几何成为可配置项
   config/sim_tool_geometry.yaml + stage2 的 tool_tip_below_flange_m 启动参数：
   仿真里规会在指间下滑，工具长每次装夹都不同（实测见过 243/254mm），
   悬停高度与 S07 下探预算必须按实测值算，不能再写死。

应用层同时跟进：gauge_fsm / gauge_config 把「悬停时工具尖端入没入工件」的判据
（hover_z_guard）与拖件判定（drag_verdict）收成纯函数并补测；stage3 搜孔、
site 导出、run_gauge_assembly 与若干文档同步。
新增 test_gauge_exit_lift / test_gauge_gripper_client / test_node_handler_collisions。

测试：全仓库 1085 passed。

devices/sim/gazebo/config/grasp_lock.yaml

@@ -0,0 +1,68 @@
+# =============================================================================
+# devices/sim/gazebo/config/grasp_lock.yaml
+#
+# 仿真"抓取锁"（grasp lock）的口径。**只对仿真有意义**，真机没有任何对应物 ——
+# 真机靠夹爪的摩擦夹持，本来就不会掉。
+#
+# -----------------------------------------------------------------------------
+# 它解决什么问题（2026-09-24 一整轮排查换来的）
+# -----------------------------------------------------------------------------
+# 仿真里的夹持力不是"设"出来的，而是**位置误差 × 接触刚度**（见 gauge/model.sdf
+# 里 kp 那段）。规站着不动时够用；机械臂一动就不行：
+#
+#     S01 绕工具轴快转 150°、pre_lift/粗定位是 60~100mm 的连续轨迹 ——
+#     这两步里规在指间打滑。实测工具长度 0.243m → 0.285m、规歪 62°，
+#     S07 随后拿到的几何全错，现象是"S07 判不到接触"或"规掉了(E1)"。
+#
+# 真机不会这样：握力 46N、µ≈0.8、规 0.265kg —— 摩擦余量两个数量级。
+# 所以这把锁补的是 **Gazebo 缺的那块物理**（刚性接触没有静摩擦迟滞、也不模拟
+# 指垫橡胶的贴合），不是绕过物理：规与指垫的接触对仍在，触觉与腕部 FT 的读数
+# 依旧是真实接触力。
+#
+# -----------------------------------------------------------------------------
+# ⚠️ 一处对不上就**静默失效**（不报错，只是规不跟着走）
+# -----------------------------------------------------------------------------
+# 本文件的名字必须与下面三处一致：
+#   ① gauge/model.sdf 里 DetachableJoint 插件的 <child_model>/<child_link>
+#   ② gauge/model.sdf 里插件的 <attach_topic>/<detach_topic>
+#   ③ 真名（Gazebo 里实际存在的连杆名）—— URDF 里与固定关节相连的连杆会被
+#      sdformat **合并（lump）**，名字会变：手指只剩 sim_left_finger_mount。
+#      查真名的办法：
+#          ros2 topic echo /sim/world_pose_info   ← 看全部 child_frame_id
+#      gazebo_sim.launch.py 启动时会读 gauge 的 SDF 核对 ① ②，不一致就直接报出来。
+# -----------------------------------------------------------------------------
+
+# 总开关。关掉它仿真照样跑，只是回到"靠摩擦夹持"，也就是上面那串毛病。
+enabled: true
+
+# 被夹住的物体（scene.yaml 里的物体名 = Gazebo 里的模型名）。
+object: "gauge"
+# 该模型里被夹的那根连杆。
+object_link: "body"
+
+# 机器人侧：模型名 = launch 里 spawn 的 -name；连杆名必须是 Gazebo 的**真名**。
+#
+# ⚠️ 必须锁到**不会跟着张爪动的**连杆上 —— 默认是 `sim_link7`（法兰）。
+#    为什么（2026-09-24 实测，一条假 E1 的真因）：
+#        起初锁到 `sim_left_finger_mount`（左手指）。夹住的规被**刚性**锁在这根
+#        手指上，而 S08 棘轮每轮都要"松爪 → 回转 → 夹紧"：张爪时手指往外走
+#        40mm，被锁住的规却还插在孔里（6mm 销 / 7mm 方孔）——两处约束打架，
+#        物理解算把规**从孔里拽出来**（实测规尖跳 +17.5mm、偏摆 64°），
+#        然后在回转那一步掉到地上。日志只看得到"通止规掉了(E1)"。
+#        `sim_grav_base_link`（夹爪本体）在 sdformat 里被**合并**进了 `sim_link7`
+#        （见 /sim/world_pose_info 里的 `sim_link7_fixed_joint_lump__...`），
+#        所以锚点就写 `sim_link7`：张爪不影响它，松爪/夹紧都不会带偏规。
+#    `sim_grasp_lock.load_grasp_lock()` 会拒绝含 `finger` 的连杆名（带这段原因）。
+robot_model: "rizon_sim"
+robot_link: "sim_link7"
+
+# 触发话题（gz 侧，由本模块用 ign topic 发一个空消息触发）。
+# 名字必须与 gauge/model.sdf 里插件的 attach_topic/detach_topic 一致。
+attach_topic: "/sim/grasp_lock/attach"
+detach_topic: "/sim/grasp_lock/detach"
+# 插件状态回执（会 attach/detach 成功时各发一条）。用它来判断"锁到底有没有生效"，
+# 而不是靠"看规掉没掉"——后者要等故障发生才知道。
+state_topic: "/sim/grasp_lock/state"
+
+# 发一次触发消息后等多久 [ms]（ign topic 是短命进程，给足启动时间）。
+publish_timeout_ms: 3000

devices/sim/gazebo/config/scene.yaml

@@ -99,6 +99,58 @@ gates_px:
   insert_ring_radius: [6, 30]
   hole_spacing: [90, 700]
 
+# ---------------------------------------------------------------------------
+# 世界模型位姿回读 —— 仿真里唯一能发现"物体被撞走/掉下去了"的手段
+# ---------------------------------------------------------------------------
+# 为什么需要它（2026-09-24 花了一整轮排查换来的，值得完整读一遍）：
+#     `perception.source=truth` 是**纯几何**的：它按下面 objects 里的**名义位姿**
+#     把孔投到像素上，**从不问 Gazebo 物体现在到底在哪**。
+#     于是工件一旦被碰走，"真值"就开始撒谎，而且每一处现象都指向别的原因：
+#
+#         实测：工件掉进台面通槽、落到地面（z≈0），而 scene.yaml 仍写着 z=0.40。
+#         S05 拿"名义孔位"的像素去查**真实**深度图 → 孔位处看到的是槽底/地面
+#         （0.6m，而工件面其实只有 0.21m）→ 三角化出 k1/k2 间距 169mm
+#         （真值 58mm）→ 被间距门槛丢掉 → 精拍连丢 9 拍 → S05 超时 ABORT。
+#         现场现象像"检测坏了 / 标定坏了 / 阈值调高了"，全都不是。
+#         真正的信息只有一句："**工件现在不在 scene.yaml 说的位置**。"
+#
+# Gazebo 自己会把每个模型的实际位姿发在 /world/<world>/pose/info
+# （gz.msgs.Pose_V），桥成 tf2_msgs/TFMessage 之后，漂移监视器
+# （sim_truth_perception）就能把"名义位姿 vs 实际位姿"直接对比出来并**指名报错**。
+#
+# 话题名里的 world 名必须与 worlds/assembly.world 的文件名一致（`assembly`）。
+pose_info:
+  gz_topic: "/world/assembly/pose/info"
+  ros_topic: "/sim/world_pose_info"
+  # 类型固定这两条：ros_gz_bridge 原生支持 Pose_V ↔ TFMessage（实测确认，
+  # 见 ros_gz_sim_demos 的 tf_bridge.launch.py）。写错类型不会有任何报错，
+  # 只会"桥接器起来了但一个消息都没有" —— 所以别改。
+  ros_type: "tf2_msgs/msg/TFMessage"
+  gz_type: "gz.msgs.Pose_V"
+  # 漂移判据：**两个**阈值，别合成一个（2026-09-24 的教训）。
+  #
+  # drift_tol_m（2mm）= **告警**线：真值几何已经跟着**实测**位姿走
+  #     （见 arm_sim_gazebo/sim_live_pose.py），所以 2mm 不再意味着"几何错了"，
+  #     只意味着"这个场景跟 scene.yaml 写的不一样了"。而 2~3mm 是**天天都有**的：
+  #     spawn 落地就偏 2.6mm，工件骑在通槽上静置十几分钟还会再挪 1.9mm。
+  #     2mm 的来历：真值投影在相机到工件 0.17m 处，fx≈447 ⇒ 1mm 位姿误差 ≈ 2.6px，
+  #     而 S05 的稳定判据是 2px —— 2mm 是"开始影响二阶段"的量级。
+  #     yaw 取 2°：两孔间距 58mm，绕中心转 2° 会让孔横移 1mm（约 2.6px），同量级。
+  drift_tol_m: 0.002
+  drift_tol_deg: 2.0
+  # drift_stop_m 才是**停发**线：到这个量级说明工件掉了/被搬走了，场景前提没了
+  # （再发点等于指挥机械臂去装一个不存在的工件）。20mm 的来历：工件一半宽 70mm、
+  # 掉下台面是 400mm 量级，而"顺手碰一下"是几毫米 —— 20mm 两边都分得开。
+  drift_stop_m: 0.02
+  # 启动复位：开局把被盯的物体 set_pose 回名义位姿（仿真专有，默认关，这里开）。
+  #    为什么还要它：上面两条线是"容差"，这一条是**消除源头** ——
+  #    物体是"在名义位姿上方 spawn、再落下来"的，落地那 2.6mm 偏差纯属 spawn
+  #    的落点问题（实测 set_pose 回名义位姿后它就停在那儿不动）。
+  #    只在开局 20s 窗口内、且只做一次 —— 运行中的漂移必须报错，不能悄悄搬回去
+  #    （那会抹掉"谁把它撞歪了"的现场，还可能把工件从爪里拽走）。
+  #    实现见 sim_truth_perception._maybe_startup_reset（含完整理由）。
+  reset_on_start: true
+
 # ---------------------------------------------------------------------------
 # 场景物体
 # ---------------------------------------------------------------------------

devices/sim/gazebo/config/sim_tool_geometry.yaml

@@ -0,0 +1,12 @@
+# ⚠ 本文件由 scripts/sim_load_gauge.py 自动生成（每次仿真上料都覆盖）。
+#    它是**这次装夹**实测到的工具几何，run_gauge_assembly.sh --sim 会读它
+#    并作为启动参数覆盖 site.yaml 的 gauge_assembly.tool_tip_below_flange_m。
+#    为什么必须这样：仿真里规会在指间下滑到落座位置，下滑量随装夹变化
+#    （实测见过 243mm / 254mm 两种）。写成固定值迟早会算错悬停高度。
+#    真机没有这个文件（现场装夹后把实测值写进 site.yaml）。
+flange_z_m: 0.763
+measured_at: '2026-09-24T19:40:08'
+model: gauge
+tip_z_m: 0.5198
+tool_tip_below_flange_m: 0.2432
+world: assembly

devices/sim/gazebo/config/wrist_ft.yaml

@@ -0,0 +1,50 @@
+# =============================================================================
+# devices/sim/gazebo/config/wrist_ft.yaml
+#
+# 仿真腕部力扭矩传感器（wrist force/torque）的话题与口径。
+# sim_wrist_ft.py、gazebo_sim.launch.py 从这里读；改这里不用重编译。
+#
+# 真机的对应物：Flexiv 的 flexiv_robot_states_broadcaster 自动发
+#     /<sn>/ft_sensor_wrench        geometry_msgs/msg/WrenchStamped（法兰系）
+# 不需要任何配置 —— 它在真机 bringup 里无条件启动。
+#
+# -----------------------------------------------------------------------------
+# 为什么仿真与真机能共用一套判据（这与触觉**相反**）
+# -----------------------------------------------------------------------------
+# 手指触觉（tactile.yaml）两边单位不同，所以阈值必须各配一套；腕部 FT 不同：
+#
+#   真机 ft_sensor_wrench 单位就是牛顿，坐标系是法兰系。
+#   仿真 force_torque 传感器单位也是牛顿，挂 joint7 取 child(link7) 系，
+#   而 link7 的 z 轴 = 工具轴（工具竖直朝下时即竖直方向）。
+#
+# 于是两边的"z 分量"都是**轴向力，单位牛顿** —— 公式一致，只有具体阈值
+# 可以按现场标定分开配（见 site.yaml 的 gauge_assembly.contact_axial_delta）。
+#
+# ⚠️ 与触觉一样，这也是一处"选错不报错"的口径：口径错了只会表现为
+#    "判据不触发"或"一碰就炸"，看起来像算法问题。
+# -----------------------------------------------------------------------------
+
+# gz 侧话题名。**必须与应用层按 sn 推出来的名字一致**：
+#     应用层算 f"/{sn}/ft_sensor_wrench"，仿真臂 sn = "sim"
+#     ⇒ /sim/ft_sensor_wrench
+# 改 sn（config/site.yaml 的 robot.arms[sim]）时必须同步改这里。
+# 也同时写死在 arm.sim.sensors.xacro 的 wrist_ft_topic 默认值里 —— 两处
+# 必须一致，check_sim_wrist_ft.py 会核对 xacro 展开结果里有没有这个话题。
+topic: "/sim/ft_sensor_wrench"
+
+# 挂载关节。改这里必须同步改 arm.sim.sensors.xacro 里的 <gazebo reference>。
+# 为什么是 joint7 见 xacro 里那段注释（不能挂 fixed 关节：物理引擎不把它当
+# 独立关节求解，传感器永远没输出且不报错）。
+joint: "sim_joint7"
+
+# 期望消息类型（ROS 侧）。gz.msgs.Wrench → WrenchStamped 由 ros_gz_bridge
+# 直接支持（实测确认，无需中间适配器节点）。
+#
+# 为什么是 WrenchStamped 而不是 Wrench：真机发的就是 WrenchStamped。
+# 类型一致，应用层才能不加分支地两边通用。
+ros_type: "geometry_msgs/msg/WrenchStamped"
+gz_type: "gz.msgs.Wrench"
+
+# 传感器输出率 [Hz]。与真机 broadcaster 对齐（100Hz）。
+# 下游按"数据够不够新"判活，率太低会被误判成掉线。
+publish_hz: 100.0