左臂换成 tool10120938 后，手眼仍按旧 tool1 和 8×11 板求解，顶视迁臂又按短跳 rest-to-rest，真机上要么整体偏一截，要么中途弹射启停。

眼在手上改用该 TCP 做 AX=XB，再折回相机→法兰；A5 内角点改为 11×8，09-04 / 10-09 旧结果留在 backup。一阶段 D435 迁臂改成一条 FollowJointTrajectory（余弦加减速，近处降速，XY 进门槛后再单独收垂直），跟踪锁高与悬停齐平，避免倾斜相机下先降一截把孔位重投影偏掉。--no-descent 只降到离面 15 mm；跳过三阶段时触觉 USB 打不开不再空等几分钟。

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

config/site.example.yaml

@@ -26,6 +26,7 @@ robot:
   # >>> 切换机械臂：改成 robots 里某一台的完整 SN <<<
   sn: "Rizon4-XXXXXX"
   # Studio Tools 名（一字不差）。空=不切换。查：bash ~/ros2/scripts/rdk_switch_tool.sh --list
+  # 左臂 89 的激活工具写在 robots[] 里（tool10120938）；这里只给未覆盖的臂当缺省。
   tool_name: "tool1"
   # 每台登记 SN + IP，以及该臂自己的相机/夹爪/手眼/点位（覆盖下面的全局段）。
   robots:
@@ -33,11 +34,13 @@ robot:
       arm: left
       ip: "192.168.2.XXX"
       pc_ip: "192.168.2.XXX"
-      iface: "enxXXXXXXXXXXXX"
+      iface: "eth0"
       label: "左臂89"
       # 本臂装了 Orisys 视触觉（只有左臂有）。脚本靠这一项决定要不要拉触觉服务，
       # 而不是靠"SN 等于某个具体值"这种写死的判断（换台臂就会失效）。
       tactile: true
+      # 2026-10-12 示教的工具 TCP（tool + 1012 + 0938）。眼在手上 AX=XB 用这份 TCP。
+      tool_name: "tool10120938"
       camera:
         name: d405_left          # 话题 /d405_left/... 与 91 分开
         serial: "<序列号>"   # 左腕 D405（一直在用的那台）
@@ -63,6 +66,11 @@ robot:
         handeye_file: "/home/USER/ros2/handeye/results/89_eye_in_hand.yaml"
         handeye_waypoints: "/home/USER/ros2/handeye/poses/89_eye_in_hand.yaml"
         handeye_workdir: "/home/USER/ros2/handeye/tmp/89"
+      handeye:
+        # Studio tcp_location（法兰系，米 + qwxyz）。与 rdk Tool.params 必须一致。
+        gripper_tool: "tool10120938"
+        gripper_tcp_xyz: [-0.0005, 0.0067, 0.2245]
+        gripper_tcp_qwxyz: [1.0, 0.0, 0.0, 0.0]
       # 左臂跟踪高度（只在 --arm 89 时生效，改 91 的 servo 不影响这里）
       # 锚点 `&servo_left`：下面的"仿真台"条目直接引用它 —— 仿真建的就是左臂，
       # 悬停/锁高必须与 89 **一模一样**，抄两份迟早漂移。
@@ -70,8 +78,10 @@ robot:
         # 2026-09-29 底座加高后由 0.35 → 0.36：工件上表面实测抬高 10mm
         # （0.012 → 0.022），锁高同步 +0.011 ⇒ 相机到工件的视距**与加高前一致**
         # （≈0.213m，跟踪那一档已实拍验证过检出稳定），不是随手抬的。
-        # 规矩：ee_z_m 要高于 servo.hole_hover_z_m（二阶段得往**下**走，不能往上返）。
+        # 跟踪锁高与 hole_hover_z_m 相同：先到这个悬高，一阶段只动 XY。
-        ee_z_m: 0.36           # 跟踪锁高：法兰相对基座 Z [m]
+        # 对孔完成后再下降。以前 ee_z 高于悬停（0.36），对孔前会先降一截，
+        # 倾斜相机下孔位被重投影，偏差变大。
+        ee_z_m: 0.320          # 跟踪锁高：法兰相对基座 Z [m]，与 hole_hover_z_m 同一值
         min_ee_z_m: 0.20
         safe_z_m: 0.45
         z_min_m: 0.22
@@ -276,7 +286,7 @@ robot:
       arm: right
       ip: "192.168.2.XXX"
       pc_ip: "192.168.2.XXX"
-      iface: "enxXXXXXXXXXXXX"
+      iface: "eth0"
       label: "右臂91"
       camera:
         name: d405_right         # 话题 /d405_right/... 禁止和 89 的 /d405_left 混用
@@ -356,9 +366,9 @@ handeye:
   # 内角点 = 方格数 − 1；列/行顺序不用纠结，检测端两种顺序都会试
   #（见 handeye_lib.detect_chessboard）。
   boards:
-    eye_in_hand:            # A5 板：9×12 方格
+    eye_in_hand:            # A5 板：12×9 方格 → 内角点 11×8
-      cols: 8               # 内角点列数
+      cols: 11              # 内角点列数
-      rows: 11              # 内角点行数
+      rows: 8               # 内角点行数
       square_size_m: 0.015  # 方格边长 [m]
     eye_to_hand:            # A4 板：12×9 方格
       cols: 11
@@ -579,9 +589,10 @@ servo:
   deadzone_m: 0.003          # 3 mm；收紧提高对准精度。若抖动调回 0.006
   deadzone_z_m: 0.012
   lock_hold_sec: 0.7        # 旧死区驻留（混合模式不再用它进二阶段）
+  # D405 跟住后，法兰 XY 连续这么久位移不超过 lock_still_move_m → /visual_servo/locked
+  # 顶视一次性把臂迁过去的那段路不算：必须是腕部有效目标。大幅一动就重计时。
   lock_still_sec: 3.0
-  lock_still_move_m: 0.015
+  lock_still_move_m: 0.015   # 15 mm；超过就重计时，避免"晃一下刚好满 3 秒"
-
   e_xy_clip_m: 0.200         # 大误差仍按 v_max 走，靠 a_max 加速
   v_max_xy: 0.45             # 法兰 XY 上限 450 mm/s
   v_max_z: 0.04
@@ -732,7 +743,9 @@ hole_detect:
                              #    是拿法兰对的孔）。收到 0.25mm 才逼它必须真的对上规尖。
     align_max_iters: 12      # 拍照→修正→复拍 迭代上限（收紧容差后需要更多拍）
     align_tip_vision: true   # true=精对准用「视觉规尖」；false=退回旧的「法兰+偏置」
-    tip_near_m: 0.15         # 规尖量取：近端阈值，规比这更近 [m]
+    tip_near_m: 0.15         # 规尖量取：无工件面深度时的近端上限 [m]；
+                             # 精对准同一帧有 z_wp 时改用「工件面 − 15mm」
+                             # （3mm 挡不住噪声，会把工件面当成规尖）
     tip_min_m: 0.04          # 有效深度下限 [m]
     tip_max_m: 0.60          # 有效深度上限 [m]
     tip_search_band_frac: 0.25  # 中央竖带半宽 = 该比例 × 图宽（规夹在爪里，不会靠边）
@@ -752,7 +765,10 @@ hole_detect:
     # —— 柔顺运动（夹爪持规，防急加速/急停/过冲把规震脱）：速度放慢 + 平滑刹车 ——
     align_speed: 0.014       # 对准段笛卡尔速度 [m/s]（原0.03，放缓防冲）
     move_speed: 0.022        # 升降/粗居中段速度 [m/s]（原0.05，放缓防震规）
-    final_gap_mm: 2.0        # 对准后轻降目标：规尖悬在孔面上方这么高 [mm]（不接触；安全下限1.2mm自动停）
+    final_gap_mm: 2.0        # 对准后贴面轻降目标：规尖悬在孔面上方这么高 [mm]（不接触）
+    observe_gap_mm: 15.0     # --no-descent 时的观测下降：只降到离面这么高 [mm]，
+                             # 方便看规尖偏没偏。必须 ≥ final_gap_mm。
+                             # 左臂89：悬停间隙约 75mm（0.320−0.223−0.022）⇒ 大约下降 60mm。
     final_descent_speed: 0.005  # 最后轻降速度 [m/s]（很慢，柔顺）
     # ✅ 2026-10-08 定稿：下降量**只由几何决定**（法兰 FK − 工具长 − 工件面 − final_gap），
     #   深度读数只做交叉核对、不再参与任何决策。两段真机事故方向相反、都指向这条：
@@ -769,7 +785,7 @@ hole_detect:
     #   三道**物理/几何**保险：descent_budget_m（配置上限）+ 腕部 FT 接触检测（停+退 1mm）
     #   + 任一步 _goto 走不到位就停。
     #   缺几何（工具长/工件面没配）时**拒绝下降**并报错（保持悬停），绝不拿测距去赌。
-    final_descent_enable: true  # 对准后是否轻降到 final_gap_mm（false=停在悬停高）
+    final_descent_enable: true  # true=对准后降到 final_gap_mm；false / --no-descent=只降到 observe_gap_mm
     verticalize_tol_deg: 0.8 # 垂直朝下判据 [deg]
     settle_sec: 0.45         # 到位后静止多久再拍照
     snap_frames: 5           # 每次精拍取中值的帧数
@@ -842,22 +858,26 @@ hole_detect:
 # =============================================================================
 perception:
   source: "yolo"
-  # 一阶段：wrist=D405 主跟踪，画面内无 conf≥wrist_handoff_conf 则 D435 一次性定 K0。
+  # 一阶段跟踪用哪台相机。wrist=腕部 D405（主跟踪）+ 顶视 D435 一次性兜底；
-  # overhead=整段只用顶视（实验）。非法值启动即报错。
+  # overhead=整段只用 D435（实验分支才切）。非法值启动即报错，不会静默退回。
   track_camera: "wrist"
+  # 画面内最高检测 conf 低于此 → 马上拍一张 D435，把工件 / k0 定到基座系。
+  # 与 perception / servo 共用这一处，不许两边各写一个 0.5。
   wrist_handoff_conf: 0.5
+  # 顶视深度量程 + 一次性参数。腕部那组 0.04–0.55m 会把 D435 的点全丢掉。
+  # 缺键启动即红，不许借腕部量程、不许用代码默认冷却时间。
   overhead:
     min_depth_m: 0.25
     max_depth_m: 1.80
     feature_min_range_m: 0.30
     work_z_max_m: 1.50
     z_ref_m: 0.70
-    feature_mode: "plane"
+    feature_mode: "plane"     # 顶视与腕部视角差太大，pose 的 k0 经常看不见；优先 k0，否则框心
-    hold_point_on_miss: true
+    hold_point_on_miss: true  # 仅 overhead 整段跟踪时用；混合模式一次性不续发
-    oneshot_conf: 0.5
+    oneshot_conf: 0.5         # D435 这一张也必须 conf≥此值，不许拿噪声框当 K0
-    cooldown_s: 1.5
+    cooldown_s: 1.5           # 两次一次性之间的最短间隔，避免跟 D405 抢同一条 YOLO 队列
-    frame_max_age_s: 0.4
+    frame_max_age_s: 0.4      # 彩图/深度比这更旧就拒绝，不许拿过期图反投影
-    oneshot_hold_s: 8.0
+    oneshot_hold_s: 8.0       # 伺服保持这一次设定的最长时间；超时丢掉，等下一张或 D405
   truth:
     # 真值目标物体。留空 = 场景文件里的 operating.part_object（推荐留空：
     # "哪个是工件"是场景的知识，换场景不该改这里）。
@@ -1089,10 +1109,11 @@ go_home:
 
 network:
   # 2026-09-29 改线：两台臂 → 交换机 → USB-C 千兆网卡（ASIX AX88179）
-  #   enxXXXXXXXXXXXX = 该 USB 网卡（名字由网卡 MAC 派生，换网卡会变）
+  #   MAC 6c:1f:f7:d0:6f:17。内核现在叫 eth0（不再用 enxXXXXXXXXXXXX）。
   #   本机 <PC_IP>  ↔  <左臂SN>(左89)=.101  /  <右臂SN>(右91)=.100
+  #   NM 配置名 robot-usb-lan（静态 .10/24，不当默认路由）。
   # 说明：enp4s0 仍是办公网（192.168.1.x）；两台臂已不在 enp4s0 上。
-  prefer_iface: "enxXXXXXXXXXXXX"
+  prefer_iface: "eth0"
 
 # 仿真平台（当前 Gazebo 尚未接线，详见 devices/sim/gazebo/README.md）
 # 设计约束：仿真与真机共用同一套参数（servo.* / hole_detect.stage2.*），

docs/HANDBOOK.md

@@ -68,10 +68,26 @@ bash ~/ros2/scripts/run_camera_d405.sh --arm 89 --preview   # T2 相机预览，
 bash ~/ros2/scripts/run_all.sh --arm 89                     # T3 三段串联（默认不拧入）
 ```
 
-一阶段默认 **D405 主跟踪**：画面里没有 conf≥`perception.wrist_handoff_conf`（0.5）的目标，
+一阶段默认 **D405 实时推理**，D435 待命。连续 `D405_MISS_BEFORE_D435_S`（0.5 s）没有
-马上用顶视 D435 **拍一张** 把工件 / k0 定到基座系（眼在手外 T，不再乘法兰），把臂迁过去
+conf≥`perception.wrist_handoff_conf`（0.5）的目标，才用顶视 D435 **拍一张** 把工件 / k0
-让 D405 重新看见。D405 跟住后，左臂法兰 XY 连续 `servo.lock_still_sec`（3 s）位移不超过
+定到基座系（眼在手外 T，不再乘法兰），左臂按二阶段同一套 **`FollowJointTrajectory` action**
-`lock_still_move_m`（15 mm）→ `/visual_servo/locked` → 二阶段对孔。
+先收齐锁高，再把全部 XY 串成一条关节折线，余弦加速（100 mm）/
+巡航封顶 60 mm/s / 余弦减速（80 mm），头尾速度为 0。中途不重测、不停住。
+40 mm 爬到 80 mm/s 会在真机上像弹射，那是加速段太短，不是初速度或角速度。
+工件上方（剩差 ≤80 mm）巡航改 30 mm/s，只走 XY，不混 Z、不夹收垂直。
+结果 + 0.25 s 再重测 TF（带符号 `dxy`）。冻结 J 若还剩 >15 mm，只允许 **一刀补**
+（`tag=fix`）。XY 进 15 mm 门槛后再单独收法兰朝下。结果回来之前不往话题插点。
+到位成功不再发话题 hold。几乎没走近就钉住，不许按同一剩差再刷。
+走到该点并且法兰 XY 连续 `servo.lock_still_sec`（3 s）位移不超过
+`lock_still_move_m`（15 mm）之前，D405 的点不下发（推理不关）。停稳之后改回 D405
+跟踪；再按同一静止判据 → `/visual_servo/locked` → 二阶段对孔。
+只测 D435 这一段位移时加 `--d435-only`：D405 点不下发，到位停稳后钉住并结束一阶段，
+不交 D405、不进二阶段。眼外标定误差只会让冻结点整体偏一截，不会造成中途启停；
+启停来自以前按 80/15 mm 一跳一跳 rest-to-rest。
+
+```bash
+bash ~/ros2/scripts/run_all.sh --arm 89 --no-camera --d435-only
+```
 
 ```bash
 bash ~/ros2/scripts/run_real.sh --arm 89
@@ -93,7 +109,7 @@ bash ~/ros2/scripts/run_visual_servo_rt.sh --arm 89 --control
 | 按螺距旋入（螺纹规） | ✅ 有参数，`--depth`/`--pitch` 控制 | `run_stage3_search.sh --arm 89 --depth 6 --pitch 1.0` |
 | Orisys 双指触觉（法向 + 两路切向） | ⚠️ 能读，但**轴向映射尚未实测确认** | 见 [`TACTILE.md`](TACTILE.md) |
 | Orisys 的深度图 / 力矢量图 | ⚠️ 消息结构与话题已在，真机值待确认 | 同上 |
-| D435 手外相机（一阶段一次性 + S03） | ⚠️ **89 已标定**；一阶段 D405 看不见时拍一张定 K0（混合跟踪）；S03 粗定位真机未对过 | `handeye/results/89_d435_eye_to_hand.yaml` |
+| D435 手外相机（一阶段一次性 + S03） | ⚠️ 89 已按 `tool10120938` 重标（24 视角，10-09 旧结果在 `results/backup/`）；S03 粗定位真机未对过 | `handeye/results/89_d435_eye_to_hand.yaml` |
 | 自动启动全部服务（一条命令拉起所有进程） | ❌ 还没做，目前靠三个终端手工起 | 见第 6 节待办 |
 | 「规有没有被夹住」自检 | ❌ 还没做 | 见第 6 节待办 |
 | 装配合格判定（通/止） | ⚠️ **通止规装配流程**能按力矩判据拧到底/拧出来，但"通/止合格"结论还没做 | `run_gauge_assembly.sh --arm 89 --dry-run` |
@@ -485,7 +501,7 @@ bash scripts/run_unit_tests.sh -k gauge              # 只跑名字带 gauge 的
 |----|------|
 | **通止规** | 检验螺纹的量具。通端应能旋到底，止端应旋不进（或只进几圈） |
 | **89 / 91** | 左臂 / 右臂的现场编号，**各有独立的手眼、相机、夹爪、锁高，禁止混用** |
-| **法兰** | 机械臂末端安装面；手眼标定求的就是相机→法兰的 `T_4x4` |
+| **法兰** | 机械臂末端安装面；手眼 `T_4x4` 仍是相机→法兰。左臂 89 现行按 Studio `tool10120938` 的 TCP 求解，再折回法兰 |
 | **eye-in-hand / eye-to-hand** | 相机装在手上 / 装在手上之外（D405 在手上，D435 在外部） |
 | **stage1/2/3** | 跟踪 / 对孔 / 搜孔，见 1.3 |
 | **S03 / S04 / S07 / S08** | 整体装配任务书里的阶段编号（S03 粗定位、S04 跟踪、S07 柔顺下压、S08 夹爪旋转循环）。代码里对应的是 stage1/2/3 的拆分实现，**没有一一对应**，别硬套 |
@@ -501,7 +517,7 @@ bash scripts/run_unit_tests.sh -k gauge              # 只跑名字带 gauge 的
    且顺序错了会出"控制器没起来但脚本继续跑"这类问题。
 2. **夹爪持规自检** —— 启动后先用触觉判断"规真的被夹着"，没有就报错停机
    （现在没有这一步，规掉了会一路跑到撞工件才发现）。
-3. **D435 手眼标定（91）** —— 89 已写入 `handeye/results/89_d435_eye_to_hand.yaml`（18 视角）。91 还没标。S03 粗定位要用这份 TF 在真机上对一次工件才算链路验过。
+3. **D435 手眼标定（91）** —— 89 按 `tool10120938` 重标（旧 10-09 / 18 视角在 `handeye/results/backup/`）。91 还没标。S03 粗定位要用这份 TF 在真机上对一次工件才算链路验过。
 4. **装配合格判定** —— 把力曲线转成"通/止"结论。当前只输出原始几何与力，
    判定逻辑（含螺纹旋入圈数）还没写。
 

docs/HANDEYE.md

@@ -4,8 +4,8 @@
 
 | 模式 | 相机 | 板 | 板怎么摆 | 结果文件 | `T_4x4` 含义 |
 |------|------|----|----------|----------|--------------|
-| `eye_in_hand` | 腕部 D405 | A5，内角点 8×11，15 mm | 板放台面不动 | `handeye/results/<臂号>_eye_in_hand.yaml` | 相机 → 法兰 |
+| `eye_in_hand` | 腕部 D405 | A5，内角点 11×8，15 mm | 板放台面不动 | `handeye/results/<臂号>_eye_in_hand.yaml` | 相机 → 法兰（89 按 `tool10120938` TCP 求解后再折算） |
-| `eye_to_hand` | 顶置 D435 | A4，内角点 11×8，20 mm | **板装到法兰上** | `handeye/results/<臂号>_d435_eye_to_hand.yaml` | 相机 → 这条臂的基座 |
+| `eye_to_hand` | 顶置 D435 | A4，内角点 11×8，20 mm | **板夹在 `tool10120938` 上** | `handeye/results/<臂号>_d435_eye_to_hand.yaml` | 相机 → 这条臂的基座 |
 
 | 臂 | 眼在手上结果 | 眼在手外结果 | 示教关节 | 工作目录 |
 |----|--------------|--------------|----------|----------|
@@ -18,9 +18,19 @@
 
 ## 眼在手上（腕部 D405 + A5 板）
 
+左臂 89 现行工具是 Studio **`tool10120938`**（2026-10-12 示教的 TCP）。
+`run_real.sh --arm 89` 会切到这个工具；标定用 `/tcp_pose`（即该 TCP）做
+AX=XB，再左乘 `T_flange_to_tool` 折成 **相机 → 法兰** 写入 `T_4x4`
+（伺服 / 静态 TF 的父系仍是法兰）。旧的 2026-09-04 / tool1 结果在
+`handeye/results/backup/89_eye_in_hand_flange_tool1_20260904.yaml`。
+
 `teach`（RDK 软拖拽）与 `run_real.sh` **互斥**。全程带同一 `--arm`。
+板：A5，内角点 **11×8**，方格 15 mm，放台面后不要动。
 
 ```bash
+# 0) 确认配置（应看到 tool10120938 与 11x8 / 0.015 m）
+bash ~/ros2/scripts/run_handeye.sh --arm 89 show
+
 # 1) 停掉 run_real.sh 和视觉伺服
 # 2) 示教 8–12 个姿态（棋盘清晰、占画面 30–70%，姿态拉开）
 bash ~/ros2/scripts/run_handeye.sh --arm 89 teach
@@ -30,7 +40,7 @@ bash ~/ros2/scripts/run_handeye.sh --arm 89 teach
 # 3) 写入该臂 handeye/poses/89_eye_in_hand.yaml
 bash ~/ros2/scripts/run_handeye.sh --arm 89 merge
 
-# 4) 再开实机 + 腕部相机，棋盘不要动
+# 4) 再开实机（会切到 tool10120938）+ 腕部相机，棋盘不要动
 bash ~/ros2/scripts/run_real.sh --arm 89
 bash ~/ros2/scripts/run_camera_d405.sh --arm 89 --preview
 bash ~/ros2/scripts/run_handeye.sh --arm 89 calibrate
@@ -43,32 +53,36 @@ bash ~/ros2/scripts/run_handeye.sh --arm 89 calibrate
 物理摆法与上面**反过来**：相机固定，板跟着臂动。板若还摆在台面上，
 `target→camera` 不变，方程退化（解不出来或解出垃圾）。
 
+左臂 89 的“手”同样是 **`tool10120938`**：把 A4 板**刚性夹在该工具的夹爪里**
+（夹正、标定过程不要松）。`run_real.sh` 会切到这个工具，AX=XB 用 `/tcp_pose`。
+`T_4x4` 仍是 **相机 → 这条臂的基座**，**不要**再乘 `T_flange_to_tool`
+（相机和基座都在世界里不动）。2026-10-09 那份在
+`handeye/results/backup/89_d435_eye_to_hand_20261009.yaml`。
+
+板：A4，内角点 **11×8**，方格 20 mm（12×9 格）。印面朝上对着 D435。
+
 ```bash
-# 1) 把 A4 板刚性固定在法兰 / 夹爪上（夹在正中间，标定过程不要松）
+# 0) 确认配置（应看到 d435、11x8 / 0.02 m、89_d435_eye_to_hand.yaml、tool10120938）
-# 2) 起臂 + 顶置相机（两个终端，常驻）
+bash ~/ros2/scripts/run_handeye.sh --arm 89 show --mode eye_to_hand
-bash ~/ros2/scripts/run_real.sh --arm 89
-bash ~/ros2/scripts/run_camera_d435.sh --preview
 
-# 3) 采点：推着臂走，让板在 D435 画面里出现在不同位置、不同角度
+# 1) 夹好 A4 板。停掉 run_real.sh、视觉伺服、腕部 D405（RDK + USB 独占）
-#    teach 与 run_real.sh 互斥 —— 用 RDK 拖拽前先停掉 run_real.sh
+# 2) 示教 8–12 个姿态：板在 D435 画面里换位置、换倾角，占画面 30–70%
 bash ~/ros2/scripts/run_handeye.sh --arm 89 teach --mode eye_to_hand --points 12
+#    窗口：d=拖拽  Enter=拍照  q=结束（窗口只显示画面，不做棋盘检测）
 
-# 4) 合并 + 自动走点求解（此时再开 run_real.sh + D435）
+# 3) 写入 handeye/poses/89_eye_to_hand.yaml（不要手抄眼在手上那份）
 bash ~/ros2/scripts/run_handeye.sh --arm 89 merge --mode eye_to_hand
