工具几何进配置：S07 悬停/下探不再靠拍，预算由几何推导

真机与仿真**同参数同结果**的一个几何错误：流程里的"悬停高度"是按**相机**
定的（要看清孔得离工件面 ~0.17m），不是按**工具**定的 —— 而工具装在法兰
下面、比相机长得多。仿真里实测：

    悬停法兰 0.632 − 工具长 0.243 = 尖端 0.389
    工件上表面                             0.412
    → **悬停这一刻，通止规已经插进工件 23mm**

后果极具误导性：S07 一路压到顶都等不到"台肩落面"那一下，最后报
"下探 N mm 都没触发接触判据" —— 看起来像**力阈值标错了**，实际是几何错。
上一轮就是照这个假象去调阈值的，白费。

新增三个纯函数 + 两个配置项：
  - gauge_fsm.tool_clearance_m(法兰 z, 工具长, 面 z)  → 尖端离面多高（负=已插入）
  - gauge_fsm.descend_needed_m(...)                   → 台肩落面还需下压多少
        判"到底了"的是**台肩落面**，不是尖端碰面。台肩在规尖上方 30mm，
        所以按尖端算会**少算 30mm** —— 那正是"预算 25mm 永远到不了落面"的 bug。
  - gauge_fsm.descend_budget_m(needed)                → 预算 = 1.5×需求，含绝对下限
  - site.yaml: tool_tip_below_flange_m（0.243 的来历写清了：
        TCP 0.200 + 规尖到指腹接触面 0.043，而 0.043 不是"夹持段中心到尖端"
        的 0.0555 —— 指腹接触面比 TCP 原点高 12.5mm）
  - site.yaml: workpiece_surface_z_m（0 = 用 stage2 实测孔位兜底）

S07 启动时现在会打印（本轮实测输出）：
    S07 工具几何：悬停法兰 z=0.6317、工具尖端在法兰下方 243.0mm
      → 尖端 z=0.3887，工件上表面 z=0.4117
      悬停时尖端离面 -23.0mm（**已经插进工件了**）；台肩落面还需下压 +7.0mm
配的预算不够走完落面那一段时，就地按几何改大并告警（不是静默用错的数）。

回归测试 5 项，其中 test_shipped_budget_covers_shipped_gauge_and_hover
专防"预算又调回不够"这个 bug 复发。

Co-authored-by: Cursor <cursoragent@cursor.com>

src/arm_application/arm_application/gauge_assembly_node.py

