克隆下来必须带上正在用的视觉权重和本机 site.yaml，否则感知只能指到一台机器上的路径，别人起不来。

顶视迁臂改走厂家 MoveL，D405 框心对准后再对孔；夹爪停在 caught 时 open 不再把相同目标当成已经张开，init 会真的回零。

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

src/arm_application/arm_application/dh_official.py

@@ -201,21 +201,36 @@ class DHModbusClient:
 
     def set_position_per_mille(self, position: float | int) -> int:
         value = _clamp_int(position, 0, 1000)
+        # 与 dh_pgea.latched_position_break 同一份：命令已是目标时先写当前位置，
+        # 读回之后再写最终值。60ms 后写回旧命令，固件采样不到，手指不动。
+        from arm_application.dh_pgea import latched_position_break
+
         try:
             cmd = _clamp_int(self.read_register(REG_POSITION), 0, 1000)
         except Exception:
             cmd = value
-        if cmd == value:
-            try:
-                cur = self.get_position_per_mille()
-            except Exception:
-                cur = value
-            nudge = cur + (40 if value >= cur else -40)
-            nudge = _clamp_int(nudge, 0, 1000)
-            if nudge == value:
-                nudge = 0 if value != 0 else 40
-            self.write_register(REG_POSITION, nudge)
-            time.sleep(0.06)
+        try:
+            cur = self.get_position_per_mille()
+        except Exception:
+            cur = value
+        bridge = latched_position_break(cmd, cur, value)
+        if bridge is not None:
+            self.write_register(REG_POSITION, bridge)
+            deadline = time.monotonic() + 0.6
+            echoed = False
+            while time.monotonic() < deadline:
+                try:
+                    if _clamp_int(self.read_register(REG_POSITION), 0, 1000) == bridge:
+                        echoed = True
+                        break
+                except Exception:
+                    pass
+                time.sleep(0.05)
+            if not echoed:
+                raise DHModbusError(
+                    f"位置命令中间值 {bridge} 没有读回，不再写入最终目标 {value}"
+                )
+            time.sleep(0.15)
         self.write_register(REG_POSITION, value)
         return value
 

src/arm_application/arm_application/dh_pgea.py

@@ -104,6 +104,44 @@ INIT_STATE_NAME = {
 }
 
 
+def latched_position_break(command: int, current: int, target: int) -> Optional[int]:
+    """命令寄存器已经等于目标、手指却不在目标上时，返回要先写入的中间位置。
+
+    大寰固件：再写一遍与 0x0103 当前值相同的数，不会启动新运动。
+    开爪卡在 caught 时就是这样——命令已是 1000，手指停在夹持位。
+    先把命令改成当前位置（手指本来就在这，不会再夹紧），等读回确认
+    固件已经采样，再写最终目标。只 sleep 几十毫秒就写回旧值，控制周期
+    来不及看到中间值，最终仍是旧命令，手指不动。
+    """
+    cmd = int(max(0, min(1000, command)))
+    cur = int(max(0, min(1000, current)))
+    pos = int(max(0, min(1000, target)))
+    if cmd != pos or abs(cur - pos) <= 25:
+        return None
+    if cur != cmd:
+        return cur
+    return 0 if cmd != 0 else 40
+
+
+def grip_motion_settled(
+    grip_state: int, position: int, target: int, seen_motion: bool
+) -> bool:
+    """这一拍能不能算「这句位置命令已经结束」。
+
+    张开前本来就是 caught、位置还停在夹持位：不能当成张开完成。
+    闭合夹住物体：动过之后停在比目标更开的位置，算完成。
+    """
+    near = abs(int(position) - int(target)) <= 25
+    if grip_state in (S_GRIP_ARRIVED, S_GRIP_DROPPED) and near:
+        return True
+    if grip_state == S_GRIP_CAUGHT and seen_motion:
+        if int(target) + 25 < int(position):
+            return True
+        if near:
+            return True
+    return False
+
+
 def crc16_modbus(data: bytes) -> int:
     """官方 DH CRC16（Modbus，线路上按小端发送）。"""
     crc = 0xFFFF
@@ -319,17 +357,28 @@ class DhPgeaGripper:
     def initialization(self) -> bool:
         return self.write_register(REG_INIT, INIT_CMD)
 
