悬停走完后常数前馈会往 +X 过补约 1.3mm、插不进，贴面改成笛卡尔锁 XY、只降 Z 的厂家 MoveL；D435 交回也不再杀栈，只等法兰停稳 0.5 秒。

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

src/arm_application/arm_application/overhead_oneshot.py

@@ -154,9 +154,9 @@ def d405_commands_arm(
 ) -> bool:
     """D405 这一帧的点能不能下发给左臂。
 
-    推理一直开着。正在用 D435 那一个点时，杀栈 MoveL 冷启动结束并且停稳之前不下发。
-    停稳从控制栈回来之后才计，用 ``d435_handback_sec`` 和
-    ``lock_still_move_m``。赶路时 XY 已经进 15 mm 也不算到位。
+    推理一直开着。正在用 D435 那一个点时，进程内 MoveL 结束并且停稳之前不下发。
+    停稳用 ``d435_handback_sec`` 和 ``lock_still_move_m``。赶路时 XY 已经进 15 mm
+    也不算到位。不许在 MoveL 期间下发 D405（两套控制互抢会晃）。
     ``move_only``：只测 D435 位移，D405 点一律不下发（到位之后也不交）。
     """
     if move_only:
@@ -541,6 +541,112 @@ HOLE_PAIR_TRIM_MAX_M = 0.070
 # 悬停 → 观察高（约 15mm）单步上限。左臂间隙约 58mm，20mm × 3 步走完。
 # 观察高 → 贴面不再走这段，改斜线一步（XY 补偿 + 剩余 Z）。
 ONCE_APPROACH_STEP_M = 0.020
+# 悬停已对上时 ΔXY 必须为 0。site ``once_ff_look_per_m`` 同源。
+# 2026-10-14 下午：框心改平面交点后两条链路下臂都仍 +1.3mm X 过补插不进。
+# k=0.018 是深度框心时代的对冲，不是下降几何。笛卡尔 MoveL 只降 Z。
+ONCE_FF_LOOK_PER_M = 0.0
+ONCE_FF_MOVEL_VEL_MPS = 0.05
+
+
+def once_look_ff_xy(look_xy, descent_m: float, k: float):
+    """视线前馈 ΔXY [m]。k≤0 是笛卡尔锁 XY：返回 (0,0)，不是 None。
+
+    k>0 时沿 -look 走 k×下降。下降≤0 / 视线退化 → None。
+    """
+    if float(descent_m) <= 1e-9:
+        return None
+    if float(k) <= 1e-9:
+        return np.array([0.0, 0.0], dtype=float)
+    look = np.asarray(look_xy, dtype=float).reshape(2)
+    n = float(np.linalg.norm(look))
+    if n < 1e-6 or not np.all(np.isfinite(look)):
+        return None
+    look = look / n
+    dxy = -look * float(k) * float(descent_m)
+    if not np.all(np.isfinite(dxy)):
+        return None
+    return dxy
+
+
+def once_ff_tcp_xyz(
+    flange_xy,
+    tip_off_xy,
+    look_xy,
+    descent_m: float,
+    k: float,
+    surface_z_m: float,
+    gap_m: float,
+):
+    """规尖（厂家 MoveL 的 TCP）目标 [m]。Z 是面 + 间隙，不是法兰。"""
+    dxy = once_look_ff_xy(look_xy, descent_m, k)
+    if dxy is None:
+        return None
+    off = np.asarray(tip_off_xy, dtype=float).reshape(2)
+    fl = np.asarray(flange_xy, dtype=float).reshape(2)
+    if not np.all(np.isfinite(off)) or not np.all(np.isfinite(fl)):
+        return None
+    tcp = np.array(
+        [
+            float(fl[0]) + float(off[0]) + float(dxy[0]),
+            float(fl[1]) + float(off[1]) + float(dxy[1]),
+            float(surface_z_m) + float(gap_m),
+        ],
+        dtype=float,
+    )
+    if not np.all(np.isfinite(tcp)):
+        return None
+    return tcp, dxy
+
+
+def d435_movel_from_tcp(tcp_xyz, flange_xy, k0_xy):
+    """D435 粗定位 MoveL 目标 [m]：当前厂家 TCP + (K0−法兰) 的 ROS XY。
+
+    keep_z 由硬件用当前 TCP 的 Z 覆盖。ROS 绝对 K0 不能当 WORLD 目标
+    （2026-10-14 贴面 MoveL 已踩过）。缺数 / 非有限 → None。
+    """
+    try:
+        t = np.asarray(tcp_xyz, dtype=float).reshape(3)
+        fl = np.asarray(flange_xy, dtype=float).reshape(2)
+        k0 = np.asarray(k0_xy, dtype=float).reshape(2)
+    except (TypeError, ValueError):
+        return None
+    if t.size != 3 or fl.size != 2 or k0.size != 2:
+        return None
+    if not np.all(np.isfinite(t)) or not np.all(np.isfinite(fl)) or not np.all(np.isfinite(k0)):
+        return None
+    out = np.array(
+        [float(t[0]) + float(k0[0]) - float(fl[0]),
+         float(t[1]) + float(k0[1]) - float(fl[1]),
+         float(t[2])],
+        dtype=float,
+    )
+    if not np.all(np.isfinite(out)):
+        return None
+    return out
+
+
+def once_ff_movel_from_tcp(tcp_xyz, dxy, descent_m: float):
+    """厂家 MoveL 目标 = 当前 Studio TCP + (ΔXY, −下降) [m]。
+
+    2026-10-14：把 ROS 规尖绝对坐标发给 MoveL，k=0.018 与 0.035 都落到
+    WORLD (0.595, −0.468, 0.024)，法兰实走约 (+9, −8)mm。WORLD 原点与
+    ROS 基座不是同一点，绝对坐标不能用。相对位移才跟 k 有关。
+    """
+    if float(descent_m) <= 1e-9:
+        return None
+    t = np.asarray(tcp_xyz, dtype=float).reshape(3)
+    d = np.asarray(dxy, dtype=float).reshape(2)
+    if t.size != 3 or d.size != 2:
+        return None
+    if not np.all(np.isfinite(t)) or not np.all(np.isfinite(d)):
+        return None
+    out = np.array(
+        [float(t[0]) + float(d[0]), float(t[1]) + float(d[1]), float(t[2]) - float(descent_m)],
+        dtype=float,
+    )
+    if not np.all(np.isfinite(out)):
+        return None
+    return out
 
 
 def oneshot_align_step_m(err_m: float) -> float:
@@ -570,6 +676,46 @@ def flange_z_for_gap_m(tip_len_m: float, surface_z_m: float, gap_m: float) -> fl
     return float(tip_len_m) + float(surface_z_m) + float(gap_m)
 
 
+def final_gap_refusal(
+    gap_m: float,
+    tool_len_m: float,
+    surface_z_m: float,
+    hover_z_m: float,
+    z_min_m: float,
+    z_max_m: float,
+) -> str:
+    """笛卡尔锁 XY 的贴面间隙能不能用。空串表示可以；非空就是拒绝原因。
+
+    参数是规尖离面 [m]，不是法兰 Z：法兰 = 工具长 + 工件面 + 间隙，只在这里算一次。
+    """
+    gap = float(gap_m)
+    if not np.isfinite(gap):
+        return "贴面间隙不是有限数，拒绝开跑"
+    if gap <= 1e-9:
+        return (
+            f"贴面间隙 {gap * 1000:.1f}mm ≤ 0，规尖会进工件/贴面接触，拒绝开跑"
+        )
+    tool = float(tool_len_m)
+    surf = float(surface_z_m)
+    if tool <= 0.0 or surf <= 0.0:
+        return "写了贴面间隙，但工具长度或工件面未知，无法确认尖端在面之上，拒绝开跑"
+    hover_clear = float(hover_z_m) - tool - surf
+    if gap > hover_clear + 1e-6:
+        return (
+            f"贴面间隙 {gap * 1000:.1f}mm 比悬停离面 {hover_clear * 1000:.1f}mm 还高，"
+            "锁 XY 只会抬臂，拒绝开跑"
+        )
+    flange = flange_z_for_gap_m(tool, surf, gap)
+    z_lo = float(z_min_m)
+    z_hi = float(z_max_m)
+    if flange < z_lo - 1e-6 or flange > z_hi + 1e-6:
+        return (
+            f"贴面法兰 Z {flange:.3f}m（规尖离面 {gap * 1000:.1f}mm）"
+            f"超出工作盒 Z [{z_lo:.3f}, {z_hi:.3f}]，拒绝开跑"
+        )
+    return ""
+
+
 def decompose_base_xy(delta_xy, T_cam_to_base: np.ndarray):
     """把基座 XY 残差拆成沿视线 / 沿画面右 [m]。投影退化返回 None。"""
     axes = cam_xy_bias_axes(T_cam_to_base)

src/arm_application/arm_application/servo_node.py

@@ -14,12 +14,11 @@ from __future__ import annotations
 
 import os
 import math
-import subprocess
 import sys
 import threading
 import time
 from collections import deque
-from typing import Deque, List, Optional
+from typing import Deque, List, Optional, Tuple
 
 import numpy as np
 import rclpy
@@ -35,6 +34,7 @@ from rclpy.qos import (
     HistoryPolicy,
     QoSProfile,
     ReliabilityPolicy,
+    qos_profile_sensor_data,
     qos_profile_system_default,
 )
 from sensor_msgs.msg import JointState