+
+# 4) 再开实机（会切到 tool10120938）+ 顶置 D435，板不要松
 bash ~/ros2/scripts/run_real.sh --arm 89
 bash ~/ros2/scripts/run_camera_d435.sh --preview
 bash ~/ros2/scripts/run_handeye.sh --arm 89 calibrate --mode eye_to_hand
 ```
 
-查看当前模式选中的板 / 相机 / 文件：
+`show` 必须同时看到 `camera=d435`、`board 11x8 inner, square=0.02 m`、
+`89_d435_eye_to_hand.yaml`、`tool10120938`。不是这四样就停。
 
-```bash
+**不要**改 `paths.handeye_file` 去指向这份结果 —— 那会把腕部 D405 的 TF 换掉。
-bash ~/ros2/scripts/run_handeye.sh --arm 89 show --mode eye_to_hand
-```
-
-应看到 `camera=d435`、`board 11x8 inner, square=0.02 m`、
-结果路径 `handeye/results/89_d435_eye_to_hand.yaml`。不是这三样就停，
-不要带着错配置开标定。
 
 ## 结果怎么用
 

handeye/README.md

@@ -65,8 +65,8 @@ handeye/
 
 | 模式 | 物理板 | 内角点（列 × 行） | 方格边长 | `site.yaml` 里的位置 |
 |------|--------|-------------------|----------|----------------------|
-| `eye_in_hand` | A5（腕部相机看台面） | 8 × 11 | 15 mm (`0.015`) | `handeye.boards.eye_in_hand` |
+| `eye_in_hand` | A5（腕部相机看台面） | 11 × 8 | 15 mm (`0.015`) | `handeye.boards.eye_in_hand` |
-| `eye_to_hand` | A4（板装在法兰上） | 11 × 8 | 20 mm (`0.020`) | `handeye.boards.eye_to_hand` |
+| `eye_to_hand` | A4（板夹在 `tool10120938` 上） | 11 × 8 | 20 mm (`0.020`) | `handeye.boards.eye_to_hand` |
 
 权威只有 `config/site.yaml` 的 `handeye.boards.<模式>`。`run_handeye.sh` 按
 `--mode` 取对应那一组，用 `-p` 传给节点。`handeye/calibrate.yaml` 和节点默认值
@@ -74,8 +74,8 @@ handeye/
 尺寸去标（那会让结果系统性偏，还不报错）。
 
 怎么看内角点：把棋盘当成一张网格，**数内部的黑白交叉点**，
-是「方格数 − 1」。A5 板上画了 9×12 个格子 → 内角点 8×11；
+是「方格数 − 1」。A5 板上画了 12×9 个格子 → 内角点 11×8；
-A4 板上画了 12×9 个格子 → 内角点 11×8。列/行顺序不用纠结，检测端两种顺序都会试。
+A4 板上画了 12×9 个格子 → 内角点 11×8（方格 20 mm）。列/行顺序不用纠结，检测端两种顺序都会试。
 
 > ⚠️ **历史上这里出过问题**：曾经三处分别写着 `9×6/0.025`、`11×16/0.011`、
 > `8×11/0.015`，谁生效取决于覆盖顺序。后来又把两块板合成一组键，于是标眼在手外

handeye/calibrate.yaml

@@ -49,11 +49,11 @@ handeye_calibrate:
     camera_dist: [0.0, 0.0, 0.0, 0.0, 0.0]
 
     # 板尺寸的权威在 site.yaml → handeye.boards.<模式>。
-    # 这里只写眼在手上那块板的兜底（A5 / 8×11 / 15mm）；run_handeye.sh 会按
+    # 这里只写眼在手上那块板的兜底（A5 / 11×8 / 15mm）；run_handeye.sh 会按
     # --mode 用 -p 覆盖成对应模式的值。没配过的模式会被脚本当场拦下，
     # 不会静默用这份兜底去标另一块板。
-    board_cols: 8
+    board_cols: 11
-    board_rows: 11
+    board_rows: 8
     square_size_m: 0.015
 
     settle_sec: 0.4

handeye/poses/89_eye_in_hand.yaml

@@ -8,31 +8,49 @@
 
 /**:
   ros__parameters:
-    num_poses: 12
+    num_poses: 21
     # 2026-09-02 通过 handeye_teach 示教（RDK 自由驱动）
     waypoint_positions: [
-      # 位姿 0
+      # pose 0
-      -0.751865, -0.769544, 0.713051, 1.140021, -0.189690, 0.133294, -0.342184,
+      -0.928421, -1.237477, 1.528823, 1.526334, -1.230146, 0.098562, 0.538531,
-      # 位姿 1
+      # pose 1
-      -0.541318, -0.867448, 0.864868, 1.233428, -0.348623, 0.146046, -0.249719,
+      -0.970203, -1.315554, 1.553230, 1.669383, -1.407869, 0.125907, 0.578173,
-      # 位姿 2
+      # pose 2
-      -0.343594, -0.948720, 0.848287, 1.273279, -0.954339, 0.019624, 0.071314,
+      -0.967819, -1.525572, 1.706057, 1.357062, -1.211051, 0.023232, 0.193541,
-      # 位姿 3
+      # pose 3
-      -0.379884, -1.002193, 1.166394, 1.579194, -1.097644, -0.115142, -0.347640,
+      -1.065106, -1.624218, 1.793644, 1.279402, -1.120929, 0.019151, 0.139658,
-      # 位姿 4
+      # pose 4
-      -0.694826, -0.943610, 0.820178, 0.963907, -0.427763, 0.165888, -0.761562,
+      -0.386279, -1.551838, 1.563578, 1.552444, -1.775337, -0.341974, 0.957445,
-      # 位姿 5
+      # pose 5
-      -0.964023, -1.025531, 1.013898, 1.032490, -0.167151, 0.017429, -0.566073,
+      -0.227779, -1.219414, 1.026882, 0.828801, -0.916809, -0.166985, 0.034154,
-      # 位姿 6
+      # pose 6
-      -0.812664, -0.984951, 1.072695, 1.473414, -0.772970, 0.267382, 0.401278,
+      -0.324968, -1.273082, 1.092344, 0.723132, -0.922095, -0.206413, 0.152082,
-      # 位姿 7
+      # pose 7
-      -0.967527, -1.067191, 1.101680, 1.434183, -0.915388, 0.259486, 0.205330,
+      -0.421966, -1.309101, 1.143065, 0.606456, -0.906673, -0.219282, 0.237953,
-      # 位姿 8
+      # pose 8
-      -0.451483, -1.257408, 0.895481, 0.806594, -0.652401, -0.221453, 0.184234,
+      -0.851598, -1.260600, 1.302895, 1.127033, -0.969809, 0.032856, 0.642673,
-      # 位姿 9
+      # pose 9
-      -0.981877, -1.415295, 1.339004, 1.131348, -0.863194, 0.170952, 1.615403,
+      -0.951418, -0.550124, 0.848598, 2.139787, -1.025778, 1.184418, 0.598394,
-      # 位姿 10
+      # pose 10
-      -0.967163, -1.377051, 1.337671, 1.399175, -1.241886, 0.284547, 1.927756,
+      -0.834498, -0.635767, 0.970038, 2.058410, -1.035138, 1.032040, 0.738619,
-      # 位姿 11
+      # pose 11
-      -0.575271, -1.141574, 1.121935, 1.502172, -1.452029, 0.088544, 2.200019,
+      -0.519528, -0.883321, 1.135035, 1.830274, -1.100901, 0.234065, -0.069068,
+      # pose 12
+      -0.347358, -1.081426, 1.192233, 1.514557, -1.244594, -0.116668, -0.564362,
+      # pose 13
+      -0.364832, -1.102689, 1.224844, 1.520320, -1.265760, -0.136855, -0.582459,
+      # pose 14
+      -1.130796, -1.316186, 1.711089, 1.821409, -1.278003, 0.010885, -1.337210,
+      # pose 15
+      -0.732222, -1.376363, 1.812339, 1.293854, -1.410826, -0.226771, -0.217230,
+      # pose 16
+      -0.885017, -1.493416, 1.894070, 1.379021, -1.520015, -0.257115, 1.025331,
+      # pose 17
+      -0.911769, -1.532695, 1.944123, 1.427787, -1.560513, -0.267425, 1.004398,
+      # pose 18
+      -1.436271, -1.768595, 1.559397, 2.244717, -2.057112, 0.341785, 0.749883,
+      # pose 19
+      -1.511828, -1.759337, 1.620075, 2.131574, -1.946595, 0.328227, 0.584657,
+      # pose 20
+      -1.180482, -1.643241, 1.865851, 1.597699, -1.464982, 0.097605, -0.217780,
     ]

handeye/poses/89_eye_to_hand.yaml

@@ -1,4 +1,4 @@
-# 眼在手外示教关节（7 关节 × N）。板装在法兰上，顶置 D435 看它。
+# 眼在手外示教关节（7 关节 × N）。板夹在 tool10120938 上，顶置 D435 看它。
 #
 # 由 teach → merge 写入，不要手抄眼在手上那份（两种模式的姿态完全不同）。
 #
@@ -7,44 +7,54 @@
 
 /**:
   ros__parameters:
-    num_poses: 19
+    num_poses: 24
     waypoint_positions: [
       # pose 0
-      -1.045969, -0.575935, 0.132913, 2.251024, -0.297265, 1.274437, -0.219817,
+      -1.241563, -0.359631, 0.346153, 2.172565, -0.179027, 0.929920, -0.778876,
       # pose 1
-      -1.221272, -0.582225, 0.269777, 2.060852, -0.180051, 1.157081, -0.070498,
+      -1.388832, -0.554698, 0.416075, 1.897617, -0.358341, 0.827495, -0.762302,
       # pose 2
-      -1.222672, -0.605722, 0.273858, 2.111021, -0.181634, 1.193047, -0.059758,
+      -1.436048, -0.573046, 0.461762, 1.870967, -0.387696, 0.813090, -0.762917,
       # pose 3
-      -1.219127, -0.576923, 0.281368, 2.310613, -0.162188, 1.339573, -0.026065,
+      -1.614491, -0.519365, 0.693991, 2.156193, -0.499113, 0.968762, 0.073287,
       # pose 4
-      -1.100853, -0.552692, 0.291246, 2.473326, -0.116403, 1.504852, 0.051283,
+      -2.020087, -0.869088, 1.059166, 2.155478, -1.198473, 0.740567, 0.478840,
       # pose 5
-      -1.022050, -0.394038, 0.375788, 2.311711, -0.503617, 1.217768, 1.268088,
+      -2.082145, -1.122210, 1.203011, 1.950272, -1.269847, 0.434138, -0.641975,
       # pose 6
-      -1.180556, -0.574370, 0.471095, 2.055400, -0.490664, 1.086456, 1.216527,
+      -1.983153, -0.886082, 0.999329, 2.324821, -1.272773, 0.754127, 0.313613,
       # pose 7
-      -2.039707, -0.492556, 1.462952, 2.222064, -0.329990, 0.209789, 0.523258,
+      -2.021479, -0.886609, 1.055679, 2.337716, -1.282773, 0.669319, -0.028355,
       # pose 8
-      -1.959748, -0.554596, 1.469405, 2.293419, -0.165008, 0.422761, 0.844081,
+      -2.022792, -0.947211, 1.118194, 2.127275, -1.211995, 0.527528, -0.169900,
       # pose 9
-      -1.666347, -0.747468, 1.236192, 2.176000, -0.303646, 0.773895, 1.279040,
+      -2.004766, -1.065820, 1.183799, 1.901084, -1.217488, 0.380285, -0.312733,
       # pose 10
-      -1.689100, -0.879982, 1.168013, 2.057620, -0.424417, 0.752234, 1.241290,
+      -1.942863, -1.219617, 1.256733, 1.653113, -1.222302, 0.261818, -0.420706,
       # pose 11
-      -1.691875, -0.889908, 1.154014, 2.062930, -0.549154, 0.850985, 1.257704,
+      -2.039558, -1.082441, 1.189833, 1.996967, -1.241252, 0.521591, 0.526563,
       # pose 12
-      -1.715420, -0.986765, 1.125361, 1.948461, -0.638012, 0.809355, 1.173290,
+      -2.098074, -1.006236, 1.538183, 2.118382, -0.737422, 0.019903, 0.648802,
       # pose 13
-      -1.729057, -1.004106, 1.126932, 1.961482, -0.727920, 0.900058, 1.188069,
+      -2.039609, -1.159222, 1.488501, 1.908579, -1.168793, -0.140451, 0.034889,
       # pose 14
-      -2.034166, -0.748356, 1.234076, 2.252718, -1.542249, 0.093972, 0.640260,
+      -1.880783, -1.104601, 1.579382, 2.036427, -0.665049, 0.254105, 1.518504,
       # pose 15
-      -2.155450, -0.769613, 1.249491, 2.275528, -1.648348, 0.073256, 0.495109,
+      -2.015530, -1.535468, 1.711220, 1.836908, -1.707450, -0.400234, -0.708683,
       # pose 16
-      -1.959788, -0.979802, 1.271261, 2.222882, -1.560991, 0.057204, 0.296625,
+      -1.997672, -1.626362, 1.529443, 2.200206, -1.291013, 0.398664, 1.954208,
       # pose 17
-      -1.933247, -0.398944, 1.211885, 2.312756, -0.424981, 0.358678, 0.347203,
+      -1.981875, -1.620651, 1.545443, 2.373140, -1.167365, 0.396487, 2.012547,
       # pose 18
-      -1.944530, -0.399220, 1.244635, 2.293245, -0.423596, 0.317991, 0.358565,
+      -2.179415, -1.511968, 1.578832, 2.401706, -1.326480, -0.051353, 1.137181,
+      # pose 19
+      -2.173463, -1.530413, 1.575534, 2.173673, -1.289673, -0.090117, 0.867214,
+      # pose 20
+      -2.096314, -1.569352, 1.579820, 1.942057, -1.458598, -0.057003, 0.572953,
+      # pose 21
+      -2.129376, -1.566787, 1.903585, 1.962858, -1.454137, -0.335990, -0.148441,
+      # pose 22
+      -2.020280, -1.527165, 1.887130, 1.728957, -1.491867, -0.370585, -0.277940,
+      # pose 23
+      -2.140817, -1.607357, 1.617236, 2.153850, -1.571057, -0.147540, 0.770535,
     ]

handeye/results/89_d435_eye_to_hand.yaml

@@ -1,13 +1,22 @@
 # 手眼标定结果
 # mode: eye_to_hand
 # camera_source: d435
-# views: 18
+# views: 24
-# utc: 2026-10-09T02:56:24.621687+00:00
+# utc: 2026-10-12T05:20:30.137123+00:00
 # board: 11x8 square_m=0.02
-# T_cam_to_base (eye_to_hand)
+# gripper_tool: tool10120938
-translation_m: [0.37495201, -0.52156937, 0.72589194]
+# T_4x4 = cam→基座（AX=XB 的手是 TCP，不折到法兰）
-T_4x4:
+# 旧结果备份在 results/backup/
-  - [-0.04575158, 0.99819969, -0.03878355, 0.37495201]
+gripper_tool: tool10120938
-  - [0.99893621, 0.04594039, 0.00399073, -0.52156937]
+T_flange_to_tool:
-  - [0.00576527, -0.03855971, -0.99923967, 0.72589194]
+  - [1.00000000, 0.00000000, 0.00000000, -0.00050000]
+  - [0.00000000, 1.00000000, 0.00000000, 0.00670000]
+  - [0.00000000, 0.00000000, 1.00000000, 0.22450000]
+  - [0.00000000, 0.00000000, 0.00000000, 1.00000000]
+# T_cam_to_base (eye_to_hand)
+translation_m: [0.37094529, -0.52805928, 0.73745585]
+T_4x4:
+  - [-0.00990083, 0.99909706, -0.04131632, 0.37094529]
+  - [0.99991380, 0.01024832, 0.00820703, -0.52805928]
+  - [0.00862304, -0.04123150, -0.99911241, 0.73745585]
   - [0.00000000, 0.00000000, 0.00000000, 1.00000000]

handeye/results/89_eye_in_hand.bak_20260904_ee-was-tool1.yaml

@@ -0,0 +1,18 @@
+# 【备份】2026-10-12 从 89_eye_in_hand.yaml 挪出。
+# 这是 2026-09-04 那份：AX=XB 的 gripper 用的是 GetArmState.ee_pose
+# （当时激活工具是 tool1，不是法兰，也不是后来的 tool10120938）。
+# 棋盘当时按 8x11 / 15mm 写。新标定不要覆盖本文件。
+# 左臂 89 eye-in-hand. 不要用于 91。
+# 手眼标定结果
+# mode: eye_in_hand
+# camera_source: d405
+# views: 12
+# utc: 2026-09-04T10:15:27.480727+00:00
+# board: 8x11 square_m=0.015
+# T_cam_to_gripper (eye_in_hand)
+translation_m: [-0.06085181, -0.00864320, 0.12364093]
+T_4x4:
+  - [0.01784499, -0.86878381, 0.49486994, -0.06085181]
+  - [0.99977570, 0.02115147, 0.00108124, -0.00864320]
+  - [-0.01140659, 0.49473964, 0.86896638, 0.12364093]
+  - [0.00000000, 0.00000000, 0.00000000, 1.00000000]

handeye/results/89_eye_in_hand.yaml

@@ -1,14 +1,28 @@
-# 左臂 89 eye-in-hand. 不要用于 91。
 # 手眼标定结果
 # mode: eye_in_hand
-# camera_source: d405
+# camera_source: d405_left
-# views: 12
+# views: 21
-# utc: 2026-09-04T10:15:27.480727+00:00
+# utc: 2026-10-12T03:29:51.243487+00:00
-# board: 8x11 square_m=0.015
+# board: 11x8 square_m=0.015
-# T_cam_to_gripper (eye_in_hand)
+# gripper_tool: tool10120938
-translation_m: [-0.06085181, -0.00864320, 0.12364093]
+# T_4x4 = cam→法兰（AX=XB 在 TCP 上解，再左乘 T_flange_to_tool）
-T_4x4:
+# T_cam_to_tool 是原始求解；旧的法兰/tool1 备份在 results/backup/
-  - [0.01784499, -0.86878381, 0.49486994, -0.06085181]
+# 2026-10-12 第一次写出的 T_4x4 乘序反了（tz≈-296 mm），已按同一份 T_cam_to_tool 重折。
-  - [0.99977570, 0.02115147, 0.00108124, -0.00864320]
+gripper_tool: tool10120938
-  - [-0.01140659, 0.49473964, 0.86896638, 0.12364093]
+T_flange_to_tool:
+  - [1.00000000, 0.00000000, 0.00000000, -0.00050000]
+  - [0.00000000, 1.00000000, 0.00000000, 0.00670000]
+  - [0.00000000, 0.00000000, 1.00000000, 0.22450000]
+  - [0.00000000, 0.00000000, 0.00000000, 1.00000000]
+T_cam_to_tool:
+  - [-0.03435184, -0.86951953, 0.49270249, -0.05961995]
+  - [0.99932767, -0.03620511, 0.00577974, -0.01523625]
+  - [0.01281276, 0.49256977, 0.87017863, -0.09761517]
+  - [0.00000000, 0.00000000, 0.00000000, 1.00000000]
+# T_cam_to_flange (eye_in_hand，由工具 TCP 折算)
+translation_m: [-0.06011995, -0.00853625, 0.12688483]
+T_4x4:
+  - [-0.03435184, -0.86951953, 0.49270249, -0.06011995]
+  - [0.99932767, -0.03620511, 0.00577974, -0.00853625]
+  - [0.01281276, 0.49256977, 0.87017863, 0.12688483]
   - [0.00000000, 0.00000000, 0.00000000, 1.00000000]

handeye/results/backup/89_d435_eye_to_hand_20261009.yaml

@@ -0,0 +1,19 @@
+# 备份：不要当现行结果用。
+# 原文件：handeye/results/89_d435_eye_to_hand.yaml
+# 备份原因：2026-10-12 起左臂 89 眼在手外改按 Studio 工具 tool10120938
+# 的 TCP 做 AX=XB（板夹在该工具上）。本份是 2026-10-09、18 视角、
+# A4 11×8 / 20 mm 的结果，当时激活工具未经这次对齐。
+# T_4x4 仍是 cam→基座。现行结果仍写回 89_d435_eye_to_hand.yaml。
+# 手眼标定结果
+# mode: eye_to_hand
+# camera_source: d435
+# views: 18
+# utc: 2026-10-09T02:56:24.621687+00:00
+# board: 11x8 square_m=0.02
+# T_cam_to_base (eye_to_hand)
+translation_m: [0.37495201, -0.52156937, 0.72589194]
+T_4x4:
+  - [-0.04575158, 0.99819969, -0.03878355, 0.37495201]
+  - [0.99893621, 0.04594039, 0.00399073, -0.52156937]
+  - [0.00576527, -0.03855971, -0.99923967, 0.72589194]
+  - [0.00000000, 0.00000000, 0.00000000, 1.00000000]

handeye/results/backup/89_eye_in_hand_flange_tool1_20260904.yaml

@@ -0,0 +1,20 @@
+# 备份：不要当现行结果用。
+# 原文件：handeye/results/89_eye_in_hand.yaml
+# 备份原因：2026-10-12 起左臂 89 眼在手上改按 Studio 工具 tool10120938 求解。
+# 本份是 2026-09-04 那次标定：AX=XB 的 gripper 按当时激活的 tool1 / 法兰口径，
+# 板为内角点 8×11、方格 15 mm。T_4x4 当时标注为 T_cam_to_gripper。
+# 现行结果仍写回 89_eye_in_hand.yaml；要比对旧外参请打开本文件。
+# 左臂 89 eye-in-hand. 不要用于 91。
+# 手眼标定结果
+# mode: eye_in_hand
+# camera_source: d405
+# views: 12
+# utc: 2026-09-04T10:15:27.480727+00:00
+# board: 8x11 square_m=0.015
+# T_cam_to_gripper (eye_in_hand)
+translation_m: [-0.06085181, -0.00864320, 0.12364093]
+T_4x4:
+  - [0.01784499, -0.86878381, 0.49486994, -0.06085181]
+  - [0.99977570, 0.02115147, 0.00108124, -0.00864320]
+  - [-0.01140659, 0.49473964, 0.86896638, 0.12364093]
+  - [0.00000000, 0.00000000, 0.00000000, 1.00000000]

scripts/fix_robot_lan.sh

@@ -42,7 +42,29 @@ pick_iface() {
     echo "${SITE_NET_IFACE}"
     return
   fi
-  for d in enp4s0 enp5s0 eth0; do
+  # 内核有时把 USB 网卡叫 eth0，不再用 enx+MAC。按 MAC 找回，禁止误选办公网 enp4s0。
+  local want="${SITE_NET_IFACE:-}"
+  if [[ "${want}" == enx* && ${#want} -ge 15 ]]; then
+    local mac
+    mac="$(printf '%s' "${want#enx}" | sed 's/../&:/g;s/:$//')"
+    local d
+    for d in /sys/class/net/*; do
+      [[ -f "${d}/address" ]] || continue
+      if [[ "$(cat "${d}/address")" == "${mac}" ]]; then
+        echo "$(basename "$d")"
+        return
+      fi
+    done
+  fi
+  local d
+  for d in /sys/class/net/*; do
+    [[ -f "${d}/address" ]] || continue
+    if ip -4 -o addr show "$(basename "$d")" 2>/dev/null | grep -q '192.168.2.10'; then
+      echo "$(basename "$d")"
+      return
+    fi
+  done
+  for d in eth0 enp4s0 enp5s0; do
     [[ -d /sys/class/net/$d ]] || continue
     echo "$d"
     return

scripts/lib/export_site_env.py

@@ -627,6 +627,13 @@ def flatten(data: dict) -> dict:
         # 两个模式各自的名字在 *_EYE_IN_HAND / *_EYE_TO_HAND 里。
         "SITE_HANDEYE_FILE": he_modes_out["SITE_HANDEYE_FILE_EYE_IN_HAND"],
         "SITE_HANDEYE_MODE": he_mode,
+        "SITE_HANDEYE_GRIPPER_TOOL": str(he.get("gripper_tool") or ""),
+        "SITE_HANDEYE_GRIPPER_TCP_XYZ": ",".join(
+            f"{float(v):.6f}" for v in list(he.get("gripper_tcp_xyz") or [])
+        ),
+        "SITE_HANDEYE_GRIPPER_TCP_QWXYZ": ",".join(
+            f"{float(v):.6f}" for v in list(he.get("gripper_tcp_qwxyz") or [])
+        ),
         **he_modes_out,
         # 不带后缀的板尺寸 = **当前模式**那一组，供 `show` 之类只看当前模式的调用方；
         # 标定脚本用带后缀的那三组（按 --mode 选），不依赖这个。
@@ -985,6 +992,10 @@ def main() -> int:
         print(f"  手眼         : {flat['SITE_HANDEYE_FILE']}")
         print(f"  手眼路点     : {flat['SITE_HANDEYE_WAYPOINTS']}")
         print(f"  手眼目录     : {flat['SITE_HANDEYE_WORKDIR']}")
+        print(
+            f"  手眼工具     : {flat.get('SITE_HANDEYE_GRIPPER_TOOL') or '（无，ee_pose 原样）'}  "
+            f"tcp=[{flat.get('SITE_HANDEYE_GRIPPER_TCP_XYZ') or ''}]"
+        )
         print(f"  命名位姿     : {flat.get('SITE_NAMED_POSES_FILE')}")
         print(f"  眼在手外结果 : {flat['SITE_HANDEYE_FILE_EYE_TO_HAND']}")
         print(f"  眼在手外路点 : {flat['SITE_HANDEYE_WAYPOINTS_EYE_TO_HAND']}")

scripts/lib/rdk_helpers.py

@@ -23,6 +23,7 @@ RDK 对同一台机器人是**独占访问**：同时有两个进程连着同一
 
 from __future__ import annotations
 
+import os
 import time
 from typing import Any, Dict, List, Optional
 
@@ -155,6 +156,77 @@ def tool_params(tool: Any, group: Any) -> Dict[str, Any]:
     }
 
 
+def iface_ipv4s(name: str) -> List[str]:
+    """网卡名 → 其上的 IPv4。FastDDS 白名单只认地址，不认 ``eth0`` / ``enx…``。"""
+    import fcntl
+    import socket
+    import struct
+    import subprocess
+
+    name = (name or "").strip()
+    if not name:
+        return []
+    if name.count(".") == 3:
+        return [name]
+    addrs: List[str] = []
+    try:
+        s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+        ifreq = struct.pack("256s", name.encode("utf-8")[:15])
+        addrs.append(socket.inet_ntoa(fcntl.ioctl(s.fileno(), 0x8915, ifreq)[20:24]))
+        s.close()
+    except OSError:
+        pass
+    try:
+        out = subprocess.check_output(
+            ["ip", "-4", "-o", "addr", "show", "dev", name],
+            text=True,
+            stderr=subprocess.DEVNULL,
+        )
+        for line in out.splitlines():
+            parts = line.split()
+            if "inet" in parts:
+                addrs.append(parts[parts.index("inet") + 1].split("/")[0])
+    except Exception:
+        pass
+    seen = set()
+    uniq: List[str] = []
+    for a in addrs:
+        if a and a not in seen:
+            seen.add(a)
+            uniq.append(a)
+    return uniq
+
+
+def rdk_whitelist_lists(iface: str = "", pc_ip: str = "") -> List[List[str]]:
+    """给 ``flexivrdk.Robot(sn, listen)`` 用的白名单候选。
+
+    只放 IPv4。把 ``eth0`` 这种网卡名塞进去，FastDDS 会把真地址全部滤掉，
+    症状是 ping 通、RDK 报 Failed to connect。
+    """
+    lists: List[List[str]] = []
+    pc = (pc_ip or os.environ.get("SITE_PC_IP") or "").strip()
+    if pc.count(".") == 3:
+        lists.append([pc])
+    iface = (iface or "").strip()
+    if iface.count(".") == 3:
+        lists.append([iface])
+    elif iface:
+        lan = [a for a in iface_ipv4s(iface) if a.startswith("192.168.2.")]
+        if lan:
+            lists.append(lan)
+        all_ips = iface_ipv4s(iface)
+        if all_ips and all_ips != lan:
+            lists.append(all_ips)
+    seen = set()
+    out: List[List[str]] = []
+    for item in lists:
+        key = tuple(item)
+        if key not in seen:
+            seen.add(key)
+            out.append(item)
+    return out
+
+
 def switch_tool(tool: Any, group: Any, target: str) -> None:
     """切换工具。失败抛异常。"""
     if group is None:

scripts/merge_handeye_poses.py

@@ -61,6 +61,16 @@ def parse_poses(path: Path) -> list[list[float]]:
     return poses
 
 
+def unique_poses(poses: list[list[float]], tol: float = 0.015) -> list[list[float]]:
+    """关节几乎不动的连拍只留一条。阈值 0.015 rad ≈ 0.86°。"""
+    kept: list[list[float]] = []
+    for pose in poses:
+        if any(max(abs(a - b) for a, b in zip(pose, old)) < tol for old in kept):
+            continue
+        kept.append(pose)
+    return kept
+
+
 def format_positions_block(poses: list[list[float]], indent: str = "    ") -> str:
     rows = [f"{indent}waypoint_positions: ["]
     for i, q in enumerate(poses):
@@ -131,9 +141,19 @@ def main(argv: list[str]) -> int:
         print(f"Not found: {dst}", file=sys.stderr)
         return 3
     poses = parse_poses(src)
-    if len(poses) < 5:
+    uniq = unique_poses(poses)
-        print(f"Need >=5 poses, got {len(poses)}", file=sys.stderr)
+    if len(uniq) < 5:
+        print(
+            f"Need >=5 unique poses, got {len(poses)} lines / {len(uniq)} unique",
+            file=sys.stderr,
+        )
         return 4
+    if len(uniq) < len(poses):
+        print(
+            f"警告: {len(poses)} 条里只有 {len(uniq)} 个姿态不同，已丢掉连按重复",
+            file=sys.stderr,
+        )
+        poses = uniq
     merge(dst, poses)
     print(f"Merged {len(poses)} poses -> {dst}")
     return 0

scripts/rdk_drag_bridge.py

@@ -55,6 +55,7 @@ from rdk_helpers import (  # noqa: E402
     clear_fault as rdk_clear_fault,
     ensure_idle,
     ensure_operational,
+    rdk_whitelist_lists,
     resolve_tool_name,
     single_arm_groups,
     switch_tool,
@@ -228,8 +229,11 @@ def main() -> int:
                 robot = None
                 last_err = None
                 attempts = []
-                if iface:
+                # 白名单必须是 IPv4。传 eth0 / enx… 会被 FastDDS 整段滤掉，
-                    attempts.extend([(sn, [iface]), (sn, [iface], True)])
+                # ping 通也会 Failed to connect。
+                for listen in rdk_whitelist_lists(iface):
+                    log('白名单: ' + ', '.join(listen))
+                    attempts.extend([(sn, listen), (sn, listen, True)])
                 attempts.append((sn,))
                 for args in attempts:
                     try:

scripts/rdk_goto_xyz.py

@@ -0,0 +1,204 @@
+#!/usr/bin/env python3
+"""用厂家 Flexiv RDK 的原语 MoveL，把当前工具尖走到世界系里的一个点。
+
+必须用 RDK 自带的 Python 3.10，不能用 ROS 的 3.11：
+
+    # 先在跑 run_real.sh 的那个终端 Ctrl-C。RDK 同时只能有一个连接。
+    /home/cc/rdk_py310/bin/python ~/ros2/scripts/rdk_goto_xyz.py
+    /home/cc/rdk_py310/bin/python ~/ros2/scripts/rdk_goto_xyz.py --xyz 0.45 -0.30 0.40
+
+不给 --xyz 时只打印当前工具尖坐标，不动臂。
+
+这个脚本做的事，按顺序只有这些：
+
+1. 用网卡上的 IPv4 连上序列号那台臂。网卡名（eth0）不能当白名单。
+2. 清故障、使能，等到 operational。示教器要在 AUTO Remote，急停松开。
+3. 读 states().tcp_pose。这 7 个数是
+   [x, y, z, qw, qx, qy, qz]，单位米，坐标系是机器人世界系（基座那套 WORLD）。
+   位置是当前 Studio 工具的 TCP（现在是 tool10120938 的尖），不是法兰，也不是相机。
+4. 目标只换 x、y、z。姿态沿用现在的朝向，折成厂家 MoveL 要的欧拉角（度，ZYX）。
+5. SwitchMode(NRT_PRIMITIVE_EXECUTION)，ExecutePrimitive("MoveL", ...)。
+6. 轮询 primitive_states()["reachedTarget"]。到了再读一次 tcp_pose，印出还差多少毫米。
+
+厂家示例：flexiv_rdk example_py/basics3_primitive_execution.py 第 (3) 段。
+"""
+
+from __future__ import annotations
+
+import argparse
+import math
+import sys
+import time
+from pathlib import Path
+
+ROOT = Path(__file__).resolve().parents[1]
+sys.path.insert(0, str(ROOT / "scripts" / "lib"))
+
+from rdk_helpers import rdk_whitelist_lists  # noqa: E402
+
+
+def site_robot() -> tuple[str, str]:
+    """读 config/site.yaml 里第一台臂的序列号和这台电脑的机器人网 IP。"""
+    text = (ROOT / "config" / "site.yaml").read_text(encoding="utf-8")
+    sn = ""
+    pc_ip = ""
+    for raw in text.splitlines():
+        line = raw.split("#", 1)[0].strip()
+        if line.startswith("sn:") and not sn:
+            sn = line.split(":", 1)[1].strip().strip('"').strip("'")
+        elif line.startswith("pc_ip:") and not pc_ip:
+            pc_ip = line.split(":", 1)[1].strip().strip('"').strip("'")
+        if sn and pc_ip:
+            break
+    if not sn:
+        raise RuntimeError(f"config/site.yaml 里没有 sn：{ROOT / 'config' / 'site.yaml'}")
+    return sn, pc_ip
+
+
+def quat_wxyz_to_euler_xyz_deg(q) -> list[float]:
+    """[qw,qx,qy,qz] → MoveL 要的 [rot_x, rot_y, rot_z] 度。
+
+    与厂家 example_py/utility.py 的 quat2eulerZYX 相同：
+    scipy 的 from_quat([x,y,z,w]).as_euler('xyz', degrees=True)。
+    这里不 import scipy，避免和 ROS 环境的 scipy 打架。
+    """
+    qw, qx, qy, qz = (float(v) for v in q)
+    n = math.sqrt(qw * qw + qx * qx + qy * qy + qz * qz)
+    if n < 1e-12:
+        raise RuntimeError(f"四元数是零：{q}")
+    qw, qx, qy, qz = qw / n, qx / n, qy / n, qz / n
+    xx, yy, zz = qx * qx, qy * qy, qz * qz
+    xy, xz, yz = qx * qy, qx * qz, qy * qz
+    wx, wy, wz = qw * qx, qw * qy, qw * qz
+    r00 = 1.0 - 2.0 * (yy + zz)
+    r10 = 2.0 * (xy + wz)
+    r20 = 2.0 * (xz - wy)
+    r21 = 2.0 * (yz + wx)
+    r22 = 1.0 - 2.0 * (xx + yy)
+    r01 = 2.0 * (xy - wz)
+    r11 = 1.0 - 2.0 * (xx + zz)
+    r12 = 2.0 * (yz - wx)
+    # R = Rx(x) @ Ry(y) @ Rz(z)
+    sy = math.sqrt(r00 * r00 + r10 * r10)
+    if sy > 1e-6:
+        x = math.atan2(r21, r22)
+        y = math.atan2(-r20, sy)
+        z = math.atan2(r10, r00)
+    else:
+        x = math.atan2(-r12, r11)
+        y = math.atan2(-r20, sy)
+        z = 0.0
+    return [math.degrees(x), math.degrees(y), math.degrees(z)]
+
+
+def connect(flexivrdk, sn: str, pc_ip: str):
+    """白名单只放 IPv4。连不上就换下一组，最后再试不带白名单。"""
+    last = None
+    attempts = []
+    for listen in rdk_whitelist_lists(pc_ip=pc_ip):
+        attempts.append((sn, listen))
+    attempts.append((sn,))
+    for args in attempts:
+        try:
+            print(f"连接 {args}")
+            return flexivrdk.Robot(*args)
+        except Exception as exc:  # noqa: BLE001 — 下一组白名单再试，最后一组失败才抛
+            last = exc
+            print(f"  失败：{exc}")
+    raise RuntimeError(f"RDK 连不上 {sn}：{last}")
+
+
+def wait_operational(robot, timeout_s: float) -> None:
+    if robot.fault():
+        print("有故障，尝试 ClearFault")
+        if not robot.ClearFault():
+            raise RuntimeError("故障清不掉。看示教器。")
+    print("Enable …（示教器：松开急停，AUTO Remote）")
+    robot.Enable()
+    t0 = time.monotonic()
+    while not robot.operational():
+        if time.monotonic() - t0 > timeout_s:
+            raise RuntimeError(f"{timeout_s:.0f}s 内没有 operational")
+        time.sleep(0.2)
+    print("operational")
+
+
+def fmt_pose(pose) -> str:
+    x, y, z = pose[0], pose[1], pose[2]
+    return f"xyz=({x:.4f}, {y:.4f}, {z:.4f}) m   qwxyz=({pose[3]:.4f}, {pose[4]:.4f}, {pose[5]:.4f}, {pose[6]:.4f})"
+
+
+def main() -> int:
+    parser = argparse.ArgumentParser(description="RDK MoveL：工具尖走到世界系的一个点")
+    parser.add_argument("--xyz", nargs=3, type=float, metavar=("X", "Y", "Z"), help="目标点，米，机器人世界系")
+    parser.add_argument("--vel", type=float, default=0.05, help="TCP 线速度 m/s，默认 0.05")
+    parser.add_argument("--max-step-m", type=float, default=0.40, help="离当前点超过这么多米就拒绝，除非 --go")
+    parser.add_argument("--go", action="store_true", help="允许超过 --max-step-m")
+    parser.add_argument("--timeout", type=float, default=60.0, help="等 reachedTarget 的秒数")
+    args = parser.parse_args()
+
+    sn, pc_ip = site_robot()
+    print(f"臂 {sn}    本机 {pc_ip or '（site 里没有 pc_ip）'}")
+
+    import flexivrdk
+
+    robot = connect(flexivrdk, sn, pc_ip)
+    try:
+        wait_operational(robot, args.timeout)
+        pose = list(robot.states().tcp_pose)
+        if len(pose) < 7:
+            raise RuntimeError(f"tcp_pose 不是 7 个数：{pose}")
+        print("当前 TCP（世界系）：", fmt_pose(pose))
+        if args.xyz is None:
+            print("没有 --xyz，只看坐标，不动。")
+            return 0
+
+        target_xyz = [float(v) for v in args.xyz]
+        dist = math.dist(pose[:3], target_xyz)
+        print(f"目标 xyz=({target_xyz[0]:.4f}, {target_xyz[1]:.4f}, {target_xyz[2]:.4f}) m")
+        print(f"直线距离 {dist * 1000:.0f} mm    速度 {args.vel:.3f} m/s    姿态保持当前")
+        if dist > args.max_step_m and not args.go:
+            print(f"超过 {args.max_step_m * 1000:.0f} mm，拒绝。确认无误后加 --go。")
+            return 2
+
+        euler_deg = quat_wxyz_to_euler_xyz_deg(pose[3:7])
+        print(f"MoveL 欧拉角 deg xyz=({euler_deg[0]:.2f}, {euler_deg[1]:.2f}, {euler_deg[2]:.2f})")
+
+        robot.SwitchMode(flexivrdk.Mode.NRT_PRIMITIVE_EXECUTION)
+        robot.ExecutePrimitive(
+            "MoveL",
+            {
+                "target": flexivrdk.Coord(
+                    target_xyz,
+                    euler_deg,
+                    ["WORLD", "WORLD_ORIGIN"],
+                ),
+                "vel": float(args.vel),
+            },
+        )
+        print("MoveL 已下发，等 reachedTarget …")
+        t0 = time.monotonic()
+        while True:
+            states = robot.primitive_states()
+            if states.get("reachedTarget"):
+                break
+            if robot.fault():
+                raise RuntimeError(f"运动中故障：{states}")
+            if time.monotonic() - t0 > args.timeout:
+                raise RuntimeError(f"{args.timeout:.0f}s 内没有 reachedTarget。states={states}")
+            time.sleep(0.2)
+
+        end = list(robot.states().tcp_pose)
+        err_mm = math.dist(end[:3], target_xyz) * 1000.0
+        print("到达 TCP：", fmt_pose(end))
+        print(f"相对目标还差 {err_mm:.1f} mm")
+        return 0
+    finally:
+        try:
+            robot.Stop()
+        except Exception:
+            pass
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())

scripts/rdk_switch_tool.py

@@ -158,27 +158,11 @@ def _iface_ipv4s(name: str) -> list:
 
 
 def _rdk_whitelist(iface: str) -> list[list[str]]:
-    """RDK 手册要的是「连机器人的那张网卡」。enp4s0 上还有办公网 IP，不能整张卡的地址都塞进白名单。"""
+    """只放 IPv4。网卡名进白名单会被 FastDDS 滤掉（见 rdk_helpers）。"""
-    import os
+    sys.path.insert(0, str(__import__("pathlib").Path(__file__).resolve().parent / "lib"))
+    from rdk_helpers import rdk_whitelist_lists  # noqa: PLC0415
 
-    lists: list[list[str]] = []
+    return rdk_whitelist_lists(iface)
-    pc = (os.environ.get("SITE_PC_IP") or "").strip()
-    if pc:
-        lists.append([pc])
-    if iface:
-        lists.append([iface])
-        robot_lan = [a for a in _iface_ipv4s(iface) if a.startswith("192.168.2.")]
-        if robot_lan:
-            lists.append(robot_lan)
-    # 去重
-    seen: set[tuple[str, ...]] = set()
-    out: list[list[str]] = []
-    for item in lists:
-        key = tuple(item)
-        if key not in seen:
-            seen.add(key)
-            out.append(item)
-    return out
 
 
 def connect_rdk(sn: str, iface: str = ""):

scripts/run_all.sh

@@ -20,7 +20,8 @@
 #   bash ~/ros2/scripts/run_all.sh --arm 89 --camera-preview     # 相机带 Qt 预览窗
 #   bash ~/ros2/scripts/run_all.sh --arm 89 --no-camera          # 相机已在别处拉起了
 #   bash ~/ros2/scripts/run_all.sh --arm 89 --no-tactile         # 不起触觉服务
-#   bash ~/ros2/scripts/run_all.sh --arm 89 --no-descent         # 只对准不轻降（验证精对准）
+#   bash ~/ros2/scripts/run_all.sh --arm 89 --no-descent         # 对准后只降到观测高度（默认离面 15mm）
+#   bash ~/ros2/scripts/run_all.sh --arm 89 --d435-only          # 只测 D435 单次位移，到位后不交 D405、不进二阶段
 #
 # 前置（另开一个终端，先跑起来）：
 #   bash ~/ros2/scripts/run_real.sh --arm 89
@@ -43,6 +44,7 @@ WITH_CAMERA=true
 WITH_TACTILE=true
 CAMERA_PREVIEW=false
 NO_DESCENT=false
+D435_ONLY=false
 
 # --sn/--arm 必须先解析：load_site_config.sh 靠 ROBOT_SN 决定加载哪套机器人配置
 # shellcheck disable=SC1091
@@ -59,14 +61,15 @@ while [[ $# -gt 0 ]]; do
     --depth)          DEPTH="${2:?--depth 需要毫米数}"; shift 2 ;;
     --pitch)          PITCH="${2:?--pitch 需要螺距毫米数}"; shift 2 ;;
     --resnap)         RESNAP=true; shift ;;
-    # 只验证**精对准**、不轻降。对准逻辑改完先这么跑，确认规真被对到孔心再放开下降
+    # 不到贴面 2mm。对准后仍降到 site 的 observe_gap_mm（默认离面 15mm），
-    # （对不准就下压会把规顶在孔沿上：2026-10-08 真机顶到 63N）。
+    # 方便看规尖偏没偏；对不准就下到 2mm 会把规顶在孔沿上（2026-10-08 真机 63N）。
     --no-descent)     NO_DESCENT=true; shift ;;
+    --d435-only)      D435_ONLY=true; SKIP_STAGE2=true; SKIP_STAGE3=true; shift ;;
     --no-preview)     PIPELINE_PREVIEW=false; shift ;;
     --camera-preview) CAMERA_PREVIEW=true; shift ;;
     --no-camera)      WITH_CAMERA=false; shift ;;
     --no-tactile)     WITH_TACTILE=false; shift ;;
-    -h|--help)        sed -n '2,30p' "$0"; exit 0 ;;
+    -h|--help)        sed -n '2,31p' "$0"; exit 0 ;;
     *) echo "Unknown: $1" >&2; exit 1 ;;
   esac
 done
@@ -174,12 +177,21 @@ else
   echo "=== --no-camera：跳过相机检查 ==="
 fi
 
-# ---- 3) 触觉服务。只有左臂装了传感器 ----
+# ---- 3) 触觉服务。只有左臂装了传感器；只有三阶段搜孔才用力 ----
+# --skip-stage3 时如果还去等，USB 打不开会假死：话题在、connected 却是 false，
+# 每次 echo 吃满超时 × 60 次 ≈ 3 分钟，终端上没有任何新行（2026-10-09 真机）。
+_SKIP_TACTILE_REASON=""
+if [[ "${SKIP_STAGE3}" == true && "${WITH_TACTILE}" == true ]]; then
+  echo "=== --skip-stage3：一/二阶段不用触觉，跳过拉起 ==="
+  WITH_TACTILE=false
+  _SKIP_TACTILE_REASON="stage3"
+fi
+
 # 同样先查 topic 列表再 echo，避免服务没起时反复撞超时。
 TACTILE_TOPIC="/tactile/left/status"
 _tactile_ok() {
   timeout 2 ros2 topic list 2>/dev/null | grep -qx "${TACTILE_TOPIC}" || return 1
-  timeout 3 ros2 topic echo "${TACTILE_TOPIC}" --once 2>/dev/null | grep -q "connected: true"
+  timeout 1 ros2 topic echo "${TACTILE_TOPIC}" --once 2>/dev/null | grep -q "connected: true"
 }
 
 if [[ "${WITH_TACTILE}" == true ]]; then
@@ -199,12 +211,19 @@ if [[ "${WITH_TACTILE}" == true ]]; then
       sleep 1.0
       nohup ros2 launch arm_application tactile.launch.py > /tmp/run_all_tactile.log 2>&1 &
       echo "    tactile pid=$! log=/tmp/run_all_tactile.log"
+      echo "    等触觉 connected（USB 打不开会在十几秒内跳过，不会空等几分钟）"
       t_ok=false
-      for _i in $(seq 1 60); do   # 最多等 ~30s；传感器要在 Python 侧加载模型
+      for _i in $(seq 1 8); do
         if _tactile_ok; then
           t_ok=true
           break
         fi
+        # 日志已经写明打不开：再给两轮自愈，然后放行。不许在这里空转到超时。
+        if [[ "${_i}" -ge 4 ]] && grep -q "connect failed" /tmp/run_all_tactile.log 2>/dev/null; then
+          echo "    触觉 USB 打不开，停止等待" >&2
+          break
+        fi
+        echo "    触觉还未 connected（${_i}/8）"
         sleep 0.5
       done
       if [[ "${t_ok}" != true ]]; then
@@ -219,7 +238,7 @@ if [[ "${WITH_TACTILE}" == true ]]; then
   else
     echo "=== 触觉只有左臂装了；当前 ${SITE_ROBOT_ARM:-?}，跳过 ==="
   fi
-else
+elif [[ "${_SKIP_TACTILE_REASON}" != "stage3" ]]; then
   echo "=== --no-tactile：跳过触觉检查 ==="
 fi
 
@@ -239,10 +258,11 @@ ARGS=(
   "preview:=${PIPELINE_PREVIEW}"
   "resnap:=${RESNAP}"
   "no_descent:=${NO_DESCENT}"
+  "d435_move_only:=${D435_ONLY}"
 )
 [[ -n "${HOLE}" ]]  && ARGS+=("target_hole:=${HOLE}")
 [[ -n "${DEPTH}" ]] && ARGS+=("insert_depth_mm:=${DEPTH}")
 [[ -n "${PITCH}" ]] && ARGS+=("screw_pitch_mm:=${PITCH}")
 
-echo "=== launch pipeline  hole=${HOLE:-site} depth=${DEPTH:-site} pitch=${PITCH:-site} skip1=${SKIP_STAGE1} skip2=${SKIP_STAGE2} skip3=${SKIP_STAGE3} no_descent=${NO_DESCENT} ==="
+echo "=== launch pipeline  hole=${HOLE:-site} depth=${DEPTH:-site} pitch=${PITCH:-site} skip1=${SKIP_STAGE1} skip2=${SKIP_STAGE2} skip3=${SKIP_STAGE3} no_descent=${NO_DESCENT} d435_only=${D435_ONLY} ==="
 exec ros2 launch arm_application pipeline.launch.py "${ARGS[@]}"

scripts/run_handeye.sh

@@ -202,6 +202,7 @@ cmd_show() {
   echo "  result      : ${RES}"
   echo "  workdir     : ${WORKDIR}"
   echo "  camera      : ${CAM} serial=${CAM_SERIAL:-（空）}"
+  echo "  tool        : ${SITE_ROBOT_TOOL_NAME:-（不切换）}  gripper_tool=${SITE_HANDEYE_GRIPPER_TOOL:-（无，ee_pose 原样）}"
   echo "  board       : $(_he BOARD_COLS)x$(_he BOARD_ROWS) inner, square=$(_he SQUARE) m  (来源=$(_he BOARD_SRC))"
   echo "                （另一种模式的板见下面 export 的 标定板[...] 两行）"
   python3 "${ROOT}/scripts/lib/export_site_env.py" --print || true
@@ -240,6 +241,20 @@ cmd_calibrate() {
     exit 2
   fi
   _need_files "${CAL}" "${WP}"
+  local grip_tool="${SITE_HANDEYE_GRIPPER_TOOL:-}"
+  local grip_xyz="${SITE_HANDEYE_GRIPPER_TCP_XYZ:-}"
+  local grip_q="${SITE_HANDEYE_GRIPPER_TCP_QWXYZ:-}"
+  if [[ -n "${grip_tool}" ]]; then
+    if [[ "${SITE_ROBOT_TOOL_NAME:-}" != "${grip_tool}" ]]; then
+      echo "ERROR: 手眼要按工具 ${grip_tool} 求解，但 site 当前激活工具是 '${SITE_ROBOT_TOOL_NAME:-}'。" >&2
+      echo "       robots[].tool_name 必须和 handeye.gripper_tool 同一份名字。" >&2
+      exit 2
+    fi
+    if [[ -z "${grip_xyz}" || -z "${grip_q}" ]]; then
+      echo "ERROR: 配了 gripper_tool=${grip_tool} 但没写 gripper_tcp_xyz / gripper_tcp_qwxyz。" >&2
+      exit 2
+    fi
+  fi
   _need_robot
   _need_camera
   echo "=== kill visual_servo (avoid traj fight) ==="
@@ -260,6 +275,19 @@ cmd_calibrate() {
   echo "    motion v=${vel} T≥${mt}s settle=${st}s qd≤${qd} pin=${ph}x${ps}s"
   echo "    poses=${WP}"
   echo "    result → ${RES}"
+  local grip_args=()
+  if [[ -n "${grip_tool}" ]]; then
+    if [[ "${HANDEYE_MODE}" == "eye_in_hand" ]]; then
+      echo "    gripper_tool=${grip_tool}  tcp_xyz=[${grip_xyz}]  （AX=XB 用 TCP，T_4x4 折成 cam→法兰）"
+    else
+      echo "    gripper_tool=${grip_tool}  tcp_xyz=[${grip_xyz}]  （AX=XB 用 TCP，T_4x4 仍是 cam→基座，不折）"
+    fi
+    grip_args+=(
+      -p gripper_tool:="${grip_tool}"
+      -p gripper_tcp_xyz:="[${grip_xyz}]"
+      -p gripper_tcp_qwxyz:="[${grip_q}]"
+    )
+  fi
   exec ros2 run arm_application handeye_calibrate --ros-args \
     --params-file "${CAL}" \
     --params-file "${WP}" \
@@ -280,7 +308,8 @@ cmd_calibrate() {
     -p settle_timeout_sec:="${sto}" \
     -p pin_holds:="${ph}" \
     -p pin_hold_sec:="${ps}" \
-    -p skip_unstable_poses:="${skip}"
+    -p skip_unstable_poses:="${skip}" \
+    "${grip_args[@]}"
 }
 
 cmd_waypoints() {
@@ -359,6 +388,7 @@ cmd_teach() {
     -p joint_prefix:="${PREFIX}"
     -p camera_source:="${CAM}"
     -p output_dir:="${WORKDIR}"
+    -p poses_file:="${TAUGHT}"
     -p target_points:="${points}"
   )
   if [[ -n "${CAM_SERIAL}" ]]; then

src/arm_application/arm_application/handeye_calibrate.py

@@ -33,8 +33,11 @@ from arm_application.handeye_lib import (
     BoardConfig,
     CameraIntrinsics,
     ChessboardStabilizer,
+    T_from_flexiv_tcp,
     calibrate_hand_eye,
+    cam_to_flange_via_tool,
     detect_chessboard,
+    format_matrix_yaml,
     format_transform_yaml,
     is_pose_filled,
     pose_to_Rt,
@@ -113,11 +116,10 @@ class HandeyeCalibrate(TolerantNode):
 
         # 棋盘（可自定义）
         # 默认值必须与 site.yaml 的 handeye.board_* 及结果文件头里的实际板子一致。
-        # 真实使用的板：内角点 8x11，方格 15mm（见 handeye/results/*.yaml 的头部注释）。
+        # 真实使用的板：A5 内角点 11x8，方格 15mm（site.yaml handeye.boards.eye_in_hand）。
-        # 这里原先写的是 9x6 / 0.025（另一张早期的板），一旦 site.yaml 漏配就会被
+        # 这里原先写的是 9x6 / 0.025，后来是 8x11；漏配就会静默用错尺寸。
-        # 静默启用 —— 症状是"标定能顺利跑完，但结果明显不对"，极难排查。
+        self.declare_parameter('board_cols', 11)  # 内角点
-        self.declare_parameter('board_cols', 8)  # 内角点
+        self.declare_parameter('board_rows', 8)
-        self.declare_parameter('board_rows', 11)
         self.declare_parameter('square_size_m', 0.015)
 
         self.declare_parameter('stable_window', 8)
@@ -138,6 +140,11 @@ class HandeyeCalibrate(TolerantNode):
         self.declare_parameter('output_dir', '/home/cc/ros2/handeye/tmp')
         self.declare_parameter('result_file', '')
         self.declare_parameter('cleanup_images', True)
+        # 眼在手上：AX=XB 的 gripper 用 Studio 工具 TCP，再折成 cam→法兰写 T_4x4。
+        # 空 = 旧口径（ee_pose 当 gripper，T_4x4 原样写出）。配了就必须带 TCP。
+        self.declare_parameter('gripper_tool', '')
+        self.declare_parameter('gripper_tcp_xyz', [0.0, 0.0, 0.0])
+        self.declare_parameter('gripper_tcp_qwxyz', [1.0, 0.0, 0.0, 0.0])
 
         prefix = str(self.get_parameter('joint_prefix').value or '')
         base = list(
@@ -152,6 +159,21 @@ class HandeyeCalibrate(TolerantNode):
         self.start_index = max(0, int(self.get_parameter('start_index').value))
         self.min_poses = int(self.get_parameter('min_poses').value)
         self.cleanup_images = bool(self.get_parameter('cleanup_images').value)
+        self.gripper_tool = str(self.get_parameter('gripper_tool').value or '').strip()
+        tcp_xyz = list(
+            self.get_parameter('gripper_tcp_xyz').get_parameter_value().double_array_value
+            or [0.0, 0.0, 0.0]
+        )
+        tcp_q = list(
+            self.get_parameter('gripper_tcp_qwxyz').get_parameter_value().double_array_value
+            or [1.0, 0.0, 0.0, 0.0]
+        )
+        self.T_flange_to_tool = T_from_flexiv_tcp(tcp_xyz[:3], tcp_q[:4])
+        if self.gripper_tool and float(np.linalg.norm(self.T_flange_to_tool[:3, 3])) < 1e-4:
+            raise RuntimeError(
+                f'gripper_tool={self.gripper_tool!r} 但 gripper_tcp_xyz 几乎是零：'
+                '等于没把 Studio TCP 传进来。不许按法兰口径假装在用工具系。'
+            )
 
         self.camera_source = str(self.get_parameter('camera_source').value).strip().lower()
         self.camera_serial = str(self.get_parameter('camera_serial').value or '').strip()
@@ -265,10 +287,12 @@ class HandeyeCalibrate(TolerantNode):
         self.images_dir.mkdir(parents=True, exist_ok=True)
 
         eye_to_hand = self.mode == 'eye_to_hand'
+        grip = self.gripper_tool or '（ee_pose 原样，无工具折算）'
         self.get_logger().info(
             f'手眼标定 [{self.mode}] camera_source={self.camera_source}：'
             f'{len(poses)} 个位姿，棋盘={self.board.cols}x{self.board.rows} '
             f'方格={self.board.square_size}m，v={self.velocity_scale} '
+            f'gripper_tool={grip}'
         )
         self.get_logger().warn('清理工作空间；确认急停就绪。Ctrl+C 可中止。')
 
@@ -368,11 +392,57 @@ class HandeyeCalibrate(TolerantNode):
 
         self.get_logger().info(f'用 {len(Rg)} 个视角求解手眼标定...')
         R, t = calibrate_hand_eye(Rg, tg, Rc, tc, eye_to_hand=eye_to_hand)
-        name = (
+        extra = ''
-            'T_cam_to_base (eye_to_hand)'
+        if eye_to_hand:
-            if eye_to_hand
+            name = 'T_cam_to_base (eye_to_hand)'
-            else 'T_cam_to_gripper (eye_in_hand)'
+            yaml_txt = format_transform_yaml(R, t, name)
+            if self.gripper_tool:
+                extra = (
+                    f'gripper_tool: {self.gripper_tool}\n'
+                    + format_matrix_yaml(self.T_flange_to_tool, 'T_flange_to_tool')
                 )
+                self.get_logger().info(
+                    f'gripper={self.gripper_tool}：AX=XB 的手是 TCP；'
+                    f'T_4x4 是 cam→基座，不折  t={t.reshape(3)*1000} mm'
+                )
+        elif self.gripper_tool:
+            T_cam_tool = np.eye(4)
+            T_cam_tool[:3, :3] = R
+            T_cam_tool[:3, 3] = t.reshape(3)
+            T_cam_flange = cam_to_flange_via_tool(T_cam_tool, self.T_flange_to_tool)
+            delta = float(np.linalg.norm(T_cam_flange[:3, 3] - T_cam_tool[:3, 3]))
+            tool_len = float(np.linalg.norm(self.T_flange_to_tool[:3, 3]))
+            if tool_len > 0.05 and delta < 1e-4:
+                self.get_logger().error(
+                    'T_cam_to_flange 与 T_cam_to_tool 平移几乎相同，'
+                    '折算没生效。结果作废。'
+                )
+                return 11
+            # 腕部相机在法兰前方（法兰 +Z）。乘反了 Z 会落到 −200 mm 一带。
+            if (
+                self.T_flange_to_tool[2, 3] > 0.05
+                and float(T_cam_flange[2, 3]) < 0.0
+            ):
+                self.get_logger().error(
+                    f'T_cam_to_flange tz={float(T_cam_flange[2, 3])*1000:.1f} mm '
+                    '在法兰后方，乘序反了。结果作废。'
+                )
+                return 12
+            R, t = T_cam_flange[:3, :3], T_cam_flange[:3, 3].reshape(3, 1)
+            name = 'T_cam_to_flange (eye_in_hand，由工具 TCP 折算)'
+            yaml_txt = format_transform_yaml(R, t, name)
+            extra = (
+                f'gripper_tool: {self.gripper_tool}\n'
+                + format_matrix_yaml(self.T_flange_to_tool, 'T_flange_to_tool')
+                + format_matrix_yaml(T_cam_tool, 'T_cam_to_tool')
+            )
+            self.get_logger().info(
+                f'gripper={self.gripper_tool}：'
+                f'T_cam_to_tool t={T_cam_tool[:3, 3]*1000} mm → '
+                f'T_cam_to_flange t={T_cam_flange[:3, 3]*1000} mm'
+            )
+        else:
+            name = 'T_cam_to_gripper (eye_in_hand)'
             yaml_txt = format_transform_yaml(R, t, name)
         header = (
             f'# 手眼标定结果\n'
@@ -383,8 +453,20 @@ class HandeyeCalibrate(TolerantNode):
             f'# board: {self.board.cols}x{self.board.rows} '
             f'square_m={self.board.square_size}\n'
         )
+        if self.gripper_tool and eye_to_hand:
+            header += (
+                f'# gripper_tool: {self.gripper_tool}\n'
+                f'# T_4x4 = cam→基座（AX=XB 的手是 TCP，不折到法兰）\n'
+                f'# 旧结果备份在 results/backup/\n'
+            )
+        elif self.gripper_tool:
+            header += (
+                f'# gripper_tool: {self.gripper_tool}\n'
+                f'# T_4x4 = cam→法兰（AX=XB 在 TCP 上解，再左乘 T_flange_to_tool）\n'
+                f'# T_cam_to_tool 是原始求解；旧的法兰/tool1 备份在 results/backup/\n'
+            )
         self.result_file.parent.mkdir(parents=True, exist_ok=True)
-        self.result_file.write_text(header + yaml_txt, encoding='utf-8')
+        self.result_file.write_text(header + extra + yaml_txt, encoding='utf-8')
         (self.output_dir / 'captures.json').write_text(
             json.dumps(meta, indent=2), encoding='utf-8'
         )

src/arm_application/arm_application/handeye_lib.py

@@ -342,6 +342,38 @@ def calibrate_hand_eye(
     return calibrate_hand_eye_tsai(Rg, tg, R_target2cam, t_target2cam)
 
 
+def T_from_flexiv_tcp(xyz_m, qwxyz) -> np.ndarray:
+    """Studio ``tcp_location`` → 法兰→工具 4×4。
+
+    Flexiv 顺序是 ``[x, y, z, q_w, q_x, q_y, q_z]``，四元数是 wxyz，
+    和本仓库其余地方的 xyzw 不一样。只在这里换一次。
+    """
+    xyz = np.asarray(xyz_m, dtype=float).reshape(3)
+    q = np.asarray(qwxyz, dtype=float).reshape(7) if np.asarray(qwxyz).size == 7 else np.asarray(qwxyz, dtype=float).reshape(4)
+    if q.size == 7:
+        qw, qx, qy, qz = (float(q[3]), float(q[4]), float(q[5]), float(q[6]))
+    else:
+        qw, qx, qy, qz = (float(q[0]), float(q[1]), float(q[2]), float(q[3]))
+    R = quat_xyzw_to_R((qx, qy, qz, qw))
+    return Rt_to_T(R, xyz.reshape(3, 1))
+
+
+def cam_to_flange_via_tool(T_cam_to_tool: np.ndarray, T_flange_to_tool: np.ndarray) -> np.ndarray:
+    """AX=XB 在工具 TCP 上解出 cam→工具后，再折成 cam→法兰。
+
+    ``T_flange_to_tool`` 是 Studio TCP：``p_法兰 = T_flange_to_tool · p_工具``。
+    OpenCV 的 ``T_cam_to_tool`` 是 ``p_工具 = T_cam_to_tool · p_相机``。
+    所以 ``T_cam_to_flange = T_flange_to_tool · T_cam_to_tool``。
+
+    乘反了（``T_cam_to_tool · inv(T_ft)``）旋转看着还像，平移会把相机
+    扔到法兰后方约 30 cm。忘了乘则 ``T_4x4`` 停在 TCP 上，沿工具偏 ~225 mm。
+    两种都不报错。
+    """
+    T_ct = np.asarray(T_cam_to_tool, dtype=float).reshape(4, 4)
+    T_ft = np.asarray(T_flange_to_tool, dtype=float).reshape(4, 4)
+    return T_ft @ T_ct
+
+
 def format_transform_yaml(R: np.ndarray, t: np.ndarray, name: str) -> str:
     T = Rt_to_T(R, t)
     lines = [
@@ -354,6 +386,14 @@ def format_transform_yaml(R: np.ndarray, t: np.ndarray, name: str) -> str:
     return '\n'.join(lines) + '\n'
 
 
+def format_matrix_yaml(T: np.ndarray, key: str) -> str:
+    arr = np.asarray(T, dtype=float).reshape(4, 4)
+    lines = [f'{key}:']
+    for row in arr:
+        lines.append('  - [' + ', '.join(f'{v:.8f}' for v in row) + ']')
+    return '\n'.join(lines) + '\n'
+
+
 def is_pose_filled(joints: Sequence[float]) -> bool:
     """若全为零 / NaN 占位则返回 False。"""
     arr = np.asarray(joints, dtype=float)

src/arm_application/arm_application/handeye_teach.py

@@ -122,6 +122,8 @@ class RdkBridge:
             'PATH': f'{Path(py).parent}:/usr/bin:/bin',
             'LD_LIBRARY_PATH': flexiv_lib,
             'LANG': 'C.UTF-8',
+            'SITE_PC_IP': os.environ.get('SITE_PC_IP', ''),
+            'SITE_NET_IFACE': os.environ.get('SITE_NET_IFACE', ''),
         }
         # 不要继承 conda 的 PYTHONPATH。
         # 重要：stderr 不能用 PIPE —— RDK 日志会填满管道并让桥接进程死锁
@@ -254,6 +256,9 @@ class HandeyeTeach(TolerantNode):
         # 设了就只是个"提醒"：采够时提示可以收工，不强制关窗 ——
         # 常常采到一半发现某个角度还没覆盖，强制退出反而要重开。
         self.declare_parameter('target_points', 0)
+        # 必须和 run_handeye.sh 的 TAUGHT 是同一个文件。只写 taught_poses.txt、
+        # 脚本去读 taught_poses_<模式>.txt，merge 就会「文件在、内容空」。
+        self.declare_parameter('poses_file', '')
 
         self.backend = str(self.get_parameter('backend').value).strip().lower()
         self.robot_sn = str(self.get_parameter('robot_sn').value)
@@ -261,8 +266,12 @@ class HandeyeTeach(TolerantNode):
 
         self.output_dir = Path(str(self.get_parameter('output_dir').value)).expanduser()
         self.images_dir = self.output_dir / 'images'
-        self.poses_file = self.output_dir / 'taught_poses.txt'
+        _pf = str(self.get_parameter('poses_file').value).strip()
+        self.poses_file = (
+            Path(_pf).expanduser() if _pf else self.output_dir / 'taught_poses.txt'
+        )
         self.yaml_file = self.output_dir / 'taught_waypoint_positions.yaml'
+        self._last_capture_t = 0.0
         self.images_dir.mkdir(parents=True, exist_ok=True)
 
         self._captures: List[dict] = []
@@ -531,6 +540,20 @@ class HandeyeTeach(TolerantNode):
             if q is None:
                 self.get_logger().error('读取关节失败')
                 return
+            now = time.monotonic()
+            if now - self._last_capture_t < 0.8:
+                self.get_logger().warn('按太快，忽略（防 Enter 连发）')
+                return
+            if self._captures:
+                last = self._captures[-1]['joints']
+                delta = max(abs(a - b) for a, b in zip(q, last))
+                if delta < 0.015:
+                    self.get_logger().warn(
+                        f'关节几乎没动（Δ={delta:.4f} rad），忽略这次拍照。'
+                        '先拖到另一个姿态再按 Enter。'
+                    )
+                    return
+            self._last_capture_t = now
 
             idx = len(self._captures)
             stamp = datetime.now(timezone.utc).strftime('%Y%m%d_%H%M%S')

src/arm_application/arm_application/overhead_oneshot.py

@@ -17,6 +17,32 @@ from typing import Iterable, Optional
 import numpy as np
 
 from arm_application.handeye_lib import cam_point_to_base
+from arm_application.path_profile import (  # 伺服和测试从本模块再导出，不另抄一份
+    D435_FULL_ACC_M,
+    D435_FULL_CRUISE_MPS,
+    D435_FULL_DEC_M,
+    D435_MID_HOP_MAX_M,
+    D435_MID_LEFTOVER_M,
+    D435_MOVE_ACC_MPS2,
+    D435_MOVE_SPEED_SCALE,
+    D435_NEAR_CRUISE_MPS,
+    D435_NEAR_HOP_MAX_M,
+    D435_NEAR_LEFTOVER_M,
+    D435_VERT_TILT_RAD,
+    D435_SETTLE_S,
+    FROZEN_J_HOP_MAX_M,
+    build_arc_xy_move,
+    flange_down_w,
+    flange_tilt_rad,
+    hybrid_cruise_vxy,
+    oneshot_hop_limits,
+    smooth_joint_schedule,
+    trap_acc_for_hop,
+    translate_hold_ori_waypoints,
+)
+
+# 上一跳实际少走这么多，就认定冻结 J 在当前姿态走不动，不许按同一剩差再刷。
+D435_MIN_PROGRESS_FRAC = 0.40
 
 
 def best_det_conf(dets: Optional[Iterable]) -> Optional[float]:
@@ -30,6 +56,301 @@ def best_det_conf(dets: Optional[Iterable]) -> Optional[float]:
     return max(vals)
 
 
+def z_locked_qos():
+    """``visual_servo/z_locked`` 的 QoS。发布端和订阅端必须用这一份。
+
+    感知比伺服晚启动。默认 volatile 会丢掉伺服启动时已经发出的「已到锁高」，
+    感知就停在初值 False，D435 永远不拍。
+    """
+    from rclpy.qos import (  # noqa: PLC0415 — 纯函数测试不需要在导入时拉起 ROS
+        DurabilityPolicy,
+        HistoryPolicy,
+        QoSProfile,
+        ReliabilityPolicy,
+    )
+
+    return QoSProfile(
+        history=HistoryPolicy.KEEP_LAST,
+        depth=1,
+        reliability=ReliabilityPolicy.RELIABLE,
+        durability=DurabilityPolicy.TRANSIENT_LOCAL,
+    )
+
+
+def at_lock_height(z_m: float, lock_z_m: float, gate_m: float) -> bool:
+    """法兰是否在锁高带内。
+
+    低于锁高**不算**到位。上一轮观测下降停在 0.26 m 时，若把「不高于锁高」
+    当成到位，XY 会在低位开跑，D435 又因高度不对拒绝拍照，两边都不再动。
+    ``lock_z_m <= 0`` 表示没启用锁高。
+    """
+    lock = float(lock_z_m)
+    if lock <= 0.0:
+        return True
+    gate = max(0.02, float(gate_m))
+    return abs(float(z_m) - lock) <= gate
+
+
+def lock_z_velocity(z_m: float, lock_z_m: float, *, kp: float, v_lim: float) -> float:
+    """回到锁高的竖直速度 [m/s]，基座 +Z 朝上。
+
+    低于锁高为正（上抬），高于为负（下降）。4 mm 内视为到位。
+    不许在低于锁高时把速度写成 0：那会和「没到锁高不拍 D435」卡死。
+    """
+    lock = float(lock_z_m)
+    if lock <= 0.0:
+        return 0.0
+    ez = lock - float(z_m)
+    if abs(ez) < 0.004:
+        return 0.0
+    lim = abs(float(v_lim))
+    return float(np.clip(float(kp) * ez, -lim, lim))
+
+
+def hold_translation_vz(z_m: float, z_hold_m: float, *, kp: float, v_lim: float) -> float:
+    """平移时把法兰钉在**这一段开始时的高度**。
+
+    只写 v_z=0 不够：大范围平移加姿态校正时，阻尼求逆会把竖直速度吃掉，
+    2026-10-09 真机从 0.327 m 漂到 0.307 m 又抬到 0.337 m。这里用开始那一
+    帧的高度当设定，4 mm 内不动，避免来回抖。不追 site 里的锁高数字，
+    也不因此停掉 XY。下臂仍只在二阶段。
+    """
+    ez = float(z_hold_m) - float(z_m)
+    if abs(ez) < 0.004:
+        return 0.0
+    lim = abs(float(v_lim))
+    return float(np.clip(float(kp) * ez, -lim, lim))
+
+
+# D405 连续这么久没有高置信度目标，才让待命的 D435 拍一张。
+# 单帧漏检就拍，会在平移半路把目标换掉。
+D405_MISS_BEFORE_D435_S = 0.5
+# 拍了但伺服没 ``d435_commanding``（工作盒拒收等）：超过这时限就允许再拍。
+# 不清的话 ``_oneshot_shot`` 会永远真，臂停着、感知还以为在赶路。
+ONESHOT_ACCEPT_TIMEOUT_S = 0.8
+
+
+def oneshot_shot_timed_out(
+    *,
+    shot: bool,
+    commanding: bool,
+    age_s: float,
+    timeout_s: float = ONESHOT_ACCEPT_TIMEOUT_S,
+) -> bool:
+    """拍过一张、伺服却没接住 → True。已经在赶路不许清。"""
+    if not bool(shot) or bool(commanding):
+        return False
+    return float(age_s) >= float(timeout_s)
+
+
+def d405_commands_arm(
+    *,
+    using_d435: bool,
+    arrived: bool,
+    settled: bool,
+    move_only: bool = False,
+) -> bool:
+    """D405 这一帧的点能不能下发给左臂。
+
+    推理一直开着。正在用 D435 那一个点时，点到并且停稳之前不下发。
+    停稳沿用一阶段的 ``StillLock``（site 里的 ``lock_still_sec`` /
+    ``lock_still_move_m``），不另写一套计时。
+    ``move_only``：只测 D435 位移，D405 点一律不下发（到位之后也不交）。
+    """
+    if move_only:
+        return False
+    if not using_d435:
+        return True
+    return bool(arrived) and bool(settled)
+
+
+def oneshot_needs_verticalize(
+    tilt_rad: float,
+    leftover_m: float,
+    arrive_m: float,
+    *,
+    tilt_lim: float = D435_VERT_TILT_RAD,
+    already_done: bool = False,
+) -> bool:
+    """XY 已经进到位门槛、倾角还大，才单独收垂直。不许和平移捆在一起。"""
+    if already_done:
+        return False
+    if float(leftover_m) > float(arrive_m):
+        return False
+    return float(tilt_rad) > float(tilt_lim)
+
+
+def oneshot_next_hop_m(
+    dist_m: float, hop_max_m: float = FROZEN_J_HOP_MAX_M
+) -> float:
+    """这一跳最多走 ``hop_max_m``。冻结 J 一次规划 400 mm 会少走一大截。"""
+    return min(max(0.0, float(dist_m)), float(hop_max_m))
+
+
+def oneshot_cartesian_dp(
+    p_ee,
+    goal_xy,
+    lock_z: float,
+    *,
+    gate_m: float,
+    hop_max_m: Optional[float] = None,
+    leftover_xy_m: Optional[float] = None,
+):
+    """这一跳的笛卡尔位移。没到锁高只收 Z；远处 XY 带锁高；近处只走 XY。
+
+    工件上方最后一跳若再把 7 mm 的 Z 和 60 mm 的 XY 捆在一起，冻结 J 会把腕部带偏。
+    远处仍带 ``ee_z_m``，避免长跳把高度抬出锁高带。
+    """
+    p = np.asarray(p_ee, dtype=float).reshape(3)
+    z_lock = float(lock_z)
+    leftover = (
+        float(leftover_xy_m)
+        if leftover_xy_m is not None
+        else float(
+            np.hypot(float(goal_xy[0]) - float(p[0]), float(goal_xy[1]) - float(p[1]))
+        )
+    )
+    hop_cap = (
+        float(hop_max_m)
+        if hop_max_m is not None
+        else oneshot_hop_limits(leftover)[0]
+    )
+    if not at_lock_height(float(p[2]), z_lock, float(gate_m)):
+        dz = z_lock - float(p[2])
+        mag = oneshot_next_hop_m(abs(dz), hop_cap)
+        if mag < 1e-6:
+            return np.zeros(3, dtype=float)
+        return np.array([0.0, 0.0, float(np.sign(dz)) * mag], dtype=float)
+    dz = 0.0 if leftover <= D435_NEAR_LEFTOVER_M else (z_lock - float(p[2]))
+    dp = np.array(
+        [
+            float(goal_xy[0]) - float(p[0]),
+            float(goal_xy[1]) - float(p[1]),
+            dz,
+        ],
+        dtype=float,
+    )
+    n = float(np.linalg.norm(dp))
+    hop = oneshot_next_hop_m(n, hop_cap)
+    if n > hop > 1e-9:
+        dp = dp * (hop / n)
+    return dp
+
+
+def oneshot_full_segments(
+    p_ee,
+    goal_xy,
+    lock_z: float,
+    *,
+    gate_m: float,
+):
+    """整段一次走完的笛卡尔分段。先收齐锁高，再走全部 XY，不切 80/15 mm。
+
+    近处（剩差 ≤80 mm）仍然只走 XY，不混 Z。收垂直不在这里。
+    返回 ``[(dp, tag), ...]``，``tag`` 是 ``lift_z`` 或 ``xy``。
+    """
+    p = np.asarray(p_ee, dtype=float).reshape(3).copy()
+    z_lock = float(lock_z)
+    goal = np.asarray(goal_xy, dtype=float).reshape(2)
+    out = []
+    if not at_lock_height(float(p[2]), z_lock, float(gate_m)):
+        dz = z_lock - float(p[2])
+        if abs(dz) >= 1e-6:
+            out.append((np.array([0.0, 0.0, dz], dtype=float), "lift_z"))
+            p[2] = z_lock
+    leftover = float(np.hypot(float(goal[0]) - float(p[0]), float(goal[1]) - float(p[1])))
+    dz = 0.0 if leftover <= D435_NEAR_LEFTOVER_M else (z_lock - float(p[2]))
+    dp = np.array(
+        [float(goal[0]) - float(p[0]), float(goal[1]) - float(p[1]), dz],
+        dtype=float,
+    )
+    if float(np.linalg.norm(dp)) >= 1e-6:
+        out.append((dp, "xy"))
+    return out
+
+
+def oneshot_concat_hold_ori(q0, J, segments, *, R=None):
+    """把分段 ``dp`` 用同一份冻结 J 串成一条关节折线。中途不回零速。"""
+    q = np.asarray(q0, dtype=float).copy()
+    wps = [q.copy()]
+    planned = 0.0
+    asked = 0.0
+    tags = []
+    for dp, tag in segments:
+        hop = float(np.linalg.norm(np.asarray(dp, dtype=float).reshape(3)))
+        if hop < 1e-6:
+            continue
+        asked += hop
+        part, remain = translate_hold_ori_waypoints(
+            q, J, dp, R=R, verticalize=False
+        )
+        if len(part) < 2:
+            break
+        wps.extend(part[1:])
+        q = wps[-1]
+        planned += hop - float(remain)
+        tags.append(str(tag))
+    tag = "full"
+    if tags == ["lift_z"]:
+        tag = "lift_z"
+    elif "lift_z" in tags and "xy" in tags:
+        tag = "full"
+    elif tags == ["xy"]:
+        tag = "full"
+    return wps, planned, asked, tag
+
+
+def oneshot_full_cruise_mps(leftover_m: float, far_cruise_mps: float) -> float:
+    """整段巡航：工件上方 30 mm/s；远处封顶 ``D435_FULL_CRUISE_MPS``，不许再弹到 80。"""
+    far = max(1e-4, float(far_cruise_mps))
+    if float(leftover_m) <= D435_NEAR_LEFTOVER_M:
+        return min(D435_NEAR_CRUISE_MPS, far)
+    return min(D435_FULL_CRUISE_MPS, far)
+
+
+def oneshot_may_send_another(
+    leftover_m: float,
+    last_dist_m: Optional[float],
+    *,
+    arrive_m: float,
+    hop_max_m: float = FROZEN_J_HOP_MAX_M,
+    min_progress_frac: float = D435_MIN_PROGRESS_FRAC,
+) -> bool:
+    """上一跳走完后还要不要再发一条。
+
+    剩差已经进到位门槛 → 钉住。
+    上一跳几乎没缩短剩差 → 不许按同一距离再刷（真机 28 mm 死循环）。
+    """
+    if float(leftover_m) <= float(arrive_m):
+        return False
+    if last_dist_m is None:
+        return True
+    progressed = float(last_dist_m) - float(leftover_m)
+    planned = min(float(last_dist_m), float(hop_max_m))
+    if progressed < float(min_progress_frac) * max(planned, 1e-6):
+        return False
+    return True
+
+
+def oneshot_should_submit(
+    *,
+    handoff: bool,
+    low_for_s: float,
+    already_shot: bool,
+    wait_s: float = D405_MISS_BEFORE_D435_S,
+) -> bool:
+    """腕部连续 ``wait_s`` 秒没有高置信度目标、这一轮还没拍成，才让 D435 拍一张。
+
+    高度不在这里再判一次。进一阶段之前已经抬到锁高；再等伺服报「已到锁高」
+    会和「没到锁高就只收 Z、不平移」卡死。拍成一张之后不许连拍，顶视不是第二套跟踪。
+    """
+    return (
+        bool(handoff)
+        and (not already_shot)
+        and float(low_for_s) >= float(wait_s)
+    )
+
+
 def select_oneshot_det(dets: Optional[Iterable], min_conf: float):
     """顶视一次性：只收 conf≥门槛的框，取最高分。没有就 None，不许降级。"""
     if not dets:

src/arm_application/arm_application/path_profile.py

@@ -0,0 +1,550 @@
+"""路径弧长上的梯形速度，以及 XY 圆弧。
+
+二阶段下臂和一阶段 D435 迁臂共用这一份。不许再抄一套加减速。
+"""
+
+from __future__ import annotations
+
+import math
+from dataclasses import dataclass, field
+from typing import Optional, Tuple
+
+import numpy as np
+
+# 整段同一巡航：site ``v_slow_xy`` × 本系数。1.0 → 80 mm/s（二阶段对孔同档上限）。
+D435_MOVE_SPEED_SCALE = 1.0
+# 缺路程时的兜底加速度。真机 hop 用 ``trap_acc_for_hop``，和二阶段同一条刹车。
+D435_MOVE_ACC_MPS2 = 0.125
+# action 走完后再停这么久，才重测 TF。二阶段 ``_goto`` 的 settle_s。
+D435_SETTLE_S = 0.25
+# 冻结雅可比一次只信这么远。二阶段 ``_plan_and_send`` 默认 ``max_seg`` 同源。
+# 400 mm 一次规划，真机剩 117 mm，随后 28 mm 死循环刷轨迹。
+FROZEN_J_HOP_MAX_M = 0.08
+# 剩差进入这一带 = 工件上方。真机最后一跳 37–75 mm @ 80 mm/s 会晃并偏转。
+D435_NEAR_LEFTOVER_M = 0.08
+D435_NEAR_HOP_MAX_M = 0.015
+D435_NEAR_CRUISE_MPS = 0.030
+# 法兰倾角超过这个才单独收垂直。和 XY 捆在同一跳会把腕部漏成 Y 过冲。
+D435_VERT_TILT_RAD = 0.015
+D435_MID_LEFTOVER_M = 0.15
+D435_MID_HOP_MAX_M = 0.040
+# 弦长的 12% 当矢高，封顶 50 mm，避免大圆弧甩出工作盒。
+D435_ARC_SAGITTA_FRAC = 0.12
+D435_ARC_SAGITTA_MAX_M = 0.05
+# 整段一次走完。起步必须够长：40 mm 就爬到 80 mm/s 是真机那下弹射
+# （头尾速度本来就是 0，不是初速度；平移也不下发角速度）。
+# 峰值加速度 a_max = π v² / (4 d_acc)；100 mm @ 60 mm/s → 约 0.028 m/s²。
+D435_FULL_ACC_M = 0.100
+D435_FULL_DEC_M = 0.080
+D435_FULL_CRUISE_MPS = 0.060
+
+
+def hybrid_cruise_vxy(
+    v_slow_xy: float, scale: float = D435_MOVE_SPEED_SCALE
+) -> float:
+    """远处巡航。近处由 ``oneshot_hop_limits`` 另给一档，不许再抄 15 mm/s。"""
+    return max(1e-4, float(v_slow_xy) * float(scale))
+
+
+def oneshot_hop_limits(
+    leftover_m: float, far_cruise_mps: float = 0.08
+) -> Tuple[float, float]:
+    """这一跳的 ``(hop_max_m, cruise_mps)``。近处必须比 80 mm / 80 mm/s 短且慢。"""
+    d = max(0.0, float(leftover_m))
+    far = max(1e-4, float(far_cruise_mps))
+    if d <= D435_NEAR_LEFTOVER_M:
+        return D435_NEAR_HOP_MAX_M, min(D435_NEAR_CRUISE_MPS, far)
+    if d <= D435_MID_LEFTOVER_M:
+        return D435_MID_HOP_MAX_M, far
+    return FROZEN_J_HOP_MAX_M, far
+
+
+def flange_down_w(
+    R,
+    *,
+    dead_rad: float = 0.006,
+    gain: float = 2.0,
+    mag_lim: float = 0.5,
+) -> Tuple[np.ndarray, float]:
+    """让法兰 +Z 指向世界 −Z 的角速度 + 当前倾角。
+
+    和二阶段 ``_w_down`` 同一份。死区 0.006 rad（约 0.34°）以下不再修。
+    """
+    z_now = np.asarray(R, dtype=float).reshape(3, 3)[:, 2]
+    z_des = np.array([0.0, 0.0, -1.0], dtype=float)
+    c = float(np.clip(np.dot(z_now, z_des), -1.0, 1.0))
+    ang = float(math.acos(c))
+    if ang < float(dead_rad):
+        return np.zeros(3, dtype=float), ang
+    axis = np.cross(z_now, z_des)
+    n = float(np.linalg.norm(axis))
+    axis = axis / n if n > 1e-9 else np.array([1.0, 0.0, 0.0], dtype=float)
+    mag = float(np.clip(float(gain) * ang, -float(mag_lim), float(mag_lim)))
+    return axis * mag, ang
+
+
+def flange_tilt_rad(R) -> float:
+    """法兰 +Z 相对世界 −Z 的倾角 [rad]。只读角度，不带死区。"""
+    z_now = np.asarray(R, dtype=float).reshape(3, 3)[:, 2]
+    z_des = np.array([0.0, 0.0, -1.0], dtype=float)
+    c = float(np.clip(np.dot(z_now, z_des), -1.0, 1.0))
+    return float(math.acos(c))
+
+
+def trap_acc_for_hop(dist: float, v: float) -> float:
+    """二阶段 ``_plan_and_send`` 同一条刹车：路程越长刹车越远，不许另写一套。"""
+    v_cmd = max(min(float(v), 0.08), 1e-3)
+    d_brake = min(0.030, max(0.006, 0.32 * max(float(dist), 1e-6)))
+    return max(v_cmd * v_cmd / (2.0 * d_brake), 0.030)
+
+
+def trap_params(dist: float, v: float, a: float) -> Tuple[float, float, float, float, float]:
+    """Rest-to-rest 梯形：返回 (T, v_peak, t_acc, t_cruise, a)。巡航速度保持 v。"""
+    dist = max(float(dist), 1e-6)
+    v = max(float(v), 1e-4)
+    a = max(float(a), 1e-3)
+    d_acc = v * v / (2.0 * a)
+    if 2.0 * d_acc >= dist:
+        v_peak = math.sqrt(max(a * dist, 1e-12))
+        t_acc = v_peak / a
+        return 2.0 * t_acc, v_peak, t_acc, 0.0, a
+    t_acc = v / a
+    t_cruise = (dist - 2.0 * d_acc) / v
+    return 2.0 * t_acc + t_cruise, v, t_acc, t_cruise, a
+
+
+def trap_sv(
+    t: float, dist: float, v_peak: float, t_acc: float, t_cruise: float, a: float,
+) -> Tuple[float, float]:
+    """时刻 t 的路径弧长 s 与切向速度。"""
+    t = max(0.0, float(t))
+    t_dec0 = t_acc + t_cruise
+    t_end = t_dec0 + t_acc
+    if t >= t_end:
+        return dist, 0.0
+    if t < t_acc:
+        return 0.5 * a * t * t, a * t
+    if t < t_dec0:
+        return 0.5 * a * t_acc * t_acc + v_peak * (t - t_acc), v_peak
+    td = t - t_dec0
+    s0 = 0.5 * a * t_acc * t_acc + v_peak * t_cruise
+    return s0 + v_peak * td - 0.5 * a * td * td, max(0.0, v_peak - a * td)
+
+
+def smooth_path_params(
+    dist: float,
+    v: float,
+    *,
+    d_acc: float = D435_FULL_ACC_M,
+    d_dec: float = D435_FULL_DEC_M,
+) -> Tuple[float, float, float, float, float, float]:
+    """整段一次走完的余弦加减速：(T, v_peak, t_acc, t_cruise, d_acc, d_dec)。
+
+    加速/减速段速度按半个余弦走，两端加速度为 0，没有梯形那种加速度台阶。
+    路程不够巡航时整段一条余弦，峰值不超过 ``v``。
+    """
+    dist = max(float(dist), 1e-6)
+    v = max(float(v), 1e-4)
+    acc = min(max(float(d_acc), 1e-4), 0.45 * dist)
+    dec = min(max(float(d_dec), 1e-4), 0.45 * dist)
+    if acc + dec >= dist - 1e-6:
+        # 整段一条正弦速度：v = v_peak sin(π t / T)，s(T)=2 v_peak T / π。
+        T = math.pi * dist / (2.0 * v)
+        return T, v, 0.0, 0.0, 0.0, dist
+    t_acc = 2.0 * acc / v
+    t_dec = 2.0 * dec / v
+    t_cruise = (dist - acc - dec) / v
+    T = t_acc + t_cruise + t_dec
+    return T, v, t_acc, t_cruise, acc, dec
+
+
+def cosine_peak_acc(v: float, d_acc: float) -> float:
+    """余弦加速段的峰值切向加速度 [m/s²]。``d_acc`` 越长，起步越不弹。"""
+    d = max(float(d_acc), 1e-6)
+    return math.pi * float(v) * float(v) / (4.0 * d)
+
+
+def smooth_path_sv(
+    t: float,
+    dist: float,
+    v_peak: float,
+    t_acc: float,
+    t_cruise: float,
+    d_acc: float,
+    d_dec: float,
+) -> Tuple[float, float]:
+    """时刻 t 的弧长与切向速度。头尾速度为 0。"""
+    t = max(0.0, float(t))
+    dist = max(float(dist), 1e-6)
+    v_peak = max(float(v_peak), 1e-4)
+    if t_acc <= 1e-9 and t_cruise <= 1e-9:
+        T = math.pi * dist / (2.0 * v_peak)
+        if t >= T:
+            return dist, 0.0
+        u = math.pi * t / T
+        s = (v_peak * T / math.pi) * (1.0 - math.cos(u))
+        vs = v_peak * math.sin(u)
+        return min(s, dist), max(0.0, vs)
+    t_dec = 2.0 * float(d_dec) / v_peak if v_peak > 1e-9 else 0.0
+    t_end = float(t_acc) + float(t_cruise) + t_dec
+    if t >= t_end:
+        return dist, 0.0
+    if t < t_acc:
+        u = math.pi * t / t_acc
+        s = 0.5 * v_peak * (t - t_acc / math.pi * math.sin(u))
+        vs = 0.5 * v_peak * (1.0 - math.cos(u))
+        return s, vs
+    if t < t_acc + t_cruise:
+        return float(d_acc) + v_peak * (t - t_acc), v_peak
+    td = t - t_acc - t_cruise
+    u = math.pi * td / t_dec if t_dec > 1e-9 else math.pi
+    s0 = float(d_acc) + v_peak * float(t_cruise)
+    s = s0 + 0.5 * v_peak * (td + t_dec / math.pi * math.sin(u))
+    vs = 0.5 * v_peak * (1.0 + math.cos(u))
+    return min(s, dist), max(0.0, vs)
+
+
+def _circle_3(
+    a: np.ndarray, b: np.ndarray, c: np.ndarray
+) -> Optional[Tuple[np.ndarray, float]]:
+    ax, ay = float(a[0]), float(a[1])
+    bx, by = float(b[0]), float(b[1])
+    cx, cy = float(c[0]), float(c[1])
+    d = 2.0 * (ax * (by - cy) + bx * (cy - ay) + cx * (ay - by))
+    if abs(d) < 1e-9:
+        return None
+    a2 = ax * ax + ay * ay
+    b2 = bx * bx + by * by
+    c2 = cx * cx + cy * cy
+    ux = (a2 * (by - cy) + b2 * (cy - ay) + c2 * (ay - by)) / d
+    uy = (a2 * (cx - bx) + b2 * (ax - cx) + c2 * (bx - ax)) / d
+    center = np.array([ux, uy], dtype=float)
+    r = float(np.hypot(ax - ux, ay - uy))
+    if r < 1e-4:
+        return None
+    return center, r
+
+
+def _ccw(a0: float, a1: float) -> float:
+    return (a1 - a0) % (2.0 * math.pi)
+
+
+def _xy_in_box(xy: np.ndarray, lo: np.ndarray, hi: np.ndarray) -> bool:
+    return bool(
+        float(lo[0]) <= float(xy[0]) <= float(hi[0])
+        and float(lo[1]) <= float(xy[1]) <= float(hi[1])
+    )
+
+
+@dataclass
+class ArcXyMove:
+    """从 p0 走到 p1 的 XY 圆弧（或退化成直线），弧长上跑梯形速度。"""
+
+    p0: np.ndarray
+    p1: np.ndarray
+    length: float
+    v_cruise: float
+    T: float
+    v_peak: float
+    t_acc: float
+    t_cruise: float
+    a: float
+    t0: float
+    linear: bool
+    center: Optional[np.ndarray] = None
+    radius: float = 0.0
+    ang0: float = 0.0
+    sweep: float = 0.0
+    s_last: float = 0.0
+    _dir: np.ndarray = field(default_factory=lambda: np.array([1.0, 0.0]))
+
+    def sample(self, now: float) -> Tuple[np.ndarray, np.ndarray, bool]:
+        """返回 (计划 XY, 切向速度, 是否走完)。"""
+        t = max(0.0, float(now) - float(self.t0))
+        s, sdot = trap_sv(
+            t, self.length, self.v_peak, self.t_acc, self.t_cruise, self.a
+        )
+        s = min(float(s), float(self.length))
+        self.s_last = s
+        done = t >= float(self.T) - 1e-4 or s >= float(self.length) - 1e-5
+        if self.linear or self.radius <= 1e-6 or self.center is None:
+            L = max(float(self.length), 1e-6)
+            u = s / L
+            xy = (1.0 - u) * self.p0 + u * self.p1
+            vxy = self._dir * float(sdot)
+            return xy, vxy, done
+        frac = 0.0 if abs(self.sweep) < 1e-9 else s / (self.radius * abs(self.sweep))
+        frac = min(1.0, max(0.0, frac))
+        ang = self.ang0 + self.sweep * frac
+        xy = self.center + self.radius * np.array(
+            [math.cos(ang), math.sin(ang)], dtype=float
+        )
+        tang = np.array([-math.sin(ang), math.cos(ang)], dtype=float)
+        if self.sweep < 0.0:
+            tang = -tang
+        return xy, tang * float(sdot), done
+
+
+def build_arc_xy_move(
+    p0,
+    p1,
+    *,
+    v: float,
+    a: float,
+    now: float,
+    ws_min=None,
+    ws_max=None,
+    sag_frac: float = D435_ARC_SAGITTA_FRAC,
+    sag_max: float = D435_ARC_SAGITTA_MAX_M,
+) -> ArcXyMove:
+    """p0→p1 的圆弧。工作盒装不下就换侧，再不行走直线。速度整段同一档。"""
+    p0 = np.asarray(p0, dtype=float).reshape(2)
+    p1 = np.asarray(p1, dtype=float).reshape(2)
+    chord = p1 - p0
+    L = float(np.hypot(chord[0], chord[1]))
+    direction = (
+        chord / L if L > 1e-6 else np.array([1.0, 0.0], dtype=float)
+    )
+    cruise = max(float(v), 1e-4)
+    acc = max(float(a), 1e-3)
+
+    def _linear() -> ArcXyMove:
+        T, v_peak, t_acc, t_cruise, a_use = trap_params(max(L, 1e-6), cruise, acc)
+        return ArcXyMove(
+            p0=p0.copy(),
+            p1=p1.copy(),
+            length=max(L, 1e-6),
+            v_cruise=cruise,
+            T=T,
+            v_peak=v_peak,
+            t_acc=t_acc,
+            t_cruise=t_cruise,
+            a=a_use,
+            t0=float(now),
+            linear=True,
+            _dir=direction,
+        )
+
+    if L < 0.003:
+        return _linear()
+
+    n = np.array([-direction[1], direction[0]], dtype=float)
+    h = min(float(sag_max), float(sag_frac) * L)
+    mid = 0.5 * (p0 + p1)
+    lo = None if ws_min is None else np.asarray(ws_min, dtype=float).reshape(-1)[:2]
+    hi = None if ws_max is None else np.asarray(ws_max, dtype=float).reshape(-1)[:2]
+
+    def _try(sign: float) -> Optional[ArcXyMove]:
+        apex = mid + n * (h * sign)
+        cir = _circle_3(p0, apex, p1)
+        if cir is None:
+            return None
+        center, r = cir
+        a0 = math.atan2(p0[1] - center[1], p0[0] - center[0])
+        a1 = math.atan2(p1[1] - center[1], p1[0] - center[0])
+        a_via = math.atan2(apex[1] - center[1], apex[0] - center[0])
+        ccw_end = _ccw(a0, a1)
+        ccw_via = _ccw(a0, a_via)
+        if 1e-6 < ccw_via < ccw_end - 1e-6:
+            sweep = ccw_end
+        else:
+            sweep = ccw_end - 2.0 * math.pi
+        if abs(sweep) < 1e-6:
+            return None
+        length = r * abs(sweep)
+        if lo is not None and hi is not None:
+            for k in range(9):
+                frac = k / 8.0
+                ang = a0 + sweep * frac
+                xy = center + r * np.array([math.cos(ang), math.sin(ang)])
+                if not _xy_in_box(xy, lo, hi):
+                    return None
+        T, v_peak, t_acc, t_cruise, a_use = trap_params(length, cruise, acc)
+        return ArcXyMove(
+            p0=p0.copy(),
+            p1=p1.copy(),
+            length=length,
+            v_cruise=cruise,
+            T=T,
+            v_peak=v_peak,
+            t_acc=t_acc,
+            t_cruise=t_cruise,
+            a=a_use,
+            t0=float(now),
+            linear=False,
+            center=center,
+            radius=r,
+            ang0=a0,
+            sweep=sweep,
+        )
+
+    for sign in (1.0, -1.0):
+        got = _try(sign)
+        if got is not None:
+            return got
+    return _linear()
+
+
+def path_interp(waypoints, u: float):
+    """沿关节路点折线插值。u∈[0,1] → (q, dq/du)。"""
+    nseg = len(waypoints) - 1
+    if nseg <= 0:
+        z = np.zeros_like(waypoints[0])
+        return waypoints[0].copy(), z
+    u = float(min(1.0, max(0.0, u)))
+    x = u * nseg
+    i = min(nseg - 1, int(math.floor(x)))
+    a = x - i
+    q = (1.0 - a) * waypoints[i] + a * waypoints[i + 1]
+    dq_du = (waypoints[i + 1] - waypoints[i]) * nseg
+    return q, dq_du
+
+
+def translate_hold_ori_waypoints(
+    q0,
+    J,
+    dp,
+    *,
+    damping: float = 0.08,
+    max_dq: float = 0.06,
+    R=None,
+    verticalize: bool = False,
+):
+    """冻结当前雅可比，把 ``dp`` 拆成关节路点。
+
+    和二阶段 ``_plan_and_send`` 同一份逆解。``verticalize`` 时顺带收法兰朝下，
+    不许另抄一份 ``_w_down``。返回 ``(路点, 还剩多少米)``。
+    """
+    from arm_application.geom_ik import dls_translate_hold_ori, dls_twist
+
+    q = np.asarray(q0, dtype=float).copy()
+    remain = np.asarray(dp, dtype=float).reshape(3).copy()
+    dist = float(np.linalg.norm(remain))
+    wps = [q.copy()]
+    inner_tol = min(0.00012, max(0.00005, 0.05 * max(dist, 1e-6)))
+    n_max = int(math.ceil(dist / 0.008)) + 16
+    Jj = np.asarray(J, dtype=float)
+    w_step = np.zeros(3, dtype=float)
+    ang = 0.0
+    if verticalize:
+        if R is None:
+            raise ValueError("verticalize 必须传入当前法兰 R，不许假装已经朝下")
+        w_step, ang = flange_down_w(R)
+    for _ in range(n_max):
+        if float(np.linalg.norm(remain)) < inner_tol and (
+            (not verticalize) or ang < 0.006
+        ):
+            break
+        nseg = max(1, int(math.ceil(float(np.linalg.norm(remain)) / 0.012)))
+        step = remain / nseg
+        if verticalize and ang > 0.008:
+            qdot = dls_twist(Jj, step / 0.12, w_base=w_step, damping=damping, max_qdot=0.45)
+            dq = np.clip(qdot * 0.12, -float(max_dq), float(max_dq))
+        else:
+            dq = dls_translate_hold_ori(Jj, step, damping=damping, max_dq=max_dq)
+        if float(np.max(np.abs(dq))) < 1e-5:
+            break
+        q = q + dq
+        wps.append(q.copy())
+        remain = remain - (Jj[:3, :] @ dq)
+        ang = max(0.0, ang - float(np.linalg.norm(w_step)) * 0.12)
+    return wps, float(np.linalg.norm(remain))
+
+
+def trap_joint_schedule(
+    waypoints,
+    dist: float,
+    v: float,
+    a: float,
+    *,
+    t0: float = 0.06,
+    dt: float = 0.04,
+    t_hold: float = 0.25,
+):
+    """路点 + 梯形速度 → ``[(q, qdot, t), ...]``。头尾速度为 0，时间严格递增。
+
+    一次下发整段。中途再发下一条会把这条掐掉，臂就重新加速 —— 那就是抖。
+    """
+    dist = max(float(dist), 1e-6)
+    T_move, v_peak, t_acc, t_cruise, a_use = trap_params(dist, v, a)
+    n_samp = max(8, int(math.ceil(T_move / max(float(dt), 1e-3))))
+    zeros = np.zeros_like(waypoints[0])
+    out = []
+    last_t = -1.0
+    for k in range(n_samp + 1):
+        t_local = T_move * k / n_samp
+        s, vs = trap_sv(t_local, dist, v_peak, t_acc, t_cruise, a_use)
+        u = s / dist
+        qi, dq_du = path_interp(waypoints, u)
+        qdot = dq_du * (vs / dist) if k < n_samp else zeros
+        if k >= n_samp:
+            qi = waypoints[-1]
+            qdot = zeros
+        t = float(t0) + t_local
+        if t <= last_t:
+            t = last_t + 0.02
+        out.append((np.asarray(qi, dtype=float).copy(), np.asarray(qdot, dtype=float).copy(), t))
+        last_t = t
+    hold = max(0.12, float(t_hold))
+    for frac in (0.40, 0.70, 1.00):
+        t = float(t0) + T_move + hold * frac
+        if t < last_t + 0.05:
+            t = last_t + 0.05
+        out.append((np.asarray(waypoints[-1], dtype=float).copy(), zeros.copy(), t))
+        last_t = t
+    return out, T_move, v_peak
+
+
+def smooth_joint_schedule(
+    waypoints,
+    dist: float,
+    v: float,
+    *,
+    t0: float = 0.06,
+    dt: float = 0.04,
+    t_hold: float = 0.25,
+    d_acc: float = D435_FULL_ACC_M,
+    d_dec: float = D435_FULL_DEC_M,
+):
+    """路点 + 余弦加减速 → ``[(q, qdot, t), ...]``。头尾速度为 0。
+
+    D435 整段一次下发用这一份。二阶段短跳仍走 ``trap_joint_schedule``。
+    """
+    dist = max(float(dist), 1e-6)
+    T_move, v_peak, t_acc, t_cruise, acc_m, dec_m = smooth_path_params(
+        dist, v, d_acc=d_acc, d_dec=d_dec
+    )
+    n_samp = max(12, int(math.ceil(T_move / max(float(dt), 1e-3))))
+    zeros = np.zeros_like(waypoints[0])
+    out = []
+    last_t = -1.0
+    for k in range(n_samp + 1):
+        t_local = T_move * k / n_samp
+        s, vs = smooth_path_sv(
+            t_local, dist, v_peak, t_acc, t_cruise, acc_m, dec_m
+        )
+        u = min(1.0, s / dist)
+        qi, dq_du = path_interp(waypoints, u)
+        qdot = dq_du * (vs / dist) if k < n_samp else zeros
+        if k >= n_samp:
+            qi = waypoints[-1]
+            qdot = zeros
+        t = float(t0) + t_local
+        if t <= last_t:
+            t = last_t + 0.02
+        out.append(
+            (
+                np.asarray(qi, dtype=float).copy(),
+                np.asarray(qdot, dtype=float).copy(),
+                t,
+            )
+        )
+        last_t = t
+    hold = max(0.12, float(t_hold))
+    for frac in (0.40, 0.70, 1.00):
+        t = float(t0) + T_move + hold * frac
+        if t < last_t + 0.05:
+            t = last_t + 0.05
+        out.append((np.asarray(waypoints[-1], dtype=float).copy(), zeros.copy(), t))
+        last_t = t
+    return out, T_move, v_peak

src/arm_application/arm_application/perception_node.py

@@ -31,7 +31,7 @@ from rclpy.qos import (
     qos_profile_sensor_data,
 )
 from sensor_msgs.msg import CameraInfo, Image
-from std_msgs.msg import Float32, Float32MultiArray
+from std_msgs.msg import Bool, Float32, Float32MultiArray
 from tf2_ros import TransformBroadcaster
 
 from arm_application.box_tracker import BoxTracker
@@ -46,11 +46,16 @@ from arm_application.handeye_realsense import (
     resolve_realsense_topics,
 )
 from arm_application.overhead_oneshot import (
+    D405_MISS_BEFORE_D435_S,
+    ONESHOT_ACCEPT_TIMEOUT_S,
     best_det_conf,
     depth_raw_to_m,
     oneshot_k0_base,
+    oneshot_shot_timed_out,
+    oneshot_should_submit,
     select_oneshot_det,
     wrist_needs_handoff,
+    z_locked_qos,
 )
 from arm_application.kalman_filter import KalmanFilter3D
 from arm_application.plane_feature import (
@@ -161,6 +166,7 @@ class PerceptionNode(TolerantNode):
         # ---- D405 主跟踪 + D435 一次性（键必须从 site 接入，缺了启动即红）----
         self.declare_parameter("wrist_handoff_conf", -1.0)
         self.declare_parameter("overhead_oneshot_enable", False)
+        self.declare_parameter("d435_move_only", False)
         self.declare_parameter("overhead_min_depth_m", -1.0)
         self.declare_parameter("overhead_max_depth_m", -1.0)
         self.declare_parameter("overhead_oneshot_conf", -1.0)
@@ -210,9 +216,10 @@ class PerceptionNode(TolerantNode):
         # 跟踪点三道守卫（单一实现见 feature_guard.py）
         self.keypoint_border_px = float(self.get_parameter("keypoint_border_px").value)
         self.track_max_step_m = float(self.get_parameter("track_max_step_m").value)
+        self.reacquire_far_px = float(self.get_parameter("reacquire_far_px").value)
         self._slew = TrackSlewGuard(self.track_max_step_m)
         self._far = FarReacquireConfirm(
-            float(self.get_parameter("reacquire_far_px").value),
+            self.reacquire_far_px,
             int(self.get_parameter("reacquire_confirm_frames").value),
         )
         self.plane_inlier_m = float(self.get_parameter("plane_inlier_m").value)
@@ -220,6 +227,7 @@ class PerceptionNode(TolerantNode):
         self.overhead_oneshot_enable = bool(
             self.get_parameter("overhead_oneshot_enable").value
         )
+        self.d435_move_only = bool(self.get_parameter("d435_move_only").value)
         self._T_oh: Optional[np.ndarray] = None
         self._oh_image: Optional[Image] = None
         self._oh_depth: Optional[Image] = None
@@ -230,6 +238,11 @@ class PerceptionNode(TolerantNode):
         self._oneshot_t = -1.0e9
         self._need_oneshot = False
         self._oneshot_inflight = False
+        self._oneshot_shot = False
+        self._oneshot_shot_t = -1.0e9
+        self._low_since: Optional[float] = None
+        self._d435_commanding = False
+        self._z_locked = False
         self._oneshot_img: Optional[Image] = None
         self._oneshot_depth: Optional[Image] = None
         if self.overhead_oneshot_enable:
@@ -441,10 +454,28 @@ class PerceptionNode(TolerantNode):
                     CameraInfo, oh_info, self._on_oh_info, image_qos,
                     callback_group=self._io_cbg,
                 )
+            self.create_subscription(
+                Bool, "visual_servo/z_locked", self._on_z_locked, z_locked_qos(),
+                callback_group=self._io_cbg,
+            )
+            self.create_subscription(
+                Bool,
+                "visual_servo/d435_commanding",
+                self._on_d435_commanding,
+                z_locked_qos(),
+                callback_group=self._io_cbg,
+            )
+            extra = (
+                "；d435_move_only：不等 D405 丢目标，0.5s 后必拍一张"
+                if self.d435_move_only
+                else ""
+            )
             self.get_logger().info(
-                f"D435 一次性 image={oh_img} depth={oh_dep} "
+                f"D435 待命 image={oh_img} depth={oh_dep} "
                 f"handoff_conf={self.wrist_handoff_conf:.2f} "
+                f"看不到满 {D405_MISS_BEFORE_D435_S:.1f}s 才拍一张 "
                 f"T={self.get_parameter('overhead_handeye_file').value}"
+                f"{extra}"
             )
 
         point_qos = QoSProfile(
@@ -865,9 +896,52 @@ class PerceptionNode(TolerantNode):
         self._publish_wrist_conf(best_conf)
         handoff = bool(
             self.overhead_oneshot_enable
-            and wrist_needs_handoff(best_conf, self.wrist_handoff_conf)
+            and (
+                self.d435_move_only
+                or wrist_needs_handoff(best_conf, self.wrist_handoff_conf)
+            )
+        )
+        now_m = time.monotonic()
+        if self._d435_commanding:
+            # 臂还在赶 D435 那个点。漏检计时清掉，走到并停稳之后再重新计。
+            self._low_since = None
+            self._need_oneshot = False
+        elif not handoff:
+            self._low_since = None
+            self._oneshot_shot = False
+            self._need_oneshot = False
+        else:
+            if oneshot_shot_timed_out(
+                shot=self._oneshot_shot,
+                commanding=self._d435_commanding,
+                age_s=now_m - float(self._oneshot_shot_t),
+                timeout_s=ONESHOT_ACCEPT_TIMEOUT_S,
+            ):
+                self.get_logger().warn(
+                    "D435 拍了但伺服没接住（工作盒拒收等），允许再拍"
+                )
+                self._oneshot_shot = False
+                self._low_since = None
+            if self._low_since is None:
+                self._low_since = now_m
+            low_for = now_m - self._low_since
+            self._need_oneshot = oneshot_should_submit(
+                handoff=True,
+                low_for_s=low_for,
+                already_shot=self._oneshot_shot,
+                wait_s=D405_MISS_BEFORE_D435_S,
+            )
+            if self._oneshot_shot:
+                self.get_logger().info(
+                    "D435 这一轮已经拍过一张，等这一段走完，不再连拍",
+                    throttle_duration_sec=2.0,
+                )
+            elif not self._need_oneshot:
+                self.get_logger().info(
+                    f"D405 已连续 {low_for:.2f}s 没有高置信度目标，"
+                    f"未满 {D405_MISS_BEFORE_D435_S:.1f}s，D435 继续待命",
+                    throttle_duration_sec=0.5,
                 )
-        self._need_oneshot = handoff
         now = time.monotonic()
         pred_at_img = self._box_trk.predict(t_img, self._img_wh())
         with self._lock:
@@ -1326,6 +1400,21 @@ class PerceptionNode(TolerantNode):
         msg.data = float("nan") if best_conf is None else float(best_conf)
         self._wrist_conf_pub.publish(msg)
 
+    def _on_z_locked(self, msg: Bool) -> None:
+        """锁高状态只记着。拍不拍 D435 不看它，看 D405 连续漏检。"""
+        self._z_locked = bool(msg.data)
+
+    def _on_d435_commanding(self, msg: Bool) -> None:
+        """伺服还在执行 D435 那一个点时，不再提交下一张。
+
+        这一段结束（到位并停稳）后清掉「已拍过」，漏检重新计 0.5 s。
+        """
+        commanding = bool(msg.data)
+        if self._d435_commanding and not commanding:
+            self._oneshot_shot = False
+            self._low_since = None
+        self._d435_commanding = commanding
+
     def _publish_overhead_k0(self, p_base: np.ndarray, is_k0: bool) -> None:
         pt = PointStamped()
         pt.header.stamp = self.get_clock().now().to_msg()
@@ -1447,6 +1536,9 @@ class PerceptionNode(TolerantNode):
             )
             return
         p_base, is_k0 = got
+        self._oneshot_shot = True
+        self._oneshot_shot_t = now
+        self._need_oneshot = False
         self._publish_overhead_k0(p_base, is_k0)
         self.get_logger().info(
             f"D435 一次性定位 {'k0' if is_k0 else '框心（顶视看不见 k0）'} "

src/arm_application/arm_application/pipeline_node.py

@@ -26,16 +26,33 @@ from std_msgs.msg import Bool, String
 from arm_application.stage3_search_node import Stage3Search
 
 
-#: 一阶段跟踪高相对二阶段悬停高的**抬升量** [m]。
+#: 悬停时规尖离面不够，跟踪高再抬的量 [m]。
 #:
-#: 为什么要抬：跟踪时通止规已经夹在爪上，规尖比相机更靠下；悬停高是"相机拍清
+#: 只在「悬停高上规尖离工件面 < :data:`TRACK_LOCK_GAP_M`」时加上。仿真悬停按
-#: 孔"的高度，直接在那儿跟踪会让规尖刮到工件。抬 5cm 让它离开工件，二阶段再
+#: 工具几何只留约 2mm，不抬就会在跟踪时刮到工件。真机 89 悬停 0.320 时规尖
-#: 小幅下降（只需 5cm 以内的行程）。
+#: 已经在面上方约 75mm，再抬 50mm 会让对孔前先改一次高度（0.37→0.32）。
 #:
 #: ⚠️ 抽成常量是因为它不只被流程用：``scripts/check_sim_scene.py`` 要用它核对
 #: "跟踪高有没有超出工作盒上界"（仿真里悬停 0.632 + 0.05 = 0.682 > 真机那套
 #: 0.65 上界，就是被这条查出来的）。两处各写一个字面量迟早漂移。
 TRACK_CLEARANCE_M = 0.05
+#: 悬停时规尖离面已经不少于这个值 [m]，一阶段就锁在悬停高，对孔完成后再下降。
+TRACK_LOCK_GAP_M = 0.040
+
+
+def track_lock_z(hover_z: float, tool_len_m: float, surface_z_m: float) -> float:
+    """一阶段锁高。
+
+    悬停时规尖离面已经 ≥ ``TRACK_LOCK_GAP_M``：锁在悬停高，跟踪和对孔都不改 Z，
+    对孔完成后再下降。离面不够（仿真悬停只留 2mm）：再抬 ``TRACK_CLEARANCE_M``，
+    否则跟踪会刮到工件。工具长或工件面缺失时抬升 —— 缺数时锁在悬停高等于没检查。
+    """
+    tool = float(tool_len_m)
+    surf = float(surface_z_m)
+    hover = float(hover_z)
+    if tool > 0.0 and surf > 0.0 and (hover - tool - surf) >= TRACK_LOCK_GAP_M:
+        return hover
+    return hover + TRACK_CLEARANCE_M
 
 
 def stage1_timeout_reason(points_seen: int, timeout_s: float, log_path: str) -> str:
@@ -79,6 +96,7 @@ class AssemblyPipeline(Stage3Search):
         self.declare_parameter("skip_stage1", False)
         self.declare_parameter("skip_stage2", False)
         self.declare_parameter("skip_stage3", False)  # 默认搜孔+轻压；拧入靠 --depth
+        self.declare_parameter("d435_move_only", False)
         self.declare_parameter("stage1_timeout_sec", 150.0)
         self.linger3 = 1.2  # 串联结束短保持；单独跑 stage3 仍用 site linger_sec
 
@@ -113,9 +131,11 @@ class AssemblyPipeline(Stage3Search):
         skip1 = bool(self.get_parameter("skip_stage1").value)
         skip2 = bool(self.get_parameter("skip_stage2").value)
         skip3 = bool(self.get_parameter("skip_stage3").value)
+        self.d435_move_only = bool(self.get_parameter("d435_move_only").value)
         self.get_logger().info(
             f"pipeline arm_ee={self.ee_frame} hole={self.target_hole} "
-            f"skip1={skip1} skip2={skip2} skip3={skip3} insert={self.insert_depth_mm:.1f}mm"
+            f"skip1={skip1} skip2={skip2} skip3={skip3} "
+            f"d435_only={self.d435_move_only} insert={self.insert_depth_mm:.1f}mm"
         )
 
     def _state(self, s: str) -> None:
@@ -163,8 +183,12 @@ class AssemblyPipeline(Stage3Search):
         self._kill_old_stage1()
 
     def _track_z(self) -> float:
-        """跟踪高度 = 孔悬停高 + :data:`TRACK_CLEARANCE_M`，通止规离开工件。"""
+        """一阶段锁高：见 :func:`track_lock_z`。对孔完成前不再改这个高度。"""
-        return float(self.hover_z) + TRACK_CLEARANCE_M
+        return track_lock_z(
+            float(self.hover_z),
+            float(getattr(self, "tool_tip_below_flange_m", 0.0) or 0.0),
+            float(getattr(self, "surface_z_m", 0.0) or 0.0),
+        )
 
     def _ensure_clearance(self) -> None:
         """上一轮轻压可能还在孔里：先纯 Z 抬到跟踪高，禁止 0.23↔0.35 来回。"""
@@ -177,7 +201,7 @@ class AssemblyPipeline(Stage3Search):
             return
         self._state("pre_lift")
         self.get_logger().info(
-            f"先离开工件/孔口：Z {z:.3f} → {tz:.3f}m（跟踪高=hover+50mm）"
+            f"先到锁高：Z {z:.3f} → {tz:.3f}m（跟踪与对孔都停在这个高度，对完孔再下降）"
         )
         _, q, Tee = self._goto(
             q, Tee, np.array([float(Tee[0, 3]), float(Tee[1, 3]), tz]),
@@ -206,7 +230,13 @@ class AssemblyPipeline(Stage3Search):
             "publish_handeye_tf:=false",
             f"ee_z_m:={lock_z:.3f}",
             f"use_sim_time:={str(use_sim).lower()}",
+            f"d435_move_only:={str(self.d435_move_only).lower()}",
         ]
+        if self.d435_move_only:
+            self.get_logger().info(
+                f"启动 D435 位移测试（锁高 {lock_z:.3f}m，到位后不交 D405、不进二阶段）"
+            )
+        else:
             self.get_logger().info(
                 f"启动一阶段跟踪（锁高 {lock_z:.3f}m，锁定后自动交二阶段）"
             )
@@ -229,6 +259,11 @@ class AssemblyPipeline(Stage3Search):
                 stage1_timeout_reason(self._point_msgs, timeout, log_path)
             )
             return 1
+        if self.d435_move_only:
+            self.get_logger().info(
+                "D435 位移测试结束 — 停跟踪、钉住当前关节，跳过二阶段"
+            )
+        else:
             self.get_logger().info("一阶段已锁定 — 停跟踪、钉住当前关节，直接进二阶段")
         self._stop_stage1()
         time.sleep(0.35)
@@ -241,6 +276,9 @@ class AssemblyPipeline(Stage3Search):
         skip1 = bool(self.get_parameter("skip_stage1").value)
         skip2 = bool(self.get_parameter("skip_stage2").value)
         skip3 = bool(self.get_parameter("skip_stage3").value)
+        if self.d435_move_only:
+            skip2 = True
+            skip3 = True
         if not self._cli.wait_for_server(timeout_sec=5.0):
             self.get_logger().error("JTC action 不在 —— 先开 T1: run_real.sh --arm 89")
             return 2
@@ -268,17 +306,30 @@ class AssemblyPipeline(Stage3Search):
                     return code
                 self.get_logger().info(
                     f"二阶段 OK 残差 {final.get('residual_mm', -1)}mm"
-                    + (" — 跳过搜孔，保持悬停" if skip3 else " — 立刻下压搜孔并轻压（不拧入）")
+                    + (
+                        " — 跳过搜孔，停在观测高度（看规尖是否对中；进程退出后臂保持）"
+                        if skip3
+                        else " — 立刻下压搜孔并轻压（不拧入）"
+                    )
                 )
             else:
                 self._state("skip_stage2")
 
             if skip3:
                 self._state("skip_stage3")
-                self.get_logger().info("跳过搜孔 —— 臂保持在孔上方")
+                if self.d435_move_only:
+                    self.get_logger().info(
+                        "D435 位移测试结束 —— 臂停在当前高度，不交 D405、不对孔；"
+                        "15s 后本进程退出，臂保持不动"
+                    )
+                else:
+                    self.get_logger().info(
+                        "跳过搜孔 —— 臂停在当前高度。请看规尖与孔是否对中；"
+                        "15s 后本进程退出，臂保持不动"
+                    )
                 if final:
                     self._publish_result(final)
-                self._linger_hold(final if final else {"ok": True}, seconds=0.8)
+                self._linger_hold(final if final else {"ok": True}, seconds=15.0)
                 payload = {"ok": True, "stage": "pipeline", "exit": 0, "skipped": "stage3"}
                 try:
                     payload["stage2"] = json.loads(

src/arm_application/arm_application/servo_node.py

@@ -24,9 +24,17 @@ import rclpy
 from geometry_msgs.msg import PointStamped, Twist
 from std_msgs.msg import Bool, Float32
 from rclpy.callback_groups import MutuallyExclusiveCallbackGroup, ReentrantCallbackGroup
+from rclpy.action import ActionClient
 from rclpy.executors import MultiThreadedExecutor
 from rclpy.node import Node
-from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy, qos_profile_system_default
+from control_msgs.action import FollowJointTrajectory
+from rclpy.qos import (
+    DurabilityPolicy,
+    HistoryPolicy,
+    QoSProfile,
+    ReliabilityPolicy,
+    qos_profile_system_default,
+)
 from sensor_msgs.msg import JointState
 from tf2_ros import Buffer, TransformListener
 from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
@@ -39,7 +47,26 @@ from arm_application.geom_ik import (
     quat_xyzw_to_R,
 )
 from arm_application.handeye_lib import cam_point_to_base, cam_to_base_T
-from arm_application.overhead_oneshot import StillLock, wrist_needs_handoff
+from arm_application.overhead_oneshot import (
+    D435_NEAR_CRUISE_MPS,
+    D435_SETTLE_S,
+    StillLock,
+    at_lock_height,
+    d405_commands_arm,
+    flange_tilt_rad,
+    hold_translation_vz,
+    hybrid_cruise_vxy,
+    lock_z_velocity,
+    oneshot_concat_hold_ori,
+    oneshot_full_cruise_mps,
+    oneshot_full_segments,
+    oneshot_hop_limits,
+    oneshot_may_send_another,
+    oneshot_needs_verticalize,
+    wrist_needs_handoff,
+    z_locked_qos,
+)
+from arm_application.path_profile import smooth_joint_schedule, translate_hold_ori_waypoints
 from arm_application.pid_controller import PidController
 from arm_application.kalman_filter import KalmanFilter3D
 from arm_application.feature_jump_guard import (
@@ -83,6 +110,9 @@ class ServoNode(TolerantNode):
         self.declare_parameter("ee_frame", ee_frame_default())
         self.declare_parameter("camera_frame", "d405_color_optical_frame")
         self.declare_parameter("traj_topic", "rizon_arm_controller/joint_trajectory")
+        self.declare_parameter(
+            "action_name", "rizon_arm_controller/follow_joint_trajectory"
+        )
         self.declare_parameter("point_topic", "visual_servo/filtered_point")
         # 默认值只作兜底：真机运行一定由 launch 传入 site.yaml 的 paths.handeye_file。
         # 指向 /tmp 是历史遗留（标定临时产物），会让人误以为"标定结果在那里"。
@@ -187,6 +217,8 @@ class ServoNode(TolerantNode):
         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)
+        # 只测 D435 位移：到位后不交 D405、不靠 D405 静止去锁二阶段。
+        self.declare_parameter("d435_move_only", False)
         self.declare_parameter("overhead_k0_topic", "visual_servo/overhead_k0")
         self.declare_parameter("wrist_conf_topic", "visual_servo/wrist_conf")
 
@@ -271,6 +303,7 @@ class ServoNode(TolerantNode):
         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.lock_still_move_m >= 0.0
@@ -281,6 +314,26 @@ class ServoNode(TolerantNode):
             still_sec=max(0.0, self.lock_still_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_settled = False
+        self._d435_exec_until = 0.0
+        self._d435_pinned = False
+        self._d435_sent = False
+        self._d435_last_dist = None
+        self._d435_give_up = False
+        self._d435_plan_lock = threading.Lock()
+        self._d435_inflight = False
+        self._d435_settle_until = 0.0
+        self._d435_last_z_err = None
+        self._d435_z_stall = 0
+        self._d435_remeasure_log = False
+        self._d435_vert_done = False
+        self._d435_full_done = False
+        self._d435_fix_done = False
         self._track_source = "wrist"  # wrist | overhead
         self._wrist_conf: Optional[float] = None
         self._wrist_conf_t = -1.0
@@ -337,6 +390,11 @@ class ServoNode(TolerantNode):
                 f"optical_vs_flangeZ={self._cam_tilt_deg:.1f}deg "
                 f"— 悬停使用这个 T，而不是用 TF 查询"
             )
+        if self.d435_move_only:
+            self.get_logger().warn(
+                "d435_move_only：只跑 D435 单次位移。到位停稳后钉住，"
+                "不交 D405；locked 只用来结束一阶段，不进二阶段"
+            )
 
         self.pid_x = PidController(
             float(self.get_parameter("kp_xy").value),
@@ -391,6 +449,7 @@ class ServoNode(TolerantNode):
         self._jump_gate = FeatureJumpGate(self.predict_jump_m, self.jump_confirm_frames)
         # 只有当法兰 Z 已经落到锁定平面上（深度新鲜）才开启 XY 悬停。
         self._xy_armed = bool(self.ee_z_m <= 0.0)
+        self._z_hold: Optional[float] = None
         # Seek（搜索）状态：在锁定高度且没有目标时扫描工件。
         self._seek_t0 = 0.0
         self._seek_theta = 0.0
@@ -447,11 +506,30 @@ class ServoNode(TolerantNode):
         )
         # 一阶段锁定信号（串联编排器用它判断可以进入二阶段）
         self._lock_pub = self.create_publisher(Bool, "visual_servo/locked", 1)
+        # TRANSIENT_LOCAL：感知节点比伺服晚起来，默认 QoS 会丢掉启动时那一拍
+        # True，感知就永远停在「还没到锁高」、D435 不拍。两边必须同一份 QoS。
+        self._z_lock_pub = self.create_publisher(
+            Bool, "visual_servo/z_locked", z_locked_qos()
+        )
+        # 感知用它挡住「平移半路再拍一张」。晚启动也能拿到当前值。
+        self._d435_cmd_pub = self.create_publisher(
+            Bool, "visual_servo/d435_commanding", z_locked_qos()
+        )
+        self._d435_cmd_sent: Optional[bool] = None
+        self._z_locked_sent: Optional[bool] = None
+        self._z_lock_pub_mono = 0.0
         self._lock_since: Optional[float] = None
         self._locked = False
         self._lock_pub_t = 0.0
         traj_topic = str(self.get_parameter("traj_topic").value)
         self._traj_pub = self.create_publisher(JointTrajectory, traj_topic, cmd_qos)
+        self._jtc_cbg = ReentrantCallbackGroup()
+        self._jtc = ActionClient(
+            self,
+            FollowJointTrajectory,
+            str(self.get_parameter("action_name").value),
+            callback_group=self._jtc_cbg,
+        )
 
         self._tf = Buffer()
         TransformListener(self._tf, self, spin_thread=True)
@@ -477,6 +555,36 @@ class ServoNode(TolerantNode):
             f"hold_xy={int(self.ee_hold_xy)} down={int(self.ee_vertical_down)} "
             f"XY_after_lock_z±{self.approach_xy_gate_m*1000:.0f}mm"
         )
+        self._publish_z_locked(self._xy_armed)
+        self._publish_d435_commanding(False)
+
+    def _publish_z_locked(self, locked: bool) -> None:
+        """法兰是否在锁高带里。只记日志，不拦 D435。
+
+        状态没变也每 0.5 s 再发一次：只发一次的话，晚订阅的感知收不到。
+        """
+        locked = bool(locked)
+        now = time.monotonic()
+        changed = self._z_locked_sent is not locked
+        if (not changed) and (now - self._z_lock_pub_mono) < 0.5:
+            return
+        self._z_locked_sent = locked
+        self._z_lock_pub_mono = now
+        self._z_lock_pub.publish(Bool(data=locked))
+        if not changed:
+            return
+        if locked:
+            self.get_logger().info("锁高已到位")
+        else:
+            self.get_logger().info("离开锁高带 —— 平移仍保持当前高度，不改去追锁高")
+
+    def _publish_d435_commanding(self, commanding: bool) -> None:
+        """正在用 D435 那一个点赶路。感知看到 True 就不再拍第二张。"""
+        commanding = bool(commanding)
+        if self._d435_cmd_sent is commanding:
+            return
+        self._d435_cmd_sent = commanding
+        self._d435_cmd_pub.publish(Bool(data=commanding))
 
     def _on_js(self, msg: JointState) -> None:
         self._last_js = msg
@@ -504,8 +612,19 @@ class ServoNode(TolerantNode):
         if not np.all(np.isfinite(p_base)):
             return
         if not self._feat_workspace_ok(p_base):
+            m = float(self.feat_ws_margin_m)
             self.get_logger().warn(
-                f"D435 一次性点 ({p_base[0]:.3f},{p_base[1]:.3f}) 在工作盒外 —— 丢弃",
+                f"D435 一次性点 ({p_base[0]:.3f},{p_base[1]:.3f}) 在工作盒外 "
+                f"（x[{self.ws_min[0]:.2f},{self.ws_max[0]:.2f}] "
+                f"y[{self.ws_min[1]:.2f},{self.ws_max[1]:.2f}] ±{m:.2f}）"
+                "—— 丢弃，不发轨迹。这不是起步加速度。"
+            )
+            return
+        with self._lock:
+            busy = self._track_source == "overhead" and not self._d435_settled
+        if busy:
+            self.get_logger().info(
+                "D435 这一段还没走完，不再换目标",
                 throttle_duration_sec=1.0,
             )
             return
@@ -520,15 +639,31 @@ class ServoNode(TolerantNode):
             self._have_target = True
             self._target_lost_flag = False
             self._track_source = "overhead"
+            self._d435_settled = False
             self._overhead_t = now_m
             self._last_seen_time = now_m
             self._stamp_sec = now_sec
             self._seek_t0 = 0.0
             self._seek_warned = False
         self._still.reset()
+        self._arrive.reset()
+        self._d435_exec_until = 0.0
+        self._d435_pinned = False
+        self._d435_sent = False
+        self._d435_last_dist = None
+        self._d435_give_up = False
+        self._d435_inflight = False
+        self._d435_settle_until = 0.0
+        self._d435_last_z_err = None
+        self._d435_z_stall = 0
+        self._d435_remeasure_log = False
+        self._d435_vert_done = False
+        self._d435_full_done = False
+        self._d435_fix_done = 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}) "
-            f"—— 迁过去等 D405 重新看见"
+            f"—— 走到这个点并停稳之前，不下发 D405 的点"
         )
 
     def _on_point(self, msg: PointStamped) -> None:
@@ -541,16 +676,26 @@ class ServoNode(TolerantNode):
             # 已经收到腕部 conf 且低于门槛：这条 D405 点不能当跟踪源。
             # conf 还是 None = 感知还没报到，不要把开场几帧也丢掉。
             return
+        with self._lock:
+            using_d435 = self._track_source == "overhead"
+            settled = self._d435_settled
+        if self._hybrid and not d405_commands_arm(
+            using_d435=using_d435,
+            arrived=settled,
+            settled=settled,
+            move_only=self.d435_move_only,
+        ):
+            # 推理服务不关。这一帧的点不下发给臂，目标仍是 D435 那一个点。
+            self.get_logger().info(
+                "D435 位置还没到并且停稳，D405 这帧不下发",
+                throttle_duration_sec=1.0,
+            )
+            return
         p_cam = np.array(
             [float(msg.point.x), float(msg.point.y), float(msg.point.z)], dtype=float
         )
         T_ee = self._lookup_T(self.base_frame, self.ee_frame)
         p_base = self._feature_in_base(T_ee, p_cam) if T_ee is not None else None
-        at_lock = True
-        if self.ee_z_m > 0.0 and T_ee is not None:
-            at_lock = abs(float(T_ee[2, 3]) - self.ee_z_m) <= max(
-                0.02, float(self.approach_xy_gate_m)
-            )
         if not self._he_cmp_done:
             self._he_cmp_done = True
             self._log_handeye_tf_vs_yaml()
@@ -558,12 +703,9 @@ class ServoNode(TolerantNode):
         rejected = False
         with self._lock:
             was_lost = self._target_lost_flag
-            if self._track_source == "overhead":
-                # 从顶视切回腕部：允许一次重定位，跳变门不拿顶视点比。
-                was_lost = True
             self._track_source = "wrist"
             self._p_meas[:] = p_cam
-            if p_base is not None and at_lock and self._xy_armed:
+            if p_base is not None:
                 verdict = self._update_feat_track(p_base, was_lost=was_lost, now_m=now_m)
                 if verdict == ACCEPT:
                     self._p_feat_base = np.asarray(p_base, dtype=float).copy()
@@ -747,28 +889,25 @@ class ServoNode(TolerantNode):
             lost = self._target_lost_flag
             source = self._track_source
         if self._hybrid:
+            self._tick_d435_arrive()
             self._tick_still_lock()
+        if self.enable_control and self._d435_oneshot_holds_the_arm():
+            return
 
-        # ----- 启动阶段：**先**下降到锁定高度，甚至在任何检测到来之前。
+        # 还没有目标、人又停在锁高下面：只抬一次 Z。已经有 D435 点或腕部点之后
-        # 在第一个 PointStamped 到达之前 have 一直是 False，所以这是一个纯启动
+        # 不再插这一步 —— 平移途中改高度会抖，也会把正在走的 XY 掐掉。
-        # 阶段：只动 Z 下降，XY 锁死，姿态朝下。
+        if self.enable_control and self.ee_z_m > 0.0 and not have:
-        if (
-            self.enable_control
-            and self.ee_z_m > 0.0
-            and not have
-            and not self._xy_armed
-        ):
             T_ee0 = self._lookup_T(self.base_frame, self.ee_frame)
             if T_ee0 is None:
-                return  # TF 还没就绪 —— 等待，不要触发保持急停
+                return
-            self._update_xy_arm(float(T_ee0[2, 3]))
+            z_now = float(T_ee0[2, 3])
+            self._update_xy_arm(z_now)
             if not self._xy_armed:
                 self.get_logger().info(
-                    f"启动下降至 lock_z {self.ee_z_m:.3f}m "
+                    f"先回锁高 {self.ee_z_m:.3f}m 当前={z_now:.3f}m（还没有目标，只动 Z）",
-                    f"当前={T_ee0[2, 3]:.3f}m（还没有目标，XY 锁死）",
                     throttle_duration_sec=1.0,
                 )
-                self._target_lost_flag = False  # 盲降期间允许执行
+                self._target_lost_flag = False
                 self._execute_twist(np.zeros(3, dtype=float), p_cam=None, hold_xy=True)
                 self._lock_feedback(False)
                 return
@@ -821,13 +960,10 @@ class ServoNode(TolerantNode):
         seen_age = now_mono - last_seen
         stamp_age = now_sec - stamp if stamp >= 0.0 else 1e9
         if self._hybrid and source == "overhead":
-            if seen_age > self.overhead_hold_s:
+            # 一次性设定一直用到 D405 接上。按秒数冻住的话，臂还在平移就被看门狗
-                self._lock_feedback(False)
+            # 掐掉，D435 又不再拍，停在半路。
-                self._watchdog_trip(
+            stamp_age = 0.0
-                    f"D435 一次性设定已过期 {seen_age:.1f}s > {self.overhead_hold_s:.1f}s"
+            seen_age = 0.0
-                )
-                return
-            stamp_age = 0.0  # 一次性不是流：别按图像戳判过期
         elif seen_age > self.watchdog_timeout or stamp_age > self.lost_timeout:
             self._watchdog_trip(
                 f"目标丢失 seen_age={seen_age:.3f}s stamp_age={stamp_age:.3f}s"
@@ -1176,12 +1312,343 @@ class ServoNode(TolerantNode):
             self._xy_in_dead = dead
         return dead
 
+    def _jacobian_at(self, p_ee: np.ndarray):
+        link_R: List[np.ndarray] = []
+        link_p: List[np.ndarray] = []
+        for fr in self.link_frames:
+            Tl = self._lookup_T(self.base_frame, fr)
+            if Tl is None:
+                return None
+            link_R.append(Tl[:3, :3])
+            link_p.append(Tl[:3, 3])
+        return geometric_jacobian(link_R, link_p, p_ee, RIZON_LINK_AXES)
+
+    def _d435_oneshot_holds_the_arm(self) -> bool:
+        """D435 到点平移：action 短跳，等结果再重测。D405 主跟踪仍走原来的伺服。"""
+        if self.d435_move_only and self._hybrid and self._track_source == "overhead" and self._d435_settled:
+            # 位移测完了：守着，不许掉进 D405 的 _execute_twist。hold 只在到位那一拍发一次。
+            return True
+        if not (self._hybrid and self._track_source == "overhead" and not self._d435_settled):
+            return False
+        if not self._d435_plan_lock.acquire(blocking=False):
+            return True
+        try:
+            return self._d435_oneshot_holds_locked()
+        finally:
+            self._d435_plan_lock.release()
+
+    def _d435_pin_and_stop_hops(self, why: str) -> None:
+        if not self._d435_pinned:
+            self._send_hold()
+            self._d435_pinned = True
+        self._d435_give_up = True
+        self.get_logger().error(why)
+
+    def _sanitize_d435_traj(self, traj: JointTrajectory, min_dt: float = 0.02) -> JointTrajectory:
+        """与二阶段 ``_sanitize_traj`` 同一条：time_from_start 必须严格递增。"""
+        last = -min_dt
+        for p in traj.points:
+            t = float(p.time_from_start.sec) + float(p.time_from_start.nanosec) * 1e-9
+            if t < last + min_dt:
+                t = last + min_dt
+            ns = int(round(t * 1e9))
+            p.time_from_start.sec = ns // 1_000_000_000
+            p.time_from_start.nanosec = ns % 1_000_000_000
+            last = t
+        return traj
+
+    def _d435_send_action(
+        self,
+        sched,
+        tag: str,
+        hop_m: float,
+        leftover_m: float,
+        v_peak: float,
+        T_move: float,
+        *,
+        ee_z: float,
+        tilt_deg: float,
+        max_dq: float,
+    ) -> None:
+        """先占住 in-flight 再 ``send_goal_async``。话题插点会把这条掐掉。"""
+        traj = JointTrajectory()
+        traj.header.stamp.sec = 0
+        traj.header.stamp.nanosec = 0
+        traj.joint_names = list(self.joint_names)
+        for qi, qdot, t_sec in sched:
+            pt = JointTrajectoryPoint()
+            pt.positions = [float(v) for v in np.asarray(qi).tolist()]
+            pt.velocities = [float(v) for v in np.asarray(qdot).tolist()]
+            ns = int(round(max(0.0, float(t_sec)) * 1e9))
+            pt.time_from_start.sec = ns // 1_000_000_000
+            pt.time_from_start.nanosec = ns % 1_000_000_000
+            traj.points.append(pt)
+        traj = self._sanitize_d435_traj(traj)
+        goal = FollowJointTrajectory.Goal()
+        goal.trajectory = traj
+        self._d435_inflight = True
+        self._d435_sent = True
+        self._d435_pinned = False
+        fut = self._jtc.send_goal_async(goal)
+        fut.add_done_callback(self._d435_on_goal)
+        self.get_logger().info(
+            f"D435 action 下发 {len(sched)} 点 tag={tag} 本跳={hop_m*1000:.0f}mm "
+            f"剩={leftover_m*1000:.0f}mm v={v_peak*1000:.0f}mm/s T={T_move:.1f}s "
+            f"ee_z={ee_z:.3f} tilt={tilt_deg:.2f}deg max|Δq|={max_dq:.4f}rad "
+            f"—— 结果回来之前不再发下一条"
+        )
+
+    def _d435_on_goal(self, fut) -> None:
+        try:
+            handle = fut.result()
+        except Exception as exc:
+            self.get_logger().error(f"D435 JTC 目标回调失败：{exc}")
+            self._d435_inflight = False
+            self._d435_give_up = True
+            return
+        if handle is None or not bool(getattr(handle, "accepted", False)):
+            self.get_logger().error("JTC 拒绝了 D435 轨迹")
+            self._d435_inflight = False
+            self._d435_give_up = True
+            return
+        handle.get_result_async().add_done_callback(self._d435_on_result)
+
+    def _d435_on_result(self, fut) -> None:
+        ok = True
+        try:
+            r = fut.result()
+            inner = getattr(r, "result", None)
+            code = getattr(inner, "error_code", 0) if inner is not None else 0
+            if code not in (0, None):
+                ok = False
+                msg = getattr(inner, "error_string", "")
+                self.get_logger().error(f"D435 JTC 执行失败 error_code={code} {msg}")
+        except Exception as exc:
+            ok = False
+            self.get_logger().error(f"D435 JTC 结果失败：{exc}")
+        self._d435_inflight = False
+        self._d435_settle_until = time.monotonic() + float(D435_SETTLE_S)
+        self._d435_remeasure_log = True
+        if not ok:
+            self._d435_give_up = True
+
+    def _d435_oneshot_holds_locked(self) -> bool:
+        now = time.monotonic()
+        if self._d435_inflight:
+            return True
+        if now < self._d435_settle_until:
+            return True
+        T_ee = self._lookup_T(self.base_frame, self.ee_frame)
+        q = self._current_joints()
+        if T_ee is None or q is None:
+            return True
+        # 只更新锁高标志。这一段不许把控制权交给回锁高流式。
+        self._update_xy_arm(float(T_ee[2, 3]))
+        with self._lock:
+            goal = None if self._p_feat_base is None else self._p_feat_base[:2].copy()
+        if goal is None:
+            return True
+        p = np.asarray(T_ee[:3, 3], dtype=float)
+        dist = float(np.hypot(float(goal[0]) - p[0], float(goal[1]) - p[1]))
+        arrive = float(self.lock_still_move_m)
+        z_lock = float(self.ee_z_m)
+        tilt = flange_tilt_rad(T_ee[:3, :3])
+        at = at_lock_height(float(p[2]), z_lock, self.approach_xy_gate_m)
+        if self._d435_remeasure_log:
+            self._d435_remeasure_log = False
+            self.get_logger().info(
+                f"D435 重测 leftover={dist*1000:.0f}mm "
+                f"dxy=({(p[0]-goal[0])*1000:+.0f},{(p[1]-goal[1])*1000:+.0f})mm "
+                f"ee_xy=({p[0]:.3f},{p[1]:.3f}) goal=({goal[0]:.3f},{goal[1]:.3f}) "
+                f"ee_z={p[2]:.3f} tilt={math.degrees(tilt):.2f}deg"
+            )
+        if self._d435_give_up:
+            self._d435_pinned = True
+            return True
+        hop_max, _cruise_unused = oneshot_hop_limits(
+            dist, hybrid_cruise_vxy(self.v_slow_xy)
+        )
+        if at and dist <= arrive:
+            if oneshot_needs_verticalize(
+                tilt, dist, arrive, already_done=self._d435_vert_done
+            ):
+                return self._d435_send_verticalize(q, T_ee, leftover=dist, tilt=tilt)
+            # action 已经 rest-to-rest。到位成功不许再往话题插 hold，那会再抖一下。
+            self._d435_pinned = True
+            return True
+        if self._d435_full_done and self._d435_fix_done:
+            self._d435_pin_and_stop_hops(
+                f"D435 整段+补刀后还差 {dist*1000:.0f}mm —— 钉住不再补刀"
+            )
+            return True
+        if at and not oneshot_may_send_another(
+            dist, self._d435_last_dist, arrive_m=arrive, hop_max_m=hop_max
+        ):
+            self._d435_pin_and_stop_hops(
+                f"D435 上一跳几乎没走近，剩差还是 {dist*1000:.0f}mm "
+                f"—— 钉住不再补刀，等停稳后交回 D405"
+            )
+            return True
+        if not at:
+            z_err = abs(float(p[2]) - z_lock)
+            if self._d435_last_z_err is not None and z_err > self._d435_last_z_err * 0.8:
+                self._d435_z_stall += 1
+            else:
+                self._d435_z_stall = 0
+            self._d435_last_z_err = z_err
+            if self._d435_z_stall >= 2:
+                self._d435_pin_and_stop_hops(
+                    f"D435 收锁高连续无进展 当前={p[2]:.3f}m 期望={z_lock:.3f}m"
+                )
+                return True
+        if not self._jtc.server_is_ready():
+            self.get_logger().warn("D435 等 JTC action 就绪", throttle_duration_sec=1.0)
+            return True
+        J = self._jacobian_at(p)
+        if J is None:
+            self.get_logger().warn("D435 平移缺 link TF，等下一拍", throttle_duration_sec=1.0)
+            return True
+        segs = oneshot_full_segments(
+            p, goal, z_lock, gate_m=self.approach_xy_gate_m
+        )
+        if not segs:
+            return True
+        # XY / 收 Z 都不许夹收垂直：冻结 J 会把腕部转动漏成 Y 过冲。
+        wps, planned, asked, tag = oneshot_concat_hold_ori(
+            q, J, segs, R=T_ee[:3, :3]
+        )
+        if self._d435_full_done:
+            tag = "fix"
+        if len(wps) < 2 or asked < 1e-5 or planned < 0.75 * asked:
+            self._d435_pin_and_stop_hops(
+                f"D435 平移规划停滞 {planned*1000:.0f}/{asked*1000:.0f}mm —— 钉住不瞎发"
+            )
+            return True
+        max_dq = float(np.max(np.abs(wps[-1] - q)))
+        if max_dq > 1.0:
+            self._d435_pin_and_stop_hops("D435 平移关节行程 >1 rad，拒绝执行")
+            return True
+        cruise = oneshot_full_cruise_mps(dist, hybrid_cruise_vxy(self.v_slow_xy))
+        sched, T_move, v_peak = smooth_joint_schedule(wps, asked, cruise)
+        if tag in ("full", "fix", "xy"):
+            self._d435_last_dist = dist
+        if not self._d435_full_done:
+            self._d435_full_done = True
+        else:
+            self._d435_fix_done = True
+        self._d435_send_action(
+            sched,
+            tag,
+            asked,
+            dist,
+            v_peak,
+            T_move,
+            ee_z=float(p[2]),
+            tilt_deg=float(np.degrees(tilt)),
+            max_dq=max_dq,
+        )
+        return True
+
+    def _d435_send_verticalize(self, q, T_ee, *, leftover: float, tilt: float) -> bool:
+        """XY 已到位后单独收法兰朝下。和平移拆开，避免 Y 过冲。"""
+        if not self._jtc.server_is_ready():
+            self.get_logger().warn("D435 收垂直等 JTC action 就绪", throttle_duration_sec=1.0)
+            return True
+        p = np.asarray(T_ee[:3, 3], dtype=float)
+        J = self._jacobian_at(p)
+        if J is None:
+            self.get_logger().warn("D435 收垂直缺 link TF，等下一拍", throttle_duration_sec=1.0)
+            return True
+        wps, _remain = translate_hold_ori_waypoints(
+            q, J, np.zeros(3, dtype=float), R=T_ee[:3, :3], verticalize=True
+        )
+        if len(wps) < 2:
+            self._d435_vert_done = True
+            self.get_logger().info(
+                f"D435 收垂直无需再动 tilt={math.degrees(tilt):.2f}deg"
+            )
+            return True
+        max_dq = float(np.max(np.abs(wps[-1] - q)))
+        if max_dq < 1e-4:
+            self._d435_vert_done = True
+            return True
+        if max_dq > 1.0:
+            self._d435_pin_and_stop_hops("D435 收垂直关节行程 >1 rad，拒绝执行")
+            return True
+        hop = max(0.008, float(tilt) * 0.05)
+        cruise = D435_NEAR_CRUISE_MPS
+        sched, T_move, v_peak = smooth_joint_schedule(wps, hop, cruise)
+        self._d435_vert_done = True
+        self._d435_send_action(
+            sched,
+            "vert",
+            hop,
+            leftover,
+            v_peak,
+            T_move,
+            ee_z=float(p[2]),
+            tilt_deg=float(np.degrees(tilt)),
+            max_dq=max_dq,
+        )
+        return True
+
+    def _tick_d435_arrive(self) -> None:
+        """走到 D435 那个点，并且法兰按一阶段的静止判据停稳，才允许下发 D405。"""
+        if not self._hybrid:
+            return
+        if self._d435_inflight or time.monotonic() < self._d435_settle_until:
+            self._arrive.reset()
+            return
+        with self._lock:
+            if self._track_source != "overhead" or self._d435_settled:
+                return
+            goal = None if self._p_feat_base is None else self._p_feat_base[:2].copy()
+        if goal is None:
+            return
+        T = self._lookup_T(self.base_frame, self.ee_frame)
+        if T is None:
+            return
+        xy = np.asarray(T[:2, 3], dtype=float)
+        dist = float(np.hypot(float(xy[0]) - goal[0], float(xy[1]) - goal[1]))
+        arrived = dist <= float(self.lock_still_move_m) or self._d435_give_up
+        settled = self._arrive.update(
+            xy, time.monotonic(), tracking_ok=arrived
+        )
+        if not settled:
+            return
+        with self._lock:
+            self._d435_settled = True
+        self._d435_exec_until = 0.0
+        if self.d435_move_only:
+            # 保持 d435_commanding=True，感知不会重拍；D405 被 move_only 挡住。
+            # locked 只让 pipeline 结束一阶段，后面会 skip_stage2。
+            # action 已 rest-to-rest，到位不再往话题插 hold。
+            now_m = time.monotonic()
+            if not self._locked:
+                self._locked = True
+                self.get_logger().info(
+                    f"已到 D435 位置（还差 {dist*1000:.0f}mm）并停稳 "
+                    f"{self.lock_still_sec:.0f}s —— D435 位移测试结束，"
+                    "不交 D405、不进二阶段"
+                )
+                self._lock_pub.publish(Bool(data=True))
+                self._lock_pub_t = now_m
+            elif now_m - self._lock_pub_t > 1.0:
+                self._lock_pub_t = now_m
+                self._lock_pub.publish(Bool(data=True))
+            return
+        self._publish_d435_commanding(False)
+        self.get_logger().info(
+            f"已到 D435 位置（还差 {dist*1000:.0f}mm）并停稳 "
+            f"{self.lock_still_sec:.0f}s —— 改回下发 D405"
+        )
+
     def _tick_still_lock(self) -> None:
         """D405 跟住且法兰 XY 连续 still_sec 秒没大幅动 → 锁定进二阶段。
 
         顶视一次性迁臂不算。门槛与位移上限只从 site 来，这里不再写死 3 / 15mm。
         """
-        if not self._hybrid:
+        if not self._hybrid or self.d435_move_only:
             return
         T = self._lookup_T(self.base_frame, self.ee_frame)
         xy = None if T is None else np.asarray(T[:2, 3], dtype=float)
@@ -1284,48 +1751,31 @@ class ServoNode(TolerantNode):
         return out
 
     def _update_xy_arm(self, ee_z: float) -> bool:
-        """只有当法兰到达 lock_z（真实深度、自适应悬停）后才做 XY 悬停。"""
+        """只有法兰落在锁高带内才做 XY。低于锁高要先抬回去，不算到位。"""
         if self.ee_z_m <= 0.0:
             self._xy_armed = True
+            self._publish_z_locked(True)
             return True
-        gate = max(0.02, float(self.approach_xy_gate_m))
         z = float(ee_z)
-        # 已经在锁高或更低：立刻开 XY，禁止再抬回 0.35（否则二阶段又要降下来）。
+        at = at_lock_height(z, self.ee_z_m, self.approach_xy_gate_m)
-        if z <= float(self.ee_z_m) + gate:
+        if at and not self._xy_armed:
-            if not self._xy_armed:
             self._xy_armed = True
-                self._feat_kf.initialized = False
-                self._p_feat_base = None
             self._v_xy_cmd[:] = 0.0
             self.pid_x.reset()
             self.pid_y.reset()
-                self.get_logger().warn(
+            self.get_logger().info(
-                    f"lock-z 就绪 ee_z={z:.3f}m（期望 {self.ee_z_m:.3f}，不再回抬） "
+                f"锁高到位 ee_z={z:.3f}m（期望 {self.ee_z_m:.3f}）"
-                    "—— 在实时 handeye 上开始 XY 悬停"
+                " —— 开始 XY；腕部无目标时 D435 可以拍一张"
             )
-            return True
+        elif (not at) and self._xy_armed:
-        err = abs(z - float(self.ee_z_m))
-        if not self._xy_armed:
-            if err <= gate:
-                self._xy_armed = True
-                self._feat_kf.initialized = False
-                self._p_feat_base = None
-                self._v_xy_cmd[:] = 0.0
-                self.pid_x.reset()
-                self.pid_y.reset()
-                self.get_logger().warn(
-                    f"已到达 lock-z ee_z={z:.3f}m（期望 {self.ee_z_m:.3f}）"
-                    " —— 在实时 handeye 上开始 XY 悬停"
-                )
-        elif err > gate + 0.05:
             self._xy_armed = False
-            self._p_feat_base = None
-            self._feat_kf.initialized = False
             self._v_xy_cmd[:] = 0.0
             self.get_logger().info(
-                f"已离开 lock-z 区间 ee_z={z:.3f}m —— XY 保持，重新下降",
+                f"离开锁高 ee_z={z:.3f}m（期望 {self.ee_z_m:.3f}）"
+                " —— 先回高度，XY 暂停",
                 throttle_duration_sec=1.0,
             )
+        self._publish_z_locked(at)
         return self._xy_armed
 
     def _seek_velocity(self, p_ee: np.ndarray) -> Optional[np.ndarray]:
@@ -1392,7 +1842,8 @@ class ServoNode(TolerantNode):
 
         now_m = time.monotonic()
         dt_cmd = now_m - self._last_traj_mono
-        if self._last_traj_mono > 0.0 and dt_cmd < (1.0 / self.traj_stream_hz):
+        min_dt = 1.0 / self.traj_stream_hz
+        if self._last_traj_mono > 0.0 and dt_cmd < min_dt:
             return
         dt_use = float(np.clip(dt_cmd if self._last_traj_mono > 0.0 else 0.04, 0.015, 0.08))
 
@@ -1422,6 +1873,7 @@ class ServoNode(TolerantNode):
         # hover：在任意锁定高度上把法兰 XY 停在三维特征点上。
         # lookat：把光轴命中点（图像中心）驱到特征点上。
         xy_ok = self._update_xy_arm(float(p_ee[2]))
+        cruise = hybrid_cruise_vxy(self.v_slow_xy)
         if (
             xy_ok
             and (not hold_xy)
@@ -1437,19 +1889,10 @@ class ServoNode(TolerantNode):
                 if nerr > clip_xy:
                     err = err * (clip_xy / nerr)
                     nerr = clip_xy
-                # ---- 远处限速：抓错目标时只走一小段 ----
+                # 锁定前整段同一巡航速度。不再用 50mm 远近两档：
-                # 2026-10-08 真机：一阶段被一串高置信度假 k0 顶掉，伺服 0.7s 内以
+                # 远处本来就慢，近处一放开就按 kp×误差冲，越近越抖。
-                # v_max_xy(0.45m/s) 把臂从 home (0.686,-0.113) 带到工作盒角落
+                if self._hybrid and not self._locked:
-                # (0.083,-0.515)，之后 148s 全是"检测漏检"。留一道**与"目标真不真"
+                    v_xy = min(float(v_xy), cruise)
-                # 无关**的防线：目标离得远就慢走 —— 真目标（真 k0 离 145mm）只是晚
-                # ~1.8s 到位，假目标则最多把臂带走几厘米。
-                v_xy, why_slow = self._slow_xy_limit(nerr, v_xy)
-                if why_slow:
-                    self.get_logger().info(
-                        f"{why_slow} —— 限速 {self.v_slow_xy*1000:.0f}mm/s 逼近"
-                        "（防追错目标大范围移动）",
-                        throttle_duration_sec=1.0,
-                    )
                 if self._xy_deadzone(nerr, update=False):
                     v_base[0] = 0.0
                     v_base[1] = 0.0
@@ -1480,9 +1923,10 @@ class ServoNode(TolerantNode):
                 )
 
         nxy = float(np.hypot(v_base[0], v_base[1]))
-        if nxy > v_xy > 0.0:
+        cap_xy = cruise if (self._hybrid and not self._locked) else v_xy
-            v_base[0] *= v_xy / nxy
+        if nxy > cap_xy > 0.0:
-            v_base[1] *= v_xy / nxy
+            v_base[0] *= cap_xy / nxy
+            v_base[1] *= cap_xy / nxy
 
         # 在 base 系里保持法兰 XYZ（可配置，默认约 250 mm）。当 ee_hold_xy
         # 为真时，视觉算出的 XY 会被覆盖。
@@ -1493,28 +1937,33 @@ class ServoNode(TolerantNode):
                 v_base[0] = 0.0
             if abs(self.ee_y_m - p_ee[1]) < 0.004:
                 v_base[1] = 0.0
-        if (not xy_ok) or hold_xy:
+        # 有目标：XY 平移，Z 钉在这一段开始的高度。没目标才回锁高。
+        translating = (not hold_xy) and self._have_target
+        if (not translating) and ((not xy_ok) or hold_xy):
             v_base[0] = 0.0
             v_base[1] = 0.0
             self._v_xy_cmd[:] = 0.0
             if self.ee_z_m > 0.0:
                 self.get_logger().info(
-                    f"下降至 lock_z {self.ee_z_m:.3f}m  当前={p_ee[2]:.3f}m  XY 保持",
+                    f"回锁高 {self.ee_z_m:.3f}m  当前={p_ee[2]:.3f}m  XY 保持",
                     throttle_duration_sec=1.0,
                 )
         shaped = self._shape_xy_vel(v_base[:2], dt_use)
         v_base[0] = float(shaped[0])
         v_base[1] = float(shaped[1])
-        if self.ee_z_m > 0.0:
+        if translating:
-            ez_b = float(self.ee_z_m - p_ee[2])
+            if self._z_hold is None:
-            # 已在锁高以下：不再抬臂。二阶段会降到 hole_hover_z。
+                self._z_hold = float(p_ee[2])
-            if ez_b > 0.0:
+            v_base[2] = hold_translation_vz(
-                v_base[2] = 0.0
+                float(p_ee[2]), self._z_hold, kp=kp, v_lim=float(self.v_max_z)
+            )
         else:
-                vz_lim = float(self.v_max_z) if xy_ok else max(float(self.v_max_z), 0.08)
+            self._z_hold = None
-                v_base[2] = float(np.clip(kp * ez_b, -vz_lim, vz_lim))
+            if self.ee_z_m > 0.0:
-                if abs(ez_b) < 0.004:
+                vz_lim = max(float(self.v_max_z), 0.08)
-                    v_base[2] = 0.0
+                v_base[2] = lock_z_velocity(
+                    float(p_ee[2]), self.ee_z_m, kp=kp, v_lim=vz_lim
+                )
 
         w_base = (
             self._w_align_flange_down(R_ee)
@@ -1523,9 +1972,9 @@ class ServoNode(TolerantNode):
         )
 
         p_next = p_ee + v_base * T
-        if self.ee_z_m > 0.0:
+        if self.ee_z_m > 0.0 and not translating:
-            # 绝不指令到配置平面以下
+            # 向下只许到锁高死区内。已经低于锁高时 v_z>0，这条不挡上抬。
-            floor = max(self.min_ee_z, self.ee_z_m - 0.02)
+            floor = float(self.ee_z_m) - 0.004
             if p_next[2] < floor and v_base[2] < 0.0:
                 v_base[2] = 0.0
                 p_next = p_ee + v_base * T

src/arm_application/arm_application/stage2_locate_node.py

@@ -45,6 +45,12 @@ from arm_application.geom_ik import (
     geometric_jacobian,
     quat_xyzw_to_R,
 )
+from arm_application.path_profile import (
+    FROZEN_J_HOP_MAX_M,
+    path_interp as _path_interp,
+    trap_params as _trap_params,
+    trap_sv as _trap_sv,
+)
 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
@@ -82,54 +88,6 @@ def _pt(q: np.ndarray, qdot: np.ndarray, t_sec: float) -> JointTrajectoryPoint:
     return pt
 
 
-def _trap_params(dist: float, v: float, a: float) -> Tuple[float, float, float, float, float]:
-    """Rest-to-rest 梯形：返回 (T, v_peak, t_acc, t_cruise, a)。巡航速度保持 v。"""
-    dist = max(float(dist), 1e-6)
-    v = max(float(v), 1e-4)
-    a = max(float(a), 1e-3)
-    d_acc = v * v / (2.0 * a)
-    if 2.0 * d_acc >= dist:
-        v_peak = math.sqrt(max(a * dist, 1e-12))
-        t_acc = v_peak / a
-        return 2.0 * t_acc, v_peak, t_acc, 0.0, a
-    t_acc = v / a
-    t_cruise = (dist - 2.0 * d_acc) / v
-    return 2.0 * t_acc + t_cruise, v, t_acc, t_cruise, a
-
-
-def _trap_sv(
-    t: float, dist: float, v_peak: float, t_acc: float, t_cruise: float, a: float,
-) -> Tuple[float, float]:
-    """时刻 t 的路径弧长 s 与切向速度。"""
-    t = max(0.0, float(t))
-    t_dec0 = t_acc + t_cruise
-    t_end = t_dec0 + t_acc
-    if t >= t_end:
-        return dist, 0.0
-    if t < t_acc:
-        return 0.5 * a * t * t, a * t
-    if t < t_dec0:
-        return 0.5 * a * t_acc * t_acc + v_peak * (t - t_acc), v_peak
-    td = t - t_dec0
-    s0 = 0.5 * a * t_acc * t_acc + v_peak * t_cruise
-    return s0 + v_peak * td - 0.5 * a * td * td, max(0.0, v_peak - a * td)
-
-
-def _path_interp(waypoints: List[np.ndarray], u: float) -> Tuple[np.ndarray, np.ndarray]:
-    """沿关节路点折线插值。u∈[0,1] → (q, dq/du)。"""
-    nseg = len(waypoints) - 1
-    if nseg <= 0:
-        z = np.zeros_like(waypoints[0])
-        return waypoints[0].copy(), z
-    u = float(min(1.0, max(0.0, u)))
-    x = u * nseg
-    i = min(nseg - 1, int(math.floor(x)))
-    a = x - i
-    q = (1.0 - a) * waypoints[i] + a * waypoints[i + 1]
-    dq_du = (waypoints[i + 1] - waypoints[i]) * nseg
-    return q, dq_du
-
-
 def _sanitize_traj(traj: JointTrajectory, min_dt: float = 0.02) -> JointTrajectory:
     """JTC 要求 time_from_start 严格递增；短 hop 保持点曾经倒退导致拒轨。"""
     last = -min_dt
@@ -246,6 +204,15 @@ class Stage2Locate(TolerantNode):
         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（贴面 2mm）高一截，对偏了也不会把规压上孔沿。
+        self.observe_gap_m = float(stage2.get("observe_gap_mm") or 15.0) / 1000.0
+        if self.observe_gap_m < self.final_gap_m:
+            self.get_logger().warn(
+                f"observe_gap_mm={self.observe_gap_m*1000:.1f} 比贴面目标 "
+                f"{self.final_gap_m*1000:.1f}mm 还近 —— 收到贴面高度，避免观测下降钻进孔"
+            )
+            self.observe_gap_m = self.final_gap_m
         # 悬停高度**必须**让工具尖端留在工件面上方（2026-09-24 的教训，完整说明见
         # gauge_fsm.hover_z_guard）：悬停高是按相机视距定的，而工具（夹爪+通止规）
         # 比相机长 —— 实测悬停时尖端已经钻进工件 25mm，于是 S05 的横向对准把工件
@@ -511,18 +478,10 @@ class Stage2Locate(TolerantNode):
         return geometric_jacobian(link_R, link_p, p_ee, RIZON_LINK_AXES)
 
     def _w_down(self, R: np.ndarray) -> Tuple[np.ndarray, float]:
-        """让法兰 +Z 指向世界 -Z（垂直朝下）的角速度向量 + 当前倾角。"""
+        """让法兰 +Z 指向世界 -Z。公式只在 ``path_profile.flange_down_w``。"""
-        z_now = R[:, 2]
+        from arm_application.path_profile import flange_down_w
-        z_des = np.array([0.0, 0.0, -1.0])
+
-        c = float(np.clip(np.dot(z_now, z_des), -1.0, 1.0))
+        return flange_down_w(R)
-        ang = float(np.arccos(c))
-        if ang < 0.006:  # 0.34° 以下不再修，避免原地抖动
-            return np.zeros(3), ang
-        axis = np.cross(z_now, z_des)
-        n = float(np.linalg.norm(axis))
-        axis = axis / n if n > 1e-9 else np.array([1.0, 0.0, 0.0])
-        mag = float(np.clip(2.0 * ang, -0.5, 0.5))
-        return axis * mag, ang
 
     def _wait_state(self, timeout_s: float) -> Tuple[Optional[np.ndarray], Optional[np.ndarray]]:
         t0 = time.monotonic()
@@ -595,7 +554,7 @@ class Stage2Locate(TolerantNode):
         target: np.ndarray,
         verticalize: bool,
         tag: str,
-        max_seg: float = 0.08,
+        max_seg: float = FROZEN_J_HOP_MAX_M,
         v: float = 0.05,
         min_dur: float = 1.6,
     ) -> bool:
@@ -982,9 +941,12 @@ class Stage2Locate(TolerantNode):
         except Exception:
             return None
         # 两孔统一用工件表面深度 z_wp（中值，稳健）。
-        # 孔是垂直孔，XY 位置与深度无关；而 _ring_depth 在通止规逼近目标孔时会采到
+        # 不用每孔的环形深度：通止规逼近时环上会采到更近的规（~88mm vs 面 ~115mm），
-        # 更近的通止规（~88mm vs 面 ~115mm），把孔深拉偏 → 孔基座坐标随手臂移动漂移、
+        # 孔的基座坐标就跟着臂飘、精对准追不动。
-        # 精对准追不动目标而振荡。倾斜仅 ~0.4°，左右孔 camera-Z 差 <0.5mm，对 XY 影响 <0.2mm。
+        # 法兰相对竖直大约 0.4°，但相机光轴相对法兰大约 30°。深度误差会进 XY：
+        # 2026-10-09 观测高度实测 dXY/dz ≈ 0.46 mm/mm。z_wp 必须是工件面，
+        # 不能是规。同一孔在悬停和观测高度上反投影，基座 XY 可以差约 2mm，
+        # 所以下降之后还要在当前高度再对一次（见 _trim_xy_after_descent）。
         z_left = z_right = z_wp
         try:
             left_b = self._cam_to_base(self._px_to_cam(pts[0][0], pts[0][1], z_left, intr))
@@ -1360,65 +1322,108 @@ class Stage2Locate(TolerantNode):
         return np.array([p[0] + off[0], p[1] + off[1]])
 
     def _tip_offset_from_depth(
-        self, depth_m: np.ndarray, intr: Dict[str, float], Tee: np.ndarray
+        self,
+        depth_m: np.ndarray,
+        intr: Dict[str, float],
+        Tee: np.ndarray,
+        z_wp_cam: Optional[float] = None,
     ) -> Tuple[Optional[np.ndarray], Optional[float], str]:
         """从一帧深度里找**规尖**，返回 (规尖相对法兰的基座 XY 偏移, 工具长, 说明)。
 
         找不到/不可信时返回 (None, None, 原因)。判据（每条都能真的失败）：
           1. 只看法兰轴线附近的**中央竖带**——规就夹在爪里，不会跑到画面边上；
-          2. 近端连通域要够大（规杆），且连通域内最深 3% 就是**规尖**；
+          2. 多个连通域里取画面**更靠下**（v 更大）的那一个，不按面积、也不按
-          3. 反投出的"法兰 z − 规尖 z"必须落在 [tip_len_min, tip_len_max] ——
+             「最接近配置工具长」——按面积会选爪子；贴配置长会选法兰轴上的假深点
-             这一条同时挡住"深度读到别的东西"（如台面/工件），是主干守卫。
+             （2026-10-09 真机工具长 225mm、偏移约 0，残差 0.14mm 但孔偏了）；
+          3. 有同一帧工件面深度时，只留**比工件面至少近 8mm** 的像素。相机前倾
+             ~30°，规尖沿光轴只比面近十来毫米（2026-10-09 观测高度：面 127mm、
+             尖 115mm）。再往前砍 15mm 会把尖切掉，只剩更近的夹爪/杆身（~80mm），
+             反投影偏移 ~2mm，残差 0.2mm 看着达标，针却偏在孔沿上。
+             没给工件面深度时，退回固定近端（默认 150mm）；
+          4. 反投出的"法兰 z − 规尖 z"必须落在 [tip_len_min, tip_len_max]，
+             并且基座 z 要比工件面高出一截 —— 挡住"深度读到台面/工件"。
 
         ⚠️ 工具长这一项**只在单帧内**可信；实测它会跳（192~223mm）——规尖细、反光，
         掠射角下深度容易丢，丢了以后"最深有效像素"就往上跑、工具长只会**变短**。
-        所以：
+        XY 偏移对这件事不敏感（杆细且接近竖直），见 _refresh_tip_offset。
-          · **XY 偏移**对这件事不敏感（规杆细且接近竖直，杆上任一像素反投回基座 XY
-            都在杆轴 ~3mm 内）⇒ 逐帧拿来用是稳的；
-          · **工具长**必须挑"端头看得全"的拍（最长那一簇），见 _refresh_tip_offset。
         """
         import cv2
 
         h, w = depth_m.shape
+        # 8mm：3mm 挡不住深度噪声（工件漏进来，工具长测成 296–315mm）；
+        # 15mm 又把真规尖切掉（面 127mm、尖 115mm，只剩夹爪）。两条都在真机上红过。
+        wp_gap = 0.008
+        # z_wp 只在像相机深度时才当上限（真机孔 ~0.13–0.18m）。基座 z~0.03 会把近端切空。
+        _z_wp = float(z_wp_cam) if z_wp_cam is not None else 0.0
+        if 0.08 <= _z_wp <= 0.45:
+            ceiling = min(_z_wp - wp_gap, self.tip_max_m)
+            ceil_s = f"比工件面{_z_wp*1000:.0f}近{wp_gap*1000:.0f}"
+        else:
+            ceiling = self.tip_near_m
+            ceil_s = f"固定近端{self.tip_near_m*1000:.0f}"
         valid = (depth_m > self.tip_min_m) & (depth_m < self.tip_max_m)
-        near = (valid & (depth_m < self.tip_near_m)).astype(np.uint8)
+        near = (valid & (depth_m < ceiling)).astype(np.uint8)
         band = np.zeros_like(near)
         half = max(1, int(self.tip_band_frac * w))
         c0, c1 = int(intr["cx"]) - half, int(intr["cx"]) + half
         band[:, max(0, c0):min(w, c1)] = 1
         near = near & band
         if int(near.sum()) < self.tip_min_px:
-            return None, None, f"近端(<{self.tip_near_m*1000:.0f}mm)中央竖带内像素 {int(near.sum())} < {self.tip_min_px}"
+            return None, None, (
+                f"近端(<{ceiling*1000:.0f}mm,{ceil_s})中央竖带内像素 "
+                f"{int(near.sum())} < {self.tip_min_px}"
+            )
         n, lab, stats, _ = cv2.connectedComponentsWithStats(near, 8)
         if n <= 1:
             return None, None, "近端无连通域（规不见了吗）"
-        big = max(range(1, n), key=lambda i: stats[i, 4])
+
-        ys, xs = np.where(lab == big)
+        # 掩膜已经丢掉贴着工件面的像素。基座 z 再挡一层：反投影落在面上的
+        # 仍不是规尖。裕量 8mm 而不是 20mm——观测高度规尖离面只有约 15mm，
+        # 20mm 会把真尖一起丢掉（2026-10-09）。悬停时尖离面约 75mm，8mm 仍够。
+        surf = float(getattr(self, "surface_z_m", 0.0) or 0.0)
+        above_surface_m = 0.008
+        cands = []
+        stuck = 0
+        for i in range(1, n):
+            if int(stats[i, 4]) < self.tip_min_px:
+                continue
+            ys, xs = np.where(lab == i)
             dv = depth_m[ys, xs]
-        k = np.percentile(dv, 97)  # 连通域内最深 3% ≈ 规尖
+            k = float(np.percentile(dv, 97))
             sel = dv >= k - 1e-6
             u = float(np.median(xs[sel]))
             v = float(np.median(ys[sel]))
-        y0, y1 = max(0, int(v) - 2), min(h, int(v) + 3)
+            # 用连通域自己的近端深度，不用周围 5×5：规尖只有几像素宽，
-        x0, x1 = max(0, int(u) - 2), min(w, int(u) + 3)
+            # 邻域中值会落到工件面上，尖又被当成面丢掉。
-        patch = depth_m[y0:y1, x0:x1]
+            z_tip = float(np.median(dv[sel]))
-        pv = patch[(patch > self.tip_min_m) & (patch < self.tip_max_m)]
-        if pv.size == 0:
-            return None, None, "规尖邻域深度无效"
-        z_tip = float(np.median(pv))
             try:
                 p_tip = self._cam_to_base(self._px_to_cam(u, v, z_tip, intr))
             except Exception as exc:  # noqa: BLE001
                 return None, None, f"规尖反投影失败（手眼 TF 缺？）: {exc}"
             tool_len = float(Tee[2, 3]) - float(p_tip[2])
             if not (self.tip_len_min <= tool_len <= self.tip_len_max):
+                continue
+            if surf > 0.0 and float(p_tip[2]) < surf + above_surface_m:
+                stuck += 1
+                continue
+            off = np.array(
+                [float(p_tip[0]) - float(Tee[0, 3]), float(p_tip[1]) - float(Tee[1, 3])]
+            )
+            cands.append((off, tool_len, u, v))
+        if not cands:
+            if stuck:
                 return None, None, (
-                f"反投影出的工具长 {tool_len*1000:.0f}mm 不在 "
+                    f"近端点贴着工件面（高出不足 {above_surface_m*1000:.0f}mm），不是规尖"
+                )
+            return None, None, (
+                f"反投影出的工具长不在 "
                 f"[{self.tip_len_min*1000:.0f},{self.tip_len_max*1000:.0f}]mm —— "
                 f"近端那个物体不是规"
             )
-        off = np.array([float(p_tip[0]) - float(Tee[0, 3]), float(p_tip[1]) - float(Tee[1, 3])])
+        # v 优先：画面更靠下的才是伸向工件的那一端。工具长只作同分时的次序，
-        return off, tool_len, f"px=({u:.0f},{v:.0f}) 工具长 {tool_len*1000:.0f}mm"
+        # 不拿去贴近 site.yaml —— 贴配置长会把法兰轴上的点当成规尖。
+        off, tool_len, u, v = max(cands, key=lambda c: (c[3], c[1]))
+        return off, tool_len, f"px=({u:.0f},{v:.0f}) 工具长 {tool_len*1000:.0f}mm {ceil_s}"
 
     def _refresh_tip_offset(self) -> None:
         """攒够**一致**样本后，定下并**冻结**「规尖相对法兰」的偏移。
@@ -1440,6 +1445,10 @@ class Stage2Locate(TolerantNode):
         med = np.median(arr, axis=0)
         inl = np.linalg.norm(arr - med, axis=1) <= self.tip_off_tol
         if int(inl.sum()) < self.tip_calib_n:
+            self.get_logger().warn(
+                f"  规尖样本 {len(self._tip_samp)} 拍、一致的只有 {int(inl.sum())}/"
+                f"{self.tip_calib_n}（tol={self.tip_off_tol*1000:.0f}mm）—— 还冻不住，继续采"
+            )
             return
         idx = np.where(inl)[0]
         self._tip_off = arr[idx].mean(0)
@@ -1540,8 +1549,11 @@ class Stage2Locate(TolerantNode):
           2. 腕部 FT 接触检测：``force.z`` 相对悬空基线突变 ⇒ 立即停 + 回退 1mm；
           3. 任一步 ``_goto`` 没走到位（多半是碰到东西/被挡）⇒ 停止，保持当前高度。
         """
-        if not self.final_descent_enable:
+        # final_descent_enable：降到贴面 final_gap（约 2mm）。
-            return True, q, Tee
+        # 关掉时（--no-descent）仍做**观测下降**，停在 observe_gap（约 15mm），
+        # 让人能看出规尖偏没偏，又不把 φ6 规压到 φ7 孔沿上。
+        target_gap = self.final_gap_m if self.final_descent_enable else self.observe_gap_m
+        kind = "贴面轻降" if self.final_descent_enable else "观测下降"
         self._state("descend_above_hole")
         tip_len = float(self.tool_tip_below_flange_m)
         surf = float(self.surface_z_m)
@@ -1557,7 +1569,7 @@ class Stage2Locate(TolerantNode):
             return False, q, Tee
 
         gap0 = float(Tee[2, 3]) - tip_len - surf  # 几何间隙（唯一决定下降量的量）
-        descent = gap0 - self.final_gap_m
+        descent = gap0 - target_gap
         if descent > self.descent_budget_m + 5e-4:
             self.get_logger().warn(
                 f"轻降超过配置几何预算：按 FK 算需降 {descent * 1000:.1f}mm > "
@@ -1585,9 +1597,9 @@ class Stage2Locate(TolerantNode):
             )
 
         self.get_logger().info(
-            f"轻降准备: 几何间隙 {gap0 * 1000:.1f}mm → 目标 {self.final_gap_m * 1000:.1f}mm，"
+            f"{kind}准备: 几何间隙 {gap0 * 1000:.1f}mm → 目标 {target_gap * 1000:.1f}mm，"
             f"需下降 {descent * 1000:.1f}mm（法兰 {Tee[2, 3]:.4f} − 工具长 {tip_len * 1000:.0f}mm"
-            f" − 工件面 {surf:.4f}；很慢很柔）"
+            f" − 工件面 {surf:.4f}；很慢，XY 保持）"
         )
         if descent <= 0.001:
             self.get_logger().info("规尖已在目标间隙附近，无需下降")
@@ -1632,7 +1644,7 @@ class Stage2Locate(TolerantNode):
                 self.descent_final_gap_m = float(Tee[2, 3]) - tip_len - surf
                 return True, q, Tee
             # 用 FK 现算剩余下降量（不是累加指令）—— 单步没走到也不会积累误差。
-            remaining = (float(Tee[2, 3]) - tip_len - surf) - self.final_gap_m
+            remaining = (float(Tee[2, 3]) - tip_len - surf) - target_gap
             if remaining <= 1e-4:
                 break
             step = min(remaining, 0.004)  # 每步 ≤4mm，很慢很柔
@@ -1653,12 +1665,128 @@ class Stage2Locate(TolerantNode):
         self.descent_final_gap_m = gap_f
         lvl = self.get_logger().info if gap_f >= 0.0 else self.get_logger().warn
         lvl(
-            f"=== 轻降完成: 规尖悬在孔面上方 ≈{gap_f * 1000:.1f}mm（几何：法兰 "
+            f"=== {kind}完成: 规尖悬在孔面上方 ≈{gap_f * 1000:.1f}mm（几何：法兰 "
             f"{Tee[2, 3]:.4f} − 工具长 {tip_len * 1000:.0f}mm − 工件面 {surf:.4f}；"
-            f"目标 {self.final_gap_m * 1000:.1f}mm，不接触）==="
+            f"目标 {target_gap * 1000:.1f}mm，不接触）==="
         )
         return True, q, Tee
 
+    def _trim_xy_after_descent(self, q, Tee, final: Dict[str, Any]):
+        """下降之后、保持当前高度，用这一高度的规尖和孔再对一次 XY。
+
+        悬停残差不能拿来当下降后的孔位。相机光轴相对法兰约 30°，孔的基座 XY
+        对深度大约 0.5 mm/mm。2026-10-09：同一孔在悬停（相机深度 180mm，残差
+        0.22mm）和观测高度（127mm）上反投影，基座 XY 差约 2mm；法兰 XY 下降时
+        只动了 0.2mm。画面里针在孔的上方一截，主要是 15mm 间隙的透视，横着的
+        那 2mm 才是要补的。
+
+        只用本高度新量的规尖，不沿用悬停时冻住的偏移。总修正超过 8mm 就不动
+        —— 那一档是夹爪/杆身，不是针。量不到规尖也不动，悬停对准的结果留着。
+        """
+        if not self.tip_vision:
+            return q, Tee, final
+        final_in = final
+        z_hold = float(Tee[2, 3])
+        saved = self._tip_off
+        budget = 0.008
+        moved = 0.0
+        prev: Optional[np.ndarray] = None
+        for it in range(4):
+            if not rclpy.ok():
+                break
+            try:
+                _bgr, depth_m, intr, res = self._snap_holes(skip_center_gate=True)
+            except Exception as exc:  # noqa: BLE001
+                self.get_logger().warn(f"下降后复拍失败，保持悬停对准的位置：{exc}")
+                self._tip_off = saved
+                return q, Tee, final
+            if not res.get("status"):
+                self.get_logger().warn(f"下降后丢孔（{res.get('msg')}），不再改 XY")
+                self._tip_off = saved
+                return q, Tee, final
+            hb = res.get("hole_base")
+            z_wp = float(res.get("z_wp") or 0.0)
+            if hb is None or len(hb) != 2 or z_wp <= 0.0:
+                self.get_logger().warn("下降后没有孔的基座坐标，不再改 XY")
+                self._tip_off = saved
+                return q, Tee, final
+            left_b = np.asarray(hb[0], dtype=float)
+            right_b = np.asarray(hb[1], dtype=float)
+            k1_b, k2_b = (left_b, right_b) if self.k1_label == "left" else (right_b, left_b)
+            tgt = k1_b if self.target_hole == "k1" else k2_b
+            Tee_now = self._lookup(self.base_frame, self.ee_frame)
+            if Tee_now is not None:
+                Tee = Tee_now
+            off, tlen, why = self._tip_offset_from_depth(
+                depth_m, intr, Tee, z_wp_cam=z_wp
+            )
+            if off is None or tlen is None:
+                self.get_logger().warn(f"下降后没量到规尖（{why}），不再改 XY")
+                self._tip_off = saved
+                return q, Tee, final
+            if prev is not None and float(np.linalg.norm(off - prev)) > 0.003:
+                self.get_logger().warn(
+                    f"下降后规尖两拍差 {float(np.linalg.norm(off - prev))*1000:.1f}mm，这一拍不动"
+                )
+                prev = np.array(off, dtype=float)
+                continue
+            prev = np.array(off, dtype=float)
+            if it == 0:
+                continue
+            gxy = np.array([
+                float(Tee[0, 3]) + float(off[0]), float(Tee[1, 3]) + float(off[1])
+            ])
+            raw = np.array([float(tgt[0]) - gxy[0], float(tgt[1]) - gxy[1]])
+            err = float(np.linalg.norm(raw))
+            final = dict(final)
+            final["residual_mm"] = round(err * 1000.0, 2)
+            final["tip_offset_m"] = [round(float(off[0]), 5), round(float(off[1]), 5)]
+            final["tip_len_m"] = round(float(tlen), 5)
+            final["gauge_xy_m"] = [round(float(gxy[0]), 5), round(float(gxy[1]), 5)]
+            final["gauge_source"] = "视觉规尖(下降后重测)"
+            self._tip_off = np.array(off, dtype=float)
+            self.get_logger().info(
+                f"[下降后 {it}] 工具=({gxy[0]:.4f},{gxy[1]:.4f}) "
+                f"残差={err*1000:.2f}mm {why}"
+            )
+            if err <= self.align_tol:
+                self.get_logger().info(
+                    f"下降后 XY 已在容差内（{err*1000:.2f}mm），高度保持 {z_hold:.4f}"
+                )
+                return q, Tee, final
+            if moved + err > budget:
+                self.get_logger().error(
+                    f"下降后要对 {err*1000:.1f}mm（已走 {moved*1000:.1f}mm），"
+                    f"超过 {budget*1000:.0f}mm —— 不像针，停在原地"
+                )
+                self._tip_off = saved
+                return q, Tee, final_in
+
+            step = raw
+            if err > 0.004:
+                step = raw * (0.004 / err)
+            move = np.array([
+                float(Tee[0, 3]) + float(step[0]),
+                float(Tee[1, 3]) + float(step[1]),
+                z_hold,
+            ])
+            ok, q, Tee = self._goto(
+                q, Tee, move, verticalize=True,
+                tol_m=0.0004, tol_tilt=self.vert_tol_rad * 2.0,
+                tag="trim_xy", v=min(float(self.align_speed), 0.010),
+                max_hops=4, settle_s=0.30, strict_xy=True,
+            )
+            self._pin(q)
+            moved += float(np.linalg.norm(step))
+            if not ok:
+                self.get_logger().warn("下降后 XY 修正没走到，停")
+                return q, Tee, final
+            if abs(float(Tee[2, 3]) - z_hold) > 0.002:
+                self.get_logger().error(
+                    f"下降后修正把高度从 {z_hold:.4f} 带到了 {float(Tee[2, 3]):.4f}，停下"
+                )
+                return q, Tee, final
+        return q, Tee, final
 
     def _snap_holes(self, *, skip_center_gate: bool) -> Tuple[np.ndarray, np.ndarray, Dict[str, float], dict]:
         """多帧拍孔：先对 N 帧做时间中值得到干净图，再在其上检测螺纹嵌件对。
@@ -2059,7 +2187,9 @@ class Stage2Locate(TolerantNode):
             # 把法兰对到孔上等于把规偏着对过去。偏移量准一次后冻结（刚体量）。
             tip_src = "法兰+偏置"
             if self.tip_vision and self._tip_off is None:
-                off, tlen, why = self._tip_offset_from_depth(depth_m, intr, Tee)
+                off, tlen, why = self._tip_offset_from_depth(
+                    depth_m, intr, Tee, z_wp_cam=float(z_wp)
+                )
                 if off is not None and tlen is not None:
                     self._tip_samp.append((off, tlen))
                     self._refresh_tip_offset()
@@ -2263,6 +2393,10 @@ class Stage2Locate(TolerantNode):
         okd, q, Tee = self._descend_above_hole(q, Tee)
         if not okd:
             self.get_logger().warn("轻降未完全到位，保持当前高度（安全优先）")
+        else:
+            # 悬停残差在下降 60mm 之后会变大约 2mm（相机 30°，同一孔两个高度
+            # 反投影对不齐）。在观测高度重测规尖，只补 XY。
+            q, Tee, final = self._trim_xy_after_descent(q, Tee, final)
         # 如实汇报**实际**间隙（几何量）——不是把目标值念一遍。停在离面 50mm 时
         # 也显示 ~50mm，现场才看得出"轻降没做到"（R1.1：不许把没做到说成做到）。
         gap_txt = (

src/arm_application/launch/pipeline.launch.py

@@ -81,6 +81,7 @@ def _launch_setup(context, *args, **kwargs):
     # 对到孔心"时先别让规往下走 —— 对不准就下压会把规顶在孔沿上（2026-10-08 真机
     # 顶到 63N）。默认关；验证对准时用 --no-descent 打开。
     no_descent = _as_bool(LaunchConfiguration("no_descent").perform(context))
+    d435_only = _as_bool(LaunchConfiguration("d435_move_only").perform(context))
     target_hole = LaunchConfiguration("target_hole").perform(context).strip()
     depth = LaunchConfiguration("insert_depth_mm").perform(context).strip()
     pitch = LaunchConfiguration("screw_pitch_mm").perform(context).strip()
@@ -117,8 +118,9 @@ def _launch_setup(context, *args, **kwargs):
                 "insert_depth_mm": float(depth) if depth else -1.0,
                 "screw_pitch_mm": float(pitch) if pitch else -999.0,
                 "skip_stage1": skip1,
-                "skip_stage2": skip2,
+                "skip_stage2": skip2 or d435_only,
-                "skip_stage3": skip3,
+                "skip_stage3": skip3 or d435_only,
+                "d435_move_only": d435_only,
                 # -1=按 site 的 stage2.final_descent_enable；0=强制不降（调试）
                 "final_descent_enable": 0 if no_descent else -1,
             }
@@ -131,6 +133,7 @@ def _launch_setup(context, *args, **kwargs):
         LogInfo(
             msg=f"[pipeline] hole={target_hole or 'site'} depth={depth or 'site'} "
             f"pitch={pitch or 'site'} skip1={skip1} skip2={skip2} skip3={skip3}"
+            + (" [d435_only：只测 D435 位移]" if d435_only else "")
             + (" [no_descent：只对准，不轻降]" if no_descent else "")
         ),
         *nodes,
@@ -156,6 +159,7 @@ def generate_launch_description() -> LaunchDescription:
             DeclareLaunchArgument("resnap", default_value="false"),
             # 只验证精对准、不轻降（见 _launch_setup 里的说明）
             DeclareLaunchArgument("no_descent", default_value="false"),
+            DeclareLaunchArgument("d435_move_only", default_value="false"),
             DeclareLaunchArgument("preview", default_value="true"),
             OpaqueFunction(function=_launch_setup),
         ]

src/arm_application/launch/visual_servo.launch.py

@@ -60,6 +60,10 @@ def _load_site():
     return merged, path, perception_overrides(data), servo_overrides(data), robot
 
 
+def _as_bool(s: str) -> bool:
+    return s.strip().lower() in ("1", "true", "yes")
+
+
 def _handeye_xyz_xyzw(path: str):
     """读手眼文件 → ``(tx, ty, tz, qx, qy, qz, qw)``。
 
@@ -171,6 +175,9 @@ def _launch_setup(context, *args, **kwargs):
             servo_params["lock_hold_sec"] = float(lock_hold_arg)
         except ValueError:
             pass
+    d435_only = _as_bool(LaunchConfiguration("d435_move_only").perform(context))
+    perc_params["d435_move_only"] = d435_only
+    servo_params["d435_move_only"] = d435_only
 
     # ------------------------------------------------------------------
     # 感知来源：谁来产生"视觉测量"（site.yaml 的 perception.source）
@@ -240,6 +247,10 @@ def _launch_setup(context, *args, **kwargs):
         ),
         LogInfo(msg=f"[site] 感知来源 perception.source = {perc_src}"),
     ]
+    if d435_only:
+        nodes.append(
+            LogInfo(msg="[d435_only] 只测 D435 单次位移：不交 D405、不进二阶段")
+        )
     if perc_src == "yolo":
         nodes.append(LogInfo(msg=f"[site] YOLO weights: {weights}"))
         nodes.append(
@@ -345,6 +356,10 @@ def generate_launch_description() -> LaunchDescription:
                 "lock_hold_sec",
                 default_value="",
             ),
+            DeclareLaunchArgument(
+                "d435_move_only",
+                default_value="false",
+            ),
             OpaqueFunction(function=_launch_setup),
         ]
     )

src/arm_application/package.xml

@@ -16,6 +16,7 @@
   <depend>std_msgs</depend>
   <depend>std_srvs</depend>
   <depend>trajectory_msgs</depend>
+  <depend>control_msgs</depend>
   <exec_depend>tf2_ros</exec_depend>
   <exec_depend>python3-yaml</exec_depend>
   <exec_depend>python3-serial</exec_depend>

src/arm_application/test/test_feature_guard.py

@@ -287,6 +287,9 @@ def test_perception_wires_slew_and_reacquire_guards():
     src = _perception_src()
     assert "TrackSlewGuard(" in src, "位移上限没接"
     assert "FarReacquireConfirm(" in src, "远框重捕获确认没接"
+    assert "self.reacquire_far_px =" in src, (
+        "参数读了没存成属性 —— 远框确认会 AttributeError，看门狗跟着急停"
+    )
 
 
 def _export_site_env():

src/arm_application/test/test_gauge_exit_lift.py

@@ -12,8 +12,8 @@
 所以这里有两条断言，缺一不可：
 * **结构**：``_step_unscrew`` 在"成功返回"之前必须调用提出动作 —— 顺序错了
   （比如把提出挪到 S10 里）就等于没做，因为横向动作已经发生了；
-* **行为**：提出目标是**跟踪高**（``_track_z()`` = 悬停高 + 50mm），不是写死的
+* **行为**：提出目标是**跟踪高**（``_track_z()``：悬停时尖端离面不够才再抬
-  毫米数；已经在跟踪高上时不动（否则每次换孔白等 1 秒多）。
+  50mm），不是写死的毫米数；已经在跟踪高上时不动（否则每次换孔白等 1 秒多）。
 """
 
 from __future__ import annotations
@@ -79,7 +79,7 @@ def test_unscrew_lifts_out_of_the_hole_before_succeeding():
 
 
 def test_lift_helper_exists_and_uses_the_track_height_idiom():
-    """提出高度用 ``_track_z()``（悬停高 + 50mm），不写死毫米数。"""
+    """提出高度用 ``_track_z()``，不写死毫米数。"""
     fn = _find_method("_lift_clear_of_hole")
     assert _call_lines(fn, "_track_z"), (
         "提出高度必须用 _track_z()：这是流程里『工具已离开工件』的既有定义，"

src/arm_application/test/test_handeye_lib.py

@@ -18,8 +18,10 @@ from arm_application.handeye_lib import (
     CameraIntrinsics,
     ChessboardStabilizer,
     Rt_to_T,
+    T_from_flexiv_tcp,
     board_object_points,
     calibrate_hand_eye,
+    cam_to_flange_via_tool,
     format_transform_yaml,
     invert_Rt,
     is_pose_filled,
@@ -420,3 +422,41 @@ def test_is_pose_filled_rejects_wrong_length():
 
 def test_is_pose_filled_accepts_real_pose():
     assert is_pose_filled([-1.107, -0.744, 0.982, 1.856, -0.735, 0.947, -0.118])
+
+
+def test_flexiv_tcp_identity_is_pure_translation():
+    """Studio tool10120938：姿态单位四元数，只沿法兰 Z 偏 224.5 mm。"""
+    T = T_from_flexiv_tcp([-0.0005, 0.0067, 0.2245], [1.0, 0.0, 0.0, 0.0])
+    assert np.allclose(T[:3, :3], np.eye(3), atol=1e-12)
+    assert T[2, 3] == pytest.approx(0.2245)
+
+
+def test_cam_to_flange_via_tool_must_not_leave_t_unchanged():
+    """忘了乘时 T_4x4 停在 TCP；乘反了相机会到法兰后方。"""
+    T_cam_tool = np.eye(4)
+    T_cam_tool[:3, 3] = [-0.05961995, -0.01523625, -0.09761517]
+    T_flange_tool = T_from_flexiv_tcp([-0.0005, 0.0067, 0.2245], [1.0, 0.0, 0.0, 0.0])
+    T_cam_flange = cam_to_flange_via_tool(T_cam_tool, T_flange_tool)
+    delta = float(np.linalg.norm(T_cam_flange[:3, 3] - T_cam_tool[:3, 3]))
+    tool_len = float(np.linalg.norm(T_flange_tool[:3, 3]))
+    assert delta == pytest.approx(tool_len, abs=1e-9)
+    assert tool_len > 0.20
+    # 相机在法兰前方（+Z ≈ 126 mm），与 2026-09-04 旧结果同量级。
+    assert T_cam_flange[2, 3] == pytest.approx(0.12688483, abs=1e-8)
+    assert T_cam_flange[0, 3] == pytest.approx(-0.06011995, abs=1e-8)
+    # 负向：乘反了会到法兰后方约 −296 mm。
+    wrong = T_cam_tool @ np.linalg.inv(T_flange_tool)
+    assert wrong[2, 3] < 0.0
+    # 负向：直接把 cam→工具当成 cam→法兰，平移几乎不动。
+    assert float(np.linalg.norm(T_cam_tool[:3, 3] - T_cam_tool[:3, 3])) < 1e-12
+    # 折回去必须还原：T_ct = inv(T_ft) · T_cf
+    T_back = np.linalg.inv(T_flange_tool) @ T_cam_flange
+    assert np.allclose(T_back, T_cam_tool, atol=1e-12)
+
+
+def test_cam_at_tcp_is_the_tool_pose_in_flange():
+    """相机与 TCP 重合时，cam→法兰必须等于法兰→工具，不能是它的逆。"""
+    T_ft = T_from_flexiv_tcp([-0.0005, 0.0067, 0.2245], [1.0, 0.0, 0.0, 0.0])
+    T_cf = cam_to_flange_via_tool(np.eye(4), T_ft)
+    assert np.allclose(T_cf, T_ft, atol=1e-12)
+    assert T_cf[2, 3] == pytest.approx(0.2245)

src/arm_application/test/test_handeye_mode.py

@@ -339,6 +339,7 @@ class TestRunHandeyeScriptMode:
         assert "camera=d435" in out
         assert "11x8 inner, square=0.02 m" in out
         assert "89_d435_eye_to_hand.yaml" in out
+        assert "tool10120938" in out
         # 负向：不能再去开腕部相机。
         assert "camera=d405" not in out
 
@@ -346,7 +347,8 @@ class TestRunHandeyeScriptMode:
         out = _show("eye_in_hand", _real_site)
         assert "mode=eye_in_hand" in out
         assert "d405" in out
-        assert "8x11 inner, square=0.015 m" in out
+        assert "11x8 inner, square=0.015 m" in out
+        assert "tool10120938" in out
         assert "89_eye_in_hand.yaml" in out
 
     def test_missing_board_blocks_calibrate(self, tmp_path: Path):
@@ -392,6 +394,58 @@ class TestRunHandeyeScriptMode:
         assert proc.returncode != 0
         assert "没有配置 [eye_to_hand] 用的标定板尺寸" in (proc.stdout + proc.stderr)
 
+    def test_left_arm_calibrate_wires_tool10120938(self):
+        """89 必须按 tool10120938 求解；脚本和节点都要接到这份 TCP。"""
+        cal = (
+            ROOT / "src" / "arm_application" / "arm_application" / "handeye_calibrate.py"
+        ).read_text(encoding="utf-8")
+        sh = SCRIPT.read_text(encoding="utf-8")
+        assert "cam_to_flange_via_tool(" in cal
+        assert "gripper_tool" in cal
+        assert "gripper_tool:=" in sh
+        assert "T_4x4 仍是 cam→基座，不折" in sh
+        backup = ROOT / "handeye" / "results" / "backup" / "89_eye_in_hand_flange_tool1_20260904.yaml"
+        assert backup.is_file()
+        e2h_bak = ROOT / "handeye" / "results" / "backup" / "89_d435_eye_to_hand_20261009.yaml"
+        assert e2h_bak.is_file()
+        e2h = (ROOT / "handeye" / "results" / "89_d435_eye_to_hand.yaml").read_text(
+            encoding="utf-8"
+        )
+        assert "gripper_tool: tool10120938" in e2h
+        assert "cam→基座" in e2h
+        import re
+
+        m_e2h = re.search(r"translation_m:\s*\[([^]]+)\]", e2h)
+        assert m_e2h, e2h
+        xyz_e2h = [float(x) for x in m_e2h.group(1).split(",")]
+        # 顶置相机在基座上方约 0.7 m；折错或写到法兰会差几十厘米。
+        assert 0.60 < xyz_e2h[2] < 0.90, xyz_e2h
+        # 眼在手外不得走 cam_to_flange_via_tool（相机和基座都在世界里）。
+        e2h_block = cal.split("if eye_to_hand:")[1].split("elif self.gripper_tool:")[0]
+        assert "cam_to_flange_via_tool" not in e2h_block
+        live = ROOT / "handeye" / "results" / "89_eye_in_hand.yaml"
+        text = live.read_text(encoding="utf-8")
+        assert "BACKUP" in text or "backup/" in text
+        # 乘反了 tz 会是负的（法兰后方）；现行必须在法兰前方。
+        import re
+
+        m = re.search(r"translation_m:\s*\[([^]]+)\]", text)
+        assert m, text
+        xyz = [float(x) for x in m.group(1).split(",")]
+        assert xyz[2] > 0.10, xyz
+        assert "inv(T_flange_to_tool)" not in text
+
+    def test_teach_and_merge_share_the_same_taught_file(self):
+        """示教写 A、merge 读 B → 采了点也报 got 0。两边必须用 TAUGHT。"""
+        sh = SCRIPT.read_text(encoding="utf-8")
+        teach = (
+            ROOT / "src" / "arm_application" / "arm_application" / "handeye_teach.py"
+        ).read_text(encoding="utf-8")
+        assert 'poses_file:="${TAUGHT}"' in sh
+        assert "taught_poses_${HANDEYE_MODE}" in sh
+        assert "declare_parameter('poses_file'" in teach
+        assert "get_parameter('poses_file')" in teach
+
     def test_illegal_mode_exits_immediately(self):
         proc = subprocess.run(
             ["bash", str(SCRIPT), "--arm", "89", "show", "--mode", "eye_on_foot"],

src/arm_application/test/test_merge_handeye_poses.py

@@ -0,0 +1,69 @@
+"""merge 必须能失败：空文件、连按重复点都不够 5 个独立姿态。"""
+
+from __future__ import annotations
+
+import importlib.util
+import sys
+from pathlib import Path
+
+ROOT = Path(__file__).resolve().parents[3]
+_SPEC = importlib.util.spec_from_file_location(
+    "merge_handeye_poses", ROOT / "scripts" / "merge_handeye_poses.py"
+)
+assert _SPEC and _SPEC.loader
+_mod = importlib.util.module_from_spec(_SPEC)
+sys.modules[_SPEC.name] = _mod
+_SPEC.loader.exec_module(_mod)
+
+
+def _pose(i: int) -> list[float]:
+    return [0.1 * i, -1.0, 1.0, 1.4, -0.9, 0.2, 0.4]
+
+
+def test_parse_taught_txt_skips_comments(tmp_path: Path):
+    text = "\n".join(
+        [
+            "# pose 0  20261012_030533",
+            "-0.730276, -1.078914, 1.182093, 1.378457, -0.914310, 0.247653, 0.409611,",
+            "# image /tmp/a.png",
+        ]
+    )
+    p = tmp_path / "taught_parse_once.txt"
+    p.write_text(text, encoding="utf-8")
+    poses = _mod.parse_poses(p)
+    assert len(poses) == 1
+    assert abs(poses[0][0] + 0.730276) < 1e-9
+
+
+def test_unique_drops_enter_repeat():
+    same = _pose(0)
+    poses = [same, list(same), list(same), _pose(1)]
+    uniq = _mod.unique_poses(poses)
+    assert len(uniq) == 2
+
+
+def test_merge_refuses_repeat_only_file(tmp_path: Path):
+    src = tmp_path / "taught.txt"
+    dst = tmp_path / "wp.yaml"
+    same = ", ".join(f"{v:.6f}" for v in _pose(0)) + ","
+    src.write_text("\n".join(f"# pose {i}\n{same}" for i in range(12)) + "\n")
+    dst.write_text("    num_poses: 0\n    waypoint_positions: []\n", encoding="utf-8")
+    rc = _mod.main(["merge_handeye_poses.py", str(src), str(dst)])
+    assert rc == 4
+    assert "waypoint_positions: []" in dst.read_text(encoding="utf-8")
+
+
+def test_merge_keeps_five_distinct(tmp_path: Path):
+    src = tmp_path / "taught.txt"
+    dst = tmp_path / "wp.yaml"
+    lines = []
+    for i in range(5):
+        lines.append(f"# pose {i}")
+        lines.append(", ".join(f"{v:.6f}" for v in _pose(i)) + ",")
+    src.write_text("\n".join(lines) + "\n", encoding="utf-8")
+    dst.write_text("    num_poses: 0\n    waypoint_positions: []\n", encoding="utf-8")
+    rc = _mod.main(["merge_handeye_poses.py", str(src), str(dst)])
+    assert rc == 0
+    text = dst.read_text(encoding="utf-8")
+    assert "num_poses: 5" in text
+    assert text.count("# pose ") == 5

src/arm_application/test/test_overhead_oneshot.py

@@ -11,14 +11,49 @@ from types import SimpleNamespace
 import numpy as np
 import pytest
 
+from arm_application.path_profile import (
+    D435_FULL_ACC_M,
+    D435_FULL_CRUISE_MPS,
+    D435_FULL_DEC_M,
+    cosine_peak_acc,
+    smooth_joint_schedule,
+    smooth_path_params,
+    smooth_path_sv,
+    trap_joint_schedule,
+)
 from arm_application.overhead_oneshot import (
     StillLock,
+    at_lock_height,
     best_det_conf,
     depth_raw_to_m,
     k0_uv_from_det,
+    lock_z_velocity,
     map_uv_to_depth,
+    D405_MISS_BEFORE_D435_S,
+    D435_MOVE_SPEED_SCALE,
+    D435_NEAR_CRUISE_MPS,
+    D435_NEAR_HOP_MAX_M,
+    D435_NEAR_LEFTOVER_M,
+    D435_SETTLE_S,
+    FROZEN_J_HOP_MAX_M,
+    build_arc_xy_move,
+    d405_commands_arm,
+    flange_down_w,
+    hold_translation_vz,
+    hybrid_cruise_vxy,
+    oneshot_cartesian_dp,
+    oneshot_concat_hold_ori,
+    oneshot_full_cruise_mps,
+    oneshot_full_segments,
+    oneshot_hop_limits,
     oneshot_k0_base,
+    oneshot_may_send_another,
+    oneshot_needs_verticalize,
+    oneshot_shot_timed_out,
+    oneshot_next_hop_m,
+    oneshot_should_submit,
     select_oneshot_det,
+    trap_acc_for_hop,
     wrist_needs_handoff,
 )
 
@@ -38,6 +73,399 @@ class TestHandoff:
         assert wrist_needs_handoff(0.5, 0.5) is False
         assert wrist_needs_handoff(0.87, 0.5) is False
 
+    def test_below_lock_height_is_not_ready_and_must_lift(self):
+        """0.26 m 是观测下降停的高度，不是锁高。必须判没到位，并且速度朝上。"""
+        assert at_lock_height(0.257, 0.320, 0.040) is False
+        assert at_lock_height(0.327, 0.320, 0.040) is True
+        assert at_lock_height(0.250, 0.320, 0.040) is False
+        vz = lock_z_velocity(0.257, 0.320, kp=1.0, v_lim=0.08)
+        assert vz > 0.0, "低于锁高必须上抬，写成 0 会和 D435 拒拍卡死"
+        assert lock_z_velocity(0.360, 0.320, kp=1.0, v_lim=0.08) < 0.0
+        assert lock_z_velocity(0.321, 0.320, kp=1.0, v_lim=0.08) == 0.0
+
+    def test_z_locked_qos_is_latched(self):
+        """晚订阅必须能拿到已经发出的锁高状态，否则 D435 会一直拒拍。"""
+        from rclpy.qos import DurabilityPolicy
+
+        from arm_application.overhead_oneshot import z_locked_qos
+
+        qos = z_locked_qos()
+        assert qos.durability == DurabilityPolicy.TRANSIENT_LOCAL
+        assert qos.depth == 1
+
+    def test_oneshot_waits_half_second_then_fires_once(self):
+        """连续 0.5 s 没有高置信度才拍。差一帧不算。拍成之后不许连拍。
+
+        不再另等「已到锁高」——那一闸和回锁高互相卡住。
+        """
+        wait = D405_MISS_BEFORE_D435_S
+        assert wait == 0.5
+        assert oneshot_should_submit(
+            handoff=True, low_for_s=wait - 0.01, already_shot=False
+        ) is False
+        assert oneshot_should_submit(
+            handoff=True, low_for_s=wait, already_shot=False
+        ) is True
+        assert oneshot_should_submit(
+            handoff=True, low_for_s=wait + 1.0, already_shot=True
+        ) is False
+        assert oneshot_should_submit(
+            handoff=False, low_for_s=wait + 1.0, already_shot=False
+        ) is False
+        # 拍了伺服没接住（工作盒拒）必须能再拍；已经在赶路不许清。
+        assert oneshot_shot_timed_out(
+            shot=True, commanding=False, age_s=0.79
+        ) is False
+        assert oneshot_shot_timed_out(
+            shot=True, commanding=False, age_s=0.80
+        ) is True
+        assert oneshot_shot_timed_out(
+            shot=True, commanding=True, age_s=2.0
+        ) is False
+        perc = (
+            __import__("pathlib").Path(__file__).resolve().parents[1]
+            / "arm_application" / "perception_node.py"
+        ).read_text(encoding="utf-8")
+        assert "oneshot_shot_timed_out(" in perc
+        assert "伺服没接住" in perc
+
+    def test_d405_points_stay_off_the_arm_until_arrived_and_settled(self):
+        """用着 D435 那个点时，到位并且停稳之前，D405 的点不下发。推理不在这里关。"""
+        assert d405_commands_arm(using_d435=False, arrived=False, settled=False) is True
+        assert d405_commands_arm(using_d435=True, arrived=False, settled=False) is False
+        assert d405_commands_arm(using_d435=True, arrived=True, settled=False) is False
+        assert d405_commands_arm(using_d435=True, arrived=False, settled=True) is False
+        assert d405_commands_arm(using_d435=True, arrived=True, settled=True) is True
+        src = (
+            __import__("pathlib").Path(__file__).resolve().parents[1]
+            / "arm_application" / "servo_node.py"
+        ).read_text(encoding="utf-8")
+        assert "d405_commands_arm(" in src
+        assert "_arrive" in src
+        perc = (
+            __import__("pathlib").Path(__file__).resolve().parents[1]
+            / "arm_application" / "perception_node.py"
+        ).read_text(encoding="utf-8")
+        assert "D405_MISS_BEFORE_D435_S" in perc
+        assert "d435_commanding" in perc
+
+    def test_d435_move_only_never_hands_back_to_d405(self):
+        """只测位移：D405 点一律不下发，到位停稳也不交。默认路径仍会交回。"""
+        assert d405_commands_arm(
+            using_d435=False, arrived=True, settled=True, move_only=True
+        ) is False
+        assert d405_commands_arm(
+            using_d435=True, arrived=True, settled=True, move_only=True
+        ) is False
+        assert d405_commands_arm(
+            using_d435=True, arrived=True, settled=True, move_only=False
+        ) is True
+        root = __import__("pathlib").Path(__file__).resolve().parents[1]
+        servo = (root / "arm_application" / "servo_node.py").read_text(encoding="utf-8")
+        perc = (root / "arm_application" / "perception_node.py").read_text(
+            encoding="utf-8"
+        )
+        pipe = (root / "arm_application" / "pipeline_node.py").read_text(
+            encoding="utf-8"
+        )
+        launch = (root / "launch" / "pipeline.launch.py").read_text(encoding="utf-8")
+        vs = (root / "launch" / "visual_servo.launch.py").read_text(encoding="utf-8")
+        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")
+        assert "move_only=self.d435_move_only" in servo
+        assert "不交 D405、不进二阶段" in servo
+        assert "改回下发 D405" in servo
+        assert "self.d435_move_only" in perc
+        assert "or wrist_needs_handoff(" in perc
+        assert "d435_move_only:={str(self.d435_move_only).lower()}" in pipe
+        assert "skip2 = True" in pipe
+        assert '"d435_move_only": d435_only' in launch
+        assert 'LaunchConfiguration("d435_move_only")' in vs
+        assert '"d435_move_only"' in vs
+        assert "--d435-only" in run_all
+
+    def test_whole_path_uses_one_cruise_and_an_arc(self):
+        """远处仍 80 mm/s。近处必须另降一档。路径是圆弧，不是对着终点做 P。"""
+        assert D435_MOVE_SPEED_SCALE == 1.0
+        assert hybrid_cruise_vxy(0.08) == pytest.approx(0.08)
+        hop_far, v_far = oneshot_hop_limits(0.20, 0.08)
+        hop_mid, v_mid = oneshot_hop_limits(0.12, 0.08)
+        hop_near, v_near = oneshot_hop_limits(0.074, 0.08)
+        assert hop_far == pytest.approx(FROZEN_J_HOP_MAX_M)
+        assert v_far == pytest.approx(0.08)
+        assert hop_mid == pytest.approx(0.040)
+        assert v_mid == pytest.approx(0.08)
+        assert hop_near == pytest.approx(D435_NEAR_HOP_MAX_M)
+        assert v_near == pytest.approx(D435_NEAR_CRUISE_MPS)
+        assert hop_near == pytest.approx(0.015)
+        assert hop_near < 0.030
+        assert v_near < 0.08
+        assert D435_NEAR_LEFTOVER_M == pytest.approx(0.08)
+        assert D435_SETTLE_S == pytest.approx(0.25)
+        p0 = np.array([0.687, -0.113])
+        p1 = np.array([0.583, -0.502])
+        move = build_arc_xy_move(p0, p1, v=0.08, a=0.40, now=0.0)
+        assert move.linear is False
+        assert move.length > float(np.linalg.norm(p1 - p0))
+        assert move.v_cruise == pytest.approx(0.08)
+        xy0, v0, done0 = move.sample(0.0)
+        np.testing.assert_allclose(xy0, p0, atol=1e-6)
+        assert float(np.hypot(v0[0], v0[1])) == pytest.approx(0.0, abs=1e-6)
+        assert done0 is False
+        t_mid = 0.5 * (move.t_acc + (move.t_acc + move.t_cruise))
+        xy_m, v_m, _ = move.sample(t_mid)
+        chord = p1 - p0
+        # 中点不在弦上，否则还是直线冲。
+        rel = xy_m - p0
+        off = abs(float(chord[0] * rel[1] - chord[1] * rel[0])) / float(
+            np.linalg.norm(chord)
+        )
+        assert off > 0.005
+        assert float(np.hypot(v_m[0], v_m[1])) == pytest.approx(0.08, abs=0.008)
+        xy1, v1, done1 = move.sample(move.T + 0.01)
+        np.testing.assert_allclose(xy1, p1, atol=2e-3)
+        assert float(np.hypot(v1[0], v1[1])) == pytest.approx(0.0, abs=1e-4)
+        assert done1 is True
+        src = (
+            __import__("pathlib").Path(__file__).resolve().parents[1]
+            / "arm_application" / "servo_node.py"
+        ).read_text(encoding="utf-8")
+        assert "hybrid_cruise_vxy(" in src
+        assert "_d435_oneshot_holds_the_arm" in src
+        assert "smooth_joint_schedule(" in src
+        assert "oneshot_full_segments(" in src
+        assert "oneshot_concat_hold_ori(" in src
+        assert "oneshot_full_cruise_mps(" in src
+        assert "oneshot_may_send_another(" in src
+        assert "oneshot_hop_limits(" in src
+        assert "oneshot_needs_verticalize(" in src
+        assert "verticalize=near" not in src
+        oneshot = (
+            __import__("pathlib").Path(__file__).resolve().parents[1]
+            / "arm_application" / "overhead_oneshot.py"
+        ).read_text(encoding="utf-8")
+        assert "verticalize=False" in oneshot
+        assert "oneshot_concat_hold_ori" in oneshot
+        assert '_d435_send_verticalize' in src
+        assert 'tag="vert"' in src or '"vert"' in src
+        assert 'tag = "fix"' in src
+        assert "_d435_full_done" in src
+        assert "_d435_fix_done" in src
+        assert "flange_tilt_rad(" in src
+        assert "D435 重测 leftover=" in src
+        assert "dxy=(" in src
+        assert "_d435_remeasure_log" in src
+        assert "到位成功不许再往话题插 hold" in src
+        assert "FollowJointTrajectory" in src
+        assert "send_goal_async(" in src
+        assert "结果回来之前不再发下一条" in src
+        assert "钉住不再补刀" in src
+        hold = src[src.index("def _d435_oneshot_holds_the_arm") : src.index("def _tick_d435_arrive")]
+        assert 'self._track_source == "overhead"' in hold
+        assert "not self._d435_settled" in hold
+        assert "_publish_joint_schedule(" not in hold
+        assert "oneshot_cartesian_dp(" not in hold
+        assert "trap_joint_schedule(" not in hold
+        assert "_hybrid_goal_xy" not in src
+        assert "def _execute_twist(" in src
+        stage2 = (
+            __import__("pathlib").Path(__file__).resolve().parents[1]
+            / "arm_application" / "stage2_locate_node.py"
+        ).read_text(encoding="utf-8")
+        assert "from arm_application.path_profile import" in stage2
+        assert "FROZEN_J_HOP_MAX_M" in stage2
+        assert "def _trap_params(" not in stage2
+        assert "def _path_interp(" not in stage2
+
+    def test_frozen_j_hop_is_capped_and_same_leftover_must_not_repeat(self):
+        """400 mm 一次规划会少走；28 mm 同剩差再发就是真机那趟死循环。"""
+        assert FROZEN_J_HOP_MAX_M == pytest.approx(0.08)
+        assert oneshot_next_hop_m(0.402) == pytest.approx(0.08)
+        assert oneshot_next_hop_m(0.028) == pytest.approx(0.028)
+        arrive = 0.015
+        assert oneshot_may_send_another(0.402, None, arrive_m=arrive) is True
+        assert oneshot_may_send_another(0.322, 0.402, arrive_m=arrive) is True
+        assert oneshot_may_send_another(0.028, 0.028, arrive_m=arrive) is False
+        assert oneshot_may_send_another(0.010, 0.028, arrive_m=arrive) is False
+
+    def test_oneshot_dp_lifts_z_first_and_caps_xy(self):
+        """没到锁高只收 Z；400 mm 的 XY 必须被砍到 80 mm，并且带着锁高。"""
+        gate = 0.040
+        lift = oneshot_cartesian_dp(
+            [0.65, -0.32, 0.26], [0.58, -0.50], 0.320, gate_m=gate
+        )
+        np.testing.assert_allclose(lift[:2], [0.0, 0.0])
+        assert lift[2] > 0.0
+        assert float(np.linalg.norm(lift)) == pytest.approx(0.060, abs=1e-9)
+        xy = oneshot_cartesian_dp(
+            [0.65, -0.11, 0.320], [0.58, -0.50], 0.320, gate_m=gate
+        )
+        assert float(np.linalg.norm(xy)) == pytest.approx(FROZEN_J_HOP_MAX_M)
+        assert abs(float(xy[2])) < 1e-9
+        acc = trap_acc_for_hop(0.08, 0.08)
+        assert acc == pytest.approx(0.08 * 0.08 / (2.0 * 0.0256), abs=1e-6)
+
+    def test_near_leftover_is_short_slow_xy_only(self):
+        """真机最后一跳 37–75 mm @ 80 mm/s 会晃。近处必须短、慢、不混 Z。"""
+        gate = 0.040
+        dp = oneshot_cartesian_dp(
+            [0.583, -0.465, 0.327], [0.583, -0.502], 0.320, gate_m=gate
+        )
+        leftover = 0.037
+        hop_max, cruise = oneshot_hop_limits(leftover, 0.08)
+        assert leftover <= D435_NEAR_LEFTOVER_M
+        assert hop_max == pytest.approx(0.015)
+        assert cruise == pytest.approx(0.030)
+        assert float(np.linalg.norm(dp)) == pytest.approx(0.015, abs=1e-9)
+        assert abs(float(dp[2])) < 1e-9
+        assert oneshot_needs_verticalize(0.03, 0.010, 0.015) is True
+        assert oneshot_needs_verticalize(0.03, 0.020, 0.015) is False
+        assert oneshot_needs_verticalize(0.03, 0.010, 0.015, already_done=True) is False
+        assert oneshot_needs_verticalize(0.005, 0.010, 0.015) is False
+        R = np.eye(3)
+        R[2, 2] = -1.0
+        w, ang = flange_down_w(R)
+        np.testing.assert_allclose(w, [0.0, 0.0, 0.0])
+        assert ang < 0.006
+        stage2 = (
+            __import__("pathlib").Path(__file__).resolve().parents[1]
+            / "arm_application" / "stage2_locate_node.py"
+        ).read_text(encoding="utf-8")
+        assert "return flange_down_w(R)" in stage2
+
+    def test_full_segments_do_not_chop_and_keep_z_then_xy(self):
+        """整段一次走完：200 mm 不许被砍成 80 mm；没到锁高先整段收 Z。"""
+        gate = 0.040
+        segs = oneshot_full_segments(
+            [0.65, -0.11, 0.26], [0.58, -0.50], 0.320, gate_m=gate
+        )
+        assert segs[0][1] == "lift_z"
+        np.testing.assert_allclose(segs[0][0][:2], [0.0, 0.0])
+        assert segs[0][0][2] == pytest.approx(0.060)
+        assert segs[1][1] == "xy"
+        xy_len = float(np.linalg.norm(segs[1][0][:2]))
+        assert xy_len > 0.20
+        assert xy_len > FROZEN_J_HOP_MAX_M
+        near = oneshot_full_segments(
+            [0.583, -0.465, 0.327], [0.583, -0.502], 0.320, gate_m=gate
+        )
+        assert len(near) == 1
+        assert near[0][1] == "xy"
+        assert float(np.linalg.norm(near[0][0])) == pytest.approx(0.037, abs=1e-9)
+        assert abs(float(near[0][0][2])) < 1e-9
+        assert oneshot_full_cruise_mps(0.20, 0.08) == pytest.approx(D435_FULL_CRUISE_MPS)
+        assert oneshot_full_cruise_mps(0.20, 0.08) < 0.08
+        assert oneshot_full_cruise_mps(0.05, 0.08) == pytest.approx(D435_NEAR_CRUISE_MPS)
+        chopped = oneshot_cartesian_dp(
+            [0.65, -0.11, 0.320], [0.58, -0.50], 0.320, gate_m=gate
+        )
+        assert float(np.linalg.norm(chopped)) == pytest.approx(FROZEN_J_HOP_MAX_M)
+
+    def test_smooth_path_is_rest_to_rest_and_slows_at_the_end(self):
+        """300 mm @ 60 mm/s：头尾速度 0，起步 ≥3 s，峰值加速度必须低于旧的弹射档。"""
+        assert D435_FULL_ACC_M == pytest.approx(0.100)
+        assert D435_FULL_DEC_M == pytest.approx(0.080)
+        assert D435_FULL_CRUISE_MPS == pytest.approx(0.060)
+        a_now = cosine_peak_acc(D435_FULL_CRUISE_MPS, D435_FULL_ACC_M)
+        a_old = cosine_peak_acc(0.08, 0.040)
+        assert a_old > 0.12
+        assert a_now < 0.035
+        assert a_now < 0.30 * a_old
+        T, v_peak, t_acc, t_cruise, d_acc, d_dec = smooth_path_params(0.30, 0.06)
+        assert v_peak == pytest.approx(0.06)
+        assert d_acc == pytest.approx(0.100)
+        assert d_dec == pytest.approx(0.080)
+        assert t_acc > 3.0
+        assert t_cruise > 0.5
+        s0, v0 = smooth_path_sv(0.0, 0.30, v_peak, t_acc, t_cruise, d_acc, d_dec)
+        assert s0 == pytest.approx(0.0, abs=1e-9)
+        assert v0 == pytest.approx(0.0, abs=1e-9)
+        s_mid, v_mid = smooth_path_sv(
+            t_acc + 0.5 * t_cruise, 0.30, v_peak, t_acc, t_cruise, d_acc, d_dec
+        )
+        assert v_mid == pytest.approx(0.06, abs=1e-6)
+        assert 0.10 < s_mid < 0.24
+        s1, v1 = smooth_path_sv(T, 0.30, v_peak, t_acc, t_cruise, d_acc, d_dec)
+        assert s1 == pytest.approx(0.30, abs=1e-6)
+        assert v1 == pytest.approx(0.0, abs=1e-6)
+        s_dec, v_dec = smooth_path_sv(
+            t_acc + t_cruise + 0.5 * (T - t_acc - t_cruise),
+            0.30,
+            v_peak,
+            t_acc,
+            t_cruise,
+            d_acc,
+            d_dec,
+        )
+        assert v_dec < 0.045
+        assert s_dec > 0.20
+        q0 = np.zeros(7)
+        q1 = np.zeros(7)
+        q1[0] = 0.3
+        sched, T_move, vp = smooth_joint_schedule([q0, q1], 0.30, 0.06)
+        assert T_move == pytest.approx(T, abs=1e-6)
+        assert vp == pytest.approx(0.06)
+        assert float(np.linalg.norm(sched[0][1])) == pytest.approx(0.0, abs=1e-9)
+        assert float(np.linalg.norm(sched[-1][1])) == pytest.approx(0.0, abs=1e-9)
+        times = [t for _, _, t in sched]
+        assert all(times[i] < times[i + 1] for i in range(len(times) - 1))
+
+    def test_concat_hold_ori_refuses_to_mix_verticalize(self):
+        """串段必须 verticalize=False。混进收垂直会把腕部漏成 Y。"""
+        J = np.zeros((6, 7))
+        J[0, 0] = 1.0
+        J[1, 1] = 1.0
+        J[2, 2] = 1.0
+        segs = [
+            (np.array([0.0, 0.0, 0.02]), "lift_z"),
+            (np.array([0.04, -0.03, 0.0]), "xy"),
+        ]
+        wps, planned, asked, tag = oneshot_concat_hold_ori(
+            np.zeros(7), J, segs, R=np.eye(3)
+        )
+        assert tag == "full"
+        assert asked == pytest.approx(0.02 + float(np.hypot(0.04, 0.03)))
+        assert len(wps) >= 3
+        src = (
+            __import__("pathlib").Path(__file__).resolve().parents[1]
+            / "arm_application" / "overhead_oneshot.py"
+        ).read_text(encoding="utf-8")
+        assert "verticalize=False" in src
+        assert "oneshot_concat_hold_ori" in src
+
+    def test_oneshot_schedule_is_rest_to_rest_and_times_increase(self):
+        """一次下发的轨迹：头尾速度 0，时间递增。中途再插点就会把这条掐掉。"""
+        q0 = np.zeros(7)
+        q1 = np.zeros(7)
+        q1[0] = 0.3
+        sched, T_move, v_peak = trap_joint_schedule(
+            [q0, q1], 0.40, 0.08, 0.40
+        )
+        assert T_move > 4.0
+        assert v_peak == pytest.approx(0.08)
+        assert len(sched) > 8
+        assert float(np.linalg.norm(sched[0][1])) == pytest.approx(0.0, abs=1e-9)
+        assert float(np.linalg.norm(sched[-1][1])) == pytest.approx(0.0, abs=1e-9)
+        times = [t for _, _, t in sched]
+        assert times == sorted(times)
+        assert all(times[i] < times[i + 1] for i in range(len(times) - 1))
+        mid = sched[len(sched) // 3]
+        assert float(np.linalg.norm(mid[1])) > 0.01
+
+    def test_translation_holds_the_height_it_started_at(self):
+        """平移钉住开始时的高度。沉下去要抬回来，已经在 4mm 内就不要再收。"""
+        assert hold_translation_vz(0.327, 0.327, kp=1.0, v_lim=0.05) == 0.0
+        assert hold_translation_vz(0.325, 0.327, kp=1.0, v_lim=0.05) == 0.0
+        assert hold_translation_vz(0.307, 0.327, kp=1.0, v_lim=0.05) > 0.0
+        assert hold_translation_vz(0.347, 0.327, kp=1.0, v_lim=0.05) < 0.0
+        src = (
+            __import__("pathlib").Path(__file__).resolve().parents[1]
+            / "arm_application" / "servo_node.py"
+        ).read_text(encoding="utf-8")
+        assert "hold_translation_vz(" in src
+
     def test_threshold_is_the_caller_value_not_hardcoded(self):
         # 负向：如果函数内部写死 0.5，改门槛 0.7 时 0.6 会判错
         assert wrist_needs_handoff(0.6, 0.7) is True

src/arm_application/test/test_pipeline_stage1_diag.py

@@ -13,7 +13,7 @@ from __future__ import annotations
 
 import pytest
 
-from arm_application.pipeline_node import stage1_timeout_reason
+from arm_application.pipeline_node import stage1_timeout_reason, track_lock_z
 
 
 def test_zero_points_blames_perception_not_calibration():
@@ -40,3 +40,21 @@ def test_timeout_seconds_and_log_path_are_in_the_message():
     msg = stage1_timeout_reason(0, 42.0, "/tmp/x.log")
     assert "42s" in msg
     assert "/tmp/x.log" in msg
+
+
+def test_real_arm_locks_track_height_at_hover():
+    """真机 89：悬停时规尖已经离面约 75mm，跟踪锁在悬停高，对孔前不再抬。"""
+    z = track_lock_z(0.320, 0.223, 0.022)
+    assert z == pytest.approx(0.320)
+
+
+def test_sim_still_lifts_when_hover_barely_clears_the_part():
+    """仿真悬停只留约 2mm，不抬就会在跟踪时刮到工件。"""
+    z = track_lock_z(0.657, 0.243, 0.412)
+    assert z == pytest.approx(0.707)
+
+
+def test_missing_geometry_does_not_pretend_the_hover_is_clear():
+    """工具长或工件面缺失时必须抬升。缺数还锁在悬停高，等于没检查。"""
+    assert track_lock_z(0.320, 0.0, 0.022) == pytest.approx(0.370)
+    assert track_lock_z(0.320, 0.223, 0.0) == pytest.approx(0.370)

src/arm_application/test/test_stage2_descent_clamp.py

@@ -314,6 +314,24 @@ def test_goto_failure_stops_the_descent():
 # --------------------------------------------------------------------------- #
 # 不许回退到"把目标值当结果汇报"
 # --------------------------------------------------------------------------- #
+def test_no_descent_still_drops_to_the_observe_gap():
+    """--no-descent 不是停在悬停高：要降到观测间隙（15mm），方便看规尖偏没偏。
+
+    不许降到贴面 2mm —— 对偏了再贴面会把规顶在孔沿上。
+    """
+    st = _StubStage2()
+    st.final_descent_enable = False
+    st.observe_gap_m = 0.015
+    ok, Tee, st = _run(st)
+
+    assert ok is True
+    gap_mm = (Tee[2, 3] - _TOOL_LEN - _SURFACE_Z) * 1000
+    assert abs(gap_mm - 15.0) < 1.0, f"观测下降应停在离面 15mm，实际 {gap_mm:.1f}mm"
+    total_mm = sum(st.steps_mm)
+    assert total_mm < _BUDGET * 1000 - 5.0, "观测下降走到了贴面预算，等于把 --no-descent 吃掉了"
+    assert any("观测下降完成" in m for m in st.logs), "日志要写明这是观测下降，不是贴面轻降"
+
+
 def test_wired_final_gap_is_reported_from_geometry_not_target():
     """收尾要汇报**几何实算**的间隙，不能把目标值（2.0mm）念一遍。
 

src/arm_application/test/test_stage2_tip_align.py

@@ -27,6 +27,7 @@ from __future__ import annotations
 from typing import Any, Dict, Optional, Tuple
 
 import numpy as np
+import pytest
 
 from arm_application.stage2_locate_node import Stage2Locate
 
@@ -101,9 +102,9 @@ class _Stub:
         return _R_CB @ np.asarray(p_cam, float) + self._t_cb
 
 
-def _call(st: _Stub, depth: np.ndarray):
+def _call(st: _Stub, depth: np.ndarray, z_wp_cam: Optional[float] = None):
     return Stage2Locate._tip_offset_from_depth(  # type: ignore[arg-type]
-        st, depth, _INTR, _TEE
+        st, depth, _INTR, _TEE, z_wp_cam=z_wp_cam
     )
 
 
@@ -143,6 +144,129 @@ def test_offset_is_stable_frame_to_frame():
 # --------------------------------------------------------------------------- #
 # 判据各自都要能真的失败（负向用例）
 # --------------------------------------------------------------------------- #
+def _gripper_and_rod_frame() -> np.ndarray:
+    """爪子块头大且更近 + 规杆更细更深：复现 2026-10-09 真机『按面积选爪子』。"""
+    depth = np.full((480, 800), 0.50, np.float32)
+    depth[20:90, 330:470] = 0.07  # 爪子：宽、浅、画面上方
+    for v in range(90, 280):
+        f = min(1.0, (v - 90) / 200.0)
+        depth[v, 392:408] = 0.08 + f * 0.05  # 规杆：细、伸向工件
+    return depth
+
+
+def test_picks_the_lower_tip_not_the_config_length_blob():
+    """画面上方、工具长刚好贴着配置的那一块不是规尖。
+
+    2026-10-09 真机：最接近配置 223mm 的点工具长 225mm、偏移约 0，冻住的是
+    法兰轴线。残差 0.14mm 达标，下降后规尖仍偏出约 10mm。规尖在画面更靠下。
+    """
+    depth = np.full((480, 800), 0.50, np.float32)
+    depth[40:100, 370:430] = 0.080  # 靠上、居中，工具长贴着配置
+    for v in range(200, 301):
+        f = min(1.0, (v - 200) / 200.0)
+        depth[v, 442:458] = 0.10 + f * 0.02  # 靠下、偏出中心
+    st = _Stub()
+    st.tool_tip_below_flange_m = 0.185
+    off, tlen, why = _call(st, depth)
+    assert off is not None and tlen is not None, f"有规尖却没量到：{why}"
+    v = float(why.split("px=(")[1].split(")")[0].split(",")[1])
+    assert v > 180, f"采到的是贴着配置工具长的上方点，不是规尖：{why}"
+    assert abs(float(off[0])) > 0.005, (
+        f"规尖偏出中心，X 偏移不该≈0（那是法兰轴）：{why} off={off}"
+    )
+
+
+def test_picks_rod_not_larger_gripper():
+    """中央竖带里爪子面积更大时，必须仍选**更深的规杆**，不能按连通域面积。"""
+    depth = _gripper_and_rod_frame()
+    off, tlen, why = _call(_Stub(), depth)
+    assert off is not None and tlen is not None, f"有规杆却没量到：{why}"
+    assert "px=" in why
+    # 规尖在杆下端 v≈270，爪子在 v≈20–90；采到爪子 v 会 <100
+    v = float(why.split("px=(")[1].split(")")[0].split(",")[1])
+    assert v > 150, f"采到的是爪子不是规尖：{why}"
+
+
+def test_workpiece_ceiling_keeps_the_real_tip():
+    """同一帧工件面深度必须抬高近端上限，否则 150mm 会切掉真规尖。
+
+    2026-10-09 真机：孔 camera-Z ≈168mm，固定 tip_near=150mm 把尖切掉，
+    像素在 v=80（杆上半）和 v=188（尖偶尔漏进来）两档跳，6 拍冻不住。
+    上限要留在工件面前方（8mm）：贴着面的像素是工件，不是规。
+    再往前砍 15mm 会把只比面近十来毫米的真尖切掉。
+    """
+    depth = np.full((480, 800), 0.50, np.float32)
+    for v in range(80, 200):
+        depth[v, 392:408] = 0.08 + (v - 80) / 120.0 * 0.06  # →0.14，在 150mm 内
+    for v in range(200, 280):
+        depth[v, 392:408] = 0.14 + (v - 200) / 80.0 * 0.024  # →0.164，会被 150mm 切掉
+    depth[300:400, 200:600] = 0.172  # 工件面；近端上限 = 面 − 15mm
+    cut, _t0, why0 = _call(_Stub(), depth)  # 无 z_wp → 固定 150mm
+    full, tlen, why = _call(_Stub(), depth, z_wp_cam=0.172)
+    assert full is not None and tlen is not None, f"给了工件面仍没量到尖：{why}"
+    v_full = float(why.split("px=(")[1].split(")")[0].split(",")[1])
+    assert v_full > 200, f"有工件面上限仍没采到杆下端：{why}"
+    if cut is not None:
+        v_cut = float(why0.split("px=(")[1].split(")")[0].split(",")[1])
+        assert v_full > v_cut + 20, (
+            f"工件面上限没有比固定 150mm 采到更靠下的尖：cut={why0} full={why}"
+        )
+
+
+def test_keeps_the_tip_when_it_is_only_slightly_in_front_of_the_surface():
+    """规尖只比工件面近十来毫米时，必须采尖，不能退回更近的夹爪。
+
+    2026-10-09 观测高度实测：工件面相机深度 127mm，规尖 115mm，夹爪/杆身 80mm。
+    近端砍到「面 − 15mm」会把尖切掉，冻住的是杆身（偏移约 2mm、工具长贴着
+    配置 223mm）。残差 0.2mm 达标，针却偏在孔沿上。
+    """
+    depth = np.full((480, 800), 0.50, np.float32)
+    depth[140:190, 360:430] = 0.080  # 夹爪：更近、更靠上
+    depth[210:236, 430:470] = 0.118  # 规尖：只比面近 12mm，画面更靠下
+    depth[250:420, 250:600] = 0.130  # 工件面
+    off, tlen, why = _call(_Stub(), depth, z_wp_cam=0.130)
+    assert off is not None and tlen is not None, f"只比面近 12mm 的尖被切掉了：{why}"
+    v = float(why.split("px=(")[1].split(")")[0].split(",")[1])
+    assert v > 200, f"采到的是夹爪不是规尖：{why}"
+    assert abs(float(off[0])) > 0.005, (
+        f"尖偏出中心，不该量成法兰轴：{why} off={off}"
+    )
+
+
+def test_rejects_pixels_stuck_to_the_workpiece():
+    """贴着工件面的近端块必须拒绝 —— 那是面，不是规尖。
+
+    2026-10-09 重跑：上限只让 3mm，噪声把工件漏进来，工具长测成 296–315mm
+    （规实际约 190mm），样本互斥，随后两孔中点一跳就被当成工件被推走。
+    """
+    depth = np.full((480, 800), 0.50, np.float32)
+    depth[200:420, 360:440] = 0.170  # 大块、深度贴着 0.172 的工件面
+    off, tlen, why = _call(_Stub(), depth, z_wp_cam=0.172)
+    assert off is None and tlen is None, f"贴着工件面的块被当成规尖：{why}"
+    assert "工件" in why
+
+
+def test_rejects_tip_that_lands_on_the_workpiece_surface():
+    """反投影落在工件面上的点必须丢掉，哪怕相机深度还在近端上限之内。
+
+    2026-10-09：相机深度留了 15mm，工具长仍测成 298mm（配置 223mm），
+    基座 z 贴在工件面上。偏移冻不住 → exit=8 → 观测下降根本不会执行。
+    """
+    st = _Stub()
+    st.surface_z_m = 1.0  # 高于任何反投影点
+    off, tlen, why = _call(st, _rod_frame())
+    assert off is None and tlen is None, f"贴在工件面上的点被当成规尖：{why}"
+    assert "工件" in why
+
+
+def test_keeps_tip_that_clears_the_workpiece_surface():
+    """规尖明显高于工件面时，表面守卫不许把真规尖一起丢掉。"""
+    st = _Stub()
+    st.surface_z_m = 0.05
+    off, tlen, why = _call(st, _rod_frame())
+    assert off is not None and tlen is not None, f"离面足够却被拒：{why}"
+
+
 def test_rejects_when_nothing_is_near():
     """近端什么都没有（规不在视野）⇒ 必须判失败，不能返回一个假偏移。"""
     depth = np.full((480, 800), 0.50, np.float32)  # 全在远端
@@ -301,8 +425,113 @@ def test_default_is_tip_vision_on_in_site():
         (Path(__file__).resolve().parents[3] / "config" / "site.yaml").read_text(encoding="utf-8")
     )
     st2 = ((site.get("hole_detect") or {}).get("stage2") or {})
+    robots = ((site.get("robot") or {}).get("robots") or [])
+    arm89 = next(r for r in robots if str(r.get("sn")) == "Rizon4-063989")
+    servo = arm89.get("servo") or {}
+    assert float(servo.get("ee_z_m")) == pytest.approx(
+        float(servo.get("hole_hover_z_m"))
+    ), "左臂跟踪锁高必须等于孔悬停高：对孔完成前不许改高度"
     assert st2.get("align_tip_vision") is True, "默认必须用视觉规尖"
     assert float(st2.get("align_tol_m")) <= 0.00025 + 1e-9, (
         f"align_tol_m={st2.get('align_tol_m')} 太松：会放过系统性的对孔偏差"
     )
     assert int(st2.get("align_max_iters")) >= 12, "收紧容差后迭代上限要跟上"
+
+
+class _TrimStub:
+    """下降后 XY 修正用到的成员。孔在基座里不动，规尖偏移是常数。"""
+
+    tip_vision = True
+    k1_label = "left"
+    target_hole = "k1"
+    align_tol = 0.00025
+    align_speed = 0.02
+    vert_tol_rad = 0.02
+    base_frame = "world"
+    ee_frame = "flange"
+    _tip_off = np.array([0.01, 0.0])
+    _tip_samp: list = []
+    _tip_len_m = 0.22
+
+    def __init__(self, err_xy=(0.0026, -0.0004), tip=None) -> None:
+        self.hole = np.array([0.50 + err_xy[0], -0.10 + err_xy[1], 0.022])
+        self.tip = None if tip is False else np.array([0.0015, -0.0012])
+        self.moves: list = []
+        self._logger = _StubLogger()
+
+    def get_logger(self):
+        return self._logger
+
+    def _lookup(self, *_a):
+        return None
+
+    def _pin(self, _q) -> None:
+        return None
+
+    def _snap_holes(self, *, skip_center_gate: bool):
+        res = {
+            "status": True,
+            "hole_base": [self.hole.tolist(), (self.hole + np.array([0.0, -0.060, 0.0])).tolist()],
+            "z_wp": 0.127,
+        }
+        return None, None, {}, res
+
+    def _tip_offset_from_depth(self, *_a, **_kw):
+        if self.tip is None:
+            return None, None, "没看到规"
+        return self.tip.copy(), 0.222, "px=(447,226)"
+
+    def _goto(self, _q, Tee, target, **_kw):
+        Tee = np.array(Tee, float)
+        self.moves.append(np.array(target, float))
+        Tee[0, 3] = float(target[0])
+        Tee[1, 3] = float(target[1])
+        Tee[2, 3] = float(target[2])
+        return True, _q, Tee
+
+
+def _trim(st: _TrimStub, z: float = 0.260):
+    import rclpy
+
+    if not rclpy.ok():
+        rclpy.init()
+    Tee = np.eye(4)
+    Tee[0, 3] = 0.50
+    Tee[1, 3] = -0.10
+    Tee[2, 3] = z
+    final = {"ok": True, "residual_mm": 0.22, "gauge_source": "视觉规尖(已冻结)"}
+    return Stage2Locate._trim_xy_after_descent(st, np.zeros(7), Tee, final)  # type: ignore[arg-type]
+
+
+def test_trim_after_descent_corrects_xy_and_holds_z():
+    """下降后孔位漂了 2mm 级，要在当前高度补上，不许改高度。
+
+    2026-10-09：悬停残差 0.22mm，降到观测高度后同一孔的基座 XY 差约 2.6mm。
+    """
+    st = _TrimStub()
+    _q, Tee, final = _trim(st)
+    assert abs(float(Tee[2, 3]) - 0.260) < 1e-9, "下降后修正不许改高度"
+    tip = np.array([Tee[0, 3] + st.tip[0], Tee[1, 3] + st.tip[1]])
+    err = float(np.hypot(tip[0] - st.hole[0], tip[1] - st.hole[1]))
+    assert err <= 0.00025 + 1e-6, f"补完针还偏 {err*1000:.2f}mm"
+    assert final["residual_mm"] <= 0.25
+    assert "下降后" in final["gauge_source"]
+
+
+def test_trim_refuses_a_centimeter_scale_correction():
+    """要补的量到了厘米，就是采到夹爪了，必须停在原地。"""
+    st = _TrimStub(err_xy=(0.014, 0.0))
+    _q, Tee, final = _trim(st)
+    assert not st.moves, "厘米级修正被执行了"
+    assert abs(float(Tee[0, 3]) - 0.50) < 1e-9
+    assert final["residual_mm"] == 0.22, "拒绝修正时不能把悬停结果改写成没补上的残差"
+    assert any("不像针" in m for m in st._logger.msgs)
+
+
+def test_trim_does_not_move_when_the_tip_is_missing():
+    """下降后没量到规尖，保持悬停对准的位置。"""
+    st = _TrimStub(tip=False)
+    _q, Tee, final = _trim(st)
+    assert not st.moves
+    assert abs(float(Tee[0, 3]) - 0.50) < 1e-9
+    assert final["gauge_source"] == "视觉规尖(已冻结)"

src/arm_driver_flexiv/resource/arm_driver_flexiv

@@ -0,0 +1 @@
+

src/arm_kinematics/resource/arm_kinematics

@@ -0,0 +1 @@
+

src/arm_sim_gazebo/resource/arm_sim_gazebo

@@ -0,0 +1 @@
+