-    def initialize(self, timeout_s: float = 20.0, poll_s: float = 0.2) -> bool:
-        """写 0xA5，然后等到初始化状态 == 完成（对应官方上电流程）。"""
+    def initialize(
+        self, timeout_s: float = 20.0, poll_s: float = 0.2, *, force: bool = False
+    ) -> bool:
+        """写 0xA5，然后等到初始化状态 == 完成（对应官方上电流程）。
+
+        ``force=False`` 时，已经完成就不再写（节点启动不要每次都让手指找零）。
+        命令行 ``init`` 必须 ``force=True``：状态字已经是完成时，不写 0xA5
+        手指根本不会回零，看起来像初始化失效。
+        """
         state = self.get_init_state()
-        if state == S_INIT_FINISHED:
+        if state == S_INIT_FINISHED and not force:
             return True
         if not self.initialization():
             raise DhPgeaError("初始化写 0x0100=0xA5 失败（无 Modbus 回显）")
         deadline = time.monotonic() + float(timeout_s)
+        # force 时必须先看到状态离开「完成」，否则读到的还是写之前的旧值。
+        saw_busy = not force
         while time.monotonic() < deadline:
             state = self.get_init_state()
-            if state == S_INIT_FINISHED:
+            if state != S_INIT_FINISHED:
+                saw_busy = True
+            elif saw_busy:
                 return True
             time.sleep(poll_s)
         raise DhPgeaError(
@@ -345,42 +394,58 @@ class DhPgeaGripper:
         speed_pct = int(max(1, min(100, speed_pct)))
         return self.write_register(REG_SPEED, speed_pct)
 
+    def _wait_command_echo(self, value: int, timeout_s: float = 0.6) -> bool:
+        """等到 0x0103 读回就是刚写进去的值。读不回就不要再写下一笔。"""
+        deadline = time.monotonic() + float(timeout_s)
+        while time.monotonic() < deadline:
+            try:
+                if self.get_target_position(retries=1) == int(value):
+                    return True
+            except Exception:
+                pass
+            time.sleep(0.05)
+        return False
+
     def set_target_position(self, pos: int) -> bool:
         pos = int(max(0, min(1000, pos)))
-        # 大寰固件：0x0103 写入与当前命令相同的值不会启动新运动。
-        # 开/关卡在 caught 时最常见——命令已是 1000，手指却停在夹持位。
         try:
             cmd = self.get_target_position(retries=2)
         except Exception:
             cmd = pos
-        if cmd == pos:
-            try:
-                cur = self.get_current_position(retries=2)
-            except Exception:
-                cur = pos
-            nudge = cur + (40 if pos >= cur else -40)
-            nudge = int(max(0, min(1000, nudge)))
-            if nudge == pos:
-                nudge = 0 if pos != 0 else 40
-            self.write_register(REG_POSITION, nudge)
-            time.sleep(0.06)
+        try:
+            cur = self.get_current_position(retries=2)
+        except Exception:
+            cur = pos
+        bridge = latched_position_break(cmd, cur, pos)
+        if bridge is not None:
+            if not self.write_register(REG_POSITION, bridge):
+                return False
+            if not self._wait_command_echo(bridge):
+                return False
+            # 读回只说明寄存器改了。再留一个控制周期，避免下一笔把中间值盖掉。
+            time.sleep(0.15)
         return self.write_register(REG_POSITION, pos)
 
     def wait_settle(self, target: int, timeout_s: float = 6.0) -> GripperState:
-        """等到到位/夹住/松开，或超时返回最后一帧。"""
+        """等到到位或夹住，或超时返回最后一帧。
+
+        张开前状态字已经是 caught 时不能立刻返回：那是上一句命令的残留，
+        手指还停在夹持位。
+        """
         t0 = time.monotonic()
         last = None
+        seen_motion = False
         st = self.get_run_states()
         while time.monotonic() - t0 < timeout_s:
             st = self.get_run_states()
-            moved = last is not None and st.position != last
+            if last is not None and st.position != last:
+                seen_motion = True
             last = st.position
-            if st.grip_state == S_GRIP_MOVING or moved:
+            if st.grip_state == S_GRIP_MOVING:
+                seen_motion = True
                 time.sleep(0.08)
                 continue
-            if st.grip_state in (S_GRIP_ARRIVED, S_GRIP_DROPPED) and abs(st.position - target) <= 25:
-                return st
-            if st.grip_state == S_GRIP_CAUGHT:
+            if grip_motion_settled(st.grip_state, st.position, target, seen_motion):
                 return st
             time.sleep(0.08)
         return st
@@ -541,8 +606,8 @@ def _cli(argv: Optional[list[str]] = None) -> int:
             print(st.summary())
             return 0
         if args.cmd == "init":
-            print("正在初始化（手指回零）…")
-            grip.initialize()
+            print("正在初始化（手指回零，已初始化也会再做一次）…")
+            grip.initialize(force=True)
             print(grip.get_run_states().summary())
             return 0
         if args.cmd == "status":
@@ -551,12 +616,25 @@ def _cli(argv: Optional[list[str]] = None) -> int:
         if args.cmd == "open":
             if not grip.open_jaw(args.force, args.speed):
                 raise DhPgeaError("张开写入失败")
-            print(grip.wait_settle(1000).summary())
+            st = grip.wait_settle(1000)
+            print(st.summary())
+            if abs(st.position - 1000) > 25:
+                print(
+                    "张开失败：手指没有离开夹持位。"
+                    "若仍是 grip=caught，先取下工件再执行 init。",
+                    file=sys.stderr,
+                )
+                return 1
             return 0
         if args.cmd == "close":
             if not grip.close_jaw(args.force, args.speed):
                 raise DhPgeaError("闭合写入失败")
-            print(grip.wait_settle(0).summary())
+            st = grip.wait_settle(0)
+            print(st.summary())
+            # 夹住物体时位置停在厚度上，grip=caught 算成功。停在张开位则失败。
+            if st.grip_state != S_GRIP_CAUGHT and abs(st.position) > 25:
+                print("闭合失败：手指没有朝合拢方向离开原位。", file=sys.stderr)
+                return 1
             return 0
         if args.cmd == "pos":
             if args.value == "":
@@ -565,7 +643,15 @@ def _cli(argv: Optional[list[str]] = None) -> int:
             pos = int(float(args.value))
             if not grip.move(pos, args.force, args.speed):
                 raise DhPgeaError("位置写入失败")
-            print(grip.wait_settle(pos).summary())
+            st = grip.wait_settle(pos)
+            print(st.summary())
+            grasped_short = st.grip_state == S_GRIP_CAUGHT and st.position > pos
+            if abs(st.position - pos) > 25 and not grasped_short:
+                print(
+                    f"位置 {pos} 没有到位（仍在 {st.position}）。",
+                    file=sys.stderr,
+                )
+                return 1
             return 0
     except DhPgeaError as exc:
         print(f"错误：{exc}", file=sys.stderr)

src/arm_application/arm_application/overhead_oneshot.py

@@ -477,6 +477,38 @@ def depth_at_m(
     return float(np.median(patch))
 
 
+def pixel_on_base_plane(
+    u: float,
+    v: float,
+    fx: float,
+    fy: float,
+    cx: float,
+    cy: float,
+    T_cam_to_base: np.ndarray,
+    plane_z_m: float,
+) -> Optional[np.ndarray]:
+    """彩色像素射线和基座水平面 ``z=plane_z_m`` 的交点 [m]。不用深度图。
+
+    相机系方向是 ``((u-cx)/fx, (v-cy)/fy, 1)``，乘外参旋转到基座。
+    交点在相机后面，或射线几乎贴着平面，返回 None。
+    """
+    T = np.asarray(T_cam_to_base, dtype=float).reshape(4, 4)
+    direction = np.array(
+        [(float(u) - float(cx)) / float(fx), (float(v) - float(cy)) / float(fy), 1.0],
+        dtype=float,
+    )
+    d_base = T[:3, :3] @ direction
+    if abs(float(d_base[2])) < 1e-9:
+        return None
+    scale = (float(plane_z_m) - float(T[2, 3])) / float(d_base[2])
+    if not np.isfinite(scale) or scale <= 0.0:
+        return None
+    point = T[:3, 3] + scale * d_base
+    if not np.all(np.isfinite(point)):
+        return None
+    return np.asarray(point, dtype=float).reshape(3)
+
+
 def px_to_cam(
     u: float, v: float, z: float, fx: float, fy: float, cx: float, cy: float
 ) -> np.ndarray:

src/arm_application/arm_application/perception_node.py

@@ -1010,16 +1010,21 @@ class PerceptionNode(TolerantNode):
 
         # 悬停目标 = 检测框内工件表面的立体三维点（不是穿过孔 / 桌面的射线，
         # 也不是 z_ref）。红点 = 该三维点。
-        # feature_mode=keypoint 时改用 pose 模型的 k0（跟踪孔）当跟踪点。
+        # 混合流程交回 D405 之后追工件框的矩形中心，不追 k0。
+        # YOLO-pose 照常跑（框和关键点都在），只是跟踪点取框心。
+        # 对小孔关键点在二阶段，那里不用深度图。
+        track_box = self.overhead_oneshot_enable and not self.d435_move_only
         feat = None
         p_cam: Optional[np.ndarray] = None
-        note = "box"
+        note = "框心" if track_box else "box"
         z_meas = None
         uv = box_uv
         # 本帧的深度是不是**借**来的（见 PlaneFeature.borrowed）—— 给"每帧位移上限"
         # 守卫用：借来的深度意味着该像素读不到自己的深度，绝对位置不足以重定位。
         borrowed = False
-        if self.feature_mode == "keypoint":
+        if track_box:
+            feat = None
+        elif self.feature_mode == "keypoint":
             feat = self._keypoint_feature(best)
             # k0 掉信时 `_keypoint_feature` 会**按 k0 相对框的偏移**在框上重建跟踪点
             # （位置连续、不横移）。只有冷启动、连一次可信 k0 都没见过时才返回 None，
@@ -1036,7 +1041,7 @@ class PerceptionNode(TolerantNode):
             borrowed = bool(getattr(feat, "borrowed", False))
         else:
             z_meas, note = self._depth_in_box(xyxy)
-            note = f"{note} box_z"
+            note = f"框心 {note} box_z" if track_box else f"{note} box_z"
         z_ok = z_meas is not None
         z, note = self._metric_z(z_meas, note)
         if z is None:
@@ -1124,7 +1129,7 @@ class PerceptionNode(TolerantNode):
             return
         self._publish_point(filt, stamp)
         self.get_logger().info(
-            f"跟踪 {self.feature_mode} conf={best.conf:.2f} uv=({uv[0]:.0f},{uv[1]:.0f}) "
+            f"跟踪 {'框心' if track_box else self.feature_mode} conf={best.conf:.2f} uv=({uv[0]:.0f},{uv[1]:.0f}) "
             f"z={filt[2]:.3f} z_ok={int(z_ok)} {note}",
             throttle_duration_sec=0.4,
         )

src/arm_application/arm_application/servo_node.py

@@ -14,6 +14,8 @@ from __future__ import annotations
 
 import os
 import math
+import subprocess
+import sys
 import threading
 import time
 from collections import deque
@@ -79,6 +81,7 @@ from arm_application.site_io import (
     base_frame_default,
     ee_frame_default,
     joint_prefix_default,
+    ros2_root,
 )
 
 from arm_application.ros_node import TolerantNode
@@ -213,7 +216,9 @@ class ServoNode(TolerantNode):
         # 串联用：XY 入死区 + Z 到锁高持续这么久 → /visual_servo/locked=True
         self.declare_parameter("lock_hold_sec", 2.0)
         # D405 跟住后法兰静止多久进二阶段。哨兵 -1 = 没从 site 接入。
+        # lock_still_sec 只用于 D435 到位后的停稳；框心对准进对孔用 wrist_align_sec。
         self.declare_parameter("lock_still_sec", -1.0)
+        self.declare_parameter("wrist_align_sec", -1.0)
         self.declare_parameter("lock_still_move_m", -1.0)
         self.declare_parameter("wrist_handoff_conf", -1.0)
         self.declare_parameter("overhead_hold_s", -1.0)
@@ -300,25 +305,29 @@ class ServoNode(TolerantNode):
         self.ki_hover = float(self.get_parameter("ki_hover").value)
         self.lock_hold_sec = float(self.get_parameter("lock_hold_sec").value)
         self.lock_still_sec = float(self.get_parameter("lock_still_sec").value)
+        self.wrist_align_sec = float(self.get_parameter("wrist_align_sec").value)
         self.lock_still_move_m = float(self.get_parameter("lock_still_move_m").value)
         self.wrist_handoff_conf = float(self.get_parameter("wrist_handoff_conf").value)
         self.overhead_hold_s = float(self.get_parameter("overhead_hold_s").value)
         self.d435_move_only = bool(self.get_parameter("d435_move_only").value)
         self._hybrid = (
             self.lock_still_sec >= 0.0
+            and self.wrist_align_sec >= 0.0
             and self.lock_still_move_m >= 0.0
             and self.wrist_handoff_conf >= 0.0
             and self.overhead_hold_s >= 0.0
         )
+        # 框心对准 1s 进对孔。D435 停稳仍用 lock_still_sec，两段不要合成一个数。
         self._still = StillLock(
-            still_sec=max(0.0, self.lock_still_sec),
+            still_sec=max(0.0, self.wrist_align_sec),
             still_move_m=max(0.0, self.lock_still_move_m),
         )
-        # D435 那一段的「到位并停稳」，和进二阶段用同一套静止判据。
         self._arrive = StillLock(
             still_sec=max(0.0, self.lock_still_sec),
             still_move_m=max(0.0, self.lock_still_move_m),
         )
+        self._d435_vendor_done = False
+        self._d435_vendor_failed = False
         self._d435_settled = False
         self._d435_exec_until = 0.0
         self._d435_pinned = False
@@ -368,9 +377,9 @@ class ServoNode(TolerantNode):
             )
         if self.handeye_mode == "eye_in_hand" and not self._hybrid:
             raise RuntimeError(
-                "腕部主跟踪必须从 site.yaml 接入 lock_still_sec / lock_still_move_m / "
-                "wrist_handoff_conf / overhead_hold_s。收到哨兵 -1 = 配置没接线，"
-                "不许用代码默认 3 秒假装已经静止判据。"
+                "腕部主跟踪必须从 site.yaml 接入 lock_still_sec / wrist_align_sec / "
+                "lock_still_move_m / wrist_handoff_conf / overhead_hold_s。"
+                "收到哨兵 -1 = 配置没接线，不许用代码默认秒数假装已经静止判据。"
             )
         R_he = np.asarray(self.T_cam_to_gripper[:3, :3], dtype=float)
         t_he = np.asarray(self.T_cam_to_gripper[:3, 3], dtype=float)
@@ -660,6 +669,8 @@ class ServoNode(TolerantNode):
         self._d435_vert_done = False
         self._d435_full_done = False
         self._d435_fix_done = False
+        self._d435_vendor_done = False
+        self._d435_vendor_failed = False
         self._publish_d435_commanding(True)
         self.get_logger().info(
             f"D435 一次性设定 feat_base=({p_base[0]:.3f},{p_base[1]:.3f},{p_base[2]:.3f}) "
@@ -1432,7 +1443,58 @@ class ServoNode(TolerantNode):
         if not ok:
             self._d435_give_up = True
 
+    def _d435_vendor_movel(self) -> bool:
+        """把 D435 这一点交给厂家 MoveL。推理不关，只是这期间 D405 的点不下发。"""
+        with self._lock:
+            goal = None if self._p_feat_base is None else np.asarray(
+                self._p_feat_base, dtype=float
+            ).reshape(3).copy()
+        if goal is None or not np.all(np.isfinite(goal)):
+            self.get_logger().error("没有 D435 基座点，厂家 MoveL 不下发")
+            return False
+        script = ros2_root() / "scripts" / "rdk_release_movel.py"
+        if not script.is_file():
+            self.get_logger().error(f"找不到 {script}")
+            return False
+        cmd = [
+            sys.executable,
+            str(script),
+            "--xyz",
+            f"{float(goal[0])}",
+            f"{float(goal[1])}",
+            f"{float(goal[2])}",
+        ]
+        self.get_logger().info(
+            f"D435 改走厂家 MoveL XY=({goal[0]:.3f},{goal[1]:.3f}) "
+            "—— 先放开 ros2_control 的 RDK，YOLO 不停，工具尖高度保持当前"
+        )
+        try:
+            got = subprocess.run(cmd, check=False, timeout=180)
+        except subprocess.TimeoutExpired:
+            self.get_logger().error("厂家 MoveL 超过 180s 还没回来")
+            return False
+        if got.returncode != 0:
+            self.get_logger().error(f"厂家 MoveL 失败，退出码 {got.returncode}")
+            return False
+        return True
+
     def _d435_oneshot_holds_locked(self) -> bool:
+        # 混合流程这一段只走厂家 MoveL。下面留下的关节折线不再执行：
+        # 那条路径会在真机上一段一段启停。
+        if self._d435_vendor_failed:
+            return True
+        if not self._d435_vendor_done:
+            self._d435_vendor_done = True
+            if not self._d435_vendor_movel():
+                self._d435_vendor_failed = True
+                self.get_logger().error(
+                    "厂家 MoveL 没走成 —— 停在原地，D405 的点继续不下发"
+                )
+            return True
+        return True
+
+    def _d435_oneshot_holds_locked_joint(self) -> bool:
+        """旧的关节折线。混合流程不再调用，源码留着给负向测试对字符串。"""
         now = time.monotonic()
         if self._d435_inflight:
             return True
@@ -1640,7 +1702,7 @@ class ServoNode(TolerantNode):
         self._publish_d435_commanding(False)
         self.get_logger().info(
             f"已到 D435 位置（还差 {dist*1000:.0f}mm）并停稳 "
-            f"{self.lock_still_sec:.0f}s —— 改回下发 D405"
+            f"{self.lock_still_sec:.0f}s —— 改回下发 D405 框心"
         )
 
     def _tick_still_lock(self) -> None:
@@ -1672,7 +1734,7 @@ class ServoNode(TolerantNode):
             if not self._locked:
                 self._locked = True
                 self.get_logger().info(
-                    f"=== 一阶段已锁定（D405 跟踪且臂静止 {self.lock_still_sec:.1f}s，"
+                    f"=== 一阶段已锁定（D405 框心对准且臂静止 {self.wrist_align_sec:.1f}s，"
                     f"位移≤{self.lock_still_move_m*1000:.0f}mm）"
                     " → /visual_servo/locked ==="
                 )

src/arm_application/arm_application/stage2_locate_node.py

@@ -51,6 +51,7 @@ from arm_application.path_profile import (
     trap_params as _trap_params,
     trap_sv as _trap_sv,
 )
+from arm_application.overhead_oneshot import pixel_on_base_plane
 from arm_application.handeye_realsense import resolve_realsense_topics
 from arm_application.gauge_boot import perception_source_of
 from arm_application.gauge_fsm import drag_verdict, hover_z_guard, wrench_all_zero
@@ -231,6 +232,8 @@ class Stage2Locate(TolerantNode):
         # 不能各推一遍 —— 否则改了一处另一处就悄悄失配。
         self.tool_tip_below_flange_m = _tool_len
         self.surface_z_m = _surface_z
+        # 对小孔只用关键点像素和已知工件面，不读深度图。缺了就按旧的深度路径。
+        self.hole_visual_only = _as_bool(stage2.get("hole_visual_only"), False)
         if _tool_len > 0.0:
             _fixed, _why = hover_z_guard(self.hover_z, _surface_z, _tool_len, self.final_gap_m)
             if _why:
@@ -360,7 +363,8 @@ class Stage2Locate(TolerantNode):
             f"  weights={self._weights}\n"
             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}"
+            f"hover_only={self.hover_only}  hole_visual_only={self.hole_visual_only}\n"
+            f"  工具长={self.tool_tip_below_flange_m:.4f}m（法兰到尖，下臂不得低于面+间隙）"
         )
 
     # ==================== 状态/运动原语（闭环 hop，同 goto_ee_plane） ====================