@@ -52,11 +52,11 @@ from arm_application.handeye_lib import cam_point_to_base, cam_to_base_T
 from arm_application.overhead_oneshot import (
     D435_NEAR_CRUISE_MPS,
     D435_SETTLE_S,
-    D435_VENDOR_MOVEL_ACC_MPS2,
     D435_VENDOR_MOVEL_VEL_MPS,
     StillLock,
     at_lock_height,
     d405_commands_arm,
+    d435_movel_from_tcp,
     flange_tilt_rad,
     hold_translation_vz,
     hybrid_cruise_vxy,
@@ -88,6 +88,11 @@ from arm_application.site_io import (
 
 from arm_application.ros_node import TolerantNode
 
+try:
+    from flexiv_msgs.msg import RobotStates
+except ImportError:  # pragma: no cover
+    RobotStates = None  # type: ignore
+
 
 DEFAULT_JOINTS = [
     "joint1", "joint2", "joint3", "joint4", "joint5", "joint6", "joint7",
@@ -219,7 +224,8 @@ class ServoNode(TolerantNode):
         self.declare_parameter("lock_hold_sec", 2.0)
         # D405 跟住后法兰静止多久进二阶段。哨兵 -1 = 没从 site 接入。
         # lock_still_sec 仍须从 site 接上（缺了就报错）。
-        # 交回 D405：MoveL 结束后杀栈冷启动，再叠 d435_handback_sec（现场 3 s）。
+        # 交回 D405：进程内 MoveL 到位后，再叠 d435_handback_sec（法兰 XY 停稳）。
+        # 旧杀栈冷启动曾把这 3s 垫在控制器重启后面；不杀栈后只留机械停稳。
         self.declare_parameter("lock_still_sec", -1.0)
         self.declare_parameter("d435_handback_sec", -1.0)
         self.declare_parameter("wrist_align_sec", -1.0)
@@ -511,6 +517,24 @@ class ServoNode(TolerantNode):
             10,
             callback_group=self._control_cbg,
         )
+        # 厂家 tcp_pose（WORLD）。D435 MoveL 必须在这上面加 ROS 相对位移。
+        self._tcp_world: Optional[Tuple[float, float, float]] = None
+        self._t_tcp: Optional[float] = None
+        prefix_ns = str(self.get_parameter("joint_prefix").value or "").rstrip("_")
+        self.robot_states_topic = f"/{prefix_ns.replace('-', '_')}/flexiv_robot_states"
+        if RobotStates is not None:
+            self.create_subscription(
+                RobotStates,
+                self.robot_states_topic,
+                self._on_servo_robot_states,
+                qos_profile_sensor_data,
+                callback_group=self._state_cbg,
+            )
+        else:
+            self.get_logger().error(
+                "没有 flexiv_msgs.RobotStates，D435 粗定位不能读厂家 tcp_pose，"
+                "不许退回杀栈 MoveL"
+            )
 
         cmd_qos = QoSProfile(
             history=HistoryPolicy.KEEP_LAST,
@@ -606,6 +630,23 @@ class ServoNode(TolerantNode):
     def _on_js(self, msg: JointState) -> None:
         self._last_js = msg
 
+    def _on_servo_robot_states(self, msg) -> None:
+        """缓存厂家 ``tcp_pose``（WORLD）。名字避开其它节点的 ``_on_robot_states``。"""
+        p = msg.tcp_pose.pose.position
+        xyz = (float(p.x), float(p.y), float(p.z))
+        if not all(math.isfinite(v) for v in xyz):
+            return
+        self._tcp_world = xyz
+        self._t_tcp = time.monotonic()
+
+    def _fresh_tcp_world(self):
+        """最近 0.5s 的厂家 TCP [m]。过期/没有 → None，不许用旧值凑。"""
+        if self._tcp_world is None or self._t_tcp is None:
+            return None
+        if time.monotonic() - float(self._t_tcp) > 0.5:
+            return None
+        return self._tcp_world
+
     def _on_wrist_conf(self, msg: Float32) -> None:
         c = float(msg.data)
         with self._lock:
@@ -1545,12 +1586,17 @@ class ServoNode(TolerantNode):
         return False, ""
 
     def _d435_vendor_movel(self) -> bool:
-        """D435 这一点：停真机栈 → 厂家 MoveL → 冷启动控制栈。
+        """D435 这一点：进程内厂家 MoveL，不杀控制栈。
 
-        线速度 / 加速度走 ``D435_VENDOR_MOVEL_VEL_MPS``、``D435_VENDOR_MOVEL_ACC_MPS2``，
-        交给 SDK 的 ``vel`` / ``acc``。杀栈再拉起大约多几秒，RDK 独占，
-        D405 的点在整段 busy 期间都不会下发。停稳计时从冷启动回来之后才开始。
+        目标 = 当前厂家 TCP + (K0−法兰) 的 ROS XY，``keep_z``。硬件在 MoveL
+        期间不灌关节指令，D405 被 ``d435_commanding`` 挡住。不许退回杀栈
+        （那会冷启动十几秒，而且两套控制会互抢晃臂）。
         """
+        lib = ros2_root() / "scripts" / "lib"
+        if str(lib) not in sys.path:
+            sys.path.insert(0, str(lib))
+        from flexiv_movel_ipc import write_request
+
         with self._lock:
             goal = None if self._p_feat_base is None else np.asarray(
                 self._p_feat_base, dtype=float
@@ -1558,38 +1604,51 @@ class ServoNode(TolerantNode):
         if goal is None or not np.all(np.isfinite(goal)):
             self.get_logger().error("没有 D435 基座点，厂家 MoveL 不下发")
             return False
-        arm = str(os.environ.get("SITE_ROBOT_ARM") or "89").strip() or "89"
-        script = ros2_root() / "scripts" / "rdk_release_movel.py"
-        if not script.is_file():
-            self.get_logger().error(f"找不到 {script}")
+        tcp_now = self._fresh_tcp_world()
+        if tcp_now is None:
+            self.get_logger().error(
+                "没有厂家 tcp_pose（话题 "
+                f"{getattr(self, 'robot_states_topic', '?')} 不新鲜），"
+                "拒绝杀栈凑 MoveL"
+            )
             return False
-        cmd = [
-            sys.executable,
-            str(script),
-            "--xyz",
-            f"{float(goal[0]):.6f}",
-            f"{float(goal[1]):.6f}",
-            f"{float(goal[2]):.6f}",
-            "--arm",
-            arm,
-            "--vel",
-            f"{float(D435_VENDOR_MOVEL_VEL_MPS):.3f}",
-            "--acc",
-            f"{float(D435_VENDOR_MOVEL_ACC_MPS2):.3f}",
-        ]
+        T_ee = self._lookup_T(self.base_frame, self.ee_frame)
+        if T_ee is None:
+            self.get_logger().error("没有法兰 TF，D435 相对位移算不出，不下发")
+            return False
+        tgt = d435_movel_from_tcp(tcp_now, T_ee[:2, 3], goal[:2])
+        if tgt is None:
+            self.get_logger().error("D435 厂家 TCP 加上相对位移算不出，不下发")
+            return False
+        seq = int(time.monotonic() * 1000.0) % 1_000_000_000
+        write_request(
+            seq,
+            float(tgt[0]),
+            float(tgt[1]),
+            float(tgt[2]),
+            True,
+            float(D435_VENDOR_MOVEL_VEL_MPS),
+        )
         self.get_logger().info(
-            f"D435 厂家 MoveL XY=({goal[0]:.3f},{goal[1]:.3f}) "
-            f"vel={D435_VENDOR_MOVEL_VEL_MPS*1000:.0f}mm/s "
-            f"acc={D435_VENDOR_MOVEL_ACC_MPS2:.1f}m/s² "
-            "—— 先停控制栈，MoveL 后再冷启动；期间不下发 D405"
+            f"D435 进程内 MoveL XY=({goal[0]:.3f},{goal[1]:.3f}) "
+            f"厂家TCP现=({tcp_now[0]:.4f},{tcp_now[1]:.4f},{tcp_now[2]:.4f}) "
+            f"MoveL=({tgt[0]:.4f},{tgt[1]:.4f},keep_z) "
+            f"vel={D435_VENDOR_MOVEL_VEL_MPS * 1000:.0f}mm/s "
+            "—— 不杀栈；期间不下发 D405"
         )
         t0 = time.monotonic()
-        got = subprocess.run(cmd, check=False)
+        acked, _why = self._wait_movel_file(seq, 2.0, want_done=False)
+        if not acked:
+            self.get_logger().error(
+                "厂家 MoveL 2s 内没 ack —— 硬件没接 /tmp/flexiv_movel_req。"
+                "真机环境重建 install/ 后再跑，不许改走杀栈假装成功"
+            )
+            return False
+        ok, detail = self._wait_movel_file(seq, 90.0, want_done=True)
         dt = time.monotonic() - t0
-        ok = got.returncode == 0
         self.get_logger().info(
-            f"rdk_release_movel {'完成' if ok else '失败'}，耗时 {dt:.2f}s"
-            + (f" 退出码 {got.returncode}" if not ok else "")
+            f"D435 进程内 MoveL {'完成' if ok else '失败'}，耗时 {dt:.2f}s"
+            + (f" {detail}" if detail else "")
         )
         return bool(ok)
 

src/arm_application/arm_application/stage2_locate_node.py

@@ -10,6 +10,7 @@
   4. fine        — 拍照，two_stage（YOLO+HoughCircles）解 k1/k2 孔心像素
   5. align       — 像素射线交工件面 + 内参 + 手眼，把规尖移到目标孔正上方。
                    ``hole_keypoint_once`` 时只走一次，核验拍只记账；
+                   ``hole_once_lock_xy_descend`` 时按视线前馈一条 MoveL 贴面；
                    否则复拍迭代到 align_tol_m
   6. hold        — 锁定关节，发布 /stage2/result(JSON) + /tmp/stage2_result.json
 
@@ -22,6 +23,7 @@ from __future__ import annotations
 import json
 import math
 import os
+import sys
 import time
 from pathlib import Path
 from typing import Any, Dict, List, Optional, Tuple
@@ -53,12 +55,18 @@ from arm_application.path_profile import (
 )
 from arm_application.overhead_oneshot import (
     ONCE_APPROACH_STEP_M,
+    ONCE_FF_LOOK_PER_M,
+    ONCE_FF_MOVEL_VEL_MPS,
     ONCE_TRIM_MAX_STEP_M,
     ONCE_TRIM_REFUSE_M,
+    cam_xy_bias_axes,
     decompose_base_xy,
     flange_z_for_gap_m,
+    final_gap_refusal,
     hole_pair_span_m,
     hole_pair_span_ok,
+    once_ff_movel_from_tcp,
+    once_ff_tcp_xyz,
     oneshot_align_step_m,
     pixel_on_base_plane,
 )
@@ -67,9 +75,14 @@ from arm_application.gauge_boot import perception_source_of
 from arm_application.gauge_fsm import drag_verdict, resolve_hover_z, wrench_all_zero
 from arm_application.hole_detect_hough import detect_holes
 from arm_application.hole_detector import config_from_site, weights_from_site
-from arm_application.site_io import deep_merge, load_site_dict
+from arm_application.site_io import deep_merge, load_site_dict, ros2_root
 from arm_application.ros_node import TolerantNode
 
+try:
+    from flexiv_msgs.msg import RobotStates
+except ImportError:  # pragma: no cover
+    RobotStates = None  # type: ignore
+
 def contact_armed_gap_m(observe_gap_m: float, final_gap_m: float = 0.002) -> float:
     """轻降力接触门槛对应的几何间隙。离面更远时腕部力会漂几牛，不能当碰到。
 
@@ -152,6 +165,9 @@ class Stage2Locate(TolerantNode):
         self.declare_parameter("hover_only", False)
         self.declare_parameter("target_hole", "")      # 空字符串 → 用 site 的 stage2.target_hole
         self.declare_parameter("hover_z_m", -1.0)      # 大于 0 → 覆盖 site 里的值
+        # 贴面规尖离面 [mm]。≥0 → 覆盖 site ``final_gap_mm``。-1=用 site。
+        # 笛卡尔锁 XY 时法兰 Z = 工具长 + 工件面 + 间隙，XY 不动。
+        self.declare_parameter("final_gap_mm", -1.0)
         # 工具尖端在法兰下方多少 [m]；大于 0 → 覆盖 site。
         # 仿真里由 run_gauge_assembly.sh --sim 传**实测值**（每次上料都可能不同），
         # 真机不传（走 site.yaml）。悬停高度与 S07 下探预算都按它算。
@@ -235,8 +251,14 @@ class Stage2Locate(TolerantNode):
         # k1/k2，而流程会把它当"孔检测失败"，现场根本想不到是粗居中把臂带走的。
         self.coarse_max_offset_m = float(stage2.get("coarse_max_offset_m") or 0.12)
         self.coarse_border_px = int(stage2.get("coarse_border_px") or 2)
-        # 最后轻降到孔面上方 final_gap_mm（不接触）
-        self.final_gap_m = float(stage2.get("final_gap_mm") or 1.5) / 1000.0
+        # 最后轻降到孔面上方 final_gap_mm（不接触）。启动参数 ≥0 覆盖 site。
+        self._final_gap_refused = ""
+        _gap_override_mm = float(self.get_parameter("final_gap_mm").value)
+        self.final_gap_user_set = _gap_override_mm >= 0.0
+        if self.final_gap_user_set:
+            self.final_gap_m = _gap_override_mm / 1000.0
+        else:
+            self.final_gap_m = float(stage2.get("final_gap_mm") or 1.5) / 1000.0
         # 观测下降：对准后降到这个间隙就停，方便看规尖是不是对着孔。
         # 比 final_gap（贴面 2mm）高一截，对偏了也不会把规压上孔沿。
         self.observe_gap_m = float(stage2.get("observe_gap_mm") or 15.0) / 1000.0
@@ -269,6 +291,16 @@ class Stage2Locate(TolerantNode):
         # 本分支实验：悬停拍一次 k1/k2，走一次，复拍只记账不改 XY。
         # 键必须接线——写了 true 却仍多走多拍，等于没做实验。
         self.hole_keypoint_once = _as_bool(stage2.get("hole_keypoint_once"), False)
+        # 悬停走完后一条厂家 MoveL 收到贴面。k=0：笛卡尔锁 XY 只降 Z。
+        # 缺键 = 仍走观察高补交（37b85aa）。
+        self.hole_once_lock_xy_descend = _as_bool(
+            stage2.get("hole_once_lock_xy_descend"), False
+        )
+        _ff = stage2.get("once_ff_look_per_m")
+        if _ff is None:
+            self.once_ff_look_per_m = float(ONCE_FF_LOOK_PER_M)
+        else:
+            self.once_ff_look_per_m = float(_ff)
         self._ws_clipped = False
         self._hole_snaps: List[Dict[str, Any]] = []
         if self.hover_z_user_set and (_tool_len <= 0.0 or _surface_z <= 0.0):
@@ -314,6 +346,28 @@ class Stage2Locate(TolerantNode):
                 f"final_descent_enable 被启动参数覆盖为 {self.final_descent_enable} "
                 f"（site 里是 {stage2.get('final_descent_enable')}）—— 只在调试时这么用"
             )
+        if self.final_gap_user_set and not self.final_descent_enable:
+            self._final_gap_refused = (
+                "同时写了 --final-gap 和 --no-descent，不知道听谁的，拒绝开跑"
+            )
+            self.get_logger().error(self._final_gap_refused)
+        elif self.final_gap_user_set and (_tool_len <= 0.0 or _surface_z <= 0.0):
+            self._final_gap_refused = (
+                "写了 --final-gap，但工具长度或工件面未知，无法确认尖端在面之上，拒绝执行"
+            )
+            self.get_logger().error(self._final_gap_refused)
+        elif _tool_len > 0.0 and _surface_z > 0.0:
+            _why_gap = final_gap_refusal(
+                self.final_gap_m,
+                _tool_len,
+                _surface_z,
+                self.hover_z,
+                float(self.ws_min[2]),
+                float(self.ws_max[2]),
+            )
+            if _why_gap:
+                self._final_gap_refused = _why_gap
+                self.get_logger().error(_why_gap)
         self.vert_tol_rad = math.radians(float(stage2.get("verticalize_tol_deg") or 0.8))
         self.settle_sec = float(stage2.get("settle_sec") or 0.45)
         self.snap_frames = int(stage2.get("snap_frames") or 5)
@@ -398,6 +452,23 @@ class Stage2Locate(TolerantNode):
         self.create_subscription(
             WrenchStamped, self.ft_wrench_topic, self._on_wrench, qos_profile_sensor_data
         )
+        # 厂家 tcp_pose（WORLD）。视线前馈 MoveL 必须在这上面加相对位移，
+        # 不许拿 ROS 法兰+规尖当绝对目标。回调名避开 stage3 的 `_on_robot_states`。
+        self._tcp_world: Optional[Tuple[float, float, float]] = None
+        self._t_tcp: Optional[float] = None
+        self.robot_states_topic = f"/{robot_ns}/flexiv_robot_states"
+        if RobotStates is not None:
+            self.create_subscription(
+                RobotStates,
+                self.robot_states_topic,
+                self._on_stage2_robot_states,
+                qos_profile_sensor_data,
+            )
+        elif self.hole_once_lock_xy_descend:
+            self.get_logger().error(
+                "没有 flexiv_msgs.RobotStates，视线前馈不能读厂家 tcp_pose，"
+                "不许退回 ROS 绝对坐标"
+            )
 
         self._state_pub = self.create_publisher(String, "/stage2/state", 10)
         self._result_pub = self.create_publisher(String, "/stage2/result", 10)
@@ -415,10 +486,33 @@ class Stage2Locate(TolerantNode):
             f"  image={self.image_topic} depth={self.depth_topic}\n"
             f"  target={self.target_hole} (k1={self.k1_label})  tol={self.align_tol*1000:.1f}mm  "
             f"hover_only={self.hover_only}  hole_visual_only={self.hole_visual_only}  "
-            f"hole_keypoint_once={self.hole_keypoint_once}\n"
+            f"hole_keypoint_once={self.hole_keypoint_once}  "
+            f"lock_xy_descend={self.hole_once_lock_xy_descend}  "
+            f"ff_look={self.once_ff_look_per_m:.3f}  "
+            f"final_gap={self.final_gap_m * 1000:.1f}mm"
+            + (
+                f"→法兰Z={flange_z_for_gap_m(self.tool_tip_below_flange_m, self.surface_z_m, self.final_gap_m):.4f}m"
+                if self.tool_tip_below_flange_m > 0.0 and self.surface_z_m > 0.0
+                else ""
+            )
+            + "\n"
             f"  工具长={self.tool_tip_below_flange_m:.4f}m（法兰到尖，下臂不得低于面+间隙）"
         )
-        if self.hole_keypoint_once:
+        if self.hole_keypoint_once and self.hole_once_lock_xy_descend:
+            k = float(self.once_ff_look_per_m)
+            if k <= 1e-9:
+                self.get_logger().warn(
+                    "对孔方案=单次精拍·笛卡尔锁 XY（实验）。悬停走完后不补拍，"
+                    "厂家 MoveL 只降 Z、ΔXY=0。k>0 会沿 -look 再加前馈。"
+                    "对照 37b85aa 把 hole_once_lock_xy_descend 改回 false。"
+                )
+            else:
+                self.get_logger().warn(
+                    "对孔方案=单次精拍·视线前馈斜线（实验）。悬停走完后不补拍，"
+                    f"ΔXY=-look×{k:.3f}×剩余下降，一条 MoveL 到贴面。"
+                    "对照 37b85aa 把 hole_once_lock_xy_descend 改回 false。"
+                )
+        elif self.hole_keypoint_once:
             self.get_logger().warn(
                 "对孔方案=单次精拍（实验分支）。拍一次→走一次，复拍和下降后只记账不改 XY。"
                 "残差以核验拍为准，不许用 hop 到位冒充。看 /tmp/stage2_result.json 的 snaps。"
@@ -438,6 +532,23 @@ class Stage2Locate(TolerantNode):
     def _on_info(self, msg: CameraInfo) -> None:
         self._info = msg
 
+    def _on_stage2_robot_states(self, msg) -> None:
+        """缓存厂家 ``tcp_pose``（WORLD）。名字必须与 stage3 ``_on_robot_states`` 分开。"""
+        p = msg.tcp_pose.pose.position
+        xyz = (float(p.x), float(p.y), float(p.z))
+        if not all(math.isfinite(v) for v in xyz):
+            return
+        self._tcp_world = xyz
+        self._t_tcp = time.monotonic()
+
+    def _fresh_tcp_world(self):
+        """最近 0.5s 的厂家 TCP [m]。过期/没有 → None，不许用旧值凑。"""
+        if self._tcp_world is None or self._t_tcp is None:
+            return None
+        if time.monotonic() - float(self._t_tcp) > 0.5:
+            return None
+        return self._tcp_world
+
     def _on_wrench(self, msg: WrenchStamped) -> None:
         """缓存腕部 FT 的**轴向力**（`geometry_msgs/WrenchStamped`，法兰系）。
 
@@ -1652,7 +1763,7 @@ class Stage2Locate(TolerantNode):
         """
         # final_descent_enable：多走多拍降到贴面 final_gap（约 2mm）。
         # 关掉时（--no-descent）仍做**观测下降**，停在 observe_gap（约 15mm）。
-        # 单次精拍由调用方传入 target_gap_m=观察高，贴面由随后斜线一步完成。
+        # 单次精拍补交路径传入观察高；锁 XY 实验传入贴面 final_gap。
         if target_gap_m is None:
             target_gap = (
                 self.final_gap_m if self.final_descent_enable else self.observe_gap_m
@@ -1817,7 +1928,12 @@ class Stage2Locate(TolerantNode):
         """把基座残差拆成视线 / 画面右，写入记录。投影退化时键仍在、值为 None。"""
         rec = dict(rec)
         rec["tag"] = tag
-        rec["scheme"] = "keypoint_once" if self.hole_keypoint_once else "multi_snap"
+        if self.hole_keypoint_once and getattr(self, "hole_once_lock_xy_descend", False):
+            rec["scheme"] = "keypoint_once_look_ff"
+        elif self.hole_keypoint_once:
+            rec["scheme"] = "keypoint_once"
+        else:
+            rec["scheme"] = "multi_snap"
         rec["cam_frame"] = self.cam_frame
         rec["image_topic"] = self.image_topic
         err_vec = np.array(
@@ -1992,6 +2108,155 @@ class Stage2Locate(TolerantNode):
             )
         return q, Tee, True
 
+    def _vendor_movel_tcp(self, xyz, vel: float) -> bool:
+        """进程内厂家 MoveL：TCP 目标，不杀控制栈。"""
+        lib = ros2_root() / "scripts" / "lib"
+        if str(lib) not in sys.path:
+            sys.path.insert(0, str(lib))
+        from flexiv_movel_ipc import read_ack, read_done, write_request
+
+        seq = int(time.monotonic() * 1000.0) % 1_000_000_000
+        write_request(
+            seq, float(xyz[0]), float(xyz[1]), float(xyz[2]), False, float(vel)
+        )
+        t0 = time.monotonic()
+        acked = False
+        while time.monotonic() - t0 < 90.0:
+            if not rclpy.ok():
+                return False
+            self._spin(0.02)
+            if not acked:
+                if read_ack() == seq:
+                    acked = True
+                elif time.monotonic() - t0 > 2.0:
+                    self.get_logger().error(
+                        "厂家 MoveL 2s 内没 ack —— 硬件没接 /tmp/flexiv_movel_req。"
+                        "真机环境重建 install/ 后再跑，不许改走关节折线假装成功"
+                    )
+                    return False
+                continue
+            got = read_done()
+            if got is not None and int(got[0]) == seq:
+                if not got[1]:
+                    self.get_logger().error(f"厂家 MoveL 失败：{got[2]}")
+                return bool(got[1])
+        self.get_logger().error("厂家 MoveL 90s 内没有 done")
+        return False
+
+    def _once_ff_diagonal_to_gap(self, q, Tee, *, target_gap_m):
+        """悬停点一条 MoveL 收到 ``target_gap_m``。k=0 只降 Z；k>0 再沿 -look 收 XY。"""
+        self._state("ff_diagonal_movel")
+        tip_len = float(self.tool_tip_below_flange_m)
+        surf = float(self.surface_z_m)
+        if tip_len <= 0.0 or surf <= 0.0:
+            self.get_logger().error("贴面下降缺工具长/工件面，停在悬停高")
+            return False, q, Tee
+        if self._tip_off is None:
+            self.get_logger().error("贴面下降需要悬停规尖偏移，没有就不许用法兰凑")
+            return False, q, Tee
+        k = float(self.once_ff_look_per_m)
+        look_xy = np.array([0.0, 0.0], dtype=float)
+        T_cam = self._lookup(self.base_frame, self.cam_frame)
+        if k > 1e-9:
+            if T_cam is None:
+                self.get_logger().error("没有 cam TF，视线轴算不出，不下发")
+                return False, q, Tee
+            axes = cam_xy_bias_axes(T_cam)
+            if axes is None:
+                self.get_logger().error("光轴水平投影退化，不下发")
+                return False, q, Tee
+            look_xy, _right = axes
+        elif T_cam is not None:
+            axes = cam_xy_bias_axes(T_cam)
+            if axes is not None:
+                look_xy, _right = axes
+        gap0 = float(Tee[2, 3]) - tip_len - surf
+        descent = gap0 - float(target_gap_m)
+        packed = once_ff_tcp_xyz(
+            [Tee[0, 3], Tee[1, 3]],
+            self._tip_off,
+            look_xy,
+            descent,
+            k,
+            surf,
+            target_gap_m,
+        )
+        if packed is None:
+            if descent <= 0.001:
+                self.get_logger().info("规尖已在目标间隙附近，无需斜线")
+                self.descent_final_gap_m = gap0
+                return True, q, Tee
+            self.get_logger().error("视线前馈算不出 ΔXY，停在悬停高")
+            return False, q, Tee
+        tcp, dxy = packed
+        n_xy = float(np.hypot(float(dxy[0]), float(dxy[1])))
+        if n_xy > ONCE_TRIM_REFUSE_M + 1e-9:
+            self.get_logger().error(
+                f"视线前馈 {n_xy * 1000:.1f}mm 超过 {ONCE_TRIM_REFUSE_M * 1000:.0f}mm，"
+                "不像交平面那几毫米，停在悬停高"
+            )
+            return False, q, Tee
+        z_flange = flange_z_for_gap_m(tip_len, surf, target_gap_m)
+        flange_tgt = np.array(
+            [
+                float(Tee[0, 3]) + float(dxy[0]),
+                float(Tee[1, 3]) + float(dxy[1]),
+                z_flange,
+            ]
+        )
+        self._ws_clipped = False
+        self._clamp_ws(flange_tgt)
+        if self._ws_clipped:
+            self.get_logger().error(
+                "视线前馈目标被工作盒截断，不下发（不许把孔位改到盒沿）"
+            )
+            return False, q, Tee
+        self._spin(0.05)
+        tcp_now = self._fresh_tcp_world()
+        if tcp_now is None:
+            self.get_logger().error(
+                "没有厂家 tcp_pose（话题 "
+                f"{getattr(self, 'robot_states_topic', '?')} 不新鲜），"
+                "拒绝用 ROS 规尖绝对坐标发 MoveL"
+            )
+            return False, q, Tee
+        movel_xyz = once_ff_movel_from_tcp(tcp_now, dxy, descent)
+        if movel_xyz is None:
+            self.get_logger().error("厂家 TCP 加上相对位移算不出，停在悬停高")
+            return False, q, Tee
+        off_xy = (
+            (float(tcp_now[0]) - float(tcp[0]) + float(dxy[0])) * 1000.0,
+            (float(tcp_now[1]) - float(tcp[1]) + float(dxy[1])) * 1000.0,
+        )
+        scheme = "笛卡尔锁XY只降Z" if n_xy < 1e-6 else "视线前馈斜线"
+        self.get_logger().info(
+            f"{scheme}：look=({look_xy[0]:+.3f},{look_xy[1]:+.3f}) "
+            f"k={k:.3f} 降 {descent * 1000:.1f}mm → "
+            f"ΔXY=({dxy[0] * 1000:+.2f},{dxy[1] * 1000:+.2f})mm "
+            f"厂家TCP现=({tcp_now[0]:.4f},{tcp_now[1]:.4f},{tcp_now[2]:.4f}) "
+            f"MoveL=({movel_xyz[0]:.4f},{movel_xyz[1]:.4f},{movel_xyz[2]:.4f}) "
+            f"ROS尖目标=({tcp[0]:.4f},{tcp[1]:.4f},{tcp[2]:.4f}) "
+            f"WORLD相对ROS XY差≈({off_xy[0]:+.1f},{off_xy[1]:+.1f})mm "
+            f"vel={ONCE_FF_MOVEL_VEL_MPS * 1000:.0f}mm/s"
+        )
+        fl0 = (float(Tee[0, 3]), float(Tee[1, 3]), float(Tee[2, 3]))
+        ok = self._vendor_movel_tcp(movel_xyz, ONCE_FF_MOVEL_VEL_MPS)
+        q2, Tee2 = self._wait_state(2.0)
+        if q2 is not None and Tee2 is not None:
+            q, Tee = q2, Tee2
+        self._pin(q)
+        self.descent_final_gap_m = float(Tee[2, 3]) - tip_len - surf
+        dax = (float(Tee[0, 3]) - fl0[0]) * 1000.0
+        day = (float(Tee[1, 3]) - fl0[1]) * 1000.0
+        daz = (float(Tee[2, 3]) - fl0[2]) * 1000.0
+        self.get_logger().info(
+            f"贴面走后：法兰 ΔXYZ=({dax:+.2f},{day:+.2f},{daz:+.2f})mm "
+            f"指令 ΔXY=({dxy[0] * 1000:+.2f},{dxy[1] * 1000:+.2f})mm "
+            f"间隙 {self.descent_final_gap_m * 1000:.1f}mm "
+            f"{'到位' if ok else 'MoveL未完成'}"
+        )
+        return bool(ok), q, Tee
+
     def _trim_xy_keep_hover_tip(self, q, Tee, final: Dict[str, Any], z_target_m=None):
         """观察高重交孔，规尖沿用悬停冻住的偏移。只许一跳、XY ≤3mm。
 
@@ -2470,6 +2735,9 @@ class Stage2Locate(TolerantNode):
         if self._hover_refused:
             self.get_logger().error(self._hover_refused)
             return 2
+        if self._final_gap_refused:
+            self.get_logger().error(self._final_gap_refused)
+            return 2
 
         if self.hover_z_user_set:
             z_lo = float(self.ws_min[2])
@@ -2641,6 +2909,37 @@ class Stage2Locate(TolerantNode):
             if truth is not None and truth.get("part_center_base"):
                 p_base = np.asarray(truth["part_center_base"][0], dtype=float)
                 src = "真值"
+            # 对孔走平面交点时，框心必须同一条射线∩面。深度框心沿 30° 视线
+            # 偏 1～2mm，符号随站位翻（D435 送到夹具位 vs 纯 D405 停在别处），
+            # 精拍再把这截当 K1.x−规尖.x 走掉，前馈又沿同轴加一次。
+            if p_base is None and self.hole_visual_only:
+                if float(self.surface_z_m) <= 0.0:
+                    self.get_logger().error(
+                        "粗居中要对孔同一平面，但 workpiece_surface_z_m 没有配，"
+                        "拒绝用深度图凑框心"
+                    )
+                    break
+                T_cam = self._lookup(self.base_frame, self.cam_frame)
+                if T_cam is None:
+                    self.get_logger().error("粗居中缺 cam TF，拒绝用深度图凑框心")
+                    break
+                hit = pixel_on_base_plane(
+                    float(bx),
+                    float(by),
+                    float(intr["fx"]),
+                    float(intr["fy"]),
+                    float(intr["cx"]),
+                    float(intr["cy"]),
+                    T_cam,
+                    float(self.surface_z_m),
+                )
+                if hit is None:
+                    self.get_logger().error(
+                        "粗居中框心射线没交到工件面，拒绝用深度图凑"
+                    )
+                    break
+                p_base = hit
+                src = "像素交工件面"
             if p_base is None:
                 z_wp = self._surface_depth(depth_m, bbox)
                 p_base = self._cam_to_base(self._px_to_cam(bx, by, z_wp, intr))
@@ -2931,21 +3230,32 @@ class Stage2Locate(TolerantNode):
                     final["align_trusted"] = False
                     final["fail_reason"] = "未量到规尖偏移，工具位置退化为法兰轴线"
                 q, Tee, _moved = self._oneshot_align_move(q, Tee, tgt_b, gxy, err)
-                q, Tee, final = self._observe_holes(
-                    q, Tee, final, tag="走后核验"
-                )
                 final["commanded_residual_mm"] = commanded_mm
-                if not final.get("residual_remeasured"):
-                    self.get_logger().error(
-                        f"走后核验没复测到。开环指令残差 {commanded_mm:.2f}mm，"
-                        "不是 hop 到位，不把这次当对准成功"
+                if self.hole_once_lock_xy_descend:
+                    final["residual_remeasured"] = False
+                    final["lock_xy_descend"] = True
+                    if self._tip_off is not None:
+                        final["ok"] = True
+                    self.get_logger().warn(
+                        f"视线前馈斜线：悬停指令 {commanded_mm:.2f}mm 已走，"
+                        "不补拍。下一步按 k×下降收 XY，一条 MoveL 到贴面。"
+                        "residual_mm 仍是走之前那一拍，不是贴面复测"
+                    )
+                else:
+                    q, Tee, final = self._observe_holes(
+                        q, Tee, final, tag="走后核验"
+                    )
+                    if not final.get("residual_remeasured"):
+                        self.get_logger().error(
+                            f"走后核验没复测到。开环指令残差 {commanded_mm:.2f}mm，"
+                            "不是 hop 到位，不把这次当对准成功"
+                        )
+                        final["ok"] = False
+                    self.get_logger().info(
+                        f"单次精拍悬停走完：指令 {commanded_mm:.2f}mm → "
+                        f"复测 {final.get('residual_mm')}mm；下降后还要补交平面漂的那几毫米"
                     )
-                    final["ok"] = False
                 self._state("aligned")
-                self.get_logger().info(
-                    f"单次精拍悬停走完：指令 {commanded_mm:.2f}mm → "
-                    f"复测 {final.get('residual_mm')}mm；下降后还要补交平面漂的那几毫米"
-                )
                 break
 
             if err <= self.align_tol:
@@ -3036,7 +3346,15 @@ class Stage2Locate(TolerantNode):
         # ---- 4.5) 柔顺轻降到孔面上方 1-2mm（不接触）----
         # 下降量由**几何**决定（法兰 FK − 工具长 − 工件面 − final_gap），深度读数只
         # 做交叉核对，见 _descend_above_hole 的说明。
-        if self.hole_keypoint_once:
+        if self.hole_keypoint_once and self.hole_once_lock_xy_descend:
+            # 视线前馈斜线：不经过观察高、不补交。一条 MoveL 收到贴面。
+            lock_gap = (
+                self.final_gap_m if self.final_descent_enable else self.observe_gap_m
+            )
+            okd, q, Tee = self._once_ff_diagonal_to_gap(
+                q, Tee, target_gap_m=lock_gap
+            )
+        elif self.hole_keypoint_once:
             # 先降到观察高（约 15mm）再交孔；贴面由补孔斜线一步完成，不再 4mm 切到 2mm。
             okd, q, Tee = self._descend_above_hole(
                 q, Tee,
@@ -3047,7 +3365,12 @@ class Stage2Locate(TolerantNode):
             okd, q, Tee = self._descend_above_hole(q, Tee)
         if not okd:
             self.get_logger().warn("轻降未完全到位，保持当前高度（安全优先）")
-        if self.hole_keypoint_once:
+        if self.hole_keypoint_once and self.hole_once_lock_xy_descend:
+            tip_len = float(self.tool_tip_below_flange_m)
+            surf = float(self.surface_z_m)
+            if tip_len > 0.0 and surf > 0.0:
+                self.descent_final_gap_m = float(Tee[2, 3]) - tip_len - surf
+        elif self.hole_keypoint_once:
             # 悬停那一拍的基座 XY 不能带到观测高：前倾 30°，降 50mm 交点会漂约 4mm。
             # 核验记账后补 XY；规尖沿用悬停冻结值，不在观测高度重测。
             q, Tee, final = self._observe_holes(q, Tee, final, tag="降后核验")

src/arm_application/launch/pipeline.launch.py

@@ -84,6 +84,7 @@ def _launch_setup(context, *args, **kwargs):
     d435_only = _as_bool(LaunchConfiguration("d435_move_only").perform(context))
     init_z = LaunchConfiguration("init_z_m").perform(context).strip()
     hover_z = LaunchConfiguration("hover_z_m").perform(context).strip()
+    final_gap = LaunchConfiguration("final_gap_mm").perform(context).strip()
     target_hole = LaunchConfiguration("target_hole").perform(context).strip()
     depth = LaunchConfiguration("insert_depth_mm").perform(context).strip()
     pitch = LaunchConfiguration("screw_pitch_mm").perform(context).strip()
@@ -125,6 +126,7 @@ def _launch_setup(context, *args, **kwargs):
                 "d435_move_only": d435_only,
                 "init_z_m": float(init_z) if init_z else -1.0,
                 "hover_z_m": float(hover_z) if hover_z else -1.0,
+                "final_gap_mm": float(final_gap) if final_gap else -1.0,
                 # -1=按 site 的 stage2.final_descent_enable；0=强制不降（调试）
                 "final_descent_enable": 0 if no_descent else -1,
             }
@@ -136,7 +138,8 @@ def _launch_setup(context, *args, **kwargs):
         LogInfo(msg=f"[pipeline] arm={robot.get('label') or robot.get('sn')} cam={cam}"),
         LogInfo(
             msg=f"[pipeline] hole={target_hole or 'site'} depth={depth or 'site'} "
-            f"pitch={pitch or 'site'} skip1={skip1} skip2={skip2} skip3={skip3}"
+            f"pitch={pitch or 'site'} final_gap={final_gap or 'site'} "
+            f"skip1={skip1} skip2={skip2} skip3={skip3}"
             + (" [d435_only：只测 D435 位移]" if d435_only else "")
             + (" [no_descent：只对准，不轻降]" if no_descent else "")
         ),
@@ -166,6 +169,7 @@ def generate_launch_description() -> LaunchDescription:
             DeclareLaunchArgument("d435_move_only", default_value="false"),
             DeclareLaunchArgument("init_z_m", default_value=""),
             DeclareLaunchArgument("hover_z_m", default_value=""),
+            DeclareLaunchArgument("final_gap_mm", default_value=""),
             DeclareLaunchArgument("preview", default_value="true"),
             OpaqueFunction(function=_launch_setup),
         ]

src/arm_application/launch/stage2_locate.launch.py

@@ -75,6 +75,7 @@ def _launch_setup(context, *args, **kwargs):
     preview = LaunchConfiguration("preview").perform(context).lower() in ("1", "true", "yes")
     target_hole = LaunchConfiguration("target_hole").perform(context).strip()
     hover_z = LaunchConfiguration("hover_z_m").perform(context).strip()
+    final_gap = LaunchConfiguration("final_gap_mm").perform(context).strip()
 
     tx, ty, tz, qx, qy, qz, qw = _handeye_xyz_xyzw(handeye)
     static_tf = Node(
@@ -101,6 +102,7 @@ def _launch_setup(context, *args, **kwargs):
                 "hover_only": hover_only,
                 "target_hole": target_hole,
                 "hover_z_m": float(hover_z) if hover_z else -1.0,
+                "final_gap_mm": float(final_gap) if final_gap else -1.0,
                 "preview": preview,
             }
         ],
@@ -123,6 +125,7 @@ def generate_launch_description() -> LaunchDescription:
             DeclareLaunchArgument("hover_only", default_value="false"),
             DeclareLaunchArgument("target_hole", default_value=""),
             DeclareLaunchArgument("hover_z_m", default_value=""),
+            DeclareLaunchArgument("final_gap_mm", default_value=""),
             DeclareLaunchArgument("preview", default_value="true"),
             OpaqueFunction(function=_launch_setup),
         ]

src/arm_application/test/test_overhead_oneshot.py

@@ -60,7 +60,14 @@ from arm_application.overhead_oneshot import (
     hole_pair_span_ok,
     oneshot_align_step_m,
     flange_z_for_gap_m,
+    final_gap_refusal,
+    d435_movel_from_tcp,
+    once_ff_movel_from_tcp,
+    once_ff_tcp_xyz,
+    once_look_ff_xy,
     ONCE_APPROACH_STEP_M,
+    ONCE_FF_LOOK_PER_M,
+    ONCE_FF_MOVEL_VEL_MPS,
     ONCE_TRIM_MAX_STEP_M,
     ONCE_TRIM_REFUSE_M,
     pixel_on_base_plane,
@@ -175,21 +182,44 @@ class TestHandoff:
         ).read_text(encoding="utf-8")
         assert "pixel_on_base_plane(" in stage2
         assert "hole_visual_only" in stage2
+        coarse = stage2[stage2.find("# ---- 3) 粗居中") : stage2.find("# ---- 4)")]
+        assert "pixel_on_base_plane" in coarse
+        assert "拒绝用深度图凑框心" in coarse
         assert "_trim_xy_after_descent" in stage2
         assert "align_max_iters" in stage2
         assert "像素交工件面" in stage2
         assert "det_depth = None if self.hole_visual_only else med_depth" in stage2
         assert 'self.hole_keypoint_once = _as_bool(stage2.get("hole_keypoint_once")' in stage2
+        assert 'self.hole_once_lock_xy_descend = _as_bool(' in stage2
         obs = stage2[stage2.find("def _observe_holes"): stage2.find("def _oneshot_align_move")]
         assert "_goto(" not in obs, "核验拍不许改 XY"
         assert "residual_remeasured" in obs
-        desc = stage2[stage2.find("okd, q, Tee = self._descend_above_hole"):]
-        desc = desc[:1800]
+        desc = stage2[stage2.find("# ---- 4.5)"):]
+        desc = desc[:2800]
+        assert "hole_once_lock_xy_descend" in desc
         assert "if self.hole_keypoint_once:" in desc
         assert 'tag="降后核验"' in desc
         assert "_trim_xy_after_descent" in desc
         assert "target_gap_m=self.observe_gap_m" in desc
         assert "ONCE_APPROACH_STEP_M" in desc
+        lock_at = stage2.find("# ---- 4.5)")
+        assert lock_at > 0
+        lock_block = stage2[lock_at: stage2.find("elif self.hole_keypoint_once:", lock_at)]
+        assert "hole_once_lock_xy_descend" in lock_block
+        assert "lock_gap" in lock_block
+        assert "_once_ff_diagonal_to_gap" in lock_block
+        assert "self._descend_above_hole" not in lock_block
+        assert "_observe_holes" not in lock_block
+        assert "_trim_xy_after_descent" not in lock_block
+        assert "once_ff_tcp_xyz" in stage2
+        assert "once_ff_movel_from_tcp" in stage2
+        assert "_fresh_tcp_world" in stage2
+        assert "没有厂家 tcp_pose" in stage2
+        assert "write_request" in stage2
+        moved_at = stage2.find("q, Tee, _moved = self._oneshot_align_move")
+        moved_blk = stage2[moved_at: stage2.find("self._state(\"aligned\")", moved_at) + 80]
+        assert "hole_once_lock_xy_descend" in moved_blk
+        assert 'tag="走后核验"' in moved_blk
         keep = stage2[
             stage2.find("def _trim_xy_keep_hover_tip"): stage2.find("def _trim_xy_after_descent")
         ]
@@ -235,12 +265,15 @@ class TestHandoff:
         assert abs(euler[0]) == pytest.approx(180.0, abs=1e-6)
         assert abs(euler[1]) == pytest.approx(0.0, abs=1e-6)
         assert abs(euler[2]) == pytest.approx(0.0, abs=1e-6)
-        assert "rdk_release_movel.py" in servo
         movel = servo[servo.find("def _d435_vendor_movel"): servo.find("def _d435_oneshot_holds_locked")]
+        assert "rdk_release_movel.py" not in movel
+        assert "write_request" in movel
+        assert "d435_movel_from_tcp" in movel
+        assert "keep_z" in movel or "True" in movel
+        assert "拒绝杀栈凑 MoveL" in movel
         assert "D435_VENDOR_MOVEL_VEL_MPS" in movel
-        assert "D435_VENDOR_MOVEL_ACC_MPS2" in movel
-        assert '"--acc"' in movel
-        assert '"0.05"' not in movel
+        assert "D435_VENDOR_MOVEL_ACC_MPS2" not in movel
+        assert '"--acc"' not in movel
         assert D435_VENDOR_MOVEL_VEL_MPS == pytest.approx(0.3)
         assert D435_VENDOR_MOVEL_ACC_MPS2 == pytest.approx(1.5)
         rel = (
@@ -251,10 +284,14 @@ class TestHandoff:
         assert f"default={D435_VENDOR_MOVEL_ACC_MPS2}" in rel
         assert '"--acc"' in rel
         assert 'prim["acc"]' in goto
+        tgt = d435_movel_from_tcp([0.580, -0.460, 0.096], [0.687, -0.113], [0.708, -0.168])
+        assert tgt is not None
+        np.testing.assert_allclose(tgt, [0.580 + 0.021, -0.460 - 0.055, 0.096], atol=1e-9)
+        assert d435_movel_from_tcp(None, [0.0, 0.0], [0.1, 0.1]) is None
         assert "d435_handback_sec" in servo
         assert "still_sec=max(0.0, self.d435_handback_sec)" in servo
         assert "wrist_align_sec" in servo
-        # 杀栈 MoveL 还在走时不许把 vendor_done 提前置上，停稳计时也不许在这段路上累计。
+        # 进程内 MoveL 还在走时不许把 vendor_done 提前置上，停稳计时也不许在这段路上累计。
         call_at = servo.find("ok = self._d435_vendor_movel()")
         done_at = servo.find("self._d435_vendor_done = True", call_at)
         assert call_at > 0 and done_at > call_at
@@ -274,6 +311,23 @@ class TestHandoff:
         flat = np.eye(4)
         flat[:3, 3] = [0.0, 0.0, 0.50]
         assert pixel_on_base_plane(320, 240, 400, 400, 320, 240, flat, 0.022) is None
+        # 前倾 30°：同一主点，深度偏远的点比平面交点更 −X。爪站在深度点上时
+        # K1.x−规尖.x > 0；深度偏近则反号。这就是两套一阶段站位符号相反的几何。
+        ca, sa = float(np.cos(np.deg2rad(30))), float(np.sin(np.deg2rad(30)))
+        tilt = np.eye(4)
+        tilt[:3, :3] = np.array(
+            [[0.0, sa, -ca], [1.0, 0.0, 0.0], [0.0, -ca, -sa]], dtype=float
+        )
+        tilt[:3, 3] = [0.65, -0.50, 0.19]
+        plane_pt = pixel_on_base_plane(320, 240, 400, 400, 320, 240, tilt, 0.022)
+        assert plane_pt is not None
+        ray = tilt[:3, :3] @ np.array([0.0, 0.0, 1.0])
+        ray = ray / float(np.linalg.norm(ray))
+        far = plane_pt[:3] + 0.020 * ray
+        near = plane_pt[:3] - 0.020 * ray
+        assert far[0] < plane_pt[0] < near[0]
+        assert (plane_pt[0] - far[0]) > 0.0
+        assert (plane_pt[0] - near[0]) < 0.0
         lib = (
             __import__("pathlib").Path(__file__).resolve().parents[3] / "scripts" / "lib"
         )
@@ -815,6 +869,10 @@ class TestHoleBiasAxes:
         st2 = (site.get("hole_detect") or {}).get("stage2") or {}
         assert st2.get("hole_keypoint_once") is True
         assert st2.get("hole_visual_only") is True
+        assert st2.get("hole_once_lock_xy_descend") is True
+        assert st2.get("once_ff_look_per_m") == pytest.approx(ONCE_FF_LOOK_PER_M)
+        servo = (site.get("servo") or {})
+        assert servo.get("d435_handback_sec") == pytest.approx(0.5)
 
     def test_oneshot_step_covers_half_hole_pair(self):
         """框心到 k1 约 29mm：必须整步走完，不许再截成 12mm。"""
@@ -836,3 +894,85 @@ class TestHoleBiasAxes:
         assert ONCE_APPROACH_STEP_M == pytest.approx(0.020)
         assert flange_z_for_gap_m(0.223, 0.022, 0.002) == pytest.approx(0.247)
         assert flange_z_for_gap_m(0.223, 0.022, 0.015) == pytest.approx(0.260)
+
+    def test_final_gap_refusal_uses_tip_clearance_not_flange_z(self):
+        """贴面参数是规尖离面。0 / 比悬停还高 / 法兰出盒必须红。"""
+        tool, surf, hover = 0.2245, 0.022, 0.320
+        z_lo, z_hi = 0.18, 0.65
+        assert final_gap_refusal(0.002, tool, surf, hover, z_lo, z_hi) == ""
+        assert final_gap_refusal(0.008, tool, surf, hover, z_lo, z_hi) == ""
+        assert "≤ 0" in final_gap_refusal(0.0, tool, surf, hover, z_lo, z_hi)
+        assert "未知" in final_gap_refusal(0.008, 0.0, surf, hover, z_lo, z_hi)
+        hover_clear = hover - tool - surf
+        why_up = final_gap_refusal(hover_clear + 0.005, tool, surf, hover, z_lo, z_hi)
+        assert "抬臂" in why_up
+        why_box = final_gap_refusal(0.008, tool, surf, hover, 0.40, z_hi)
+        assert "工作盒" in why_box
+        flange_8 = flange_z_for_gap_m(tool, surf, 0.008)
+        assert flange_8 == pytest.approx(0.2545)
+        assert flange_8 < hover
+
+    def test_final_gap_cli_overrides_site_and_conflicts_with_no_descent(self):
+        """--final-gap 必须接到 launch/节点；和 --no-descent 一起写必须在脚本里红。"""
+        from pathlib import Path
+
+        root = Path(__file__).resolve().parents[1]
+        ws = root
+        while ws != ws.parent and not (ws / "scripts" / "run_all.sh").is_file():
+            ws = ws.parent
+        run_all = (ws / "scripts" / "run_all.sh").read_text(encoding="utf-8")
+        stage2 = (root / "arm_application" / "stage2_locate_node.py").read_text(
+            encoding="utf-8"
+        )
+        launch = (root / "launch" / "pipeline.launch.py").read_text(encoding="utf-8")
+        assert "--final-gap" in run_all
+        assert "final_gap_mm:=${FINAL_GAP}" in run_all
+        assert "同时写了 --final-gap 和 --no-descent" in run_all
+        assert 'self.declare_parameter("final_gap_mm"' in stage2
+        assert "final_gap_user_set" in stage2
+        assert "target_gap_m=lock_gap" in stage2
+        assert '"final_gap_mm": float(final_gap) if final_gap else -1.0' in launch
+        assert 'DeclareLaunchArgument("final_gap_mm"' in launch
+
+    def test_once_look_ff_is_minus_look_times_k_dz(self):
+        """现行 k=0 → ΔXY=(0,0)。k=0.018、look=-X、降 71.5mm 才是 +1.287mm X。"""
+        k0 = float(ONCE_FF_LOOK_PER_M)
+        assert k0 == pytest.approx(0.0)
+        dxy0 = once_look_ff_xy([-1.0, 0.0], 0.0715, k0)
+        assert dxy0 is not None
+        np.testing.assert_allclose(dxy0, [0.0, 0.0], atol=1e-12)
+        np.testing.assert_allclose(
+            once_look_ff_xy([0.0, 0.0], 0.0715, 0.0), [0.0, 0.0], atol=1e-12
+        )
+        assert once_look_ff_xy([-1.0, 0.0], 0.0, 0.0) is None
+        k = 0.018
+        expect_x = k * 0.0715
+        dxy = once_look_ff_xy([-1.0, 0.0], 0.0715, k)
+        assert dxy is not None
+        np.testing.assert_allclose(dxy, [expect_x, 0.0], atol=1e-9)
+        assert expect_x == pytest.approx(0.001287, abs=1e-6)
+        assert once_look_ff_xy([0.0, 0.0], 0.0715, k) is None
+        assert once_look_ff_xy([-1.0, 0.0], 0.0, k) is None
+        packed = once_ff_tcp_xyz(
+            [0.50, -0.40],
+            [-0.001, -0.001],
+            [-1.0, 0.0],
+            0.0715,
+            k0,
+            0.022,
+            0.002,
+        )
+        assert packed is not None
+        tcp, dxy2 = packed
+        assert tcp[2] == pytest.approx(0.024)
+        assert tcp[2] < 0.18
+        np.testing.assert_allclose(tcp[:2], [0.50 - 0.001, -0.40 - 0.001])
+        np.testing.assert_allclose(dxy2, [0.0, 0.0], atol=1e-12)
+        assert ONCE_FF_MOVEL_VEL_MPS == pytest.approx(0.05)
+        dz_refuse = ONCE_TRIM_REFUSE_M / k + 0.01
+        assert once_look_ff_xy([-1.0, 0.0], dz_refuse, k) is not None
+        assert float(np.hypot(*once_look_ff_xy([-1.0, 0.0], dz_refuse, k))) > ONCE_TRIM_REFUSE_M
+        rel = once_ff_movel_from_tcp([0.580, -0.460, 0.096], [0.0, 0.0], 0.0715)
+        assert rel is not None
+        np.testing.assert_allclose(rel, [0.580, -0.460, 0.0245], atol=1e-9)
+        assert once_ff_movel_from_tcp([0.580, -0.460, 0.096], [0.00129, 0.0], 0.0) is None

src/arm_application/test/test_overhead_track.py

@@ -238,7 +238,7 @@ class TestRealSiteBranchDefault:
         assert s["handeye_mode"] == "eye_in_hand"
         assert s["handeye_file"].endswith("89_eye_in_hand.yaml")
         assert s["lock_still_sec"] == pytest.approx(3.0)
-        assert s["d435_handback_sec"] == pytest.approx(3.0)
+        assert s["d435_handback_sec"] == pytest.approx(0.5)
         assert s["wrist_align_sec"] == pytest.approx(0.5)
         assert s["lock_still_move_m"] == pytest.approx(0.015)
         # 负向：跟踪外参不能被换成顶视文件

src/arm_application/test/test_stage2_coarse_guard.py

@@ -254,6 +254,11 @@ def test_coarse_loop_rejects_before_moving():
         "又在咬孔检测器的 bbox 当粗居中目标了（真机那帧是整幅画面）"
     )
     assert "skip_coarse = True" in src, "'跳过粗居中'必须真的不移动"
+    coarse = src[src.find("# ---- 3) 粗居中") : src.find("# ---- 4)")]
+    assert "hole_visual_only" in coarse
+    assert "pixel_on_base_plane" in coarse
+    assert "拒绝用深度图凑框心" in coarse
+    assert 'src = "像素交工件面"' in coarse
 
 
 def test_guard_reason_is_logged_with_both_numbers():

src/arm_application/test/test_stage2_descent_clamp.py

@@ -36,6 +36,7 @@ from __future__ import annotations
 from typing import Any, List, Optional
 
 import numpy as np
+import pytest
 
 from arm_application.overhead_oneshot import ONCE_APPROACH_STEP_M
 from arm_application.stage2_locate_node import (
@@ -112,6 +113,7 @@ class _StubStage2:
         self._contact_after = contact_after_steps
         # 记录每一次实际下发的下降步长，用来核对累计行程
         self.steps_mm: List[float] = []
+        self.goto_hold_xy: List[bool] = []
         self.pins = 0
 
     # --- 被测函数会调到的钩子 ---
@@ -134,10 +136,14 @@ class _StubStage2:
 
     def _goto(self, _q: Any, Tee: Any, target: Any, **_kw: Any) -> Any:
         Tee = np.array(Tee, dtype=float)
+        self.goto_hold_xy.append(bool(_kw.get("hold_xy")))
         step_mm = (Tee[2, 3] - float(target[2])) * 1000.0
         # 记录"下降步"（正数）；回退步（负数）不计入下降行程
         if step_mm > 0.0:
             self.steps_mm.append(step_mm)
+        if not _kw.get("hold_xy"):
+            Tee[0, 3] = float(target[0])
+            Tee[1, 3] = float(target[1])
         Tee[2, 3] = float(target[2])
         self._cur_z = float(target[2])
         return True, np.zeros(7), Tee
@@ -290,6 +296,194 @@ def test_contact_not_armed_while_still_high_above_the_part():
     assert contact_armed_by_gap(0.005, 0.015) is True
 
 
+def test_lock_xy_descend_holds_xy_and_stops_at_final_gap():
+    """锁 XY 实验：从悬停纯 Z 收到 2mm，XY 不许动，步长仍 20mm。"""
+    import rclpy
+
+    if not rclpy.ok():
+        rclpy.init()
+    st = _StubStage2()
+    Tee0 = np.eye(4)
+    Tee0[0, 3] = 0.512
+    Tee0[1, 3] = -0.401
+    Tee0[2, 3] = _HOVER_Z
+    ok, _q, Tee = Stage2Locate._descend_above_hole(  # type: ignore[arg-type]
+        st, np.zeros(7), Tee0, target_gap_m=_FINAL_GAP, step_m=ONCE_APPROACH_STEP_M
+    )
+    assert ok is True
+    gap = float(Tee[2, 3]) - _TOOL_LEN - _SURFACE_Z
+    assert abs(gap - _FINAL_GAP) < 0.001, f"应停在贴面 2mm，实际 {gap * 1000:.1f}mm"
+    assert Tee[0, 3] == 0.512
+    assert Tee[1, 3] == -0.401
+    assert max(st.steps_mm) <= ONCE_APPROACH_STEP_M * 1000 + 1e-6
+    assert all(st.goto_hold_xy), "下臂每一步都必须 hold_xy"
+
+
+def _look_neg_x_T() -> np.ndarray:
+    T = np.eye(4)
+    T[:3, :3] = np.array(
+        [[0.0, 0.0, -1.0], [1.0, 0.0, 0.0], [0.0, -1.0, 0.0]],
+        dtype=float,
+    )
+    return T
+
+
+def test_once_ff_zero_k_is_cartesian_lock_xy_movel():
+    """k=0 必须下发相对 MoveL，且 ΔXY=0、只降 Z。不许停在悬停高装成功。"""
+    import rclpy
+
+    if not rclpy.ok():
+        rclpy.init()
+    st = _ff_stub_ready()
+    st.once_ff_look_per_m = 0.0
+    st._lookup = lambda *_a, **_k: np.eye(4)  # type: ignore[method-assign]
+    st._fresh_tcp_world = lambda: (0.580, -0.460, 0.096)  # type: ignore[method-assign]
+    got: list = []
+    st._vendor_movel_tcp = (  # type: ignore[method-assign]
+        lambda xyz, _vel: got.append([float(xyz[0]), float(xyz[1]), float(xyz[2])]) or True
+    )
+    Tee = np.eye(4)
+    Tee[0, 3] = 0.595
+    Tee[1, 3] = -0.467
+    Tee[2, 3] = _HOVER_Z
+    ok, _q, _Tee1 = Stage2Locate._once_ff_diagonal_to_gap(  # type: ignore[arg-type]
+        st, np.zeros(7), Tee, target_gap_m=_FINAL_GAP
+    )
+    assert ok is True
+    assert len(got) == 1
+    gap0 = _HOVER_Z - _TOOL_LEN - _SURFACE_Z
+    descent = gap0 - _FINAL_GAP
+    np.testing.assert_allclose(got[0], [0.580, -0.460, 0.096 - descent], atol=1e-9)
+    assert any("笛卡尔锁XY只降Z" in m for m in st.logs)
+
+
+def _ff_stub_ready() -> _StubStage2:
+    st = _StubStage2()
+    st.once_ff_look_per_m = 0.0
+    st._tip_off = np.array([-0.001, -0.001], dtype=float)
+    st.cam_frame = "cam"
+    st.base_frame = "base"
+    st._ws_clipped = False
+    st._lookup = lambda *_a, **_k: _look_neg_x_T()  # type: ignore[method-assign]
+    st._clamp_ws = lambda *_a, **_k: None  # type: ignore[method-assign]
+    st._spin = lambda *_a, **_k: None  # type: ignore[method-assign]
+    st._wait_state = lambda *_a, **_k: (None, None)  # type: ignore[method-assign]
+    st.robot_states_topic = "/Rizon4_063989/flexiv_robot_states"
+    return st
+
+
+def test_once_ff_refuses_without_tcp_pose():
+    """没有厂家 tcp_pose 不许退回 ROS 绝对坐标。"""
+    import rclpy
+
+    if not rclpy.ok():
+        rclpy.init()
+    st = _ff_stub_ready()
+    st._fresh_tcp_world = lambda: None  # type: ignore[method-assign]
+    called: list = []
+    st._vendor_movel_tcp = lambda *_a, **_k: called.append(1) or True  # type: ignore[method-assign]
+    Tee = np.eye(4)
+    Tee[0, 3] = 0.595
+    Tee[1, 3] = -0.467
+    Tee[2, 3] = _HOVER_Z
+    ok, _q, Tee1 = Stage2Locate._once_ff_diagonal_to_gap(  # type: ignore[arg-type]
+        st, np.zeros(7), Tee, target_gap_m=_FINAL_GAP
+    )
+    assert ok is False
+    assert called == []
+    assert Tee1[2, 3] == pytest.approx(_HOVER_Z)
+    assert any("没有厂家 tcp_pose" in m for m in st.logs)
+
+
+def test_once_ff_movel_is_flexiv_tcp_plus_delta():
+    """MoveL 必须是当前厂家 TCP + (ΔXY, −下降)，不是 ROS 规尖绝对坐标。"""
+    import rclpy
+
+    if not rclpy.ok():
+        rclpy.init()
+    st = _ff_stub_ready()
+    st._fresh_tcp_world = lambda: (0.580, -0.460, 0.096)  # type: ignore[method-assign]
+    got: list = []
+    st._vendor_movel_tcp = (  # type: ignore[method-assign]
+        lambda xyz, _vel: got.append([float(xyz[0]), float(xyz[1]), float(xyz[2])]) or True
+    )
+    Tee = np.eye(4)
+    Tee[0, 3] = 0.595
+    Tee[1, 3] = -0.467
+    Tee[2, 3] = _HOVER_Z
+    ok, _q, _Tee1 = Stage2Locate._once_ff_diagonal_to_gap(  # type: ignore[arg-type]
+        st, np.zeros(7), Tee, target_gap_m=_FINAL_GAP
+    )
+    assert ok is True
+    assert len(got) == 1
+    gap0 = _HOVER_Z - _TOOL_LEN - _SURFACE_Z
+    descent = gap0 - _FINAL_GAP
+    np.testing.assert_allclose(got[0], [0.580, -0.460, 0.096 - descent], atol=1e-9)
+    assert got[0][0] == pytest.approx(0.580)
+    assert not np.allclose(got[0][:2], [0.595 - 0.001, -0.467 - 0.001], atol=1e-4)
+
+
+def test_once_ff_custom_final_gap_only_changes_z():
+    """--final-gap 8mm：MoveL XY 仍为 0，下降量必须少 6mm，不许再走贴面 2mm。"""
+    import rclpy
+
+    if not rclpy.ok():
+        rclpy.init()
+    st = _ff_stub_ready()
+    st._fresh_tcp_world = lambda: (0.580, -0.460, 0.096)  # type: ignore[method-assign]
+    got: list = []
+    st._vendor_movel_tcp = (  # type: ignore[method-assign]
+        lambda xyz, _vel: got.append([float(xyz[0]), float(xyz[1]), float(xyz[2])]) or True
+    )
+    Tee = np.eye(4)
+    Tee[0, 3] = 0.595
+    Tee[1, 3] = -0.467
+    Tee[2, 3] = _HOVER_Z
+    gap_custom = 0.008
+    ok, _q, _Tee1 = Stage2Locate._once_ff_diagonal_to_gap(  # type: ignore[arg-type]
+        st, np.zeros(7), Tee, target_gap_m=gap_custom
+    )
+    assert ok is True
+    assert len(got) == 1
+    gap0 = _HOVER_Z - _TOOL_LEN - _SURFACE_Z
+    descent = gap0 - gap_custom
+    descent_2mm = gap0 - _FINAL_GAP
+    np.testing.assert_allclose(got[0], [0.580, -0.460, 0.096 - descent], atol=1e-9)
+    assert got[0][0] == pytest.approx(0.580)
+    assert got[0][1] == pytest.approx(-0.460)
+    assert descent == pytest.approx(descent_2mm - 0.006)
+    assert any("笛卡尔锁XY只降Z" in m for m in st.logs)
+
+
+def test_once_ff_k_018_still_adds_look_xy():
+    """有人把 k 改回 0.018 时，MoveL 必须真的加 +k×下降，不许再吞成绝对坐标。"""
+    import rclpy
+
+    if not rclpy.ok():
+        rclpy.init()
+    st = _ff_stub_ready()
+    st.once_ff_look_per_m = 0.018
+    st._fresh_tcp_world = lambda: (0.580, -0.460, 0.096)  # type: ignore[method-assign]
+    got: list = []
+    st._vendor_movel_tcp = (  # type: ignore[method-assign]
+        lambda xyz, _vel: got.append([float(xyz[0]), float(xyz[1]), float(xyz[2])]) or True
+    )
+    Tee = np.eye(4)
+    Tee[0, 3] = 0.595
+    Tee[1, 3] = -0.467
+    Tee[2, 3] = _HOVER_Z
+    ok, _q, _Tee1 = Stage2Locate._once_ff_diagonal_to_gap(  # type: ignore[arg-type]
+        st, np.zeros(7), Tee, target_gap_m=_FINAL_GAP
+    )
+    assert ok is True
+    assert len(got) == 1
+    gap0 = _HOVER_Z - _TOOL_LEN - _SURFACE_Z
+    descent = gap0 - _FINAL_GAP
+    dxy = 0.018 * descent
+    np.testing.assert_allclose(got[0], [0.580 + dxy, -0.460, 0.096 - descent], atol=1e-9)
+    assert any("视线前馈斜线" in m for m in st.logs)
+
+
 def test_oneshot_approach_stops_at_observe_gap_with_larger_steps():
     """单次精拍第一段只降到 15mm，步长 20mm（约 3 步），不许 4mm 切到 2mm。"""
     import rclpy