@@ -97,9 +97,12 @@ from arm_application.gauge_fsm import (
     Event,
     FsmError,
     GaugeContext,
+    GAUGE_SHOULDER_ABOVE_TIP_M,
     GaugeFsm,
     State,
+    descend_budget_m,
     descend_iter_budget,
+    descend_needed_m,
     descend_progress,
     descend_tol_m,
     is_joint_limit_error,
@@ -108,6 +111,7 @@ from arm_application.gauge_fsm import (
     ratchet_cycles,
     screw_stop,
     tangential_jump,
+    tool_clearance_m,
 )
 from arm_application.gauge_gripper import (
     CONTRACT_STATUS_TOPIC,
@@ -1455,6 +1459,56 @@ class GaugeAssembly(AssemblyPipeline):
         z0 = float(Tee[2, 3])
         step_m = self.cfg.contact_step_mm / 1000.0
         max_depth_m = self.cfg.contact_max_depth_mm / 1000.0
+
+        # ---- 工具几何自检：预算必须是**由几何推出来的**，不能拍 ------------------
+        # 判"到底了"的那一下是**台肩落到工件上表面**（不是尖端碰面）。工件上表面
+        # 就是 stage2 实测的孔位 z（孔在面上）；台肩在规尖上方 30mm（通端长 30）。
+        #
+        # 这里做两件事：
+        #   ① 把"悬停时工具尖端离面还有多高"算出来印出来。**负值说明悬停这一步
+        #      就已经把工具捅进工件了**（悬停高度是按相机定的、工具比相机长就
+        #      会这样，真机同参数同结果），这是个几何错误，藏得很深：现象是
+        #      S07 一路压到顶都判不到接触，看起来像力阈值标错了。
+        #   ② 如果配的预算**不够**走完"台肩落面"那一段，就地按几何**改大**并
+        #      大声报出来。2026-09-24 仿真里踩过：预算 25mm 而台肩在尖端上方
+        #      30mm → 永远到不了落面那一下，报出来的原因是假的（"没触发接触
+        #      判据"），真实原因只是预算不够。
+        surface_z = float(self.cfg.workpiece_surface_z_m)
+        if surface_z <= 0.0:
+            # 没配就取 stage2 实测的孔位 z —— 孔就在工件上表面上。
+            k1 = (self._snap_final or {}).get("k1_base_m") or []
+            surface_z = float(k1[2]) if len(k1) >= 3 else 0.0
+        if surface_z > 0.0:
+            clear_m = tool_clearance_m(
+                z0,   # 悬停时的法兰高度 = 当前 Tee 的 z（S06 刚把法兰锁到悬停高）
+                self.cfg.tool_tip_below_flange_m,
+                surface_z,
+            )
+            needed_m = descend_needed_m(
+                z0, self.cfg.tool_tip_below_flange_m, surface_z,
+                GAUGE_SHOULDER_ABOVE_TIP_M,
+            )
+            self.get_logger().info(
+                f"S07 工具几何：悬停法兰 z={z0:.4f}、工具尖端在法兰下方 "
+                f"{self.cfg.tool_tip_below_flange_m * 1000:.1f}mm → 尖端 z="
+                f"{z0 - self.cfg.tool_tip_below_flange_m:.4f}，工件上表面 z={surface_z:.4f}\n"
+                f"  悬停时尖端离面 {clear_m * 1000:+.1f}mm"
+                f"{'（**已经插进工件了**）' if clear_m < 0 else ''}；"
+                f"台肩落面还需下压 {needed_m * 1000:+.1f}mm"
+            )
+            if needed_m > max_depth_m:
+                new_depth_m = descend_budget_m(needed_m)
+                self.get_logger().warn(
+                    f"S07 下探预算不足：配的是 {max_depth_m * 1000:.1f}mm，"
+                    f"但「台肩落面」这一段就要 {needed_m * 1000:.1f}mm"
+                    f"（台肩在尖端上方 {GAUGE_SHOULDER_ABOVE_TIP_M * 1000:.0f}mm）。\n"
+                    f"  按几何自动改大为 {new_depth_m * 1000:.1f}mm。"
+                    f"要固定这个值，改 site.yaml 的 "
+                    f"gauge_assembly.contact_max_depth_mm（建议 ≥ "
+                    f"{new_depth_m * 1000:.0f}）"
+                )
+                max_depth_m = new_depth_m
+
         # 这一步的到位容差**必须小于步长**（见 gauge_fsm.descend_tol_m 的说明：
         # 容差≥步长时 _goto 判"已在容差内"直接返回，一步都不走 → 静默自旋）。
         tol_step_m = descend_tol_m(step_m)

src/arm_application/arm_application/gauge_config.py

@@ -96,6 +96,8 @@ _TYPES: Dict[str, type] = {
     "contact_speed_m_s": float,
     "contact_step_mm": float,
     "contact_max_depth_mm": float,
+    "tool_tip_below_flange_m": float,
+    "workpiece_surface_z_m": float,
     "contact_tangential_delta": float,
     "contact_safe_delta": float,
     "screw_step_deg": float,
@@ -136,6 +138,9 @@ _UNITS: Dict[str, str] = {
     "contact_speed_m_s": "m/s",
     "contact_step_mm": "mm",
     "contact_max_depth_mm": "mm",
+    # 这两个是**长度**不是"毫米量"：用 m 口径，与 hover_z 同一把尺
+    "tool_tip_below_flange_m": "m",
+    "workpiece_surface_z_m": "m",
     "contact_tangential_delta": "au",
     "contact_safe_delta": "au",
     "screw_step_deg": "deg",
@@ -200,7 +205,9 @@ DEFAULTS: Dict[str, Any] = {
     # S07
     "contact_speed_m_s": 0.02,
     "contact_step_mm": 0.5,
-    "contact_max_depth_mm": 25.0,
+    "contact_max_depth_mm": 35.0,
+    "tool_tip_below_flange_m": 0.243,
+    "workpiece_surface_z_m": 0.0,   # 0 = 未知，运行期用 stage2 实测孔位兜底
     "contact_tangential_delta": 300.0,
     "contact_safe_delta": 2000.0,
     # S08
@@ -249,6 +256,8 @@ class GaugeConfig:
     contact_speed_m_s: float
     contact_step_mm: float
     contact_max_depth_mm: float
+    tool_tip_below_flange_m: float
+    workpiece_surface_z_m: float
     contact_tangential_delta: float
     contact_safe_delta: float
     screw_step_deg: float
@@ -303,6 +312,10 @@ class GaugeConfig:
             f"  S06 对位容差 {self.align_tol_mm:.1f}mm（悬停高取自 servo.hole_hover_z_m）",
             f"  S07 切向突变阈值 +{self.contact_tangential_delta:.0f}（两通道任一，现场标定），"
             f"安全上限 +{self.contact_safe_delta:.0f}，最多下探 {self.contact_max_depth_mm:.0f}mm",
+            f"  S07 工具几何：尖端在法兰下方 {self.tool_tip_below_flange_m * 1000:.1f}mm，"
+            f"工件上表面 z={self.workpiece_surface_z_m:.3f}m"
+            "（启动时会按这两个值 + servo.hole_hover_z_m 算「悬停时工具到底入没入工件」，"
+            "入了会**明确报错**——见 gauge_fsm.tool_clearance_m）",
             f"  S07 步进 {self.contact_step_mm:.2f}mm @ {self.contact_speed_m_s * 1000:.0f}mm/s，"
             f"到位容差 {descend_tol_m(self.contact_step_mm / 1000.0) * 1000:.3f}mm"
             "（=步长的 40%，**推导出来的、不是可调项**：容差 ≥ 步长会让每一步都被判成"
@@ -436,6 +449,12 @@ def _sanity(cfg: GaugeConfig) -> None:
             "前者是「立刻抬臂」的安全上限，后者只是「停止下压」。"
             "两者相等的话，接触一开始就会触发安全上限"
         )
+    if cfg.tool_tip_below_flange_m <= 0.0:
+        raise GaugeConfigError(
+            f"tool_tip_below_flange_m 必须为正（工具尖端在法兰**下方**多少米），"
+            f"收到 {cfg.tool_tip_below_flange_m}。"
+            "填 0 会让「悬停时工具会不会已经插进工件」这个判断失效"
+        )
     if cfg.snap_tol_px <= 0.0:
         raise GaugeConfigError(f"gauge_assembly.snap_tol_px 必须为正，收到 {cfg.snap_tol_px}")
     if cfg.snap_tol_mm <= 0.0:

src/arm_application/arm_application/gauge_fsm.py

@@ -408,6 +408,81 @@ def descend_progress(z_prev: float, z_now: float, step_m: float) -> bool:
     return (float(z_prev) - float(z_now)) >= step_m * DESCEND_PROGRESS_FRAC
 
 
+#: 通止规**台肩**（COLLAR）在规尖上方的距离 [m]。
+#: 取自 devices/sim/gazebo/models/gauge/model.sdf：通端 φ6×30mm，台肩在 30~33mm。
+#: 判"到底了"的那一下就是台肩落到工件上表面 —— 所以它决定了 S07 至少要下压
+#: 多少（见 descend_needed_m）。改规的模型时必须一起改这里，否则预算会算错。
+GAUGE_SHOULDER_ABOVE_TIP_M: float = 0.030
+
+
+def tool_clearance_m(
+    flange_z_m: float,
+    tool_tip_below_flange_m: float,
+    surface_z_m: float,
+) -> float:
+    """工具尖端离工件上表面还有多高 [m]。**负 = 已经插进去了。**
+
+    为什么需要这个：流程里"悬停高度"是按**相机**定的，不是按**工具**定的。
+    腕部相机要看清楚孔，就得离工件面 ~0.17m；而工具（装在法兰下面）可能比
+    相机长得多。两者一冲突，就会出现下面这种极难查的局面：
+
+        悬停高度 0.632（法兰）− 工具长 0.243 = 0.389
+        工件上表面                                  0.412
+        → 工具尖端已经插进工件 23mm
+
+    此时"悬停"这一步本身已经把工具捅进孔里了，S07 再往下压就一路压到顶、
+    等不到"台肩落面"那一下，最后报"下探 N mm 都没触发接触判据"然后 ABORT。
+    现象看起来像力阈值标错了，实际是**几何**错了。
+
+    真机同参数同结果（不是仿真特有的）：只要悬停高度是按相机定的、而工具
+    比相机长，就会这样。所以这个值要在**启动时**算出来并报出来。
+    """
+    tip_z = float(flange_z_m) - float(tool_tip_below_flange_m)
+    return tip_z - float(surface_z_m)
+
+
+def descend_needed_m(
+    flange_z_m: float,
+    tool_tip_below_flange_m: float,
+    surface_z_m: float,
+    tool_shoulder_above_tip_m: float,
+) -> float:
+    """S07 要判"台肩落面"，从当前法兰高度起还需要下压多少 [m]。
+
+    判"到底了"的那一下不是工具尖端碰面，而是**台肩落到工件上表面**；
+    台肩在工具尖端上方 ``tool_shoulder_above_tip_m``（规是通端长 30mm）。
+    所以需要的下压量 = 台肩到面的距离，而不是尖端到面的距离。
+
+    返回值可能是负的（台肩已经在面以下）—— 那说明几何配错了，调用方应当
+    报错而不是"往下走一个负距离"。
+    """
+    shoulder_z = (
+        float(flange_z_m)
+        - float(tool_tip_below_flange_m)
+        + float(tool_shoulder_above_tip_m)
+    )
+    return shoulder_z - float(surface_z_m)
+
+
+#: S07 下探预算相对"实际需要下压量"的余量系数。
+#: 预算是**兜底**用的（不是用来判成败的）：真正让循环停下的是接触判据。
+#: 给 1.5 倍余量是为了容纳对位残差、板厚公差、台肩倒角还没完全落面等。
+DESCEND_BUDGET_FRAC: float = 1.5
+#: 下探预算的绝对下限 [m]：几何算出来很小（比如台肩已快贴面）时也要留够，
+#: 否则一旦对位有点偏，"预算用完"会先于"判到接触"触发，报出来的原因是假的。
+DESCEND_BUDGET_FLOOR_M: float = 0.008
+
+
+def descend_budget_m(needed_m: float) -> float:
+    """由"实际需要下压多少"推出 S07 的下探预算 [m]。
+
+    不要手工填这个数：填小了会在台肩落面**之前**就用完预算（报"下探 N mm
+    都没触发接触判据"，而真实原因只是预算不够）；填大了会在判据失灵时压得
+    更深（更危险）。由几何推出来，两个毛病都没有。
+    """
+    return max(float(needed_m) * DESCEND_BUDGET_FRAC, DESCEND_BUDGET_FLOOR_M)
+
+
 def screw_stop(normal_delta: float, shear_max: float, normal_th: float, shear_th: float) -> bool:
     """S08：旋转装配的停止判据 —— **切向力>阈值 且 法向力突增**。
 

src/arm_application/test/test_gauge_fsm.py

@@ -23,9 +23,14 @@ from arm_application.gauge_fsm import (
     GaugeContext,
     GaugeFsm,
     State,
+    DESCEND_BUDGET_FLOOR_M,
+    GAUGE_SHOULDER_ABOVE_TIP_M,
+    descend_budget_m,
     descend_iter_budget,
+    descend_needed_m,
     descend_progress,
     descend_tol_m,
+    tool_clearance_m,
     is_joint_limit_error,
     pixel_stable,
     points_stable,
@@ -459,6 +464,74 @@ def test_descend_iter_budget_covers_the_whole_depth():
     assert descend_iter_budget(depth_m, step_m) < 200
 
 
+def test_tool_clearance_is_negative_when_tool_already_inside():
+    """悬停高度按相机定、工具比相机长 → 悬停那一刻工具已经插进工件。
+
+    这是 2026-09-24 仿真里实测到的局面（也是真机同参数同结果）：
+        悬停法兰 0.632 − 工具长 0.243 = 尖端 0.389，工件上表面 0.412
+        → 尖端在面下 23mm
+    现象是 S07 一路压到顶都判不到接触，看起来像力阈值标错了 —— 其实是几何错。
+    """
+    gap = tool_clearance_m(0.632, 0.243, 0.412)
+    assert gap == pytest.approx(-0.023, abs=1e-9)
+
+
+def test_tool_clearance_positive_when_tool_is_short():
+    assert tool_clearance_m(0.632, 0.100, 0.412) == pytest.approx(0.120, abs=1e-9)
+
+
+def test_descend_needed_measures_shoulder_not_tip():
+    """判"到底了"的是**台肩落面**，所以需要的下压量按台肩算，不按尖端算。
+
+    台肩在尖端上方 30mm：尖端要往下 30mm，台肩才落到面上。
+    按尖端算会少算 30mm —— 那正是"预算 25mm 永远到不了落面"那个 bug。
+    """
+    # 尖端正好在面上（法兰 = 面 0.412 + 工具长 0.243 = 0.655）
+    # → 还要再下压 30mm 台肩才落面
+    assert descend_needed_m(0.655, 0.243, 0.412, 0.030) == pytest.approx(0.030)
+
+    # 悬停在 0.632（= 仿真里那个按相机定的值）→ 台肩还差 7mm
+    assert descend_needed_m(0.632, 0.243, 0.412, 0.030) == pytest.approx(0.007)
+
+    # 按尖端算会少算台肩那 30mm —— 差的就是出问题的那个量
+    tip_only = tool_clearance_m(0.655, 0.243, 0.412)
+    assert tip_only == pytest.approx(0.0)
+    assert descend_needed_m(0.655, 0.243, 0.412, 0.030) - tip_only == pytest.approx(0.030)
+
+
+def test_descend_budget_has_margin_and_floor():
+    """预算要有余量（容纳对位残差/板厚公差），且再小也有下限。
+
+    下限的理由：几何算出来"只差 2mm"时，如果预算就填 2mm，一点对位残差就会
+    让"预算用完"先于"判到接触"触发 —— 报出来的原因是假的。
+    """
+    # 需求很小时吃下限：0.002×1.5 = 0.003 < 0.008
+    assert descend_budget_m(0.002) == pytest.approx(DESCEND_BUDGET_FLOOR_M)
+    # 需求正常时按系数放大：0.030×1.5 = 0.045
+    assert descend_budget_m(0.030) == pytest.approx(0.045)
+
+
+def test_shipped_budget_covers_shipped_gauge_and_hover():
+    """出厂配置必须自洽：预算要够走完"台肩落面"。
+
+    这是那个 bug 的回归测试 —— 当时预算 25mm 而台肩在尖端上方 30mm，
+    永远到不了落面那一下，报"下探 25mm 都没触发接触判据"（假原因）。
+    以后谁把 contact_max_depth_mm 调回去、或把 tool_tip_below_flange_m 改大
+    到不匹配，这里就会红。
+    """
+    from arm_application.gauge_config import DEFAULTS
+
+    budget_m = DEFAULTS["contact_max_depth_mm"] / 1000.0
+    tip_below = DEFAULTS["tool_tip_below_flange_m"]
+    # 出厂配置里 workpiece_surface_z_m = 0（运行期用 stage2 实测孔位兜底），
+    # 所以这里取一个"最坏"的悬停-面高差：仿真里实测法兰 0.632、面 0.412。
+    needed = descend_needed_m(0.632, tip_below, 0.412, GAUGE_SHOULDER_ABOVE_TIP_M)
+    assert budget_m >= needed, (
+        f"下探预算 {budget_m*1000:.1f}mm 不够走完台肩落面所需的 "
+        f"{needed*1000:.1f}mm —— 会在到落面之前就用完预算"
+    )
+
+
 def test_descend_iter_budget_zero_depth_means_no_iterations():
     assert descend_iter_budget(0.0, 0.0005) == 0
     with pytest.raises(ValueError):