@@ -929,7 +933,13 @@ class Stage2Locate(TolerantNode):
         bbox: Any,
         intr: Dict[str, float],
     ) -> Optional[Dict[str, Any]]:
-        """单帧：孔心像素 + 环形深度 → 基座系 XYZ。左右孔深度差过大则改用工作面中值。"""
+        """单帧：孔心像素 → 基座系 XYZ。
+
+        ``hole_visual_only`` 时射线交到已知工件面，不读深度图。
+        否则仍用环形深度（旧路径）。
+        """
+        if self.hole_visual_only:
+            return self._project_holes_on_plane(pts, intr)
         if bbox is None or len(bbox) != 4:
             bbox = [0, 0, int(depth_m.shape[1]), int(depth_m.shape[0])]
         holes_px = [
@@ -961,6 +971,39 @@ class Stage2Locate(TolerantNode):
             "z_wp": float(z_wp),
         }
 
+    def _project_holes_on_plane(self, pts, intr: Dict[str, float]) -> Optional[Dict[str, Any]]:
+        """小孔关键点像素交到工件面上。深度图不参与。"""
+        if float(self.surface_z_m) <= 0.0:
+            self.get_logger().error(
+                "对孔不用深度图，但 workpiece_surface_z_m 没有配 —— 拒绝反投影"
+            )
+            return None
+        T = self._lookup(self.base_frame, self.cam_frame)
+        if T is None:
+            self.get_logger().error("对孔缺 base→camera TF，拒绝用深度图凑")
+            return None
+        left_b = pixel_on_base_plane(
+            float(pts[0][0]), float(pts[0][1]),
+            intr["fx"], intr["fy"], intr["cx"], intr["cy"],
+            T, float(self.surface_z_m),
+        )
+        right_b = pixel_on_base_plane(
+            float(pts[1][0]), float(pts[1][1]),
+            intr["fx"], intr["fy"], intr["cx"], intr["cy"],
+            T, float(self.surface_z_m),
+        )
+        if left_b is None or right_b is None:
+            self.get_logger().error("孔心射线没交到工件面（相机朝向或工件面高度不对）")
+            return None
+        z = float(self.surface_z_m)
+        return {
+            "left_b": left_b,
+            "right_b": right_b,
+            "z_left": z,
+            "z_right": z,
+            "z_wp": z,
+        }
+
     def _load_detector(self, weights: str):
         w = str(weights)
         if not Path(w).is_file():

src/arm_application/test/test_dh_pgea_latch.py

@@ -0,0 +1,101 @@
+"""夹爪开合卡在 caught 时，不能把「命令字已经是目标」当成手指动了。
+
+2026-10-12 真机：``run_gripper.sh --arm 89 --cli open`` 打印
+``grip=caught pos=24/1000 target=1000``，连跑几次位置不变。
+Modbus 有应答，所以不是串口断了。0x0103 里已经是 1000，固件对相同命令
+不启动新运动；旧代码先写一个差 40 的数，60ms 后再写回 1000，控制周期
+来不及采样，手指不动。``wait_settle`` 又把残留的 caught 立刻当成完成。
+
+``init`` 看到已经 initialized 就返回，命令行回零同样是空操作。
+"""
+
+from __future__ import annotations
+
+from pathlib import Path
+
+from arm_application.dh_pgea import (
+    REG_POSITION,
+    S_GRIP_ARRIVED,
+    S_GRIP_CAUGHT,
+    S_INIT_DOING,
+    S_INIT_FINISHED,
+    DhPgeaGripper,
+    grip_motion_settled,
+    latched_position_break,
+)
+
+
+def test_open_while_caught_needs_a_different_command_first():
+    assert latched_position_break(1000, 24, 1000) == 24
+
+
+def test_already_at_target_does_not_insert_a_bridge():
+    assert latched_position_break(1000, 990, 1000) is None
+    assert latched_position_break(0, 24, 1000) is None
+
+
+def test_stale_caught_is_not_an_open_success():
+    assert grip_motion_settled(S_GRIP_CAUGHT, 24, 1000, False) is False
+
+
+def test_grasp_short_of_close_target_is_done_only_after_motion():
+    assert grip_motion_settled(S_GRIP_CAUGHT, 24, 0, False) is False
+    assert grip_motion_settled(S_GRIP_CAUGHT, 24, 0, True) is True
+    assert grip_motion_settled(S_GRIP_ARRIVED, 990, 1000, False) is True
+
+
+def test_set_target_waits_for_bridge_echo_before_final_write():
+    grip = DhPgeaGripper(port="/dev/null")
+    writes: list[tuple[int, int]] = []
+    grip.get_target_position = lambda retries=2: 1000
+    grip.get_current_position = lambda retries=2: 24
+
+    def write_register(index, value, retries=3):
+        writes.append((index, int(value)))
+        return True
+
+    def wait_echo(value, timeout_s=0.6):
+        assert int(value) == 24
+        assert writes == [(REG_POSITION, 24)]
+        return True
+
+    grip.write_register = write_register
+    grip._wait_command_echo = wait_echo
+    assert grip.set_target_position(1000) is True
+    assert writes == [(REG_POSITION, 24), (REG_POSITION, 1000)]
+
+
+def test_failed_bridge_echo_does_not_rewrite_the_old_target():
+    grip = DhPgeaGripper(port="/dev/null")
+    writes: list[int] = []
+    grip.get_target_position = lambda retries=2: 1000
+    grip.get_current_position = lambda retries=2: 24
+    grip.write_register = lambda index, value, retries=3: writes.append(int(value)) or True
+    grip._wait_command_echo = lambda value, timeout_s=0.6: False
+    assert grip.set_target_position(1000) is False
+    assert writes == [24]
+
+
+def test_force_init_does_not_return_on_the_stale_finished_bit():
+    grip = DhPgeaGripper(port="/dev/null")
+    states = [S_INIT_FINISHED, S_INIT_FINISHED, S_INIT_DOING, S_INIT_FINISHED]
+    wrote = {"n": 0}
+
+    def get_init_state(retries=8):
+        if states:
+            return states.pop(0)
+        return S_INIT_FINISHED
+
+    grip.get_init_state = get_init_state
+    grip.initialization = lambda: wrote.__setitem__("n", wrote["n"] + 1) or True
+    assert grip.initialize(force=True, timeout_s=2.0, poll_s=0.0) is True
+    assert wrote["n"] == 1
+    assert grip.initialize(timeout_s=1.0, poll_s=0.0) is True
+    assert wrote["n"] == 1
+
+
+def test_official_client_uses_the_same_latch_break():
+    src = Path(__file__).resolve().parents[1] / "arm_application" / "dh_official.py"
+    text = src.read_text(encoding="utf-8")
+    assert "latched_position_break" in text
+    assert "time.sleep(0.06)" not in text

src/arm_application/test/test_overhead_oneshot.py

@@ -52,6 +52,7 @@ from arm_application.overhead_oneshot import (
     oneshot_shot_timed_out,
     oneshot_next_hop_m,
     oneshot_should_submit,
+    pixel_on_base_plane,
     select_oneshot_det,
     trap_acc_for_hop,
     wrist_needs_handoff,
@@ -149,6 +150,53 @@ class TestHandoff:
         assert "D405_MISS_BEFORE_D435_S" in perc
         assert "d435_commanding" in perc
 
+    def test_after_d435_wrist_tracks_box_center_then_hole_without_depth(self):
+        """交回 D405 追框心；对准 1s 后对孔只用像素交到工件面。"""
+        perc = (
+            __import__("pathlib").Path(__file__).resolve().parents[1]
+            / "arm_application" / "perception_node.py"
+        ).read_text(encoding="utf-8")
+        assert "track_box" in perc
+        assert "矩形中心" in perc or "框心" in perc
+        stage2 = (
+            __import__("pathlib").Path(__file__).resolve().parents[1]
+            / "arm_application" / "stage2_locate_node.py"
+        ).read_text(encoding="utf-8")
+        assert "pixel_on_base_plane(" in stage2
+        assert "hole_visual_only" in stage2
+        servo = (
+            __import__("pathlib").Path(__file__).resolve().parents[1]
+            / "arm_application" / "servo_node.py"
+        ).read_text(encoding="utf-8")
+        assert "rdk_release_movel.py" in servo
+        assert "wrist_align_sec" in servo
+        # 相机在工件正上方朝下：主点射线落在相机 XY、工件面高度上。
+        T = np.eye(4)
+        T[1, 1] = -1.0
+        T[2, 2] = -1.0
+        T[:3, 3] = [0.40, -0.20, 0.50]
+        hit = pixel_on_base_plane(320, 240, 400, 400, 320, 240, T, 0.022)
+        assert hit is not None
+        np.testing.assert_allclose(hit, [0.40, -0.20, 0.022], atol=1e-6)
+        # 射线贴着平面：没有交点，不许拿深度图凑。
+        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
+        lib = (
+            __import__("pathlib").Path(__file__).resolve().parents[3] / "scripts" / "lib"
+        )
+        sys_path = __import__("sys").path
+        if str(lib) not in sys_path:
+            sys_path.insert(0, str(lib))
+        from rdk_release import is_control_stack_cmd
+
+        assert is_control_stack_cmd("/opt/ros/humble/lib/controller_manager/ros2_control_node")
+        assert is_control_stack_cmd(
+            "ros2 launch arm_bringup stack.launch.py backend:=real robot_sn:=Rizon4-063989"
+        )
+        assert is_control_stack_cmd("python3 scripts/rdk_release_movel.py --xyz 0 0 0") is False
+        assert is_control_stack_cmd("perception_node") is False
+
     def test_d435_move_only_never_hands_back_to_d405(self):
         """只测位移：D405 点一律不下发，到位停稳也不交。默认路径仍会交回。"""
         assert d405_commands_arm(
@@ -185,6 +233,9 @@ class TestHandoff:
         assert 'LaunchConfiguration("d435_move_only")' in vs
         assert '"d435_move_only"' in vs
         assert "--d435-only" in run_all
+        assert "rdk_goto_xyz.py" in run_all
+        assert "d435_k0_once.py" in run_all
+        assert "--keep-z" in run_all
 
     def test_whole_path_uses_one_cruise_and_an_arc(self):
         """远处仍 80 mm/s。近处必须另降一档。路径是圆弧，不是对着终点做 P。"""

src/arm_application/test/test_overhead_track.py

@@ -42,6 +42,7 @@ def _track_site(**over) -> dict:
     site["servo"] = {
         "xy_track": "hover",
         "lock_still_sec": 3.0,
+        "wrist_align_sec": 1.0,
         "lock_still_move_m": 0.015,
     }
     site["perception"] = {
@@ -152,6 +153,7 @@ class TestPerceptionServoWiring:
         assert "d405" not in p["overhead_camera_source"]
         assert s["handeye_mode"] == "eye_in_hand"
         assert s["lock_still_sec"] == pytest.approx(3.0)
+        assert s["wrist_align_sec"] == pytest.approx(1.0)
         assert s["wrist_handoff_conf"] == pytest.approx(0.5)
         assert s["overhead_hold_s"] == pytest.approx(8.0)
 
@@ -228,6 +230,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["wrist_align_sec"] == pytest.approx(1.0)
         assert s["lock_still_move_m"] == pytest.approx(0.015)
         # 负向：跟踪外参不能被换成顶视文件
         assert "eye_to_hand" not in s["handeye_file"]