工位操作改成桌面窗口：夹爪位置、力度、速度和旋转角可以调，阶段能分步或整体跑，并能软拖拽记点后回到该点。相机预览按窗口一次铺满，不再跟着每帧慢慢放大。

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

config/site.example.yaml

@@ -590,10 +590,12 @@ 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
+  # 混合模式必填。交回 D405 的等待不再用它，见 d435_handback_sec。
-  # 顶视一次性把臂迁过去的那段路不算：必须是腕部有效目标。大幅一动就重计时。
   lock_still_sec: 3.0
-  wrist_align_sec: 1.0
+  # 进程内 MoveL 到位后，法兰 XY 连续停稳这么久才交回 D405。
+  d435_handback_sec: 0.5
+  # 框心对准并且法兰停稳这么久，才精拍一次小孔关键点、进对孔。实时点到此为止。
+  wrist_align_sec: 0.5
   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
@@ -768,7 +770,13 @@ hole_detect:
     align_speed: 0.014       # 对准段笛卡尔速度 [m/s]（原0.03，放缓防冲）
     move_speed: 0.022        # 升降/粗居中段速度 [m/s]（原0.05，放缓防震规）
     hole_visual_only: true
-    final_gap_mm: 2.0        # 对准后贴面轻降目标：规尖悬在孔面上方这么高 [mm]（不接触）
+    # 本分支实验：true=悬停拍一次并走完到孔；main 保存点应保持 false。
+    hole_keypoint_once: true
+    # true=悬停走完后一条厂家 MoveL 贴面。对照观察高补交时改 false。
+    hole_once_lock_xy_descend: true
+    # 0=笛卡尔锁 XY 只降 Z。k>0 才沿 -look 加前馈。
+    once_ff_look_per_m: 0.0
+    final_gap_mm: 2.0        # 对准后贴面目标：规尖离面 [mm]。CLI --final-gap 覆盖。
     observe_gap_mm: 15.0     # --no-descent 时的观测下降：只降到离面这么高 [mm]，
                              # 方便看规尖偏没偏。必须 ≥ final_gap_mm。
                              # 左臂89：悬停间隙约 75mm（0.320−0.223−0.022）⇒ 大约下降 60mm。

config/site.yaml

@@ -577,11 +577,13 @@ servo:
   deadzone_m: 0.003          # 3 mm；收紧提高对准精度。若抖动调回 0.006
   deadzone_z_m: 0.012
   lock_hold_sec: 0.7        # 旧死区驻留（混合模式不再用它进二阶段）
-  # D435 到位之后，法兰 XY 连续这么久位移不超过 lock_still_move_m 才算停稳、交回 D405。
+  # 混合模式必填。交回 D405 的等待不再用它，见 d435_handback_sec。
-  # 顶视一次性把臂迁过去的那段路不算：必须是腕部有效目标。大幅一动就重计时。
   lock_still_sec: 3.0
-  # D405 改追工件框中心之后，连续这么久才进对孔。不要和上面的停稳写成同一个数。
+  # 进程内 MoveL 到位后，法兰 XY 连续停稳这么久才交回 D405。
-  wrist_align_sec: 1.0
+  # 旧 3s 是杀栈冷启动的垫时间；不杀栈后只留机械停稳。晃了把数加大，不许再杀栈。
+  d435_handback_sec: 0.5
+  # 框心对准并且法兰停稳这么久，才精拍一次小孔关键点、进对孔。实时点到此为止。
+  wrist_align_sec: 0.5
   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
@@ -757,7 +759,16 @@ hole_detect:
     move_speed: 0.022        # 升降/粗居中段速度 [m/s]（原0.05，放缓防震规）
     # 对小孔关键点：只用像素射线交到 workpiece_surface_z_m，不读深度图。
     hole_visual_only: true
-    final_gap_mm: 2.0        # 对准后贴面轻降目标：规尖悬在孔面上方这么高 [mm]（不接触）
+    # 本分支实验（exp/hole-keypoint-once）：true=悬停拍一次，框心→k1 一次走完
+    # （40mm 上限）。main 保存点是 false（多走多拍）。
+    hole_keypoint_once: true
+    # true=悬停走完后一条厂家 MoveL 贴面（不补拍）。对照 37b85aa 改回 false。
+    hole_once_lock_xy_descend: true
+    # 贴面相对悬停：沿 -look 走 k×剩余下降 [m/m]。0=笛卡尔锁 XY 只降 Z。
+    # 0.018 在框心改平面交点后两条链路都 +1.3mm X 过补，插不进。
+    once_ff_look_per_m: 0.0
+    final_gap_mm: 2.0        # 对准后贴面目标：规尖离面 [mm]（不接触）。笛卡尔锁 XY
+                             # 时法兰 Z = 工具长 + 工件面 + 本值。CLI --final-gap 覆盖。
     observe_gap_mm: 15.0     # --no-descent 时的观测下降：只降到离面这么高 [mm]，
                              # 方便看规尖偏没偏。必须 ≥ final_gap_mm。
                              # 左臂89：悬停间隙约 73.5mm（0.320−0.2245−0.022）⇒ 观测下降约 58mm。

docs/HANDBOOK.md

@@ -66,18 +66,29 @@ run_all.sh
 bash ~/ros2/scripts/run_real.sh --arm 89                    # T1 控制栈，一直开着
 bash ~/ros2/scripts/run_camera_d405.sh --arm 89 --preview   # T2 相机预览，一直开着
 bash ~/ros2/scripts/run_all.sh --arm 89                     # T3 三段串联（默认不拧入）
+bash ~/ros2/scripts/run_all.sh --arm 89 --init-z 0.35 --hover-z 0.34
 ```
 
 一阶段默认 **D405 实时推理**，D435 待命。YOLO-pose 这一路不关。连续
 `D405_MISS_BEFORE_D435_S`（0.5 s）没有 conf≥`perception.wrist_handoff_conf`（0.5）
 的目标，才用顶视 D435 **拍一张**，把点用眼在手外 T 变到基座（不再乘法兰）。
-这一段运动是厂家 `MoveL`：先放开 `ros2_control` 占着的 RDK，XY 对准该点，
+这一段运动是进程内厂家 `MoveL`（不杀 `run_real.sh`）：当前厂家 `tcp_pose` 加
-工具尖高度保持当前（点的 z 是工件表面），不按直线距离拒绝；走完再用
+ROS 相对 XY，`keep_z`。硬件在 MoveL 期间不灌关节指令。到位后法兰 XY 连续停稳
-`run_real.sh --no-stop` 把控制栈拉回来（不扫感知进程）。走到该点并且法兰 XY
+`servo.d435_handback_sec`（0.5 s）才交回 D405。赶路期间不下发 D405（两套控制互抢会晃）。
-连续 `servo.lock_still_sec`（3 s）位移不超过 `lock_still_move_m`（15 mm）之前，
+停稳之后改回 D405，跟踪点是工件框的矩形中心，不追 k0。
-D405 的点不下发。停稳之后改回 D405，跟踪点是工件框的矩形中心，不追 k0。
+启动高度用 `--init-z`（法兰 Z，米），对孔悬停用 `--hover-z`。两个都是法兰高度。
-框心对准连续 `servo.wrist_align_sec`（1 s）→ `/visual_servo/locked` → 二阶段对小孔。
+下臂贴面用 `--final-gap`（规尖离面，毫米；site 默认 `final_gap_mm` 2）。笛卡尔锁 XY
-对孔只用关键点像素交到 `workpiece_surface_z_m`，不读深度图。下臂停在
+时只改 Z：法兰 = 工具长 + 工件面 + 间隙。不能和 `--no-descent` 一起写。
+会让工具尖进入工件、或超出工作盒的值直接拒绝，不会改成另一个高度再往下走。
+不写则一阶段锁高仍按 site，对孔悬停仍是 `hole_hover_z_m`（89 为 0.320）。
+框心对准连续 `servo.wrist_align_sec`（0.5 s）→ `/visual_servo/locked`。
+本分支（`exp/hole-keypoint-once`）二阶段是**单次精拍**：悬停拍一次 k1/k2，
+把框心到目标孔一次走完（步长上限 40mm）。现行实验 `hole_once_lock_xy_descend: true`：
+走完后不补拍，一条厂家 MoveL 收到贴面。`once_ff_look_per_m: 0` 时 MoveL 只降 Z
+（笛卡尔锁 XY）；k>0 才沿 -look 加前馈。目标 = 当前厂家 `tcp_pose` + 相对位移，
+不是 ROS 绝对坐标。保存点 `37b85aa` 是观察高 15mm 再斜线补 XY。看 `/tmp/stage2_result.json`。
+`main` 保存点仍是多走多拍。
+孔坐标把关键点像素交到 `workpiece_surface_z_m`，不读深度图。下臂停在
 `tool10120938` 的 TCP 长度（0.2245 m）加上 `final_gap_mm` 之上，不按旧 tool1 的 0.223 m。
 只测 D435 这一段位移时加 `--d435-only`：不交 D405、不进二阶段，并且要求先停 `run_real.sh`。
 

docs/HOLE_DETECT.md

@@ -99,8 +99,25 @@ bash ~/ros2/scripts/run_all.sh --arm 89 --depth 6 --pitch 1.0
 ## 二阶段一次性拍照定位（stage2，与一阶段跟踪分开跑）
 
 先悬停垂直朝下 → 拍一张照 → `detect_best_1.onnx` 框工件 → Hough 解 k1/k2 孔心 →
-深度（孔外工作面中值）+ 内参 + 手眼换算基座坐标 → 移动**规尖**到目标孔正上方 →
+像素射线交到 `workpiece_surface_z_m`（不读深度图）+ 内参 + 手眼换算基座坐标 →
-复拍验证（≤`align_max_iters` 次，收敛判据 `align_tol_m`）→ 锁定保持。
+移动**规尖**到目标孔正上方。`hole_visual_only` 时粗居中框心也走同一条平面交点，
+不用深度（30° 相机下深度框心会让 `K1.x−规尖.x` 随站位翻号）。
+
+本分支 `hole_keypoint_once: true`：悬停拍一次，把框心到 k1 的半孔距（约 30mm）
+一次走完（不再用多走多拍的 12mm 封顶）。
+
+现行实验 `hole_once_lock_xy_descend: true`：走完后不补拍，一条进程内厂家
+MoveL 收到 `final_gap_mm`（规尖离面，毫米；`--final-gap` 覆盖，默认 2）。
+`once_ff_look_per_m: 0` 时 ΔXY=0、只降 Z（笛卡尔锁 XY）。k>0 才
+`ΔXY = -look × k × 剩余下降`。法兰 Z = 工具长 + 工件面 + 间隙，不许直接写法兰高度。MoveL 目标是**当前厂家 `tcp_pose` 加
+相对位移**，不是 ROS 规尖绝对坐标。
+2026-10-14：k=0.018 在框心改平面交点后，D435+D405 与单 D405 下臂都往 +X 过补
+约 1.3mm、插不进——悬停 X 已对上，常数前馈是多余的。锁 XY 折线失败是关节 hop
+漂 XY，不是笛卡尔 MoveL 锁 XY。对照 `37b85aa`：观察高约 15mm 再交孔斜线补 XY。
+前馈超过 8mm 或工作盒会截断则不动。没有新鲜 `tcp_pose` 则不下发。
+
+轻降力接触门控是 `final_gap_mm + 4mm`（贴面 2mm 时 = 6mm）。看
+`/tmp/stage2_result.json` 的 `snaps[]`。`main` 保存点是多走多拍闭环。
 
 > **对的是"规尖"，不是"法兰轴线"**（2026-10-08 定稿）。通止规夹在爪里，相对法兰轴线
 > 可以整体偏出若干毫米；旧写法把**法兰**对到孔上，规尖就偏着压在孔沿上，轻降一压
@@ -119,6 +136,7 @@ bash ~/ros2/scripts/run_stage2_locate.sh --arm 89 --hover-only   # 只垂直+悬
 bash ~/ros2/scripts/run_stage2_locate.sh --arm 89 --hole k2      # 对准 k2（默认 k1）
 bash ~/ros2/scripts/run_stage2_locate.sh --arm 89 --hover-z 0.28 # 临时改悬停高，不动 yaml
 bash ~/ros2/scripts/run_all.sh --arm 89 --no-descent             # 只验证精对准、不轻降
+bash ~/ros2/scripts/run_all.sh --arm 89 --final-gap 8            # 锁 XY，规尖停在离面 8mm
 ```
 
 悬停锁高接口：`site.yaml → robots[89].servo.hole_hover_z_m`（91 在同名段）。

handeye/poses/89_eye_to_hand.yaml

@@ -7,54 +7,48 @@
 
 /**:
   ros__parameters:
-    num_poses: 24
+    num_poses: 21
     waypoint_positions: [
       # pose 0
-      -1.241563, -0.359631, 0.346153, 2.172565, -0.179027, 0.929920, -0.778876,
+      -0.798667, -0.611079, 0.054912, 2.246988, 0.049199, 1.389219, 0.196135,
       # pose 1
-      -1.388832, -0.554698, 0.416075, 1.897617, -0.358341, 0.827495, -0.762302,
+      -0.787304, -0.531383, 0.240108, 2.183440, 0.367026, 1.370311, 0.472489,
       # pose 2
-      -1.436048, -0.573046, 0.461762, 1.870967, -0.387696, 0.813090, -0.762917,
+      -1.615896, -0.687070, 1.187600, 2.243693, -0.162789, 1.650045, 1.026121,
       # pose 3
-      -1.614491, -0.519365, 0.693991, 2.156193, -0.499113, 0.968762, 0.073287,
+      -1.787431, -0.668004, 1.408195, 2.025431, -0.064718, 0.565415, 1.324635,
       # pose 4
-      -2.020087, -0.869088, 1.059166, 2.155478, -1.198473, 0.740567, 0.478840,
+      -2.024343, -0.863793, 1.349640, 2.359469, -0.720629, 0.084467, 0.821412,
       # pose 5
-      -2.082145, -1.122210, 1.203011, 1.950272, -1.269847, 0.434138, -0.641975,
+      -1.884100, -1.117584, 1.183093, 2.127937, -1.258013, 0.151934, 0.985555,
       # pose 6
-      -1.983153, -0.886082, 0.999329, 2.324821, -1.272773, 0.754127, 0.313613,
+      -1.866465, -1.194137, 1.143263, 1.872811, -1.320028, 0.280719, 0.777663,
       # pose 7
-      -2.021479, -0.886609, 1.055679, 2.337716, -1.282773, 0.669319, -0.028355,
+      -1.729402, -1.139685, 1.127574, 1.913544, -1.329630, -0.194162, 1.383834,
       # pose 8
-      -2.022792, -0.947211, 1.118194, 2.127275, -1.211995, 0.527528, -0.169900,
+      -1.611937, -1.158158, 1.064049, 1.918313, -1.480676, -0.392994, 1.833894,
       # pose 9
-      -2.004766, -1.065820, 1.183799, 1.901084, -1.217488, 0.380285, -0.312733,
+      -2.252978, -1.387014, 1.686186, 2.101298, -1.333349, -0.111994, 0.036817,
       # pose 10
-      -1.942863, -1.219617, 1.256733, 1.653113, -1.222302, 0.261818, -0.420706,
+      -2.179722, -1.297659, 1.497171, 2.167675, -1.272158, 0.100548, 0.673603,
       # pose 11
-      -2.039558, -1.082441, 1.189833, 1.996967, -1.241252, 0.521591, 0.526563,
+      -1.921897, -1.543024, 1.000863, 1.897273, -1.455972, 1.437388, -0.140968,
       # pose 12
-      -2.098074, -1.006236, 1.538183, 2.118382, -0.737422, 0.019903, 0.648802,
+      -1.884059, -1.063347, 1.439248, 2.253200, -1.164963, -0.423306, 0.907197,
       # pose 13
-      -2.039609, -1.159222, 1.488501, 1.908579, -1.168793, -0.140451, 0.034889,
+      -1.987022, -1.235459, 1.354108, 2.370862, -1.354044, 0.065378, 0.923079,
       # pose 14
-      -1.880783, -1.104601, 1.579382, 2.036427, -0.665049, 0.254105, 1.518504,
+      -1.770869, -1.333861, 1.241996, 1.800723, -1.395819, -0.255590, 1.389660,
       # pose 15
-      -2.015530, -1.535468, 1.711220, 1.836908, -1.707450, -0.400234, -0.708683,
+      -1.761759, -1.369860, 1.213137, 1.791639, -1.467956, -0.170537, 1.473557,
       # pose 16
-      -1.997672, -1.626362, 1.529443, 2.200206, -1.291013, 0.398664, 1.954208,
+      -1.504294, -1.346227, 1.206550, 1.660186, -1.320782, -0.393676, 1.657215,
       # pose 17
-      -1.981875, -1.620651, 1.545443, 2.373140, -1.167365, 0.396487, 2.012547,
+      -1.846546, -1.476930, 0.946255, 1.910615, -2.435429, 1.535168, 2.151544,
       # pose 18
-      -2.179415, -1.511968, 1.578832, 2.401706, -1.326480, -0.051353, 1.137181,
+      -1.700067, -1.452760, 0.909035, 1.758222, -2.103930, 1.445249, 2.174743,
       # pose 19
-      -2.173463, -1.530413, 1.575534, 2.173673, -1.289673, -0.090117, 0.867214,
+      -2.173713, -0.983647, 1.399849, 2.384939, -1.379509, -0.216471, 0.253561,
       # pose 20
-      -2.096314, -1.569352, 1.579820, 1.942057, -1.458598, -0.057003, 0.572953,
+      -2.160187, -0.967904, 1.376288, 2.451713, -1.393524, -0.212278, 0.473606,
-      # 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,
     ]

scripts/lib/export_site_env.py

@@ -903,6 +903,7 @@ def servo_overrides(data: dict) -> dict:
         "ee_hold_kp", "ee_w_max",
         "lock_hold_sec",
         "lock_still_sec",
+        "d435_handback_sec",
         "wrist_align_sec",
         "lock_still_move_m",
         "a_max", "max_qdot", "damping",
@@ -934,8 +935,13 @@ def servo_overrides(data: dict) -> dict:
         out["overhead_hold_s"] = float(oh["oneshot_hold_s"])
         if "lock_still_sec" not in servo:
             raise RuntimeError(
-                "servo.lock_still_sec 必须显式写"
+                "servo.lock_still_sec 必须显式写。"
-                "（D435 到位后法兰停稳多久才交回 D405）。不许用代码默认 3。"
+                "不许用代码默认秒数。"
+            )
+        if "d435_handback_sec" not in servo:
+            raise RuntimeError(
+                "servo.d435_handback_sec 必须显式写"
+                "（厂家 MoveL 到位后，再确认多久才交回 D405）。不许用代码默认秒数。"
             )
         if "wrist_align_sec" not in servo:
             raise RuntimeError(
@@ -948,6 +954,7 @@ def servo_overrides(data: dict) -> dict:
                 "（静止判据的法兰 XY 位移上限）。不许用代码默认 15mm。"
             )
         out["lock_still_sec"] = float(servo["lock_still_sec"])
+        out["d435_handback_sec"] = float(servo["d435_handback_sec"])
         out["wrist_align_sec"] = float(servo["wrist_align_sec"])
         out["lock_still_move_m"] = float(servo["lock_still_move_m"])
         out["overhead_k0_topic"] = "visual_servo/overhead_k0"

scripts/lib/flexiv_movel_ipc.py

@@ -0,0 +1,66 @@
+"""伺服和 flexiv_hardware 之间的 MoveL 交接。
+
+两边不在一个进程里，硬件接口也没有自己的 ROS 节点。用三个文件：
+
+* ``/tmp/flexiv_movel_req``  一行 ``seq x y z keep_z vel``
+* ``/tmp/flexiv_movel_ack``  硬件接住请求后立刻写序号
+* ``/tmp/flexiv_movel_done`` 一行 ``seq ok`` 或 ``seq fail 原因``
+
+先写临时文件再改名，避免对面读到半行。
+"""
+
+from __future__ import annotations
+
+from pathlib import Path
+
+REQ = Path("/tmp/flexiv_movel_req")
+ACK = Path("/tmp/flexiv_movel_ack")
+DONE = Path("/tmp/flexiv_movel_done")
+
+
+def format_request(seq: int, x: float, y: float, z: float, keep_z: bool, vel: float) -> str:
+    return (
+        f"{int(seq)} {float(x):.6f} {float(y):.6f} {float(z):.6f} "
+        f"{1 if keep_z else 0} {float(vel):.4f}\n"
+    )
+
+
+def write_request(seq: int, x: float, y: float, z: float, keep_z: bool, vel: float) -> None:
+    text = format_request(seq, x, y, z, keep_z, vel)
+    tmp = REQ.with_name(REQ.name + ".tmp")
+    tmp.write_text(text, encoding="utf-8")
+    tmp.replace(REQ)
+
+
+def parse_done(text: str):
+    """返回 ``(seq, ok, detail)``。行不完整时返回 None。"""
+    parts = text.strip().split(maxsplit=2)
+    if len(parts) < 2:
+        return None
+    try:
+        seq = int(parts[0])
+    except ValueError:
+        return None
+    if parts[1] not in ("ok", "fail"):
+        return None
+    detail = parts[2] if len(parts) > 2 else ""
+    return seq, parts[1] == "ok", detail
+
+
+def read_done():
+    try:
+        text = DONE.read_text(encoding="utf-8")
+    except OSError:
+        return None
+    return parse_done(text)
+
+
+def read_ack() -> int:
+    try:
+        text = ACK.read_text(encoding="utf-8").strip()
+    except OSError:
+        return -1
+    try:
+        return int(text.split()[0])
+    except (ValueError, IndexError):
+        return -1

scripts/rdk_drag_bridge.py

@@ -33,6 +33,7 @@ RDK 的 FloatingJoint/FloatingCartesian 通常需要额外授权。
     {"cmd":"drag_on","mode":"soft"}               # soft | floating
     {"cmd":"drag_off"}
     {"cmd":"joints"}
+    {"cmd":"move_joints","q":[0,0,0,0,0,0,0],"speed":0.15}
     {"cmd":"quit"}
 
 每个命令的应答都是单行 JSON，含 ``ok`` 字段；失败时另有 ``error``。
@@ -169,6 +170,69 @@ def _ensure_operational(robot) -> Optional[str]:
     return ensure_operational(robot)
 
 
+def _restore_tracking_impedance(robot, flexivrdk) -> None:
+    """把关节阻抗从软拖拽拉回名义值，否则回放点位时臂跟不住指令。"""
+    try:
+        info = robot.info()
+        groups = dict(getattr(info, 'single_arm_groups', None) or {})
+    except Exception as exc:
+        log(f'读名义阻抗失败: {exc}')
+        groups = {}
+    if groups:
+        for group in groups:
+            try:
+                nominal = [float(v) for v in list(info.K_q_nom[group])]
+                robot.SetJointImpedance(group, nominal)
+                log(f'恢复阻抗 {group} K={nominal}')
+            except Exception as exc:
+                log(f'恢复阻抗 {group}: {exc}')
+        return
+    try:
+        robot.SetJointImpedance([350.0] * 7, [7.0] * 7)
+    except Exception as exc:
+        log(f'恢复阻抗: {exc}')
+
+
+def _send_joint_target(robot, flexivrdk, target: List[float], grouped: Optional[bool]) -> bool:
+    """下发 7 关节目标。返回是否走了分组接口。"""
+    if grouped is True:
+        _send_grouped_joint_target(robot, flexivrdk, target)
+        return True
+    try:
+        n = len(target)
+        robot.SendJointPosition(target, [0.0] * n, [1.5] * n, [2.0] * n)
+        return False
+    except Exception as exc:
+        if grouped is False:
+            raise
+        log(f'扁平关节指令失败，改分组: {exc}')
+        _send_grouped_joint_target(robot, flexivrdk, target)
+        return True
+
+
+def _send_grouped_joint_target(robot, flexivrdk, target: List[float]) -> None:
+    info = robot.info()
+    groups = dict(getattr(info, 'single_arm_groups', None) or {})
+    if not groups:
+        raise RuntimeError('没有分组，扁平关节指令也失败了')
+    states = robot.states()
+    cmds = {}
+    placed = False
+    for group in groups:
+        try:
+            current = [float(v) for v in list(states[group].q)]
+        except Exception:
+            current = list(target)
+        use = target if (not placed and len(current) == len(target)) else current
+        if use is target:
+            placed = True
+        n = len(use)
+        cmds[group] = flexivrdk.NrtJointPositionCmd(use, [0.0] * n, [1.5] * n, [2.0] * n)
+    if not placed:
+        raise RuntimeError('没有一组关节数和示教点一致，拒绝下发')
+    robot.SendJointPosition(cmds)
+
+
 def _idle_async(robot, lock: threading.Lock, flexivrdk, timeout: float = 4.0) -> bool:
     """切到 IDLE，同时避免把 stdin 循环永久阻塞住。"""
     done = threading.Event()
@@ -423,6 +487,65 @@ def main() -> int:
                     info['tool'] = ''
                 reply(info)
 
+            elif cmd == 'move_joints':
+                if robot is None:
+                    reply({'ok': False, 'error': '尚未连接'})
+                    continue
+                raw = msg.get('q')
+                if not isinstance(raw, list) or len(raw) < 7:
+                    reply({'ok': False, 'error': '需要 7 个关节角（弧度）'})
+                    continue
+                try:
+                    target = [float(v) for v in raw[:7]]
+                except (TypeError, ValueError):
+                    reply({'ok': False, 'error': '关节角不是数字'})
+                    continue
+                if any(v != v or abs(v) > 12.6 for v in target):
+                    reply({'ok': False, 'error': '关节角不是有限值，或超出约两圈'})
+                    continue
+                try:
+                    speed = float(msg.get('speed', 0.15))
+                except (TypeError, ValueError):
+                    speed = 0.15
+                speed = max(0.05, min(0.35, speed))
+                if soft is not None:
+                    soft.stop()
+                    soft = None
+                drag_on = False
+                err = _ensure_operational(robot)
+                if err:
+                    reply({'ok': False, 'error': err})
+                    continue
+                try:
+                    with rlock:
+                        robot.SwitchMode(flexivrdk.Mode.NRT_JOINT_IMPEDANCE)
+                        current = [float(v) for v in list(robot.states().q)[:7]]
+                        _restore_tracking_impedance(robot, flexivrdk)
+                    max_delta = max(abs(a - b) for a, b in zip(current, target))
+                    step_s = 0.05
+                    steps = max(1, int(max_delta / (speed * step_s)))
+                    if steps * step_s > 150.0:
+                        reply({
+                            'ok': False,
+                            'error': f'路程太长（{max_delta:.2f} rad），请先拖近一点再回放',
+                        })
+                        continue
+                    grouped: Optional[bool] = None
+                    for index in range(1, steps + 1):
+                        alpha = index / steps
+                        sample = [a + (b - a) * alpha for a, b in zip(current, target)]
+                        with rlock:
+                            grouped = _send_joint_target(robot, flexivrdk, sample, grouped)
+                        time.sleep(step_s)
+                    for _hold in range(8):
+                        with rlock:
+                            grouped = _send_joint_target(robot, flexivrdk, target, grouped)
+                        time.sleep(step_s)
+                    cached_q = list(target)
+                    reply({'ok': True, 'q': target, 'from': current, 'steps': steps, 'speed': speed})
+                except Exception as exc:
+                    reply({'ok': False, 'error': f'回点失败: {exc}'})
+
             elif cmd == 'quit':
                 if soft is not None:
                     soft.stop()

scripts/rdk_goto_xyz.py

@@ -133,6 +133,7 @@ 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("--acc", type=float, default=None, help="TCP 加速度 m/s²，交给 SDK acc；不给则用厂家默认")
     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(
@@ -168,7 +169,11 @@ def main() -> int:
             target_xyz[2] = float(pose[2])
         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    姿态保持当前")
+        acc_txt = "厂家默认" if args.acc is None else f"{float(args.acc):.3f} m/s²"
+        print(
+            f"直线距离 {dist * 1000:.0f} mm    速度 {args.vel:.3f} m/s"
+            f"    加速度 {acc_txt}    姿态保持当前"
+        )
         if dist > args.max_step_m and not args.go:
             print(f"超过 {args.max_step_m * 1000:.0f} mm，拒绝。确认无误后加 --go。")
             return 2
@@ -177,17 +182,17 @@ def main() -> int:
         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(
+        prim = {
-            "MoveL",
+            "target": flexivrdk.Coord(
-            {
+                target_xyz,
-                "target": flexivrdk.Coord(
+                euler_deg,
-                    target_xyz,
+                ["WORLD", "WORLD_ORIGIN"],
-                    euler_deg,
+            ),
-                    ["WORLD", "WORLD_ORIGIN"],
+            "vel": float(args.vel),
-                ),
+        }
-                "vel": float(args.vel),
+        if args.acc is not None:
-            },
+            prim["acc"] = float(args.acc)
-        )
+        robot.ExecutePrimitive("MoveL", prim)
         print("MoveL 已下发，等 reachedTarget …")
         t0 = time.monotonic()
         while True:

scripts/rdk_release_movel.py

@@ -122,7 +122,8 @@ def main() -> int:
     ap = argparse.ArgumentParser()
     ap.add_argument("--xyz", nargs=3, type=float, required=True, metavar=("X", "Y", "Z"))
     ap.add_argument("--arm", default=os.environ.get("SITE_ROBOT_ARM", "89"))
-    ap.add_argument("--vel", type=float, default=0.05)
+    ap.add_argument("--vel", type=float, default=0.3, help="TCP 线速度 m/s，交给 SDK vel")
+    ap.add_argument("--acc", type=float, default=1.5, help="TCP 加速度 m/s²，交给 SDK acc")
     args = ap.parse_args()
 
     held = _control_pids()
@@ -144,6 +145,7 @@ def main() -> int:
         "--xyz", f"{x}", f"{y}", f"{z}",
         "--keep-z", "--go",
         "--vel", f"{float(args.vel)}",
+        "--acc", f"{float(args.acc)}",
     ]
     print("厂家 MoveL：XY 对准 D435 这一点，工具尖高度保持当前")
     got = subprocess.run(movel, check=False)

scripts/run_all.sh

@@ -21,6 +21,9 @@
 #   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         # 对准后只降到观测高度（默认离面 15mm）
+#   bash ~/ros2/scripts/run_all.sh --arm 89 --init-z 0.35        # 启动后先到这个法兰高度 [m]
+#   bash ~/ros2/scripts/run_all.sh --arm 89 --hover-z 0.34       # 对孔悬停法兰高度 [m]，不安全会拒绝
+#   bash ~/ros2/scripts/run_all.sh --arm 89 --final-gap 8        # 下臂锁 XY，规尖停在离面 8mm（site 默认 2）
 #   bash ~/ros2/scripts/run_all.sh --arm 89 --d435-only          # 拍一张 K0，厂家 MoveL 把工具尖 XY 对准它（先停 run_real.sh）
 #
 # 前置（另开一个终端，先跑起来）：
@@ -45,6 +48,9 @@ WITH_TACTILE=true
 CAMERA_PREVIEW=false
 NO_DESCENT=false
 D435_ONLY=false
+INIT_Z=""
+HOVER_Z=""
+FINAL_GAP=""
 
 # --sn/--arm 必须先解析：load_site_config.sh 靠 ROBOT_SN 决定加载哪套机器人配置
 # shellcheck disable=SC1091
@@ -65,15 +71,23 @@ while [[ $# -gt 0 ]]; do
     # 方便看规尖偏没偏；对不准就下到 2mm 会把规顶在孔沿上（2026-10-08 真机 63N）。
     --no-descent)     NO_DESCENT=true; shift ;;
     --d435-only)      D435_ONLY=true; SKIP_STAGE2=true; SKIP_STAGE3=true; shift ;;
+    --init-z)         INIT_Z="${2:?--init-z 需要法兰高度，单位米}"; shift 2 ;;
+    --hover-z)        HOVER_Z="${2:?--hover-z 需要法兰高度，单位米}"; shift 2 ;;
+    --final-gap)      FINAL_GAP="${2:?--final-gap 需要规尖离面间隙，单位毫米}"; shift 2 ;;
     --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,31p' "$0"; exit 0 ;;
+    -h|--help)        sed -n '2,32p' "$0"; exit 0 ;;
     *) echo "Unknown: $1" >&2; exit 1 ;;
   esac
 done
 
+if [[ -n "${FINAL_GAP}" && "${NO_DESCENT}" == true ]]; then
+  echo "ERROR: 同时写了 --final-gap 和 --no-descent，不知道听谁的。" >&2
+  exit 1
+fi
+
 # shellcheck disable=SC1091
 source "${ROOT}/scripts/env.sh"
 # shellcheck disable=SC1091
@@ -303,9 +317,12 @@ ARGS=(
   "no_descent:=${NO_DESCENT}"
   "d435_move_only:=${D435_ONLY}"
 )
+[[ -n "${INIT_Z}" ]]  && ARGS+=("init_z_m:=${INIT_Z}")
+[[ -n "${HOVER_Z}" ]] && ARGS+=("hover_z_m:=${HOVER_Z}")
+[[ -n "${FINAL_GAP}" ]] && ARGS+=("final_gap_mm:=${FINAL_GAP}")
 [[ -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} d435_only=${D435_ONLY} ==="
+echo "=== launch pipeline  hole=${HOLE:-site} depth=${DEPTH:-site} pitch=${PITCH:-site} init_z=${INIT_Z:-site} hover_z=${HOVER_Z:-site} final_gap=${FINAL_GAP:-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_camera_d435.sh

@@ -46,10 +46,13 @@ if [[ "${PREVIEW}" == "1" ]]; then
   ros2 run arm_application realsense_color_pub "${CAM_ARGS[@]}" -p show_preview:=false &
   CAM_PID=$!
   trap 'kill "${CAM_PID}" 2>/dev/null || true' EXIT INT TERM
-  # 工件框是 D405 像素，叠到 1280×720 的顶视图上位置全错，看起来像没检出。
+  # 绿框来自 workpiece/detection_overhead（D435 像素）。不要订 workpiece/detection。
   ros2 run arm_application realsense_view --ros-args \
     -p camera_name:=d435 \
-    -p overlay_detection:=false
+    -p overlay_detection:=true \
+    -p detection_topic:=workpiece/detection_overhead \
+    -p overlay_hold_sec:=1.5 \
+    -p hud_label:="D435 顶视"
 else
   exec ros2 run arm_application realsense_color_pub "${CAM_ARGS[@]}"
 fi

scripts/run_console.sh

@@ -0,0 +1,17 @@
+#!/usr/bin/env bash
+# Orisys 通止规工位桌面程序（窗口，不是网页）。
+#
+#   bash ~/ros2/scripts/run_console.sh
+
+set -eo pipefail
+ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
+# shellcheck disable=SC1091
+source "${ROOT}/scripts/env.sh"
+export PYTHONPATH="${ROOT}/src/arm_application${PYTHONPATH:+:${PYTHONPATH}}"
+if [[ -z "${DISPLAY:-}" && -S /tmp/.X11-unix/X1 ]]; then
+  export DISPLAY=:1
+fi
+if [[ -z "${XAUTHORITY:-}" && -r "/run/user/$(id -u)/gdm/Xauthority" ]]; then
+  export XAUTHORITY="/run/user/$(id -u)/gdm/Xauthority"
+fi
+exec python3 -m arm_application.operator_app

scripts/run_gripper.sh

@@ -12,7 +12,9 @@
 #   bash ~/ros2/scripts/run_gripper.sh --arm 91 --cli open
 #   bash ~/ros2/scripts/run_gripper.sh --arm 91 --cli close
 #   bash ~/ros2/scripts/run_gripper.sh --arm 91 --cli rotate 90
+#   bash ~/ros2/scripts/run_gripper.sh --arm 91 --force 40 --speed 20 --cli rotate_by -15
 #   bash ~/ros2/scripts/run_gripper.sh --arm 91 --cli revolutions 2
+#   --force / --speed 写在 --cli 前面，覆盖 site.yaml 里的力度和速度。
 #   bash ~/ros2/scripts/run_gripper.sh --arm 89          # ROS 节点 PGEA
 #   bash ~/ros2/scripts/run_gripper.sh --arm 91          # ROS 节点 RGI
 #   bash ~/ros2/scripts/run_dual_gripper.sh              # 两个 ROS 节点都起
@@ -21,6 +23,8 @@ set -eo pipefail
 ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
 CLI=()
 DUAL=0
+GRIP_FORCE=""
+GRIP_SPEED=""
 
 # shellcheck disable=SC1091
 source "${ROOT}/scripts/lib/parse_arm_args.sh"
@@ -29,6 +33,8 @@ set -- "${PARSE_ARM_REST[@]}"
 
 while [[ $# -gt 0 ]]; do
   case "$1" in
+    --force) GRIP_FORCE="${2:?}"; shift 2 ;;
+    --speed) GRIP_SPEED="${2:?}"; shift 2 ;;
     --cli) shift; CLI=("$@"); break ;;
     --dual) DUAL=1; shift ;;
     -h|--help) sed -n '2,24p' "$0"; exit 0 ;;
@@ -70,11 +76,17 @@ if [[ ${#CLI[@]} -gt 0 ]]; then
         ARGS=(--arm right --test rotate)
         if [[ -n "${REST[0]:-}" ]]; then ARGS+=(--angle "${REST[0]}"); fi
         ;;
+      rotate_by)
+        ARGS=(--arm right --test rotate_by)
+        if [[ -n "${REST[0]:-}" ]]; then ARGS+=(--angle "${REST[0]}"); fi
+        ;;
       revolutions)
         ARGS=(--arm right --test rotate)
         if [[ -n "${REST[0]:-}" ]]; then ARGS+=(--revolutions "${REST[0]}"); fi
         ;;
     esac
+    if [[ -n "${GRIP_FORCE}" ]]; then ARGS+=(--force "${GRIP_FORCE}"); fi
+    if [[ -n "${GRIP_SPEED}" ]]; then ARGS+=(--speed "${GRIP_SPEED}"); fi
     echo "=== DH RGI  SN=${SITE_GRIP_SN:-?}  ${SITE_GRIP_PORT}  arm=${SITE_ROBOT_SHORT_ID:-?} ==="
     exec python3 -m arm_application.dual_gripper_cli "${ARGS[@]}"
   fi
@@ -84,7 +96,7 @@ if [[ ${#CLI[@]} -gt 0 ]]; then
   echo "=== DH ${MODEL}  SN=${SITE_GRIP_SN:-?}  ${PORT} @${BAUD} slave=${SLAVE} ==="
   exec python3 "${ROOT}/src/arm_application/arm_application/dh_pgea.py" \
     --port "${PORT}" --baud "${BAUD}" --slave "${SLAVE}" \
-    --force "${SITE_GRIP_FORCE:-50}" --speed "${SITE_GRIP_SPEED:-50}" \
+    --force "${GRIP_FORCE:-${SITE_GRIP_FORCE:-50}}" --speed "${GRIP_SPEED:-${SITE_GRIP_SPEED:-50}}" \
     --rts "${SITE_GRIP_RS485_RTS:-}" \
     "${CLI[@]}"
 fi

scripts/run_stage2_locate.sh

@@ -12,6 +12,7 @@
 #   bash ~/ros2/scripts/run_stage2_locate.sh --arm 89 --hole k2      # 对准 k2
 #   bash ~/ros2/scripts/run_stage2_locate.sh --arm 89 --hover-only   # 只垂直+悬停锁定（调高度）
 #   bash ~/ros2/scripts/run_stage2_locate.sh --arm 89 --hover-z 0.28 # 临时改悬停高（不改 yaml）
+#   bash ~/ros2/scripts/run_stage2_locate.sh --arm 89 --final-gap 8  # 下臂锁 XY，规尖离面 8mm
 #   bash ~/ros2/scripts/run_stage2_locate.sh --arm 89 --no-tactile   # 不起触觉服务
 #
 # 悬停锁高固定接口：config/site.yaml → robots[89].servo.hole_hover_z_m
@@ -21,6 +22,7 @@ ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
 HOVER_ONLY=false
 HOLE=""
 HOVER_Z=""
+FINAL_GAP=""
 WITH_TACTILE=true
 
 # shellcheck disable=SC1091
@@ -33,8 +35,9 @@ while [[ $# -gt 0 ]]; do
     --hover-only) HOVER_ONLY=true; shift ;;
     --hole) HOLE="${2:?}"; shift 2 ;;
     --hover-z) HOVER_Z="${2:?}"; shift 2 ;;
+    --final-gap) FINAL_GAP="${2:?--final-gap 需要规尖离面间隙，单位毫米}"; shift 2 ;;
     --no-tactile) WITH_TACTILE=false; shift ;;
-    -h|--help) sed -n '2,22p' "$0"; exit 0 ;;
+    -h|--help) sed -n '2,18p' "$0"; exit 0 ;;
     *) echo "Unknown: $1" >&2; exit 1 ;;
   esac
 done
@@ -137,5 +140,6 @@ echo "=== 确认：左臂夹爪已夹持通止规，通止规轴线与法兰轴
 ARGS=(hover_only:="${HOVER_ONLY}")
 if [[ -n "${HOLE}" ]]; then ARGS+=(target_hole:="${HOLE}"); fi
 if [[ -n "${HOVER_Z}" ]]; then ARGS+=(hover_z_m:="${HOVER_Z}"); fi
-echo "=== launch stage2_locate  hover_only=${HOVER_ONLY} hole=${HOLE:-site} hover_z=${HOVER_Z:-site} ==="
+if [[ -n "${FINAL_GAP}" ]]; then ARGS+=(final_gap_mm:="${FINAL_GAP}"); fi
+echo "=== launch stage2_locate  hover_only=${HOVER_ONLY} hole=${HOLE:-site} hover_z=${HOVER_Z:-site} final_gap=${FINAL_GAP:-site} ==="
 exec ros2 launch arm_application stage2_locate.launch.py "${ARGS[@]}"

src/arm_application/arm_application/gauge_fsm.py

@@ -614,6 +614,34 @@ def hover_z_guard(
     return need_z, why
 
 
+def resolve_hover_z(
+    hover_z_m: float,
+    surface_z_m: float,
+    tool_tip_below_flange_m: float,
+    want_clearance_m: float,
+    user_set: bool,
+) -> Tuple[float, str]:
+    """返回 ``(要用的法兰悬停高, 拒绝原因)``。
+
+    启动参数写了高度：不安全就拒绝，原因非空，高度原样返回，调用方不许改成别的数。
+    没写（用 site）：沿用 :func:`hover_z_guard` 的几何抬高，拒绝原因留空。
+    """
+    fixed, why = hover_z_guard(
+        hover_z_m, surface_z_m, tool_tip_below_flange_m, want_clearance_m
+    )
+    if not why:
+        return float(hover_z_m), ""
+    if not user_set:
+        return float(fixed), ""
+    clear_m = tool_clearance_m(hover_z_m, tool_tip_below_flange_m, surface_z_m)
+    return float(hover_z_m), (
+        f"悬停高度会让工具尖端进入工件：法兰 {float(hover_z_m):.4f} − 工具长 "
+        f"{float(tool_tip_below_flange_m) * 1000:.1f}mm，工件面 {float(surface_z_m):.4f}，"
+        f"间隙 {clear_m * 1000:.1f}mm，要求 ≥ {float(want_clearance_m) * 1000:.1f}mm。"
+        "这个值是启动参数指定的，不会改成别的高度。"
+    )
+
+
 #: S05 精对位时判"工件被工具推走"的两级阈值 [m]。
 #
 #   为什么需要这条判据（2026-09-24 又栽了一次，见下面 drag_verdict 的说明）：

src/arm_application/arm_application/handeye_teach.py

@@ -220,6 +220,13 @@ class RdkBridge:
                 return self._last_q
         return self._last_q
 
+    def move_joints(self, joints: List[float], speed: float = 0.15) -> dict:
+        """按关节阻抗把臂缓慢送到已记录的 7 个关节角。speed 单位 rad/s。"""
+        return self._ask(
+            {'cmd': 'move_joints', 'q': [float(v) for v in joints[:7]], 'speed': float(speed)},
+            timeout=180.0,
+        )
+
     def close(self) -> None:
         try:
             self._ask({'cmd': 'quit'}, timeout=3.0)

src/arm_application/arm_application/operator_camera_view.py

@@ -0,0 +1,226 @@
+"""工位程序里的相机预览。
+
+只订彩色和已对齐深度，不订左右红外。画面留在本进程，不另开查看器窗口。
+"""
+
+from __future__ import annotations
+
+import subprocess
+import threading
+import time
+
+import numpy as np
+
+_LOCK = threading.Lock()
+_STOP = threading.Event()
+_THREAD: threading.Thread | None = None
+_JPEG: dict[str, bytes] = {}
+_STATE = {"on": False, "error": "", "frames": {}, "stamp": 0.0}
+
+
+def targets() -> list[dict]:
+    """真机腕部相机和手外相机。仿真相机不进预览。"""
+    from arm_application.operator_console import _arms, _site
+    from arm_application.site_io import overhead_camera
+
+    found: list[dict] = []
+    seen: set[str] = set()
+    for arm in _arms():
+        name = str(arm.get("camera") or "").strip().strip("/")
+        if not name or name.startswith("sim") or name in seen:
+            continue
+        seen.add(name)
+        found.append({"id": name, "label": f"{arm.get('label') or name} 腕部"})
+    cam, _others = overhead_camera(_site(), None)
+    name = str((cam or {}).get("name") or "").strip().strip("/")
+    if name and not name.startswith("sim") and name not in seen:
+        found.append({"id": name, "label": str((cam or {}).get("label") or "手外相机")})
+    return found
+
+
+def state() -> dict:
+    with _LOCK:
+        return {
+            "on": _STATE["on"],
+            "error": _STATE["error"],
+            "stamp": _STATE["stamp"],
+            "frames": dict(_STATE["frames"]),
+        }
+
+
+def frame(name: str, kind: str) -> bytes | None:
+    with _LOCK:
+        return _JPEG.get(f"{name}_{kind}")
+
+
+def _close_external_viewers() -> None:
+    """关掉独立的四窗格查看器。它会把左右红外挂出来，本程序不再用它。"""
+    subprocess.run(["pkill", "-f", "lib/arm_application/realsense_view"], check=False)
+    subprocess.run(["pkill", "-f", "arm_application.realsense_view"], check=False)
+
+
+def _jpeg(img: np.ndarray) -> bytes | None:
+    import cv2
+
+    if img.size == 0:
+        return None
+    view = img
+    if view.shape[1] > 640:
+        scale = 640.0 / float(view.shape[1])
+        view = cv2.resize(view, (640, max(1, int(view.shape[0] * scale))))
+    ok, buf = cv2.imencode(".jpg", view, [int(cv2.IMWRITE_JPEG_QUALITY), 70])
+    if not ok:
+        return None
+    return buf.tobytes()
+
+
+def _depth_bgr(depth: np.ndarray) -> np.ndarray:
+    import cv2
+
+    vis = np.zeros((depth.shape[0], depth.shape[1], 3), np.uint8)
+    valid = depth > 0
+    if not bool(valid.any()):
+        return vis
+    values = depth.astype(np.float32)
+    lo, hi = np.percentile(values[valid], [5, 95])
+    hi = max(float(hi), float(lo) + 1.0)
+    gray = (np.clip((values - lo) / (hi - lo), 0.0, 1.0) * 255.0).astype(np.uint8)
+    gray[~valid] = 0
+    colored = cv2.applyColorMap(gray, cv2.COLORMAP_JET)
+    colored[~valid] = 0
+    return colored
+
+
+def _color_array(msg) -> np.ndarray | None:
+    if msg.encoding not in ("bgr8", "rgb8") or msg.height <= 0 or msg.width <= 0:
+        return None
+    arr = np.frombuffer(bytes(msg.data), dtype=np.uint8)
+    need = int(msg.height) * int(msg.width) * 3
+    if arr.size < need:
+        return None
+    arr = arr[:need].reshape(int(msg.height), int(msg.width), 3)
+    if msg.encoding == "rgb8":
+        arr = arr[:, :, ::-1]
+    return np.ascontiguousarray(arr)
+
+
+def _depth_array(msg) -> np.ndarray | None:
+    if msg.encoding not in ("16UC1", "mono16") or msg.height <= 0 or msg.width <= 0:
+        return None
+    arr = np.frombuffer(bytes(msg.data), dtype=np.uint16)
+    need = int(msg.height) * int(msg.width)
+    if arr.size < need:
+        return None
+    return np.ascontiguousarray(arr[:need].reshape(int(msg.height), int(msg.width)))
+
+
+def _loop(cams: list[dict]) -> None:
+    import rclpy
+    from rclpy.node import Node
+    from rclpy.qos import qos_profile_sensor_data
+    from sensor_msgs.msg import Image
+
+    latest: dict[str, np.ndarray] = {}
+    lock = threading.Lock()
+
+    class _Node(Node):
+        def __init__(self) -> None:
+            super().__init__("operator_camera_view")
+            for cam in cams:
+                name = cam["id"]
+                self.create_subscription(
+                    Image,
+                    f"/{name}/color/image_raw",
+                    self._color(name),
+                    qos_profile_sensor_data,
+                )
+                self.create_subscription(
+                    Image,
+                    f"/{name}/aligned_depth_to_color/image_raw",
+                    self._depth(name),
+                    qos_profile_sensor_data,
+                )
+
+        def _color(self, name: str):
+            def _cb(msg: Image) -> None:
+                arr = _color_array(msg)
+                if arr is None:
+                    return
+                with lock:
+                    latest[f"{name}_color"] = arr
+            return _cb
+
+        def _depth(self, name: str):
+            def _cb(msg: Image) -> None:
+                arr = _depth_array(msg)
+                if arr is None:
+                    return
+                with lock:
+                    latest[f"{name}_depth"] = arr
+            return _cb
+
+    rclpy.init()
+    node = _Node()
+    next_encode = 0.0
+    try:
+        while not _STOP.is_set():
+            rclpy.spin_once(node, timeout_sec=0.05)
+            now = time.monotonic()
+            if now < next_encode:
+                continue
+            next_encode = now + 0.12
+            with lock:
+                snap = dict(latest)
+                latest.clear()
+            frames = {}
+            blobs = {}
+            for key, arr in snap.items():
+                name, _, kind = key.rpartition("_")
+                img = _depth_bgr(arr) if kind == "depth" else arr
+                blob = _jpeg(img)
+                if not blob:
+                    continue
+                blobs[key] = blob
+                frames[key] = True
+            if blobs:
+                with _LOCK:
+                    _JPEG.update(blobs)
+                    _STATE["frames"].update(frames)
+                    _STATE["stamp"] = time.time()
+                    _STATE["error"] = ""
+    except Exception as exc:
+        with _LOCK:
+            _STATE["error"] = str(exc)
+    finally:
+        node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+        with _LOCK:
+            _STATE["on"] = False
+
+
+def start() -> str:
+    global _THREAD
+    cams = targets()
+    if not cams:
+        with _LOCK:
+            _STATE["error"] = "site 里没有可预览的真机相机"
+        return _STATE["error"]
+    _close_external_viewers()
+    with _LOCK:
+        if _STATE["on"]:
+            return "相机预览已经在程序里"
+        _STATE["on"] = True
+        _STATE["error"] = ""
+    _STOP.clear()
+    _THREAD = threading.Thread(target=_loop, args=(cams,), daemon=True, name="cam-view")
+    _THREAD.start()
+    names = "、".join(cam["label"] for cam in cams)
+    return f"预览订了 {names} 的彩色和深度，不含左右红外。"
+
+
+def stop() -> None:
+    _STOP.set()
+    thread = _THREAD
+    if thread is not None:
+        thread.join(timeout=1.5)

src/arm_application/arm_application/operator_console.html

@@ -0,0 +1,858 @@
+<!DOCTYPE html>
+<html lang="zh-CN">
+<head>
+<meta charset="utf-8" />
+<meta name="viewport" content="width=device-width, initial-scale=1" />
+<title>ORISYS 通止规工位</title>
+<style>
+  :root {
+    --bg: #070708;
+    --panel: #101216;
+    --panel-2: #171a20;
+    --line: #2a2e36;
+    --text: #f3f4f6;
+    --muted: #8b929d;
+    --blue: #12315d;
+    --blue-2: #1c4f98;
+    --ok: #3f9d6e;
+    --warn: #c8963e;
+    --bad: #c45c5c;
+  }
+  * { box-sizing: border-box; }
+  html, body { margin: 0; height: 100%; background: var(--bg); color: var(--text); }
+  body {
+    font-family: "Noto Sans SC", "Source Han Sans SC", "PingFang SC", "Segoe UI", sans-serif;
+    display: grid;
+    grid-template-columns: 220px 1fr;
+    grid-template-rows: 72px 1fr;
+    height: 100vh;
+    overflow: hidden;
+  }
+  header {
+    grid-column: 1 / -1;
+    display: flex;
+    align-items: center;
+    gap: 16px;
+    padding: 0 18px;
+    border-bottom: 1px solid var(--line);
+    background: #0b0c0e;
+  }
+  header img { height: 46px; width: auto; }
+  header .name { display: flex; flex-direction: column; gap: 2px; }
+  header .name b { font-size: 15px; font-weight: 560; letter-spacing: 0.08em; }
+  header .name span { color: var(--muted); font-size: 12px; }
+  header .grow { flex: 1; }
+  .chips { display: flex; gap: 6px; flex-wrap: wrap; }
+  .arms { display: flex; gap: 6px; }
+  .arms button, .btn {
+    border-radius: 8px;
+    border: 1px solid var(--line);
+    background: var(--panel-2);
+    color: var(--text);
+    padding: 8px 12px;
+    cursor: pointer;
+    font-size: 13px;
+  }
+  .arms button.on { background: var(--blue); border-color: #1a4580; }
+  .btn.primary { background: var(--blue-2); border-color: #2a62b0; }
+  .btn.on { background: var(--blue); border-color: #1a4580; }
+  .btn.danger { background: #6e2a2a; border-color: #8a3a3a; }
+  .clock { color: var(--muted); font-variant-numeric: tabular-nums; font-size: 13px; min-width: 72px; text-align: right; }
+  aside {
+    border-right: 1px solid var(--line);
+    background: #0b0c0e;
+    padding: 16px 12px;
+    display: flex;
+    flex-direction: column;
+    gap: 6px;
+  }
+  nav { display: flex; flex-direction: column; gap: 6px; }
+  nav button {
+    display: block;
+    width: 100%;
+    text-align: left;
+    background: transparent;
+    color: var(--text);
+    border: 1px solid transparent;
+    border-radius: 8px;
+    padding: 11px 12px;
+    font-size: 14px;
+    cursor: pointer;
+  }
+  nav button small { display: block; color: var(--muted); font-size: 11px; margin-top: 3px; }
+  nav button.active { background: var(--blue); border-color: #1a4580; }
+  nav button.active small { color: #c5d4ea; }
+  nav button:hover { border-color: var(--line); }
+  .side-note { margin-top: auto; color: var(--muted); font-size: 12px; line-height: 1.55; padding: 0 4px; }
+  main { min-width: 0; min-height: 0; overflow: auto; }
+  section { display: none; height: 100%; padding: 16px 18px; }
+  section.on { display: flex; flex-direction: column; gap: 12px; }
+  h2 { margin: 0; font-size: 18px; font-weight: 560; }
+  .lead { margin: 0; color: var(--muted); font-size: 13px; line-height: 1.55; }
+  .dash {
+    flex: 1;
+    min-height: 0;
+    display: grid;
+    grid-template-columns: 1.35fr 0.95fr;
+    grid-template-rows: minmax(0, 1fr) 168px;
+    gap: 12px;
+  }
+  .devices, .mini-forces { display: grid; gap: 12px; min-height: 0; }
+  .devices { grid-template-columns: 1fr 1fr; grid-template-rows: 1fr 1fr; }
+  .mini-forces { grid-template-rows: 1fr 1fr; }
+  .flow { grid-column: 1 / -1; display: grid; grid-template-columns: 1fr 1fr 1fr; gap: 12px; }
+  .card {
+    background: var(--panel);
+    border: 1px solid var(--line);
+    border-radius: 12px;
+    padding: 14px;
+    min-height: 0;
+  }
+  .card h3 {
+    margin: 0 0 8px;
+    font-size: 12px;
+    color: #9aa3b2;
+    font-weight: 500;
+    letter-spacing: 0.12em;
+  }
+  .stat { font-size: 32px; font-variant-numeric: tabular-nums; letter-spacing: 0.02em; }
+  .sub { color: var(--muted); font-size: 12px; margin-top: 6px; line-height: 1.5; white-space: pre-line; }
+  .pill {
+    display: inline-block;
+    border-radius: 999px;
+    padding: 2px 8px;
+    font-size: 12px;
+    border: 1px solid var(--line);
+    margin-right: 6px;
+  }
+  .pill.ok { color: var(--ok); border-color: #245c40; }
+  .pill.warn { color: var(--warn); border-color: #6d5424; }
+  .pill.bad { color: var(--bad); border-color: #6d3030; }
+  .split { display: grid; grid-template-columns: 1fr 1fr; gap: 12px; flex: 1; min-height: 0; }
+  .split.three { grid-template-columns: 1fr 1fr 1fr; }
+  .instrument { display: flex; flex-direction: column; gap: 10px; height: 100%; }
+  .readout { display: flex; gap: 18px; }
+  .readout div { min-width: 88px; }
+  .readout em { display: block; color: var(--muted); font-style: normal; font-size: 12px; }
+  .readout strong { font-size: 28px; font-weight: 520; font-variant-numeric: tabular-nums; }
+  .meter { display: grid; grid-template-columns: 52px 1fr 72px; align-items: center; gap: 8px; font-size: 12px; color: var(--muted); }
+  .track { height: 8px; background: #1c212b; border-radius: 99px; overflow: hidden; }
+  .track i { display: block; height: 100%; width: 0; background: linear-gradient(90deg, #12315d, #4c86d4); }
+  .finder {
+    flex: 1;
+    min-height: 180px;
+    border: 1px solid #243044;
+    border-radius: 10px;
+    background:
+      linear-gradient(#4c86d4, #4c86d4) left top / 18px 2px no-repeat,
+      linear-gradient(#4c86d4, #4c86d4) left top / 2px 18px no-repeat,
+      linear-gradient(#4c86d4, #4c86d4) right top / 18px 2px no-repeat,
+      linear-gradient(#4c86d4, #4c86d4) right top / 2px 18px no-repeat,
+      linear-gradient(#4c86d4, #4c86d4) left bottom / 18px 2px no-repeat,
+      linear-gradient(#4c86d4, #4c86d4) left bottom / 2px 18px no-repeat,
+      linear-gradient(#4c86d4, #4c86d4) right bottom / 18px 2px no-repeat,
+      linear-gradient(#4c86d4, #4c86d4) right bottom / 2px 18px no-repeat,
+      #0c0e12;
+    display: flex;
+    align-items: flex-end;
+    padding: 14px;
+    color: var(--muted);
+    font-size: 13px;
+  }
+  .rail { display: grid; grid-template-columns: repeat(6, minmax(0, 1fr)); gap: 8px; }
+  .step { background: var(--panel); border: 1px solid var(--line); border-radius: 10px; padding: 12px; min-height: 78px; }
+  .step b { display: block; color: #7f93b5; font-size: 12px; letter-spacing: 0.08em; }
+  .step span { display: block; margin-top: 8px; font-size: 14px; }
+  .ops { display: grid; grid-template-columns: repeat(3, minmax(0, 1fr)); gap: 12px; }
+  .row { display: flex; gap: 8px; flex-wrap: wrap; }
+  .pair { display: grid; grid-template-columns: 1fr 1fr; gap: 8px; }
+  .pair img:not([src]) { visibility: hidden; }
+  .pair img {
+    width: 100%;
+    height: 200px;
+    object-fit: contain;
+    background: #0c0e12;
+    border-radius: 8px;
+    border: 1px solid #243044;
+  }
+  .charts { display: grid; grid-template-columns: 1fr 1fr; gap: 12px; }
+  canvas.chart { width: 100%; height: 210px; background: #0c0e12; border-radius: 8px; display: block; }
+  .legend { display: flex; gap: 14px; font-size: 12px; color: var(--muted); margin-bottom: 6px; }
+  .legend i { display: inline-block; width: 18px; height: 3px; margin-right: 6px; vertical-align: middle; }
+  pre.out {
+    margin: 0;
+    min-height: 140px;
+    max-height: 260px;
+    overflow: auto;
+    background: #0c0e12;
+    border-radius: 8px;
+    padding: 10px;
+    color: #c5d4ea;
+    font-size: 12px;
+    white-space: pre-wrap;
+  }
+  dialog {
+    border: 1px solid var(--line);
+    background: #12141a;
+    color: var(--text);
+    border-radius: 12px;
+    padding: 18px;
+    width: min(480px, calc(100vw - 32px));
+  }
+  dialog::backdrop { background: rgba(0, 0, 0, 0.55); }
+  dialog h3 { margin: 0; }
+  dialog code {
+    display: block;
+    margin: 12px 0;
+    padding: 10px;
+    background: #0b0c0e;
+    border-radius: 8px;
+    color: #c5d4ea;
+    font-size: 12px;
+    white-space: pre-wrap;
+  }
+  .toast {
+    position: fixed; right: 18px; bottom: 18px;
+    background: #151922; border: 1px solid var(--line);
+    border-radius: 10px; padding: 12px 14px; max-width: 420px;
+    display: none;
+  }
+  @media (max-width: 1100px) {
+    body { grid-template-columns: 1fr; grid-template-rows: auto auto 1fr; height: auto; overflow: auto; }
+    header { flex-wrap: wrap; height: auto; padding: 12px; }
+    .dash, .split, .split.three, .ops, .rail, .flow { grid-template-columns: 1fr 1fr; }
+  }
+</style>
+</head>
+<body>
+<header>
+  <img src="/logo.png" alt="ORISYS 叠动科技" />
+  <div class="name">
+    <b>通止规工位</b>
+    <span id="arm-line">左臂 89</span>
+  </div>
+  <div class="grow"></div>
+  <div class="chips" id="chips"></div>
+  <div class="arms" id="arms"></div>
+  <div class="clock" id="clock">--:--:--</div>
+</header>
+<aside>
+  <nav>
+    <button class="active" data-page="overview">总览<small>设备与流程</small></button>
+    <button data-page="tactile">视触觉<small>切向、热成像、曲线</small></button>
+    <button data-page="handeye">手眼<small>眼在手上 / 手外</small></button>
+    <button data-page="camera">相机<small>D405 与 D435</small></button>
+    <button data-page="gripper">夹爪<small>平行爪与旋转爪</small></button>
+    <button data-page="assembly">装配<small>S00 到 S11</small></button>
+    <button data-page="operate">操作<small>启动与确认</small></button>
+  </nav>
+  <div class="side-note">读的是本机 USB、进程和话题。动臂、张合夹爪都要再确认一次。</div>
+</aside>
+<main>
+  <section id="overview" class="on">
+    <div>
+      <h2>现场</h2>
+      <p class="lead" id="arm-lead"></p>
+    </div>
+    <div class="dash">
+      <div class="devices" id="usb-cards"></div>
+      <div class="mini-forces" id="mini-forces"></div>
+      <div class="flow" id="flow"></div>
+    </div>
+  </section>
+
+  <section id="tactile">
+    <div>
+      <h2>视触觉</h2>
+      <p class="lead">切向力是 SDK 的剪切场箭头，热成像是接触散度的伪彩。下面两条曲线是法向、切向 X、切向 Y，同一时间轴。单位是形变读数，不是牛顿。</p>
+    </div>
+    <div class="row">
+      <button class="btn primary" id="view-on" type="button">开始成像</button>
+      <button class="btn" id="view-off" type="button">停止成像</button>
+      <button class="btn" data-act="tactile_up">启动三力节点</button>
+      <button class="btn" data-act="tactile_down">停止触觉节点</button>
+    </div>
+    <p class="sub" id="view-note"></p>
+    <div class="split" id="views"></div>
+    <div class="charts">
+      <div class="card">
+        <h3>左指</h3>
+        <div class="legend"><span><i style="background:#e8e8e8"></i>法向</span><span><i style="background:#4c86d4"></i>切向 X</span><span><i style="background:#d7a15a"></i>切向 Y</span></div>
+        <canvas class="chart" id="chart-left"></canvas>
+      </div>
+      <div class="card">
+        <h3>右指</h3>
+        <div class="legend"><span><i style="background:#e8e8e8"></i>法向</span><span><i style="background:#4c86d4"></i>切向 X</span><span><i style="background:#d7a15a"></i>切向 Y</span></div>
+        <canvas class="chart" id="chart-right"></canvas>
+      </div>
+    </div>
+    <p class="sub" id="tac-note"></p>
+  </section>
+
+  <section id="handeye">
+    <div>
+      <h2>手眼标定</h2>
+      <p class="lead">眼在手上用腕部 D405 和 A5 板，结果是相机到法兰。眼在手外用顶置 D435 和 A4 板，结果是相机到基座。两份文件不能混用。示教和求解会动臂。</p>
+    </div>
+    <div class="row" id="he-modes">
+      <button class="btn on" type="button" data-mode="eye_in_hand">眼在手上</button>
+      <button class="btn" type="button" data-mode="eye_to_hand">眼在手外</button>
+    </div>
+    <div class="card" id="he-card"></div>
+    <div class="row">
+      <button class="btn" data-act="handeye_show">查看配置</button>
+      <button class="btn danger" data-act="handeye_teach">示教采点</button>
+      <button class="btn" data-act="handeye_merge">合并示教点</button>
+      <button class="btn danger" data-act="handeye_calibrate">求解手眼</button>
+    </div>
+    <pre class="out" id="he-out">查看配置后，脚本输出显示在这里。</pre>
+  </section>
+
+  <section id="camera">
+    <div>
+      <h2>相机</h2>
+      <p class="lead">腕部 D405 是 848×480 @ 30，手外 D435 是 1280×720 @ 30。预览开在桌面窗口，这一页只报链路，不造画面。</p>
+    </div>
+    <div class="split three" id="cams"></div>
+    <div class="row">
+      <button class="btn primary" data-act="camera_d405">本臂 D405 预览</button>
+      <button class="btn" data-act="camera_d435">D435 预览</button>
+    </div>
+  </section>
+
+  <section id="gripper">
+    <div>
+      <h2>夹爪</h2>
+      <p class="lead" id="grip-lead"></p>
+    </div>
+    <div class="split" id="grip-body"></div>
+  </section>
+
+  <section id="assembly">
+    <div>
+      <h2>通止规装配</h2>
+      <p class="lead">顺序与状态机一致。干跑只打印计划。真跑会动臂、会拧，而且这台机器上还没跑过完整真机装配。</p>
+    </div>
+    <div class="rail" id="rail"></div>
+    <div class="row">
+      <button class="btn" data-act="gauge_dry">干跑</button>
+      <button class="btn danger" data-act="gauge_run">真跑</button>
+    </div>
+  </section>
+
+  <section id="operate">
+    <div>
+      <h2>操作</h2>
+      <p class="lead" id="op-lead"></p>
+    </div>
+    <div class="ops" id="ops"></div>
+  </section>
+</main>
+
+<dialog id="ask">
+  <h3 id="ask-title">确认</h3>
+  <p class="sub" id="ask-text"></p>
+  <code id="ask-cmd"></code>
+  <div class="row">
+    <button class="btn" id="ask-no">取消</button>
+    <button class="btn primary" id="ask-yes">确认执行</button>
+  </div>
+</dialog>
+<div class="toast" id="toast"></div>
+
+<script>
+const titles = {overview:"总览", tactile:"视触觉", camera:"相机", assembly:"装配", operate:"操作", gripper:"夹爪", handeye:"手眼"};
+const boot = /*BOOT*/null;
+let arm = "89";
+let catalog = (boot && boot.catalog) || {actions:[], stages:[]};
+let status = (boot && boot.status) || null;
+let pending = null;
+let heMode = "eye_in_hand";
+
+function $(id){ return document.getElementById(id); }
+function pill(ok, word, warn){
+  const s = document.createElement("span");
+  s.className = "pill " + (ok ? "ok" : warn ? "warn" : "bad");
+  s.textContent = word;
+  return s;
+}
+function toast(text){
+  const el = $("toast");
+  el.textContent = text;
+  el.style.display = "block";
+  clearTimeout(toast._t);
+  toast._t = setTimeout(() => { el.style.display = "none"; }, 4200);
+}
+async function api(path, body){
+  const res = await fetch(path, body ? {
+    method:"POST", headers:{"Content-Type":"application/json"}, body: JSON.stringify(body)
+  } : undefined);
+  return res.json();
+}
+function cmdOf(action){
+  const spec = catalog.actions.find(a => a.id === action);
+  if (!spec) return action;
+  const args = (spec.args || []).map(a => a.replace("{arm}", arm).replace("{mode}", heMode)).join(" ");
+  return "bash scripts/" + ({
+    real_up:"run_real.sh", camera_d405:"run_camera_d405.sh", camera_d435:"run_camera_d435.sh",
+    tactile_up:"run_tactile.sh", gripper_up:"run_gripper.sh", gripper_probe:"run_gripper.sh",
+    gripper_open:"run_gripper.sh", gripper_close:"run_gripper.sh", gripper_rotate:"run_gripper.sh",
+    pipeline:"run_all.sh", gauge_dry:"run_gauge_assembly.sh", gauge_run:"run_gauge_assembly.sh",
+    handeye_show:"run_handeye.sh", handeye_teach:"run_handeye.sh",
+    handeye_merge:"run_handeye.sh", handeye_calibrate:"run_handeye.sh"
+  }[action] || "scripts") + (args ? " " + args : "");
+}
+async function fire(action, confirm){
+  const data = await api("/api/action", {action, arm, confirm: !!confirm, mode: heMode});
+  toast(data.message || (data.ok ? "已发出" : "失败"));
+  if (String(action).startsWith("handeye") && $("he-out")) {
+    $("he-out").textContent = data.detail || data.message || "";
+  }
+}
+function ask(action){
+  const spec = catalog.actions.find(a => a.id === action);
+  if (action === "gripper_rotate" && arm !== "91"){
+    toast("旋转只对右臂 91");
+    return;
+  }
+  if (spec && spec.moves){
+    pending = action;
+    $("ask-title").textContent = spec.title;
+    $("ask-text").textContent = spec.blurb + " 当前臂 " + arm + "。";
+    $("ask-cmd").textContent = cmdOf(action);
+    $("ask").showModal();
+    return;
+  }
+  fire(action, false);
+}
+function svc(id){ return (status?.services || []).find(s => s.id === id); }
+function meters(card, data){
+  const keys = [["法向", data.fnormal], ["切向 X", data.fshearx], ["切向 Y", data.fsheary]];
+  const peak = Math.max(1, ...keys.map(([,v]) => Math.abs(typeof v === "number" ? v : 0)));
+  keys.forEach(([name, value]) => {
+    const row = document.createElement("div");
+    row.className = "meter";
+    const lab = document.createElement("span");
+    lab.textContent = name;
+    const track = document.createElement("div");
+    track.className = "track";
+    const bar = document.createElement("i");
+    const n = typeof value === "number" ? value : null;
+    bar.style.width = n == null ? "0%" : (Math.abs(n) / peak * 100).toFixed(1) + "%";
+    track.appendChild(bar);
+    const num = document.createElement("span");
+    num.textContent = n == null ? "—" : n.toFixed(1);
+    row.append(lab, track, num);
+    card.appendChild(row);
+  });
+}
+function forceCard(title, data, tall){
+  const card = document.createElement("div");
+  card.className = "card instrument";
+  const h = document.createElement("h3");
+  h.textContent = title;
+  card.appendChild(h);
+  const has = data && (data.connected !== undefined || data.fnormal !== undefined);
+  if (tall) {
+    const row = document.createElement("div");
+    row.className = "readout";
+    [["法向", data.fnormal], ["切向 X", data.fshearx], ["切向 Y", data.fsheary], ["fps", data.fps]].forEach(([k,v]) => {
+      const d = document.createElement("div");
+      const em = document.createElement("em");
+      em.textContent = k;
+      const st = document.createElement("strong");
+      st.textContent = typeof v === "number" ? (k === "fps" ? v.toFixed(1) : v.toFixed(1)) : "—";
+      d.append(em, st);
+      row.appendChild(d);
+    });
+    card.appendChild(row);
+  }
+  meters(card, data || {});
+  const sub = document.createElement("div");
+  sub.className = "sub";
+  sub.textContent = has
+    ? ((data.contact ? "有接触" : "无接触") + (typeof data.fps === "number" ? "  ·  " + data.fps.toFixed(1) + " fps" : "") + (data.connected === false ? "  ·  未连接" : ""))
+    : "话题上还没有这一侧";
+  card.appendChild(sub);
+  return card;
+}
+function renderHeader(){
+  const box = $("arms");
+  box.replaceChildren();
+  (status?.arms || [{id:"89", label:"左臂89"}, {id:"91", label:"右臂91"}]).forEach(a => {
+    const b = document.createElement("button");
+    b.textContent = a.label;
+    b.className = a.id === arm ? "on" : "";
+    b.onclick = () => { arm = a.id; render(); };
+    box.appendChild(b);
+  });
+  const chips = $("chips");
+  chips.replaceChildren();
+  (status?.services || []).forEach(s => {
+    chips.appendChild(pill(s.up, s.label + (s.up ? " 在跑" : " 未开")));
+  });
+  const cur = (status?.arms || []).find(a => a.id === arm);
+  $("arm-line").textContent = cur ? cur.label : ("臂 " + arm);
+  $("arm-lead").textContent = cur
+    ? cur.label + "  ·  " + (cur.sn || "") + "  ·  相机 " + (cur.camera || "—") + "  ·  夹爪 " + (cur.gripper || "—") + (cur.tactile ? "  ·  本臂有视触觉" : "  ·  本臂无视触觉")
+    : "";
+  $("op-lead").textContent = "当前臂 " + arm + "。红色按钮会动设备，先弹出命令再执行。";
+  $("grip-lead").textContent = arm === "91"
+    ? "右臂 91 是旋转爪 RGI。张开、闭合、转动都要确认。探测只读状态。"
+    : "左臂 89 是平行爪 PGEA，串口以现场为准。张开和闭合要确认。探测只读状态。";
+}
+function renderUsb(){
+  const box = $("usb-cards");
+  box.replaceChildren();
+  const ports = status?.grippers || [];
+  const rows = [...(status?.usb || []), {id:"tty", label:"夹爪串口", expect:1, items: ports.map(p => ({port:p, speed:""}))}];
+  rows.forEach(dev => {
+    const card = document.createElement("div");
+    card.className = "card";
+    const h = document.createElement("h3");
+    h.textContent = dev.label;
+    const n = document.createElement("div");
+    n.className = "stat";
+    n.textContent = dev.id === "tty" ? String(dev.items.length) : (dev.items.length + " / " + dev.expect);
+    const sub = document.createElement("div");
+    sub.className = "sub";
+    sub.textContent = dev.items.length
+      ? dev.items.map(i => i.speed ? (i.port + "  ·  " + i.speed + " Mbps") : i.port).join("\n")
+      : "未检测到";
+    const ok = dev.id === "tty" ? dev.items.length > 0 : dev.items.length >= dev.expect;
+    const partial = !ok && dev.items.length > 0;
+    card.append(h, n, pill(ok, ok ? "齐" : partial ? "缺一" : "缺失", partial), sub);
+    box.appendChild(card);
+  });
+}
+function renderMini(){
+  const box = $("mini-forces");
+  const t = status?.tactile || {};
+  box.replaceChildren(forceCard("左指", t.left, false), forceCard("右指", t.right, false));
+}
+function renderFlow(){
+  const box = $("flow");
+  box.replaceChildren();
+  const pipe = svc("pipeline");
+  [["跟踪", "D405 看住工件，锁住悬停"], ["对孔", "拍照找孔，移到孔上方"], ["搜孔轻压", "力闭环入孔，再轻压"]].forEach(([name, desc]) => {
+    const card = document.createElement("div");
+    card.className = "card";
+    const h = document.createElement("h3");
+    h.textContent = name;
+    const sub = document.createElement("div");
+    sub.className = "sub";
+    sub.textContent = desc + (pipe && pipe.up ? "\n进程在跑，具体阶段看终端日志。" : "\n流程未启动。");
+    card.append(h, pill(false, pipe && pipe.up ? "进行中" : "待命", pipe && pipe.up), sub);
+    box.appendChild(card);
+  });
+}
+function renderFingers(){
+  const t = status?.tactile || {};
+  const node = svc("tactile");
+  const live = ["left", "right"].some(side => (t[side] || {}).fnormal !== undefined);
+  const note = $("tac-note");
+  if (note) note.textContent = node && !node.up && live && !status?.view?.on
+    ? "正式触觉节点没开。曲线若在走，是话题上另一个发布者。"
+    : "";
+}
+function camCard(title, desc, dev){
+  const card = document.createElement("div");
+  card.className = "card instrument";
+  const h = document.createElement("h3");
+  h.textContent = title;
+  const frame = document.createElement("div");
+  frame.className = "finder";
+  frame.textContent = dev && dev.items.length ? "链路在线  ·  预览用下面的按钮挂到桌面" : "未插入";
+  const sub = document.createElement("div");
+  sub.className = "sub";
+  const where = (dev?.items || []).map(i => i.port + "  ·  " + i.speed + " Mbps").join("\n");
+  sub.textContent = desc + (where ? "\n" + where : "");
+  card.append(h, frame, pill(!!(dev && dev.items.length), dev && dev.items.length ? dev.items.length + " 台" : "缺失"), sub);
+  return card;
+}
+function renderCams(){
+  const usb = Object.fromEntries((status?.usb || []).map(d => [d.id, d]));
+  const d405 = usb.d405 || {items:[]};
+  const d435 = usb.d435 || {items:[]};
+  const other = d405.items.length >= 2 ? {items: d405.items.slice(1)} : {items: []};
+  $("cams").replaceChildren(
+    camCard(arm === "91" ? "右腕 D405" : "左腕 D405", "848×480 @ 30  ·  彩色 + 深度", {items: d405.items.slice(0, 1)}),
+    camCard("另一只腕部", "现场期望两台。缺的那台插上后这里会亮。", other),
+    camCard("手外 D435", "1280×720 @ 30  ·  顶视", d435),
+  );
+}
+function renderGripper(){
+  const ports = status?.grippers || [];
+  const up = svc("gripper");
+  const box = $("grip-body");
+  box.replaceChildren();
+  [
+    ["89", "左臂平行爪", "PGEA-100-20-W", ["gripper_probe", "gripper_up", "gripper_open", "gripper_close"]],
+    ["91", "右臂旋转爪", "RGI-100-30-O-S-M1", ["gripper_probe", "gripper_up", "gripper_open", "gripper_close", "gripper_rotate"]],
+  ].forEach(([id, title, model, acts]) => {
+    const card = document.createElement("div");
+    card.className = "card";
+    const h = document.createElement("h3");
+    h.textContent = title;
+    const sub = document.createElement("div");
+    sub.className = "sub";
+    sub.textContent = model + "\n串口 " + (ports.join("  ") || "没有 ttyUSB") + (up && up.up ? "\n驱动在跑" : "\n驱动未开");
+    card.append(h, pill(id === arm, id === arm ? "当前臂" : "切换顶栏", id !== arm), sub);
+    const row = document.createElement("div");
+    row.className = "row";
+    acts.forEach(act => {
+      const spec = catalog.actions.find(a => a.id === act);
+      const b = document.createElement("button");
+      b.className = "btn " + (spec && spec.moves ? "danger" : "primary");
+      b.textContent = spec ? spec.title : act;
+      b.onclick = () => { if (arm !== id) arm = id; ask(act); render(); };
+      row.appendChild(b);
+    });
+    card.appendChild(row);
+    box.appendChild(card);
+  });
+}
+function renderRail(){
+  const box = $("rail");
+  if (box.childElementCount) return;
+  catalog.stages.forEach(([id, name]) => {
+    const d = document.createElement("div");
+    d.className = "step";
+    const b = document.createElement("b");
+    b.textContent = id;
+    const s = document.createElement("span");
+    s.textContent = name;
+    d.append(b, s);
+    box.appendChild(d);
+  });
+}
+function renderOps(){
+  const box = $("ops");
+  box.replaceChildren();
+  const skip = new Set(["gripper_open", "gripper_close", "gripper_rotate", "gripper_probe", "handeye_show", "handeye_teach", "handeye_merge", "handeye_calibrate"]);
+  catalog.actions.filter(spec => !skip.has(spec.id)).forEach(spec => {
+    const card = document.createElement("div");
+    card.className = "card";
+    const h = document.createElement("h3");
+    h.textContent = spec.title;
+    const sub = document.createElement("div");
+    sub.className = "sub";
+    sub.textContent = spec.blurb;
+    const b = document.createElement("button");
+    b.className = "btn " + (spec.moves ? "danger" : "primary");
+    b.textContent = spec.moves ? "确认后执行" : "执行";
+    b.style.marginTop = "10px";
+    b.onclick = () => ask(spec.id);
+    card.append(h, sub, b);
+    box.appendChild(card);
+  });
+  const stop = document.createElement("div");
+  stop.className = "card";
+  const h = document.createElement("h3");
+  h.textContent = "停止视触觉";
+  const sub = document.createElement("div");
+  sub.className = "sub";
+  sub.textContent = "关掉触觉节点和占着相机的 worker。";
+  const b = document.createElement("button");
+  b.className = "btn";
+  b.textContent = "停止";
+  b.style.marginTop = "10px";
+  b.onclick = () => ask("tactile_down");
+  stop.append(h, sub, b);
+  box.appendChild(stop);
+}
+function shot(side, kind, title){
+  const wrap = document.createElement("div");
+  const h = document.createElement("div");
+  h.className = "sub";
+  h.textContent = title;
+  const img = document.createElement("img");
+  img.alt = "";
+  img.dataset.side = side;
+  img.dataset.kind = kind;
+  const ready = status?.view?.frames?.[side + "_" + kind];
+  if (status?.view?.on && ready) {
+    img.src = "/api/tactile/frame/" + side + "/" + kind + ".jpg?t=" + Math.floor((status.view.stamp || 0) * 5);
+  }
+  wrap.append(h, img);
+  return wrap;
+}
+function renderViews(){
+  const box = $("views");
+  if (!box.childElementCount) {
+    [["left", "左指"], ["right", "右指"]].forEach(([side, name]) => {
+      const card = document.createElement("div");
+      card.className = "card";
+      const h = document.createElement("h3");
+      h.textContent = name;
+      const pair = document.createElement("div");
+      pair.className = "pair";
+      pair.append(shot(side, "shear", "切向力"), shot(side, "thermal", "热成像"));
+      card.append(h, pair);
+      box.appendChild(card);
+    });
+  }
+  const view = status?.view || {};
+  $("view-note").textContent = view.error || view.note || (view.on ? "成像已开，等第一帧。" : "成像没开。点开始成像后这里出切向力和热成像。");
+}
+function drawChart(canvas, samples){
+  if (!canvas) return;
+  const dpr = window.devicePixelRatio || 1;
+  const w = canvas.clientWidth || 360;
+  const h = canvas.clientHeight || 210;
+  canvas.width = Math.floor(w * dpr);
+  canvas.height = Math.floor(h * dpr);
+  const ctx = canvas.getContext("2d");
+  ctx.setTransform(dpr, 0, 0, dpr, 0, 0);
+  ctx.clearRect(0, 0, w, h);
+  ctx.fillStyle = "#0c0e12";
+  ctx.fillRect(0, 0, w, h);
+  const rows = (samples || []).filter(p => ["fnormal", "fshearx", "fsheary"].some(k => typeof p[k] === "number"));
+  if (rows.length < 2) {
+    ctx.fillStyle = "#8b929d";
+    ctx.font = "13px sans-serif";
+    ctx.fillText("还没有足够的采样", 16, 28);
+    return;
+  }
+  const keys = [["fnormal", "#e8e8e8"], ["fshearx", "#4c86d4"], ["fsheary", "#d7a15a"]];
+  let lo = Infinity, hi = -Infinity;
+  rows.forEach(p => keys.forEach(([k]) => {
+    if (typeof p[k] === "number") { lo = Math.min(lo, p[k]); hi = Math.max(hi, p[k]); }
+  }));
+  if (!(hi > lo)) { hi = lo + 1; }
+  const pad = (hi - lo) * 0.08;
+  lo -= pad; hi += pad;
+  const t0 = rows[0].t, t1 = rows[rows.length - 1].t;
+  const span = Math.max(t1 - t0, 1e-3);
+  const left = 46, right = 10, top = 12, bottom = 22;
+  ctx.strokeStyle = "#2a2e36";
+  ctx.beginPath();
+  const y0 = top + (hi - 0) / (hi - lo) * (h - top - bottom);
+  if (y0 > top && y0 < h - bottom) {
+    ctx.moveTo(left, y0);
+    ctx.lineTo(w - right, y0);
+    ctx.stroke();
+  }
+  keys.forEach(([k, color]) => {
+    ctx.beginPath();
+    ctx.strokeStyle = color;
+    ctx.lineWidth = 1.6;
+    let started = false;
+    rows.forEach(p => {
+      if (typeof p[k] !== "number") return;
+      const x = left + (p.t - t0) / span * (w - left - right);
+      const y = top + (hi - p[k]) / (hi - lo) * (h - top - bottom);
+      if (!started) { ctx.moveTo(x, y); started = true; }
+      else ctx.lineTo(x, y);
+    });
+    ctx.stroke();
+  });
+  ctx.fillStyle = "#8b929d";
+  ctx.font = "11px sans-serif";
+  ctx.fillText(hi.toFixed(0), 4, top + 10);
+  ctx.fillText(lo.toFixed(0), 4, h - bottom);
+  ctx.fillText((span).toFixed(0) + " 秒", w - 48, h - 6);
+}
+function mm(v){
+  if (!Array.isArray(v) || v.length < 3) return "—";
+  return v.slice(0, 3).map(x => (Number(x) * 1000).toFixed(1) + " mm").join("   ");
+}
+function renderHandeye(){
+  document.querySelectorAll("#he-modes button").forEach(b => b.classList.toggle("on", b.dataset.mode === heMode));
+  const card = (status?.handeye || {})[arm]?.[heMode];
+  const box = $("he-card");
+  if (!box) return;
+  box.replaceChildren();
+  if (!card) {
+    box.textContent = "没有这一臂的标定记录";
+    return;
+  }
+  const h = document.createElement("h3");
+  h.textContent = heMode === "eye_in_hand" ? "眼在手上" : "眼在手外";
+  const sub = document.createElement("div");
+  sub.className = "sub";
+  const board = card.board || {};
+  sub.textContent = [
+    card.exists ? "已有结果" : "还没有结果文件",
+    card.meaning || "",
+    "文件 " + card.file,
+    "视角 " + (card.views || "—") + "    时间 " + (card.utc || "—"),
+    "相机 " + (card.camera || "—") + "    工具 " + (card.tool || "—"),
+    "平移 X / Y / Z   " + mm(card.translation_m),
+    "示教点 " + (card.poses == null ? "没有位姿文件" : card.poses + " 个"),
+    "板 " + (board.cols || "—") + "×" + (board.rows || "—") + "  方格 " + (board.square_m == null ? "—" : board.square_m + " m"),
+  ].filter(Boolean).join("\n");
+  box.append(h, pill(!!card.exists, card.exists ? "有结果" : "无结果"), sub);
+}
+function render(){
+  renderHeader();
+  renderUsb();
+  renderMini();
+  renderFlow();
+  renderFingers();
+  renderViews();
+  drawChart($("chart-left"), status?.history?.left);
+  drawChart($("chart-right"), status?.history?.right);
+  renderCams();
+  renderGripper();
+  renderHandeye();
+  renderRail();
+  renderOps();
+}
+function show(page){
+  if (!titles[page]) page = "overview";
+  document.querySelectorAll("nav button").forEach(b => b.classList.toggle("active", b.dataset.page === page));
+  document.querySelectorAll("main section").forEach(s => s.classList.remove("on"));
+  $(page).classList.add("on");
+  if (location.hash !== "#" + page) history.replaceState(null, "", "#" + page);
+}
+document.querySelectorAll("nav button").forEach(btn => { btn.onclick = () => show(btn.dataset.page); });
+window.addEventListener("hashchange", () => show(location.hash.slice(1)));
+document.body.addEventListener("click", ev => {
+  const act = ev.target.closest("[data-act]");
+  if (act) ask(act.dataset.act);
+});
+document.querySelectorAll("#he-modes button").forEach(btn => {
+  btn.onclick = () => { heMode = btn.dataset.mode; renderHandeye(); };
+});
+$("view-on").onclick = async () => {
+  const data = await api("/api/tactile/view", {on: true});
+  toast(data.message || "已请求");
+};
+$("view-off").onclick = async () => {
+  const data = await api("/api/tactile/view", {on: false});
+  toast(data.message || "已停止");
+};
+setInterval(() => {
+  if (!$("tactile").classList.contains("on") || !status?.view?.on) return;
+  document.querySelectorAll("#views img").forEach(img => {
+    const side = img.dataset.side, kind = img.dataset.kind;
+    if (!status.view.frames?.[side + "_" + kind]) return;
+    img.src = "/api/tactile/frame/" + side + "/" + kind + ".jpg?t=" + Date.now();
+  });
+}, 200);
+$("ask-no").onclick = () => $("ask").close();
+$("ask-yes").onclick = () => { const action = pending; $("ask").close(); if (action) fire(action, true); };
+async function tick(){
+  try { status = await api("/api/status"); render(); }
+  catch (err) { toast("状态刷新失败"); }
+  paintClock();
+}
+function paintClock(){
+  $("clock").textContent = new Date().toLocaleTimeString("zh-CN", {hour12:false});
+}
+async function start(){
+  if (!boot) {
+    catalog = await api("/api/catalog");
+    await tick();
+  } else {
+    render();
+    paintClock();
+  }
+  show(location.hash.slice(1) || "overview");
+  setInterval(tick, 2000);
+}
+start();
+</script>
+</body>
+</html>

src/arm_application/arm_application/operator_console.py

@@ -0,0 +1,725 @@
+#!/usr/bin/env python3
+"""Orisys 工位界面。
+
+只做两件事：把本机真实状态摊开，以及按白名单启动仓库里已有的脚本。
+会动臂的动作必须带 ``confirm=true``，避免页面上误点一下就上电。
+
+    bash ~/ros2/scripts/run_console.sh
+"""
+
+from __future__ import annotations
+
+import json
+import os
+import re
+import subprocess
+import threading
+import time
+from collections import deque
+from http.server import BaseHTTPRequestHandler, ThreadingHTTPServer
+from pathlib import Path
+from urllib.parse import urlparse
+
+import yaml
+
+from arm_application import operator_tactile_view as tactile_view
+from arm_application.site_io import eye_to_hand_result_name, overhead_camera
+
+HOST = "127.0.0.1"
+PORT = 8766
+_ROOT = Path(__file__).resolve()
+for _p in _ROOT.parents:
+    if (_p / "scripts" / "run_real.sh").is_file():
+        ROOT = _p
+        break
+else:
+    ROOT = Path.home() / "ros2"
+
+LOG_DIR = Path("/tmp/orisys_console")
+ASSETS = Path(__file__).resolve().parent
+_LOCK = threading.Lock()
+_TACTILE = {"left": {}, "right": {}, "stamp": 0.0}
+_HIST = {"left": deque(maxlen=360), "right": deque(maxlen=360)}
+_PROCS: dict[str, int] = {}
+
+
+def _push_force(side: str, sample: dict) -> None:
+    if side not in _HIST:
+        return
+    point = {
+        "t": float(sample.get("t") or time.time()),
+        "fnormal": sample.get("fnormal"),
+        "fshearx": sample.get("fshearx"),
+        "fsheary": sample.get("fsheary"),
+    }
+    if not any(isinstance(point[k], (int, float)) for k in ("fnormal", "fshearx", "fsheary")):
+        return
+    with _LOCK:
+        _HIST[side].append(point)
+        _TACTILE[side] = {
+            "fnormal": point["fnormal"],
+            "fshearx": point["fshearx"],
+            "fsheary": point["fsheary"],
+            "connected": True,
+        }
+
+
+tactile_view.set_sample_hook(_push_force)
+
+
+def _site() -> dict:
+    path = ROOT / "config" / "site.yaml"
+    if not path.is_file():
+        return {}
+    data = yaml.safe_load(path.read_text()) or {}
+    return data if isinstance(data, dict) else {}
+
+
+def _arms() -> list[dict]:
+    robots = (_site().get("robot") or {}).get("robots") or []
+    out = []
+    for robot in robots:
+        if not isinstance(robot, dict):
+            continue
+        sn = str(robot.get("sn") or "")
+        arm_no = "89" if sn.endswith("063989") else "91" if sn.endswith("063991") else ""
+        if not arm_no:
+            continue
+        out.append({
+            "id": arm_no,
+            "label": str(robot.get("label") or f"臂{arm_no}"),
+            "sn": sn,
+            "tactile": bool(robot.get("tactile")),
+            "camera": str((robot.get("camera") or {}).get("name") or ""),
+            "gripper": str((robot.get("gripper") or {}).get("model") or ""),
+        })
+    return out or [
+        {"id": "89", "label": "左臂89", "sn": "", "tactile": True, "camera": "d405_left", "gripper": ""},
+        {"id": "91", "label": "右臂91", "sn": "", "tactile": False, "camera": "d405_right", "gripper": ""},
+    ]
+
+
+def _usb() -> list[dict]:
+    found = {"d405": [], "d435": [], "orisys": []}
+    want = {
+        ("8086", "0b5b"): "d405",
+        ("8086", "0b07"): "d435",
+        ("1bcf", "2281"): "orisys",
+    }
+    base = Path("/sys/bus/usb/devices")
+    if not base.is_dir():
+        return []
+    for dev in base.iterdir():
+        vid = dev / "idVendor"
+        pid = dev / "idProduct"
+        if not vid.is_file() or not pid.is_file():
+            continue
+        key = want.get((vid.read_text().strip(), pid.read_text().strip()))
+        if not key:
+            continue
+        speed = (dev / "speed").read_text().strip() if (dev / "speed").is_file() else "?"
+        found[key].append({"port": dev.name, "speed": speed})
+    return [
+        {"id": "d405", "label": "腕部 D405", "expect": 2, "items": found["d405"]},
+        {"id": "d435", "label": "手外 D435", "expect": 1, "items": found["d435"]},
+        {"id": "orisys", "label": "视触觉", "expect": 2, "items": found["orisys"]},
+    ]
+
+
+def _listeners(pattern: str) -> bool:
+    try:
+        out = subprocess.run(
+            ["pgrep", "-af", pattern],
+            capture_output=True, text=True, timeout=2,
+        ).stdout
+    except Exception:
+        return False
+    for line in out.splitlines():
+        if "pgrep" in line or "operator_console" in line:
+            continue
+        return True
+    return False
+
+
+def _services() -> list[dict]:
+    rows = [
+        ("real", "控制栈", "ros2_control_node"),
+        ("tactile", "视触觉节点", "lib/arm_application/tactile"),
+        ("camera", "相机", "realsense_color_pub"),
+        ("gripper", "夹爪", "dh_pgea|dual_gripper_driver"),
+        ("pipeline", "三段流程", "assembly_pipeline"),
+        ("gauge", "通止规装配", "gauge_assembly"),
+    ]
+    out = []
+    for sid, label, pattern in rows:
+        if "|" in pattern:
+            up = any(_listeners(part) for part in pattern.split("|"))
+        else:
+            up = _listeners(pattern)
+        out.append({"id": sid, "label": label, "up": up})
+    return out
+
+
+def _grippers() -> list[str]:
+    return sorted(p.name for p in Path("/dev").glob("ttyUSB*"))
+
+
+def _parse_tactile(text: str) -> dict:
+    got: dict[str, object] = {}
+    for line in text.splitlines():
+        if ":" not in line:
+            continue
+        key, _, raw = line.partition(":")
+        key = key.strip()
+        raw = raw.strip()
+        if key in ("fnormal", "fshearx", "fsheary", "fps"):
+            try:
+                got[key] = float(raw)
+            except ValueError:
+                continue
+        elif key in ("connected", "contact"):
+            got[key] = raw.lower() == "true"
+    return got
+
+
+def _poll_tactile() -> None:
+    while True:
+        snap = {}
+        for side in ("left", "right"):
+            try:
+                proc = subprocess.run(
+                    ["timeout", "1.2", "ros2", "topic", "echo", f"/tactile/{side}/status", "--once"],
+                    capture_output=True, text=True, timeout=2.5,
+                )
+                snap[side] = _parse_tactile(proc.stdout) if proc.returncode == 0 else {}
+            except Exception:
+                snap[side] = {}
+        view_on = bool(tactile_view.state().get("on"))
+        with _LOCK:
+            _TACTILE["stamp"] = time.time()
+            if not view_on:
+                _TACTILE["left"] = snap.get("left") or {}
+                _TACTILE["right"] = snap.get("right") or {}
+                now = time.time()
+                for side in ("left", "right"):
+                    row = snap.get(side) or {}
+                    if row:
+                        _HIST[side].append({
+                            "t": now,
+                            "fnormal": row.get("fnormal"),
+                            "fshearx": row.get("fshearx"),
+                            "fsheary": row.get("fsheary"),
+                        })
+        time.sleep(0.35)
+
+
+def _robot(arm: str) -> dict:
+    robots = (_site().get("robot") or {}).get("robots") or []
+    for robot in robots:
+        if not isinstance(robot, dict):
+            continue
+        sn = str(robot.get("sn") or "")
+        short = "89" if sn.endswith("063989") else "91" if sn.endswith("063991") else ""
+        if short == arm:
+            return robot
+    return {}
+
+
+def _comment(text: str, key: str) -> str:
+    match = re.search(rf"^#\s*{re.escape(key)}:\s*(.+)$", text, re.M)
+    return match.group(1).strip() if match else ""
+
+
+def _pose_count(path: Path) -> int | None:
+    if not path.is_file():
+        return None
+    match = re.search(r"num_poses:\s*(\d+)", path.read_text(errors="replace"))
+    return int(match.group(1)) if match else None
+
+
+def _result_card(path: Path, pose_path: Path, board: dict) -> dict:
+    text = path.read_text(errors="replace") if path.is_file() else ""
+    trans = None
+    tool = ""
+    if text:
+        try:
+            data = yaml.safe_load(text) or {}
+        except yaml.YAMLError:
+            data = {}
+        if isinstance(data, dict):
+            raw = data.get("translation_m")
+            if isinstance(raw, list) and len(raw) >= 3:
+                trans = [float(raw[0]), float(raw[1]), float(raw[2])]
+            tool = str(data.get("gripper_tool") or "")
+    meaning = ""
+    for line in text.splitlines():
+        if line.startswith("# T_4x4"):
+            meaning = line[2:].strip()
+            break
+    return {
+        "file": str(path),
+        "exists": path.is_file(),
+        "views": _comment(text, "views"),
+        "utc": _comment(text, "utc"),
+        "board_note": _comment(text, "board"),
+        "camera": _comment(text, "camera_source"),
+        "tool": tool,
+        "meaning": meaning,
+        "translation_m": trans,
+        "poses": _pose_count(pose_path),
+        "pose_file": str(pose_path),
+        "board": {
+            "cols": board.get("cols"),
+            "rows": board.get("rows"),
+            "square_m": board.get("square_size_m"),
+        },
+    }
+
+
+def _handeye(arm: str) -> dict:
+    site = _site()
+    robot = _robot(arm)
+    paths = robot.get("paths") if isinstance(robot.get("paths"), dict) else {}
+    shared = site.get("paths") if isinstance(site.get("paths"), dict) else {}
+    boards = ((site.get("handeye") or {}).get("boards") or {})
+    wrist_file = Path(str(
+        paths.get("handeye_file")
+        or shared.get("handeye_file")
+        or (ROOT / "handeye" / "results" / f"{arm}_eye_in_hand.yaml")
+    ))
+    wrist_pose = Path(str(
+        paths.get("handeye_waypoints")
+        or shared.get("handeye_waypoints")
+        or (ROOT / "handeye" / "poses" / f"{arm}_eye_in_hand.yaml")
+    ))
+    cam, _others = overhead_camera(site, robot or None)
+    cam_name = str((cam or {}).get("name") or "")
+    e2h_name = eye_to_hand_result_name(arm, cam_name)
+    e2h_file = Path(str(
+        paths.get("handeye_file_eye_to_hand")
+        or shared.get("handeye_file_eye_to_hand")
+        or (ROOT / "handeye" / "results" / e2h_name)
+    ))
+    e2h_pose = Path(str(
+        paths.get("handeye_waypoints_eye_to_hand")
+        or shared.get("handeye_waypoints_eye_to_hand")
+        or (ROOT / "handeye" / "poses" / f"{arm}_eye_to_hand.yaml")
+    ))
+    return {
+        "eye_in_hand": _result_card(wrist_file, wrist_pose, boards.get("eye_in_hand") or {}),
+        "eye_to_hand": _result_card(e2h_file, e2h_pose, boards.get("eye_to_hand") or {}),
+    }
+
+
+def status() -> dict:
+    with _LOCK:
+        tactile = {
+            "left": dict(_TACTILE["left"]),
+            "right": dict(_TACTILE["right"]),
+            "stamp": _TACTILE["stamp"],
+        }
+        history = {
+            "left": list(_HIST["left"]),
+            "right": list(_HIST["right"]),
+        }
+    return {
+        "arms": _arms(),
+        "usb": _usb(),
+        "services": _services(),
+        "grippers": _grippers(),
+        "tactile": tactile,
+        "history": history,
+        "view": tactile_view.state(),
+        "handeye": {"89": _handeye("89"), "91": _handeye("91")},
+        "now": time.time(),
+    }
+
+
+# 会动真机的动作必须 confirm。argv 里的 {arm} 只接受 89 或 91。
+_ACTIONS = {
+    "real_up": {
+        "title": "上电控制栈",
+        "script": "run_real.sh",
+        "args": ["--arm", "{arm}"],
+        "moves": True,
+        "blurb": "连接机械臂并拉起 ros2_control。",
+    },
+    "camera_d405": {
+        "title": "本臂腕部相机",
+        "script": "run_camera_d405.sh",
+        "args": ["--arm", "{arm}", "--no-preview"],
+        "moves": False,
+        "blurb": "只拉起本臂腕部相机。彩色和深度显示在程序里，不看左右红外，不另开窗口。",
+    },
+    "camera_d435": {
+        "title": "手外 D435",
+        "script": "run_camera_d435.sh",
+        "args": ["--no-preview"],
+        "moves": False,
+        "blurb": "只拉起顶视 D435。彩色和深度显示在程序里，不另开窗口。",
+    },
+    "tactile_up": {
+        "title": "视触觉三力",
+        "script": "run_tactile.sh",
+        "args": [],
+        "moves": False,
+        "blurb": "发布左右指法向力与两路切向力。",
+    },
+    "gripper_up": {
+        "title": "夹爪节点",
+        "script": "run_gripper.sh",
+        "args": ["--arm", "{arm}"],
+        "moves": False,
+        "blurb": "只拉起夹爪驱动，不主动张合。",
+    },
+    "gripper_probe": {
+        "title": "夹爪探测",
+        "script": "run_gripper.sh",
+        "args": ["--arm", "{arm}", "--cli", "probe"],
+        "moves": False,
+        "blurb": "读夹爪是否在线，不动手指。",
+    },
+    "gripper_open": {
+        "title": "夹爪张开",
+        "script": "run_gripper.sh",
+        "args": ["--arm", "{arm}", "--cli", "open"],
+        "moves": True,
+        "blurb": "张开当前臂夹爪。",
+    },
+    "gripper_close": {
+        "title": "夹爪闭合",
+        "script": "run_gripper.sh",
+        "args": ["--arm", "{arm}", "--cli", "close"],
+        "moves": True,
+        "blurb": "闭合当前臂夹爪。",
+    },
+    "gripper_rotate": {
+        "title": "右爪转 90°",
+        "script": "run_gripper.sh",
+        "args": ["--arm", "{arm}", "--cli", "rotate", "90"],
+        "moves": True,
+        "blurb": "右臂旋转爪转到 90°。左臂平行爪没有这一轴。",
+    },
+    "pipeline": {
+        "title": "整体三段",
+        "script": "run_all.sh",
+        "args": ["--arm", "{arm}"],
+        "moves": True,
+        "blurb": "跟踪、对孔、搜孔轻压一次跑完。会动臂。",
+    },
+    "stage_track": {
+        "title": "只测跟踪",
+        "script": "run_visual_servo_rt.sh",
+        "args": ["--arm", "{arm}", "--control"],
+        "moves": True,
+        "blurb": "只跑视觉跟踪。从当前位姿开始，会动臂。要先有控制栈和相机。",
+    },
+    "stage_locate": {
+        "title": "只测对孔",
+        "script": "run_stage2_locate.sh",
+        "args": ["--arm", "{arm}"],
+        "moves": True,
+        "blurb": "只跑二阶段：朝下、悬停、拍照、对准孔。会动臂。",
+    },
+    "stage_search": {
+        "title": "只测搜孔",
+        "script": "run_stage3_search.sh",
+        "args": ["--arm", "{arm}"],
+        "moves": True,
+        "blurb": "只跑三阶段：触面、搜孔、轻压。会动臂。臂应已停在孔上方。",
+    },
+    "reset_home": {
+        "title": "复位到 home",
+        "script": "go_home.sh",
+        "args": ["--arm", "{arm}", "--pose", "home"],
+        "moves": True,
+        "sync": True,
+        "timeout": 120,
+        "blurb": "先停跟踪，再回到命名位姿 home。要控制栈已经起来。会动臂。",
+    },
+    "gauge_dry": {
+        "title": "装配干跑",
+        "script": "run_gauge_assembly.sh",
+        "args": ["--arm", "{arm}", "--dry-run"],
+        "moves": False,
+        "blurb": "S00 到 S11 只打印计划，不发运动指令。",
+    },
+    "gauge_run": {
+        "title": "装配真跑",
+        "script": "run_gauge_assembly.sh",
+        "args": ["--arm", "{arm}"],
+        "moves": True,
+        "blurb": "通止规状态机。会动臂、会拧。",
+    },
+    "handeye_show": {
+        "title": "查看标定配置",
+        "script": "run_handeye.sh",
+        "args": ["--arm", "{arm}", "show", "--mode", "{mode}"],
+        "moves": False,
+        "sync": True,
+        "blurb": "打印这块板、结果文件和已有点数，不动臂。",
+    },
+    "handeye_teach": {
+        "title": "示教采点",
+        "script": "run_handeye.sh",
+        "args": ["--arm", "{arm}", "teach", "--mode", "{mode}", "--points", "12"],
+        "moves": True,
+        "blurb": "软拖拽示教，目标 12 个姿态。要先停控制栈，会动臂。",
+    },
+    "handeye_merge": {
+        "title": "合并示教点",
+        "script": "run_handeye.sh",
+        "args": ["--arm", "{arm}", "merge", "--mode", "{mode}"],
+        "moves": False,
+        "sync": True,
+        "blurb": "把刚采的点写入该模式的位姿文件，不动臂。",
+    },
+    "handeye_calibrate": {
+        "title": "求解手眼",
+        "script": "run_handeye.sh",
+        "args": ["--arm", "{arm}", "calibrate", "--mode", "{mode}"],
+        "moves": True,
+        "blurb": "按点位走一遍并写结果。需要控制栈和对应相机，会动臂。",
+    },
+}
+
+
+def catalog() -> dict:
+    return {
+        "actions": [
+            {"id": key, **{k: v for k, v in spec.items() if k not in ("script", "sync")}}
+            for key, spec in _ACTIONS.items()
+        ],
+        "stages": [
+            ("S00", "自检"),
+            ("S01", "回零预绕"),
+            ("S02", "夹持确认"),
+            ("S03", "手外粗定位"),
+            ("S04", "视觉跟踪"),
+            ("S05", "精拍"),
+            ("S06", "正悬停"),
+            ("S07", "柔顺接触"),
+            ("S08", "棘轮拧入"),
+            ("S09", "反向拧出"),
+            ("S10", "换孔"),
+            ("S11", "回零结束"),
+        ],
+    }
+
+
+def _stop_tactile() -> str:
+    subprocess.run(["pkill", "-f", "lib/arm_application/tactile"], check=False)
+    subprocess.run(["pkill", "-f", "arm_application/tactile_node.py"], check=False)
+    subprocess.run(["pkill", "orisys_tac_w"], check=False)
+    return "已发送停止视触觉节点"
+
+
+def run_action(action: str, arm: str, confirm: bool, mode: str = "eye_in_hand") -> dict:
+    if action == "tactile_down":
+        tactile_view.stop()
+        return {"ok": True, "message": _stop_tactile()}
+    if action == "tactile_view_on":
+        with _LOCK:
+            _HIST["left"].clear()
+            _HIST["right"].clear()
+        return {"ok": True, "message": tactile_view.start(_site())}
+    if action == "tactile_view_off":
+        return {"ok": True, "message": tactile_view.stop()}
+    spec = _ACTIONS.get(action)
+    if spec is None:
+        return {"ok": False, "message": "没有这个动作"}
+    if arm not in ("89", "91"):
+        return {"ok": False, "message": "臂号只接受 89 或 91"}
+    if mode not in ("eye_in_hand", "eye_to_hand"):
+        return {"ok": False, "message": "手眼模式只接受 eye_in_hand 或 eye_to_hand"}
+    if spec["moves"] and not confirm:
+        return {"ok": False, "message": "这个动作会动真机，需要确认"}
+    script = ROOT / "scripts" / spec["script"]
+    if not script.is_file():
+        return {"ok": False, "message": f"找不到 {script}"}
+    args = [a.replace("{arm}", arm).replace("{mode}", mode) for a in spec["args"]]
+    LOG_DIR.mkdir(parents=True, exist_ok=True)
+    log_path = LOG_DIR / f"{action}_{arm}.log"
+    shown = " ".join(["bash", f"scripts/{spec['script']}", *args])
+    if spec.get("sync"):
+        try:
+            proc = subprocess.run(
+                ["bash", str(script), *args],
+                cwd=str(ROOT),
+                capture_output=True,
+                text=True,
+                timeout=float(spec.get("timeout") or 40),
+            )
+        except subprocess.TimeoutExpired:
+            return {"ok": False, "message": "命令超时还没返回", "detail": shown}
+        detail = ((proc.stdout or "") + (proc.stderr or "")).strip()
+        log_path.write_text(detail)
+        ok = proc.returncode == 0
+        return {
+            "ok": ok,
+            "message": shown if ok else f"退出码 {proc.returncode}",
+            "detail": detail[-4000:],
+            "log": str(log_path),
+        }
+    log_f = log_path.open("ab")
+    proc = subprocess.Popen(
+        ["bash", str(script), *args],
+        cwd=str(ROOT),
+        stdout=log_f,
+        stderr=subprocess.STDOUT,
+        start_new_session=True,
+    )
+    _PROCS[action] = proc.pid
+    return {"ok": True, "message": shown, "pid": proc.pid, "log": str(log_path)}
+
+
+def _bounded(name: str, raw, low: float, high: float) -> float:
+    try:
+        value = float(raw)
+    except (TypeError, ValueError):
+        raise ValueError(f"{name} 不是数字") from None
+    if value != value or value < low or value > high:
+        raise ValueError(f"{name} 只接受 {low:g}–{high:g}")
+    return value
+
+
+def run_gripper_tune(arm: str, kind: str, numbers: dict) -> dict:
+    """按界面上的位置、力度、速度下发夹爪。右臂另有角度、圈数。"""
+    if arm not in ("89", "91"):
+        return {"ok": False, "message": "臂号只接受 89 或 91"}
+    if kind not in ("pos", "rotate", "rotate_by", "revolutions"):
+        return {"ok": False, "message": "没有这种夹爪调节"}
+    if kind != "pos" and arm != "91":
+        return {"ok": False, "message": "左臂平行爪没有旋转轴"}
+    try:
+        if kind == "pos":
+            pos = int(round(_bounded("位置", numbers.get("pos"), 0, 1000)))
+            force = int(round(_bounded("力度", numbers.get("force"), 20, 100)))
+            speed = int(round(_bounded("速度", numbers.get("speed"), 1, 100)))
+            cli = ["pos", str(pos)]
+            summary = f"位置 {pos}（0 合，1000 开）  力度 {force}%  速度 {speed}%"
+        else:
+            force = int(round(_bounded("力矩", numbers.get("torque"), 20, 100)))
+            speed = int(round(_bounded("旋转速度", numbers.get("rot_speed"), 1, 100)))
+            if kind == "rotate":
+                angle = _bounded("绝对角", numbers.get("angle"), -32767, 32767)
+                cli = ["rotate", f"{angle:.3f}"]
+                summary = f"绝对角 {angle:.3f}°  力矩 {force}%  速度 {speed}%"
+            elif kind == "rotate_by":
+                angle = _bounded("相对角", numbers.get("delta"), -32767, 32767)
+                cli = ["rotate_by", f"{angle:.3f}"]
+                summary = f"相对转 {angle:.3f}°  力矩 {force}%  速度 {speed}%"
+            else:
+                turns = _bounded("圈数", numbers.get("revolutions"), -50, 50)
+                cli = ["revolutions", f"{turns:.3f}"]
+                summary = f"转 {turns:.3f} 圈  力矩 {force}%  速度 {speed}%"
+    except ValueError as exc:
+        return {"ok": False, "message": str(exc)}
+    script = ROOT / "scripts" / "run_gripper.sh"
+    argv = [
+        "bash", str(script),
+        "--arm", arm,
+        "--force", str(force),
+        "--speed", str(speed),
+        "--cli", *cli,
+    ]
+    shown = " ".join(argv)
+    try:
+        proc = subprocess.run(argv, cwd=str(ROOT), capture_output=True, text=True, timeout=90)
+    except subprocess.TimeoutExpired:
+        return {"ok": False, "message": "夹爪 90 秒还没到位", "detail": shown}
+    detail = ((proc.stdout or "") + (proc.stderr or "")).strip()
+    ok = proc.returncode == 0
+    return {
+        "ok": ok,
+        "message": summary if ok else f"夹爪退出码 {proc.returncode}",
+        "detail": (summary + "\n" + shown + "\n" + detail)[-4000:],
+    }
+
+
+class Handler(BaseHTTPRequestHandler):
+    def log_message(self, fmt: str, *args) -> None:
+        return
+
+    def _send(self, code: int, body: bytes, content_type: str) -> None:
+        self.send_response(code)
+        self.send_header("Content-Type", content_type)
+        self.send_header("Content-Length", str(len(body)))
+        self.end_headers()
+        self.wfile.write(body)
+
+    def _json(self, code: int, payload: dict) -> None:
+        self._send(code, json.dumps(payload, ensure_ascii=False).encode(), "application/json; charset=utf-8")
+
+    def do_GET(self) -> None:
+        path = urlparse(self.path).path
+        if path in ("/", "/index.html"):
+            page = (ASSETS / "operator_console.html").read_text(encoding="utf-8")
+            payload = json.dumps(
+                {"status": status(), "catalog": catalog()},
+                ensure_ascii=False,
+            ).replace("<", "\\u003c")
+            page = page.replace("/*BOOT*/null", payload, 1)
+            self._send(200, page.encode(), "text/html; charset=utf-8")
+            return
+        if path == "/logo.png":
+            self._send(200, (ASSETS / "operator_console_logo.png").read_bytes(), "image/png")
+            return
+        if path == "/api/status":
+            self._json(200, status())
+            return
+        if path == "/api/catalog":
+            self._json(200, catalog())
+            return
+        parts = path.strip("/").split("/")
+        if len(parts) == 5 and parts[:3] == ["api", "tactile", "frame"] and parts[4].endswith(".jpg"):
+            side, kind = parts[3], parts[4][:-4]
+            blob = tactile_view.frame(side, kind) if side in ("left", "right") and kind in ("shear", "thermal") else None
+            if not blob:
+                self._json(404, {"ok": False, "message": "还没有这一帧"})
+                return
+            self.send_response(200)
+            self.send_header("Content-Type", "image/jpeg")
+            self.send_header("Cache-Control", "no-store")
+            self.send_header("Content-Length", str(len(blob)))
+            self.end_headers()
+            self.wfile.write(blob)
+            return
+        self._json(404, {"ok": False, "message": "没有这个路径"})
+
+    def do_POST(self) -> None:
+        path = urlparse(self.path).path
+        if path not in ("/api/action", "/api/tactile/view"):
+            self._json(404, {"ok": False, "message": "没有这个路径"})
+            return
+        length = int(self.headers.get("Content-Length") or 0)
+        try:
+            body = json.loads(self.rfile.read(length) or b"{}")
+        except json.JSONDecodeError:
+            self._json(400, {"ok": False, "message": "请求不是 JSON"})
+            return
+        if path == "/api/tactile/view":
+            action = "tactile_view_on" if body.get("on") else "tactile_view_off"
+            result = run_action(action, "89", False)
+            self._json(200 if result.get("ok") else 400, result)
+            return
+        result = run_action(
+            str(body.get("action") or ""),
+            str(body.get("arm") or "89"),
+            bool(body.get("confirm")),
+            str(body.get("mode") or "eye_in_hand"),
+        )
+        self._json(200 if result.get("ok") else 400, result)
+
+
+def main() -> None:
+    import atexit
+    atexit.register(tactile_view.stop)
+    threading.Thread(target=_poll_tactile, daemon=True).start()
+    server = ThreadingHTTPServer((HOST, PORT), Handler)
+    print(f"Orisys 工位界面  http://{HOST}:{PORT}", flush=True)
+    server.serve_forever()
+
+
+if __name__ == "__main__":
+    main()

src/arm_application/arm_application/operator_tactile_view.py

@@ -0,0 +1,353 @@
+"""工位界面用的视触觉成像。
+
+切向力是 SDK 的 ``VSHEAR`` 箭头，热成像是 ``DEPTH`` 的 JET 伪彩。
+相机独占，所以只有页面点了「开始成像」才打开，失败把原因交回去，不造图。
+"""
+
+from __future__ import annotations
+
+import os
+import re
+import threading
+import time
+from pathlib import Path
+
+import numpy as np
+
+_LOCK = threading.Lock()
+_STOP = threading.Event()
+_THREADS: list[threading.Thread] = []
+_ON_SAMPLE = None
+_STATE = {
+    "on": False,
+    "error": "",
+    "stamp": 0.0,
+    "note": "",
+    "frames": {},
+}
+_JPEG: dict[str, bytes] = {}
+
+
+def set_sample_hook(fn) -> None:
+    global _ON_SAMPLE
+    _ON_SAMPLE = fn
+
+
+def state() -> dict:
+    with _LOCK:
+        return {
+            "on": _STATE["on"],
+            "error": _STATE["error"],
+            "stamp": _STATE["stamp"],
+            "note": _STATE["note"],
+            "frames": dict(_STATE["frames"]),
+        }
+
+
+def frame(side: str, kind: str) -> bytes | None:
+    with _LOCK:
+        return _JPEG.get(f"{side}_{kind}")
+
+
+def _videos() -> list[str]:
+    found = []
+    by_path = Path("/dev/v4l/by-path")
+    if not by_path.is_dir():
+        return found
+    for link in sorted(by_path.iterdir()):
+        if not link.name.endswith("-video-index0"):
+            continue
+        real = os.path.realpath(link)
+        base = os.path.basename(real)
+        sysdev = os.path.realpath(f"/sys/class/video4linux/{base}/device")
+        dev = ""
+        for part in reversed(sysdev.split("/")):
+            if re.fullmatch(r"\d+-\d+(\.\d+)*", part):
+                dev = part
+                break
+        if not dev:
+            continue
+        vid = Path(f"/sys/bus/usb/devices/{dev}/idVendor")
+        pid = Path(f"/sys/bus/usb/devices/{dev}/idProduct")
+        if (
+            vid.is_file()
+            and pid.is_file()
+            and vid.read_text().strip() == "1bcf"
+            and pid.read_text().strip() == "2281"
+        ):
+            found.append(str(link))
+    return found
+
+
+def _resolve_cfg(site: dict) -> dict:
+    tactile = site.get("tactile") if isinstance(site.get("tactile"), dict) else {}
+    left = str(tactile.get("left_video") or "")
+    right = str(tactile.get("right_video") or "")
+    if not Path(left).exists() or not Path(right).exists():
+        found = _videos()
+        if len(found) >= 2:
+            left, right = found[0], found[1]
+        elif len(found) == 1:
+            left = right = found[0]
+    return {
+        "cfg": str(tactile.get("config_file") or ""),
+        "backend": str(tactile.get("infer_backend") or "cpu"),
+        "cuda": bool(tactile.get("cuda", False)),
+        "isstitch": bool(tactile.get("isstitch", True)),
+        "baseline": float(tactile.get("baseline_seconds") or 1.5),
+        "left": left,
+        "right": right,
+    }
+
+
+def _as_bgr(img):
+    if not isinstance(img, np.ndarray) or img.size == 0:
+        return None
+    import cv2
+
+    if img.ndim == 2:
+        return cv2.cvtColor(img, cv2.COLOR_GRAY2BGR)
+    if img.ndim == 3 and img.shape[2] >= 3:
+        return np.ascontiguousarray(img[:, :, :3])
+    return None
+
+
+def _flat(arr) -> bool:
+    if not isinstance(arr, np.ndarray) or arr.size == 0 or arr.ndim < 2:
+        return True
+    a = arr.reshape(-1, arr.shape[-1]) if arr.ndim == 3 else arr.reshape(-1)
+    finite = np.isfinite(a)
+    if not bool(finite.any()):
+        return True
+    vals = a[finite]
+    return float(np.max(vals) - np.min(vals)) < 1e-6
+
+
+def _thermal(depth):
+    import cv2
+
+    if depth is None:
+        return None
+    a = np.asarray(depth)
+    if a.size == 0:
+        return None
+    if a.ndim == 3 and a.shape[2] == 2:
+        a = np.linalg.norm(a.astype(np.float32), axis=2)
+    if a.ndim != 2:
+        return None
+    finite = np.isfinite(a)
+    if not finite.any():
+        return None
+    vals = a[finite]
+    lo, hi = float(vals.min()), float(vals.max())
+    if hi <= lo:
+        norm = np.zeros(a.shape, np.uint8)
+    else:
+        norm = np.clip((a - lo) / (hi - lo) * 255.0, 0, 255).astype(np.uint8)
+    return cv2.applyColorMap(norm, cv2.COLORMAP_JET)
+
+
+def _views(sensor, gray):
+    import cv2
+    import orisys
+
+    bgr = _as_bgr(gray)
+    h, w = (bgr.shape[0], bgr.shape[1]) if bgr is not None else (450, 420)
+    try:
+        sensor.compute_deformation(decompose=True, check_motion=False)
+    except Exception:
+        pass
+    try:
+        sensor.compute_contact()
+    except Exception:
+        pass
+    try:
+        vshear, vnormal, depth = sensor.read_info(
+            sensor.info.VSHEAR, sensor.info.VNORMAL, sensor.info.DEPTH
+        )
+    except Exception:
+        vshear, vnormal, depth = None, None, None
+    canvas = np.zeros((h, w, 3), np.uint8)
+    shear = canvas
+    if isinstance(vshear, np.ndarray) and vshear.ndim == 3 and vshear.shape[2] >= 2 and vshear.size:
+        field = np.ascontiguousarray(vshear[:, :, :2], dtype=np.float32)
+        if field.shape[0] != h or field.shape[1] != w:
+            field = cv2.resize(field, (w, h))
+        mag = np.linalg.norm(field, axis=2)
+        peak = float(np.percentile(mag, 99)) if mag.size else 0.0
+        arrow_scale = (2.0 * peak / 12.0) if peak > 1e-6 else 1.0
+        try:
+            shear = orisys.util.draw_arrows(
+                canvas,
+                field,
+                threshold=peak * 0.15 if peak > 1e-6 else 1.0,
+                grid_spacing=16,
+                arrow_scale=max(arrow_scale, 1e-6),
+                below_threshold_color=(40, 40, 40),
+            )
+        except Exception:
+            shear = canvas
+    heat = None if _flat(depth) else _thermal(depth)
+    if heat is None:
+        heat = _thermal(vnormal)
+    if not isinstance(heat, np.ndarray) or heat.ndim != 3:
+        heat = np.zeros((h, w, 3), np.uint8)
+    elif heat.shape[0] != h or heat.shape[1] != w:
+        heat = cv2.resize(heat, (w, h))
+    return shear, heat
+
+
+def _jpeg(img) -> bytes | None:
+    import cv2
+
+    if not isinstance(img, np.ndarray) or img.size == 0:
+        return None
+    ok, buf = cv2.imencode(".jpg", img, [int(cv2.IMWRITE_JPEG_QUALITY), 70])
+    if not ok:
+        return None
+    return buf.tobytes()
+
+
+def _store(side: str, shear, heat, sample: dict | None) -> None:
+    frames = {}
+    blobs = {}
+    for kind, img in (("shear", shear), ("thermal", heat)):
+        blob = _jpeg(img)
+        if blob:
+            blobs[f"{side}_{kind}"] = blob
+            frames[f"{side}_{kind}"] = True
+    with _LOCK:
+        _JPEG.update(blobs)
+        _STATE["frames"].update(frames)
+        _STATE["stamp"] = time.time()
+        if _STATE["error"].startswith(side):
+            _STATE["error"] = ""
+    if sample and _ON_SAMPLE is not None:
+        _ON_SAMPLE(side, sample)
+
+
+def _open_sensor(cfg: dict, side: str):
+    import orisys
+
+    return orisys.Sensor(
+        cfg[side],
+        cuda=cfg["cuda"],
+        isstitch=cfg["isstitch"],
+        verbose=False,
+        config_name=cfg["cfg"],
+        backend=cfg["backend"],
+    )
+
+
+def _worker(side: str, cfg: dict) -> None:
+    if not cfg.get(side):
+        with _LOCK:
+            _STATE["error"] = f"{side} 没有视频节点"
+        return
+    sensor = None
+    try:
+        sensor = _open_sensor(cfg, side)
+    except Exception as exc:
+        with _LOCK:
+            prev = _STATE["error"]
+            _STATE["error"] = (prev + "；" if prev else "") + f"{side} 打开失败：{exc}"
+        return
+    buckets = {"n": [], "x": [], "y": []}
+    deadline = time.perf_counter() + float(cfg["baseline"])
+    while time.perf_counter() < deadline and not _STOP.is_set():
+        try:
+            img = sensor.get_img()
+            sensor.compute_deformation(check_motion=False)
+            n, x, y = sensor.read_info(
+                sensor.info.FNORMAL, sensor.info.FSHEARX, sensor.info.FSHEARY
+            )
+            buckets["n"].append(float(n))
+            buckets["x"].append(float(x))
+            buckets["y"].append(float(y))
+        except Exception:
+            pass
+        time.sleep(0.02)
+    base = {
+        "n": float(np.mean(buckets["n"])) if buckets["n"] else 0.0,
+        "x": float(np.mean(buckets["x"])) if buckets["x"] else 0.0,
+        "y": float(np.mean(buckets["y"])) if buckets["y"] else 0.0,
+    }
+    with _LOCK:
+        base_note = "曲线已减去打开后短暂基线，单位仍是形变读数。"
+        if "右指" in str(_STATE["note"]):
+            _STATE["note"] = _STATE["note"] + " " + base_note
+        else:
+            _STATE["note"] = base_note
+    last_sample = 0.0
+    while not _STOP.is_set():
+        try:
+            gray = sensor.get_img()
+        except Exception:
+            gray = None
+        if gray is None:
+            time.sleep(0.05)
+            continue
+        shear, heat = _views(sensor, gray)
+        sample = None
+        now = time.time()
+        if now - last_sample >= 0.1:
+            try:
+                n, x, y = sensor.read_info(
+                    sensor.info.FNORMAL, sensor.info.FSHEARX, sensor.info.FSHEARY
+                )
+                sample = {
+                    "t": now,
+                    "fnormal": float(n) - base["n"],
+                    "fshearx": float(x) - base["x"],
+                    "fsheary": float(y) - base["y"],
+                }
+                last_sample = now
+            except Exception:
+                sample = None
+        _store(side, shear, heat, sample)
+        time.sleep(0.04)
+    try:
+        sensor.disconnect()
+    except Exception:
+        pass
+
+
+def start(site: dict) -> str:
+    global _THREADS
+    with _LOCK:
+        if _STATE["on"]:
+            return "成像已经开着"
+        _STATE["on"] = True
+        _STATE["error"] = ""
+        _STATE["frames"] = {}
+    _STOP.clear()
+    cfg = _resolve_cfg(site)
+    if not cfg["left"] and not cfg["right"]:
+        with _LOCK:
+            _STATE["on"] = False
+            _STATE["error"] = "没有找到 Orisys 相机"
+        return _STATE["error"]
+    sides = ["left"]
+    if cfg.get("right") and cfg.get("right") != cfg.get("left"):
+        sides.append("right")
+    elif not cfg.get("right") or cfg.get("right") == cfg.get("left"):
+        with _LOCK:
+            _STATE["note"] = "只找到一路 Orisys，右指没有画面。插上后重新开始成像。"
+    _THREADS = [
+        threading.Thread(target=_worker, args=(side, cfg), daemon=True, name=f"tac-{side}")
+        for side in sides
+    ]
+    for thread in _THREADS:
+        thread.start()
+    return "正在打开两只视触觉。切向力和热成像出来之前会先采一小段基线。"
+
+
+def stop() -> str:
+    _STOP.set()
+    for thread in list(_THREADS):
+        thread.join(timeout=2.0)
+    with _LOCK:
+        _STATE["on"] = False
+        _STATE["frames"] = {}
+        _JPEG.clear()
+    return "已停止视触觉成像"

src/arm_application/arm_application/operator_teach.py

@@ -0,0 +1,203 @@
+"""软拖拽示教：开拖拽、记下当前关节角、再回到该点。
+
+点位写在 ``handeye/tmp/<臂号>/operator_teach.yaml``。关节角单位是弧度，
+直接存桥接返回的 7 个数，不另做姿态换算。
+
+软拖拽占用 RDK，和 ``run_real.sh`` 不能同时连同一台臂。
+"""
+
+from __future__ import annotations
+
+import threading
+from datetime import datetime, timezone
+from pathlib import Path
+
+import yaml
+
+_LOCK = threading.Lock()
+_BRIDGE = None
+_SN = ""
+
+
+def _file(arm: str) -> Path:
+    from arm_application.operator_console import ROOT
+
+    if arm not in ("89", "91"):
+        raise ValueError("臂号只接受 89 或 91")
+    return ROOT / "handeye" / "tmp" / arm / "operator_teach.yaml"
+
+
+def _bridge_for(sn: str):
+    global _BRIDGE, _SN
+    sn = str(sn or "").strip()
+    if not sn:
+        raise RuntimeError("site.yaml 里这台臂没有序列号，拒绝连接")
+    if _BRIDGE is not None and _SN == sn and _BRIDGE._proc.poll() is None:
+        return _BRIDGE
+    close()
+    from arm_application.handeye_teach import RdkBridge
+
+    _BRIDGE = RdkBridge(sn)
+    _SN = sn
+    return _BRIDGE
+
+
+def close() -> None:
+    global _BRIDGE, _SN
+    bridge = _BRIDGE
+    _BRIDGE = None
+    _SN = ""
+    if bridge is None:
+        return
+    try:
+        bridge.close()
+    except Exception:
+        pass
+
+
+def drag_on(sn: str) -> dict:
+    with _LOCK:
+        try:
+            bridge = _bridge_for(sn)
+            result = bridge.drag_on(mode="soft")
+        except Exception as exc:
+            return {"ok": False, "message": str(exc)}
+    if not result.get("ok"):
+        return {"ok": False, "message": str(result.get("error") or result)}
+    return {
+        "ok": True,
+        "message": "软拖拽已开启。轻推机械臂，再记点。这一步要先停掉控制栈。",
+    }
+
+
+def drag_off(sn: str) -> dict:
+    with _LOCK:
+        try:
+            bridge = _bridge_for(sn)
+            result = bridge.drag_off()
+        except Exception as exc:
+            return {"ok": False, "message": str(exc)}
+    if not result.get("ok"):
+        return {"ok": False, "message": str(result.get("error") or result)}
+    return {"ok": True, "message": "软拖拽已关闭，臂保持当前位置。"}
+
+
+def list_points(arm: str) -> dict:
+    try:
+        path = _file(arm)
+    except ValueError as exc:
+        return {"ok": False, "message": str(exc), "points": []}
+    if not path.is_file():
+        return {"ok": True, "points": [], "file": str(path)}
+    try:
+        data = yaml.safe_load(path.read_text(encoding="utf-8")) or {}
+    except Exception as exc:
+        return {"ok": False, "message": f"示教文件读不了：{exc}", "points": [], "file": str(path)}
+    points = []
+    for row in data.get("points") or []:
+        if not isinstance(row, dict):
+            continue
+        joints = row.get("joints")
+        if not isinstance(joints, list) or len(joints) < 7:
+            continue
+        try:
+            values = [float(v) for v in joints[:7]]
+        except (TypeError, ValueError):
+            continue
+        if any(v != v for v in values):
+            continue
+        points.append({
+            "name": str(row.get("name") or "").strip(),
+            "utc": str(row.get("utc") or ""),
+            "joints": values,
+        })
+    points = [row for row in points if row["name"]]
+    return {"ok": True, "points": points, "file": str(path)}
+
+
+def _write(arm: str, points: list[dict]) -> Path:
+    path = _file(arm)
+    path.parent.mkdir(parents=True, exist_ok=True)
+    path.write_text(
+        yaml.safe_dump({"points": points}, allow_unicode=True, sort_keys=False),
+        encoding="utf-8",
+    )
+    return path
+
+
+def record(sn: str, arm: str, name: str) -> dict:
+    title = str(name or "").strip()
+    if not title or "/" in title or "\\" in title or title in (".", ".."):
+        return {"ok": False, "message": "点名不能为空，也不能带路径"}
+    with _LOCK:
+        try:
+            bridge = _bridge_for(sn)
+            result = bridge._ask({"cmd": "joints"}, timeout=2.0)
+        except Exception as exc:
+            return {"ok": False, "message": str(exc)}
+    if not result.get("ok"):
+        return {"ok": False, "message": str(result.get("error") or "没有读到关节，点位没写下去")}
+    raw = result.get("q") or []
+    if len(raw) < 7:
+        return {"ok": False, "message": "关节数不够 7 个，点位没写下去"}
+    try:
+        values = [round(float(v), 6) for v in raw[:7]]
+    except (TypeError, ValueError):
+        return {"ok": False, "message": "关节角不是数字，点位没写下去"}
+    if any(v != v for v in values):
+        return {"ok": False, "message": "关节角不是有限值，点位没写下去"}
+    listed = list_points(arm)
+    if not listed.get("ok"):
+        return listed
+    points = [row for row in listed["points"] if row["name"] != title]
+    points.append({
+        "name": title,
+        "utc": datetime.now(timezone.utc).strftime("%Y-%m-%dT%H:%M:%SZ"),
+        "joints": values,
+    })
+    path = _write(arm, points)
+    text = "  ".join(f"{v:.4f}" for v in values)
+    return {"ok": True, "message": f"已记录 {title}", "detail": f"{path}\n关节 rad：{text}", "points": points}
+
+
+def delete_point(arm: str, name: str) -> dict:
+    title = str(name or "").strip()
+    listed = list_points(arm)
+    if not listed.get("ok"):
+        return listed
+    kept = [row for row in listed["points"] if row["name"] != title]
+    if len(kept) == len(listed["points"]):
+        return {"ok": False, "message": f"没有名为 {title} 的点"}
+    path = _write(arm, kept)
+    return {"ok": True, "message": f"已删除 {title}", "detail": str(path), "points": kept}
+
+
+def goto(sn: str, arm: str, name: str, speed: float) -> dict:
+    title = str(name or "").strip()
+    listed = list_points(arm)
+    if not listed.get("ok"):
+        return listed
+    hit = next((row for row in listed["points"] if row["name"] == title), None)
+    if hit is None:
+        return {"ok": False, "message": f"没有名为 {title} 的点"}
+    try:
+        speed_f = float(speed)
+    except (TypeError, ValueError):
+        return {"ok": False, "message": "关节速度不是数字"}
+    if not 0.05 <= speed_f <= 0.35:
+        return {"ok": False, "message": "关节速度只接受 0.05–0.35 rad/s"}
+    with _LOCK:
+        try:
+            bridge = _bridge_for(sn)
+            bridge.drag_off()
+            result = bridge.move_joints(hit["joints"], speed_f)
+        except Exception as exc:
+            return {"ok": False, "message": str(exc)}
+    if not result.get("ok"):
+        return {"ok": False, "message": str(result.get("error") or result)}
+    text = "  ".join(f"{float(v):.4f}" for v in hit["joints"])
+    return {
+        "ok": True,
+        "message": f"已回到 {title}",
+        "detail": f"速度 {speed_f:.2f} rad/s\n关节 rad：{text}",
+    }

src/arm_application/arm_application/overhead_oneshot.py

@@ -28,6 +28,8 @@ from arm_application.path_profile import (  # 伺服和测试从本模块再导
     D435_NEAR_CRUISE_MPS,
     D435_NEAR_HOP_MAX_M,
     D435_NEAR_LEFTOVER_M,
+    D435_VENDOR_MOVEL_ACC_MPS2,
+    D435_VENDOR_MOVEL_VEL_MPS,
     D435_VERT_TILT_RAD,
     D435_SETTLE_S,
     FROZEN_J_HOP_MAX_M,
@@ -152,9 +154,9 @@ def d405_commands_arm(
 ) -> bool:
     """D405 这一帧的点能不能下发给左臂。
 
-    推理一直开着。正在用 D435 那一个点时，点到并且停稳之前不下发。
+    推理一直开着。正在用 D435 那一个点时，进程内 MoveL 结束并且停稳之前不下发。
-    停稳沿用一阶段的 ``StillLock``（site 里的 ``lock_still_sec`` /
+    停稳用 ``d435_handback_sec`` 和 ``lock_still_move_m``。赶路时 XY 已经进 15 mm
-    ``lock_still_move_m``），不另写一套计时。
+    也不算到位。不许在 MoveL 期间下发 D405（两套控制互抢会晃）。
     ``move_only``：只测 D435 位移，D405 点一律不下发（到位之后也不交）。
     """
     if move_only:
@@ -301,7 +303,7 @@ def oneshot_concat_hold_ori(q0, J, segments, *, R=None):
 
 
 def oneshot_full_cruise_mps(leftover_m: float, far_cruise_mps: float) -> float:
-    """整段巡航：工件上方 30 mm/s；远处封顶 ``D435_FULL_CRUISE_MPS``，不许再弹到 80。"""
+    """整段巡航：工件上方 45 mm/s；远处封顶 ``D435_FULL_CRUISE_MPS``，不许再弹到远处 hop 上限。"""
     far = max(1e-4, float(far_cruise_mps))
     if float(leftover_m) <= D435_NEAR_LEFTOVER_M:
         return min(D435_NEAR_CRUISE_MPS, far)
@@ -509,6 +511,226 @@ def pixel_on_base_plane(
     return np.asarray(point, dtype=float).reshape(3)
 
 
+def cam_xy_bias_axes(T_cam_to_base: np.ndarray):
+    """基座水平面上两个单位向量：``(视线前方, 画面右)``。
+
+    视线 = 相机 +Z 投到基座 XY（前倾 30° 时的交平面敏感轴）；
+    画面右 = 相机 +X 投到基座 XY（立体基线 / 图像 u 方向）。
+    水平投影退化时返回 None，不许拿 (0,0) 当轴。
+    """
+    T = np.asarray(T_cam_to_base, dtype=float).reshape(4, 4)
+    look = T[:3, :3] @ np.array([0.0, 0.0, 1.0], dtype=float)
+    right = T[:3, :3] @ np.array([1.0, 0.0, 0.0], dtype=float)
+    look_xy = np.asarray(look[:2], dtype=float)
+    right_xy = np.asarray(right[:2], dtype=float)
+    n_l = float(np.linalg.norm(look_xy))
+    n_r = float(np.linalg.norm(right_xy))
+    if n_l < 1e-6 or n_r < 1e-6:
+        return None
+    return look_xy / n_l, right_xy / n_r
+
+
+# 单次精拍这一步必须能走完「框心 → k1」。两孔间距约 60mm，半距 30mm。
+# 多走多拍的 12mm 封顶在这里会把一半孔距留下（2026-10-13 真机：指令 29mm 只走 12mm）。
+ONESHOT_ALIGN_CAP_M = 0.040
+# 下降后补 XY：一跳上限。2026-10-13 两跳合计 4.3mm 把近距检测带崩。
+ONCE_TRIM_MAX_STEP_M = 0.003
+ONCE_TRIM_REFUSE_M = 0.008
+HOLE_PAIR_TRIM_MIN_M = 0.050
+HOLE_PAIR_TRIM_MAX_M = 0.070
+# 悬停 → 观察高（约 15mm）单步上限。左臂间隙约 58mm，20mm × 3 步走完。
+# 观察高 → 贴面不再走这段，改斜线一步（XY 补偿 + 剩余 Z）。
+ONCE_APPROACH_STEP_M = 0.020
+# 悬停已对上时 ΔXY 必须为 0。site ``once_ff_look_per_m`` 同源。
+# 2026-10-14 下午：框心改平面交点后两条链路下臂都仍 +1.3mm X 过补插不进。
+# k=0.018 是深度框心时代的对冲，不是下降几何。笛卡尔 MoveL 只降 Z。
+ONCE_FF_LOOK_PER_M = 0.0
+ONCE_FF_MOVEL_VEL_MPS = 0.05
+
+
+def once_look_ff_xy(look_xy, descent_m: float, k: float):
+    """视线前馈 ΔXY [m]。k≤0 是笛卡尔锁 XY：返回 (0,0)，不是 None。
+
+    k>0 时沿 -look 走 k×下降。下降≤0 / 视线退化 → None。
+    """
+    if float(descent_m) <= 1e-9:
+        return None
+    if float(k) <= 1e-9:
+        return np.array([0.0, 0.0], dtype=float)
+    look = np.asarray(look_xy, dtype=float).reshape(2)
+    n = float(np.linalg.norm(look))
+    if n < 1e-6 or not np.all(np.isfinite(look)):
+        return None
+    look = look / n
+    dxy = -look * float(k) * float(descent_m)
+    if not np.all(np.isfinite(dxy)):
+        return None
+    return dxy
+
+
+def once_ff_tcp_xyz(
+    flange_xy,
+    tip_off_xy,
+    look_xy,
+    descent_m: float,
+    k: float,
+    surface_z_m: float,
+    gap_m: float,
+):
+    """规尖（厂家 MoveL 的 TCP）目标 [m]。Z 是面 + 间隙，不是法兰。"""
+    dxy = once_look_ff_xy(look_xy, descent_m, k)
+    if dxy is None:
+        return None
+    off = np.asarray(tip_off_xy, dtype=float).reshape(2)
+    fl = np.asarray(flange_xy, dtype=float).reshape(2)
+    if not np.all(np.isfinite(off)) or not np.all(np.isfinite(fl)):
+        return None
+    tcp = np.array(
+        [
+            float(fl[0]) + float(off[0]) + float(dxy[0]),
+            float(fl[1]) + float(off[1]) + float(dxy[1]),
+            float(surface_z_m) + float(gap_m),
+        ],
+        dtype=float,
+    )
+    if not np.all(np.isfinite(tcp)):
+        return None
+    return tcp, dxy
+
+
+def d435_movel_from_tcp(tcp_xyz, flange_xy, k0_xy):
+    """D435 粗定位 MoveL 目标 [m]：当前厂家 TCP + (K0−法兰) 的 ROS XY。
+
+    keep_z 由硬件用当前 TCP 的 Z 覆盖。ROS 绝对 K0 不能当 WORLD 目标
+    （2026-10-14 贴面 MoveL 已踩过）。缺数 / 非有限 → None。
+    """
+    try:
+        t = np.asarray(tcp_xyz, dtype=float).reshape(3)
+        fl = np.asarray(flange_xy, dtype=float).reshape(2)
+        k0 = np.asarray(k0_xy, dtype=float).reshape(2)
+    except (TypeError, ValueError):
+        return None
+    if t.size != 3 or fl.size != 2 or k0.size != 2:
+        return None
+    if not np.all(np.isfinite(t)) or not np.all(np.isfinite(fl)) or not np.all(np.isfinite(k0)):
+        return None
+    out = np.array(
+        [float(t[0]) + float(k0[0]) - float(fl[0]),
+         float(t[1]) + float(k0[1]) - float(fl[1]),
+         float(t[2])],
+        dtype=float,
+    )
+    if not np.all(np.isfinite(out)):
+        return None
+    return out
+
+
+def once_ff_movel_from_tcp(tcp_xyz, dxy, descent_m: float):
+    """厂家 MoveL 目标 = 当前 Studio TCP + (ΔXY, −下降) [m]。
+
+    2026-10-14：把 ROS 规尖绝对坐标发给 MoveL，k=0.018 与 0.035 都落到
+    WORLD (0.595, −0.468, 0.024)，法兰实走约 (+9, −8)mm。WORLD 原点与
+    ROS 基座不是同一点，绝对坐标不能用。相对位移才跟 k 有关。
+    """
+    if float(descent_m) <= 1e-9:
+        return None
+    t = np.asarray(tcp_xyz, dtype=float).reshape(3)
+    d = np.asarray(dxy, dtype=float).reshape(2)
+    if t.size != 3 or d.size != 2:
+        return None
+    if not np.all(np.isfinite(t)) or not np.all(np.isfinite(d)):
+        return None
+    out = np.array(
+        [float(t[0]) + float(d[0]), float(t[1]) + float(d[1]), float(t[2]) - float(descent_m)],
+        dtype=float,
+    )
+    if not np.all(np.isfinite(out)):
+        return None
+    return out
+
+
+def oneshot_align_step_m(err_m: float) -> float:
+    """单次精拍允许的步长 [m]。不超过半孔距余量；超过 cap 才截断。"""
+    e = abs(float(err_m))
+    cap = float(ONESHOT_ALIGN_CAP_M)
+    if e <= cap:
+        return e
+    return cap
+
+
+def hole_pair_span_m(p0, p1) -> float:
+    """两孔基座 XY 间距 [m]。"""
+    a = np.asarray(p0, dtype=float).reshape(-1)
+    b = np.asarray(p1, dtype=float).reshape(-1)
+    return float(np.hypot(float(a[0]) - float(b[0]), float(a[1]) - float(b[1])))
+
+
+def hole_pair_span_ok(span_m: float) -> bool:
+    """下降后补孔用的间距窗。真机 k1/k2 约 60mm；45mm 是近距检测漂了。"""
+    s = float(span_m)
+    return HOLE_PAIR_TRIM_MIN_M <= s <= HOLE_PAIR_TRIM_MAX_M
+
+
+def flange_z_for_gap_m(tip_len_m: float, surface_z_m: float, gap_m: float) -> float:
+    """规尖离面 ``gap_m`` 时的法兰 Z [m]。与轻降几何同一条式子。"""
+    return float(tip_len_m) + float(surface_z_m) + float(gap_m)
+
+
+def final_gap_refusal(
+    gap_m: float,
+    tool_len_m: float,
+    surface_z_m: float,
+    hover_z_m: float,
+    z_min_m: float,
+    z_max_m: float,
+) -> str:
+    """笛卡尔锁 XY 的贴面间隙能不能用。空串表示可以；非空就是拒绝原因。
+
+    参数是规尖离面 [m]，不是法兰 Z：法兰 = 工具长 + 工件面 + 间隙，只在这里算一次。
+    """
+    gap = float(gap_m)
+    if not np.isfinite(gap):
+        return "贴面间隙不是有限数，拒绝开跑"
+    if gap <= 1e-9:
+        return (
+            f"贴面间隙 {gap * 1000:.1f}mm ≤ 0，规尖会进工件/贴面接触，拒绝开跑"
+        )
+    tool = float(tool_len_m)
+    surf = float(surface_z_m)
+    if tool <= 0.0 or surf <= 0.0:
+        return "写了贴面间隙，但工具长度或工件面未知，无法确认尖端在面之上，拒绝开跑"
+    hover_clear = float(hover_z_m) - tool - surf
+    if gap > hover_clear + 1e-6:
+        return (
+            f"贴面间隙 {gap * 1000:.1f}mm 比悬停离面 {hover_clear * 1000:.1f}mm 还高，"
+            "锁 XY 只会抬臂，拒绝开跑"
+        )
+    flange = flange_z_for_gap_m(tool, surf, gap)
+    z_lo = float(z_min_m)
+    z_hi = float(z_max_m)
+    if flange < z_lo - 1e-6 or flange > z_hi + 1e-6:
+        return (
+            f"贴面法兰 Z {flange:.3f}m（规尖离面 {gap * 1000:.1f}mm）"
+            f"超出工作盒 Z [{z_lo:.3f}, {z_hi:.3f}]，拒绝开跑"
+        )
+    return ""
+
+
+def decompose_base_xy(delta_xy, T_cam_to_base: np.ndarray):
+    """把基座 XY 残差拆成沿视线 / 沿画面右 [m]。投影退化返回 None。"""
+    axes = cam_xy_bias_axes(T_cam_to_base)
+    if axes is None:
+        return None
+    look_xy, right_xy = axes
+    d = np.asarray(delta_xy, dtype=float).reshape(2)
+    return {
+        "along_look_m": float(np.dot(d, look_xy)),
+        "along_right_m": float(np.dot(d, right_xy)),
+        "look_xy": [float(look_xy[0]), float(look_xy[1])],
+        "right_xy": [float(right_xy[0]), float(right_xy[1])],
+    }
+
+
 def px_to_cam(
     u: float, v: float, z: float, fx: float, fy: float, cx: float, cy: float
 ) -> np.ndarray:

src/arm_application/arm_application/path_profile.py

@@ -11,10 +11,15 @@ from typing import Optional, Tuple
 
 import numpy as np
 
-# 整段同一巡航：site ``v_slow_xy`` × 本系数。1.0 → 80 mm/s（二阶段对孔同档上限）。
+# 整段同一巡航：site ``v_slow_xy`` × 本系数。1.5 → 120 mm/s（远处 hop 上限）。
-D435_MOVE_SPEED_SCALE = 1.0
+D435_MOVE_SPEED_SCALE = 1.5
+# D435 厂家 MoveL：SDK ``vel`` [m/s]、``acc`` [m/s²]。
+# 厂家示例自由空间 0.6 / 近接触 0.2；本档 0.3 和 1.5，短行程也能爬到巡航。
+# 只改 vel、不给 acc 时，短 hop 仍被默认加速度卡住，肉眼看不出。
+D435_VENDOR_MOVEL_VEL_MPS = 0.3
+D435_VENDOR_MOVEL_ACC_MPS2 = 1.5
 # 缺路程时的兜底加速度。真机 hop 用 ``trap_acc_for_hop``，和二阶段同一条刹车。
-D435_MOVE_ACC_MPS2 = 0.125
+D435_MOVE_ACC_MPS2 = 0.1875
 # action 走完后再停这么久，才重测 TF。二阶段 ``_goto`` 的 settle_s。
 D435_SETTLE_S = 0.25
 # 冻结雅可比一次只信这么远。二阶段 ``_plan_and_send`` 默认 ``max_seg`` 同源。
@@ -23,7 +28,7 @@ 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
+D435_NEAR_CRUISE_MPS = 0.045
 # 法兰倾角超过这个才单独收垂直。和 XY 捆在同一跳会把腕部漏成 Y 过冲。
 D435_VERT_TILT_RAD = 0.015
 D435_MID_LEFTOVER_M = 0.15
@@ -33,10 +38,11 @@ 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²。
+# 峰值加速度 a_max = π v² / (4 d_acc)；150 mm @ 90 mm/s → 约 0.042 m/s²。
-D435_FULL_ACC_M = 0.100
+# 巡航相对原 60 mm/s 加 50%；加减速路程同比加长，起步时间差不多。
-D435_FULL_DEC_M = 0.080
+D435_FULL_ACC_M = 0.150
-D435_FULL_CRUISE_MPS = 0.060
+D435_FULL_DEC_M = 0.120
+D435_FULL_CRUISE_MPS = 0.090
 
 
 def hybrid_cruise_vxy(

src/arm_application/arm_application/perception_node.py

@@ -77,6 +77,7 @@ from arm_application.ros_node import TolerantNode
 
 # D435 一次性推理用的 seq。必须远离腕部帧序号，结果绝不能当 D405 检测去立体匹配。
 _ONESHOT_SEQ = 2_000_000_000
+_ONESHOT_VIZ_SEQ = 2_000_000_001
 
 
 def _bgr_copy(msg: Image) -> np.ndarray:
@@ -245,6 +246,8 @@ class PerceptionNode(TolerantNode):
         self._z_locked = False
         self._oneshot_img: Optional[Image] = None
         self._oneshot_depth: Optional[Image] = None
+        self._oneshot_viz_only = False
+        self._oh_viz_t = -1.0e9
         if self.overhead_oneshot_enable:
             self.wrist_handoff_conf = float(
                 self.get_parameter("wrist_handoff_conf").value
@@ -499,6 +502,10 @@ class PerceptionNode(TolerantNode):
             reliability=ReliabilityPolicy.RELIABLE,
         )
         self._det_pub = self.create_publisher(Float32MultiArray, "workpiece/detection", det_qos)
+        # 顶视框单独话题：像素是 D435 的，不能叠到 D405 窗上。
+        self._oh_det_pub = self.create_publisher(
+            Float32MultiArray, "workpiece/detection_overhead", det_qos
+        )
         self._tf_br = TransformBroadcaster(self)
 
         vision_hz = max(5.0, min(40.0, float(self.get_parameter("vision_hz").value)))
@@ -601,6 +608,7 @@ class PerceptionNode(TolerantNode):
             self.detector.restart()
             self._submitted_seq = -1
             self._oneshot_inflight = False
+            self._oneshot_viz_only = False
             self._last_gpu_t = time.monotonic()
             self.get_logger().info("YOLO 工作进程已重启")
         except Exception as exc:  # noqa: BLE001
@@ -653,12 +661,17 @@ class PerceptionNode(TolerantNode):
                 continue
             if int(seq) >= _ONESHOT_SEQ:
                 try:
-                    self._finish_overhead_oneshot(dets, float(self._infer_meta.get(int(seq), 1.0)))
+                    self._finish_overhead_oneshot(
+                        dets,
+                        float(self._infer_meta.get(int(seq), 1.0)),
+                        viz_only=bool(int(seq) == _ONESHOT_VIZ_SEQ),
+                    )
                 except Exception as exc:  # noqa: BLE001
                     self.get_logger().error(
                         f"D435 一次性结果处理失败：{exc}", throttle_duration_sec=2.0
                     )
                     self._oneshot_inflight = False
+                    self._oneshot_viz_only = False
                     self._oneshot_t = time.monotonic()
                 continue
             scale = float(self._infer_meta.get(int(seq), 1.0))
@@ -670,7 +683,9 @@ class PerceptionNode(TolerantNode):
                 self.get_logger().error(f"应用检测结果出错：{exc}", throttle_duration_sec=2.0)
             self._done_seq = int(seq)
             if self._need_oneshot and not self._oneshot_inflight:
-                self._submit_overhead_oneshot()
+                self._submit_overhead_oneshot(viz_only=False)
+            elif self._should_submit_overhead_viz() and not self._oneshot_inflight:
+                self._submit_overhead_oneshot(viz_only=True)
             else:
                 self._submit_latest()
             dt_ms = (time.monotonic() - t0) * 1000.0
@@ -1430,68 +1445,105 @@ class PerceptionNode(TolerantNode):
         self._overhead_k0_pub.publish(pt)
         del is_k0  # 语义写在日志里；话题保持 PointStamped，避免再抄一份类型
 
-    def _submit_overhead_oneshot(self) -> None:
+    def _should_submit_overhead_viz(self) -> bool:
+        """低频率给 D435 窗刷绿框；不发控制点。赶路时不抢 GPU。"""
+        if not self.overhead_oneshot_enable or self.d435_move_only:
+            return False
+        if self._d435_commanding or self._need_oneshot:
+            return False
+        return (time.monotonic() - self._oh_viz_t) >= 0.5
+
+    def _publish_overhead_det(
+        self, xyxy: np.ndarray, conf: float, cls_id: float, uv: np.ndarray, z: float
+    ) -> None:
+        det = Float32MultiArray()
+        det.data = [
+            float(xyxy[0]), float(xyxy[1]), float(xyxy[2]), float(xyxy[3]),
+            float(conf), float(cls_id),
+            float(uv[0]), float(uv[1]), float(z),
+        ]
+        self._oh_det_pub.publish(det)
+
+    def _publish_overhead_det_clear(self) -> None:
+        msg = Float32MultiArray()
+        msg.data = []
+        self._oh_det_pub.publish(msg)
+
+    def _submit_overhead_oneshot(self, *, viz_only: bool = False) -> None:
         """把最新 D435 帧排进同一条 YOLO 队列。不在这里等结果 —— 等会把 D405 卡死。"""
         if self._oneshot_inflight or not self.overhead_oneshot_enable:
             return
         now = time.monotonic()
-        if now - self._oneshot_t < self.overhead_cooldown_s:
+        if (not viz_only) and now - self._oneshot_t < self.overhead_cooldown_s:
             return
         img = self._oh_image
         depth = self._oh_depth
         if img is None or depth is None:
-            self.get_logger().warn(
+            if not viz_only:
-                "D405 无合格目标，但 D435 还没出图 —— 不编造点",
+                self.get_logger().warn(
-                throttle_duration_sec=1.0,
+                    "D405 无合格目标，但 D435 还没出图 —— 不编造点",
-            )
+                    throttle_duration_sec=1.0,
+                )
             return
         if (now - self._oh_image_mono) > self.overhead_frame_max_age_s:
-            self.get_logger().warn(
+            if not viz_only:
-                f"D435 彩图过期 {(now - self._oh_image_mono)*1000:.0f}ms —— 不反投影",
+                self.get_logger().warn(
-                throttle_duration_sec=1.0,
+                    f"D435 彩图过期 {(now - self._oh_image_mono)*1000:.0f}ms —— 不反投影",
-            )
+                    throttle_duration_sec=1.0,
+                )
             return
-        if (now - self._oh_depth_mono) > self.overhead_frame_max_age_s:
+        if (not viz_only) and (now - self._oh_depth_mono) > self.overhead_frame_max_age_s:
             self.get_logger().warn(
                 f"D435 深度过期 {(now - self._oh_depth_mono)*1000:.0f}ms —— 不反投影",
                 throttle_duration_sec=1.0,
             )
             return
-        if self._oh_fx <= 1.0:
+        if (not viz_only) and self._oh_fx <= 1.0:
             self.get_logger().warn(
                 "D435 内参还没到（camera_info）—— 不反投影",
                 throttle_duration_sec=1.0,
             )
             return
-        if self._T_oh is None:
+        if (not viz_only) and self._T_oh is None:
             self.get_logger().error("眼在手外 T 没加载 —— 一次性定位拒绝")
             return
+        seq = _ONESHOT_VIZ_SEQ if viz_only else _ONESHOT_SEQ
         try:
             bgr = _bgr_copy(img)
             small, scale = self._prepare_infer(bgr)
-            self._infer_meta[_ONESHOT_SEQ] = scale
+            self._infer_meta[seq] = scale
-            if not self.detector.submit(small, _ONESHOT_SEQ):
+            if not self.detector.submit(small, seq):
                 return
         except Exception as exc:  # noqa: BLE001
             self.get_logger().warn(
-                f"D435 一次性提交失败：{exc}", throttle_duration_sec=2.0
+                f"D435 {'预览' if viz_only else '一次性'}提交失败：{exc}",
+                throttle_duration_sec=2.0,
             )
             return
         self._oneshot_inflight = True
+        self._oneshot_viz_only = bool(viz_only)
         self._oneshot_img = img
         self._oneshot_depth = depth
-        self.get_logger().info("D435 一次性已提交（不打断 D405 队列）")
+        if viz_only:
+            self._oh_viz_t = now
+        else:
+            self.get_logger().info("D435 一次性已提交（不打断 D405 队列）")
 
-    def _finish_overhead_oneshot(self, dets, scale: float) -> None:
+    def _finish_overhead_oneshot(
-        """YOLO 返回的顶视结果 → 基座系 K0。失败不发假点。"""
+        self, dets, scale: float, *, viz_only: bool = False
+    ) -> None:
+        """YOLO 返回的顶视结果。viz_only 只刷绿框；控制路径才发基座点。"""
         self._oneshot_inflight = False
+        viz_only = bool(viz_only or self._oneshot_viz_only)
+        self._oneshot_viz_only = False
         now = time.monotonic()
-        self._oneshot_t = now
+        if not viz_only:
+            self._oneshot_t = now
         img = self._oneshot_img
         depth = self._oneshot_depth
         self._oneshot_img = None
         self._oneshot_depth = None
-        if dets is INFER_BUSY or dets is None or img is None or depth is None:
+        if dets is INFER_BUSY or dets is None or img is None:
             return
         if scale != 1.0:
             for d in dets:
@@ -1499,13 +1551,27 @@ class PerceptionNode(TolerantNode):
                 if getattr(d, "kpts", None) is not None:
                     d.kpts = np.asarray(d.kpts, dtype=float).copy()
                     d.kpts[:, :2] *= scale
-        det = select_oneshot_det(dets, self.overhead_oneshot_conf)
+        det = select_oneshot_det(dets, self.overhead_oneshot_conf if not viz_only else 0.25)
         if det is None:
+            self._publish_overhead_det_clear()
+            if not viz_only:
+                self._oneshot_t = now
+                self.get_logger().warn(
+                    f"D435 一次性：画面里没有 conf≥{self.overhead_oneshot_conf:.2f} 的工件",
+                    throttle_duration_sec=1.0,
+                )
+            return
+        xyxy = np.asarray(det.xyxy, dtype=float).reshape(4)
+        uv = np.array(
+            [0.5 * (xyxy[0] + xyxy[2]), 0.5 * (xyxy[1] + xyxy[3])], dtype=float
+        )
+        self._publish_overhead_det(
+            xyxy, float(det.conf), float(det.cls_id), uv, 0.0
+        )
+        if viz_only:
+            return
+        if depth is None:
             self._oneshot_t = now
-            self.get_logger().warn(
-                f"D435 一次性：画面里没有 conf≥{self.overhead_oneshot_conf:.2f} 的工件",
-                throttle_duration_sec=1.0,
-            )
             return
         raw = self._decode_depth_m(depth)
         depth_m = depth_raw_to_m(raw, depth.encoding or "")

src/arm_application/arm_application/pipeline_node.py

@@ -40,6 +40,36 @@ TRACK_CLEARANCE_M = 0.05
 TRACK_LOCK_GAP_M = 0.040
 
 
+def init_z_refusal(
+    z_m: float,
+    tool_len_m: float,
+    surface_z_m: float,
+    gap_m: float,
+    z_min: float,
+    z_max: float,
+) -> str:
+    """初始法兰高度能不能用。空串表示可以；非空就是拒绝原因，不许悄悄抬高。"""
+    z = float(z_m)
+    if z < float(z_min) - 1e-6 or z > float(z_max) + 1e-6:
+        return (
+            f"初始法兰 Z {z:.3f}m 超出工作盒 [{float(z_min):.3f}, {float(z_max):.3f}]，"
+            "拒绝开跑"
+        )
+    tool = float(tool_len_m)
+    surf = float(surface_z_m)
+    if tool <= 0.0 or surf <= 0.0:
+        return "写了初始高度，但工具长度或工件面未知，无法确认尖端在面之上，拒绝开跑"
+    clear = z - tool - surf
+    if clear < float(gap_m) - 0.001:
+        return (
+            f"初始法兰 Z {z:.3f}m 会让工具尖进入工件："
+            f"尖端 {z - tool:.3f}，工件面 {surf:.3f}，"
+            f"间隙 {clear * 1000:.1f}mm，要求 ≥ {float(gap_m) * 1000:.1f}mm。"
+            "拒绝开跑，不会悄悄抬高。"
+        )
+    return ""
+
+
 def track_lock_z(hover_z: float, tool_len_m: float, surface_z_m: float) -> float:
     """一阶段锁高。
 
@@ -97,6 +127,7 @@ class AssemblyPipeline(Stage3Search):
         self.declare_parameter("skip_stage2", False)
         self.declare_parameter("skip_stage3", False)  # 默认搜孔+轻压；拧入靠 --depth
         self.declare_parameter("d435_move_only", False)
+        self.declare_parameter("init_z_m", -1.0)  # >0：启动后先到这个法兰高度
         self.declare_parameter("stage1_timeout_sec", 150.0)
         self.linger3 = 1.2  # 串联结束短保持；单独跑 stage3 仍用 site linger_sec
 
@@ -183,37 +214,64 @@ class AssemblyPipeline(Stage3Search):
         self._kill_old_stage1()
 
     def _track_z(self) -> float:
-        """一阶段锁高：见 :func:`track_lock_z`。对孔完成前不再改这个高度。"""
+        """一阶段锁高。写了 ``init_z_m`` 就用它，否则见 :func:`track_lock_z`。"""
+        z = float(self.get_parameter("init_z_m").value)
+        if z > 0.0:
+            return z
         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:
+    def _ensure_clearance(self) -> bool:
-        """上一轮轻压可能还在孔里：先纯 Z 抬到跟踪高，禁止 0.23↔0.35 来回。"""
+        """没写初始高度：只在低于锁高时抬上去。写了：升或降到那个法兰 Z，不安全就拒绝。"""
+        user = float(self.get_parameter("init_z_m").value) > 0.0
         q, Tee = self._wait_state(4.0)
         if q is None or Tee is None:
-            return
+            if user:
+                self.get_logger().error("设了 --init-z，但拿不到当前位姿，拒绝开跑")
+                return False
+            return True
         tz = self._track_z()
+        if user:
+            why = init_z_refusal(
+                tz,
+                float(getattr(self, "tool_tip_below_flange_m", 0.0) or 0.0),
+                float(getattr(self, "surface_z_m", 0.0) or 0.0),
+                float(self.final_gap_m),
+                float(self.ws_min[2]),
+                float(self.ws_max[2]),
+            )
+            if why:
+                self.get_logger().error(why)
+                return False
         z = float(Tee[2, 3])
-        if z >= tz - 0.008:
+        if user:
-            return
+            if abs(z - tz) <= 0.008:
+                self.get_logger().info(f"已在初始高度 Z={z:.3f}m（目标 {tz:.3f}）")
+                return True
+        elif z >= tz - 0.008:
+            return True
         self._state("pre_lift")
-        self.get_logger().info(
+        self.get_logger().info(f"先到初始高度：Z {z:.3f} → {tz:.3f}m")
-            f"先到锁高：Z {z:.3f} → {tz:.3f}m（跟踪与对孔都停在这个高度，对完孔再下降）"
+        ok, q, Tee = self._goto(
-        )
-        _, q, Tee = self._goto(
             q, Tee, np.array([float(Tee[0, 3]), float(Tee[1, 3]), tz]),
             verticalize=True, tol_m=0.003, tol_tilt=self.vert_tol_rad * 2.0,
             tag="pre_lift", v=self.move_speed, max_hops=8, hold_xy=True, settle_s=0.25,
         )
         q2 = self._q()
         self._pin(q2 if q2 is not None else q)
+        z2 = float(Tee[2, 3])
+        if user and (not ok or abs(z2 - tz) > 0.008):
+            self.get_logger().error(f"初始高度没到位：Z={z2:.3f}，目标 {tz:.3f}")
+            return False
+        return True
 
     def _run_stage1(self) -> int:
         """拉起 perception+servo，等到 /visual_servo/locked 后冻结并拆跟踪。"""
-        self._ensure_clearance()
+        if not self._ensure_clearance():
+            return 2
         self._state("stage1_track")
         self._kill_old_stage1()
         self._locked = False
@@ -289,7 +347,8 @@ class AssemblyPipeline(Stage3Search):
                 if code != 0:
                     return code
             else:
-                self._ensure_clearance()
+                if not self._ensure_clearance():
+                    return 2
                 self._state("skip_stage1")
 
             q, Tee = self._wait_state(10.0)

src/arm_application/arm_application/realsense_view.py

@@ -63,11 +63,16 @@ class RealsenseView(TolerantNode):
         self.declare_parameter('camera_name', 'd405')
         self.declare_parameter('overlay_hold_sec', 0.10)
         self.declare_parameter('overlay_detection', True)
+        # D405 用 workpiece/detection；D435 用 workpiece/detection_overhead（像素坐标系不同）。
+        self.declare_parameter('detection_topic', 'workpiece/detection')
         self.declare_parameter('view_hz', 20.0)
         self.declare_parameter('hud_label', '')
         self._name = str(self.get_parameter('camera_name').value).strip().strip('/')
         self._hold = max(0.0, float(self.get_parameter('overlay_hold_sec').value))
         self._overlay_detection = bool(self.get_parameter('overlay_detection').value)
+        self._det_topic = str(self.get_parameter('detection_topic').value or '').strip() or (
+            'workpiece/detection'
+        )
         self._hud = str(self.get_parameter('hud_label').value or '').strip()
 
         self._color: Optional[Image] = None
@@ -106,7 +111,7 @@ class RealsenseView(TolerantNode):
         )
         if self._overlay_detection:
             self.create_subscription(
-                Float32MultiArray, 'workpiece/detection', self._on_det, det_qos
+                Float32MultiArray, self._det_topic, self._on_det, det_qos
             )
         # stage2 孔对准叠图（k1/k2 标注 + 残差 HUD），有它时优先显示
         self.create_subscription(
@@ -128,6 +133,7 @@ class RealsenseView(TolerantNode):
         self.create_timer(1.0 / hz, self._push)
         self.get_logger().info(
             f'查看器已启动 —— 订阅 /{self._name}/color|depth|infra1|infra2 '
+            f'检测={self._det_topic if self._overlay_detection else "关"} '
             f'（保留 {self._hold:.2f}s）。关闭窗口 = 仅隐藏。'
         )
 
@@ -190,6 +196,9 @@ class RealsenseView(TolerantNode):
             self.get_logger().warn(f"site.yaml 重新加载失败：{exc}")
             return
         if "overlay_hold_sec" in cam:
+            # 顶视检测频率低，保留 launch 里设的较长 hold，别被 site 的 0.1s 冲掉。
+            if "overhead" in self._det_topic:
+                return
             hold = max(0.0, float(cam.get("overlay_hold_sec") or 0.0))
             if abs(hold - self._hold) > 1e-6:
                 self.get_logger().info(

src/arm_application/arm_application/servo_node.py

@@ -14,12 +14,11 @@ from __future__ import annotations
 
 import os
 import math
-import subprocess
 import sys
 import threading
 import time
 from collections import deque
-from typing import Deque, List, Optional
+from typing import Deque, List, Optional, Tuple
 
 import numpy as np
 import rclpy
@@ -35,6 +34,7 @@ from rclpy.qos import (
     HistoryPolicy,
     QoSProfile,
     ReliabilityPolicy,
+    qos_profile_sensor_data,
     qos_profile_system_default,
 )
 from sensor_msgs.msg import JointState
@@ -52,9 +52,11 @@ from arm_application.handeye_lib import cam_point_to_base, cam_to_base_T
 from arm_application.overhead_oneshot import (
     D435_NEAR_CRUISE_MPS,
     D435_SETTLE_S,
+    D435_VENDOR_MOVEL_VEL_MPS,
     StillLock,
     at_lock_height,
     d405_commands_arm,
+    d435_movel_from_tcp,
     flange_tilt_rad,
     hold_translation_vz,
     hybrid_cruise_vxy,
@@ -86,6 +88,11 @@ from arm_application.site_io import (
 
 from arm_application.ros_node import TolerantNode
 
+try:
+    from flexiv_msgs.msg import RobotStates
+except ImportError:  # pragma: no cover
+    RobotStates = None  # type: ignore
+
 
 DEFAULT_JOINTS = [
     "joint1", "joint2", "joint3", "joint4", "joint5", "joint6", "joint7",
@@ -216,8 +223,11 @@ class ServoNode(TolerantNode):
         # 串联用：XY 入死区 + Z 到锁高持续这么久 → /visual_servo/locked=True
         self.declare_parameter("lock_hold_sec", 2.0)
         # D405 跟住后法兰静止多久进二阶段。哨兵 -1 = 没从 site 接入。
-        # lock_still_sec 只用于 D435 到位后的停稳；框心对准进对孔用 wrist_align_sec。
+        # lock_still_sec 仍须从 site 接上（缺了就报错）。
+        # 交回 D405：进程内 MoveL 到位后，再叠 d435_handback_sec（法兰 XY 停稳）。
+        # 旧杀栈冷启动曾把这 3s 垫在控制器重启后面；不杀栈后只留机械停稳。
         self.declare_parameter("lock_still_sec", -1.0)
+        self.declare_parameter("d435_handback_sec", -1.0)
         self.declare_parameter("wrist_align_sec", -1.0)
         self.declare_parameter("lock_still_move_m", -1.0)
         self.declare_parameter("wrist_handoff_conf", -1.0)
@@ -305,6 +315,7 @@ class ServoNode(TolerantNode):
         self.ki_hover = float(self.get_parameter("ki_hover").value)
         self.lock_hold_sec = float(self.get_parameter("lock_hold_sec").value)
         self.lock_still_sec = float(self.get_parameter("lock_still_sec").value)
+        self.d435_handback_sec = float(self.get_parameter("d435_handback_sec").value)
         self.wrist_align_sec = float(self.get_parameter("wrist_align_sec").value)
         self.lock_still_move_m = float(self.get_parameter("lock_still_move_m").value)
         self.wrist_handoff_conf = float(self.get_parameter("wrist_handoff_conf").value)
@@ -312,21 +323,24 @@ class ServoNode(TolerantNode):
         self.d435_move_only = bool(self.get_parameter("d435_move_only").value)
         self._hybrid = (
             self.lock_still_sec >= 0.0
+            and self.d435_handback_sec >= 0.0
             and self.wrist_align_sec >= 0.0
             and self.lock_still_move_m >= 0.0
             and self.wrist_handoff_conf >= 0.0
             and self.overhead_hold_s >= 0.0
         )
-        # 框心对准 1s 进对孔。D435 停稳仍用 lock_still_sec，两段不要合成一个数。
+        # 框心对准进对孔用 wrist_align_sec。D435 交回只用 d435_handback_sec，两段不要合成一个数。
         self._still = StillLock(
             still_sec=max(0.0, self.wrist_align_sec),
             still_move_m=max(0.0, self.lock_still_move_m),
         )
         self._arrive = StillLock(
-            still_sec=max(0.0, self.lock_still_sec),
+            still_sec=max(0.0, self.d435_handback_sec),
             still_move_m=max(0.0, self.lock_still_move_m),
         )
+        self._sw_cli = None
         self._d435_vendor_done = False
+        self._d435_vendor_busy = False
         self._d435_vendor_failed = False
         self._d435_settled = False
         self._d435_exec_until = 0.0
@@ -377,8 +391,8 @@ class ServoNode(TolerantNode):
             )
         if self.handeye_mode == "eye_in_hand" and not self._hybrid:
             raise RuntimeError(
-                "腕部主跟踪必须从 site.yaml 接入 lock_still_sec / wrist_align_sec / "
+                "腕部主跟踪必须从 site.yaml 接入 lock_still_sec / d435_handback_sec / "
-                "lock_still_move_m / wrist_handoff_conf / overhead_hold_s。"
+                "wrist_align_sec / lock_still_move_m / wrist_handoff_conf / overhead_hold_s。"
                 "收到哨兵 -1 = 配置没接线，不许用代码默认秒数假装已经静止判据。"
             )
         R_he = np.asarray(self.T_cam_to_gripper[:3, :3], dtype=float)
@@ -503,6 +517,24 @@ class ServoNode(TolerantNode):
             10,
             callback_group=self._control_cbg,
         )
+        # 厂家 tcp_pose（WORLD）。D435 MoveL 必须在这上面加 ROS 相对位移。
+        self._tcp_world: Optional[Tuple[float, float, float]] = None
+        self._t_tcp: Optional[float] = None
+        prefix_ns = str(self.get_parameter("joint_prefix").value or "").rstrip("_")
+        self.robot_states_topic = f"/{prefix_ns.replace('-', '_')}/flexiv_robot_states"
+        if RobotStates is not None:
+            self.create_subscription(
+                RobotStates,
+                self.robot_states_topic,
+                self._on_servo_robot_states,
+                qos_profile_sensor_data,
+                callback_group=self._state_cbg,
+            )
+        else:
+            self.get_logger().error(
+                "没有 flexiv_msgs.RobotStates，D435 粗定位不能读厂家 tcp_pose，"
+                "不许退回杀栈 MoveL"
+            )
 
         cmd_qos = QoSProfile(
             history=HistoryPolicy.KEEP_LAST,
@@ -598,6 +630,23 @@ class ServoNode(TolerantNode):
     def _on_js(self, msg: JointState) -> None:
         self._last_js = msg
 
+    def _on_servo_robot_states(self, msg) -> None:
+        """缓存厂家 ``tcp_pose``（WORLD）。名字避开其它节点的 ``_on_robot_states``。"""
+        p = msg.tcp_pose.pose.position
+        xyz = (float(p.x), float(p.y), float(p.z))
+        if not all(math.isfinite(v) for v in xyz):
+            return
+        self._tcp_world = xyz
+        self._t_tcp = time.monotonic()
+
+    def _fresh_tcp_world(self):
+        """最近 0.5s 的厂家 TCP [m]。过期/没有 → None，不许用旧值凑。"""
+        if self._tcp_world is None or self._t_tcp is None:
+            return None
+        if time.monotonic() - float(self._t_tcp) > 0.5:
+            return None
+        return self._tcp_world
+
     def _on_wrist_conf(self, msg: Float32) -> None:
         c = float(msg.data)
         with self._lock:
@@ -608,6 +657,13 @@ class ServoNode(TolerantNode):
         """D435 一次性：点已经在基座系。再乘腕部 T 会差几十厘米还看起来合理。"""
         if not self._hybrid:
             return
+        if not self.enable_control:
+            # 仅观察时不占臂：占着 d435_commanding 会把感知的预览框也卡死。
+            self.get_logger().info(
+                "仅观察：收到 D435 点，不占臂、不挡 D405 预览",
+                throttle_duration_sec=2.0,
+            )
+            return
         fid = str(msg.header.frame_id or "").strip()
         if fid and self.base_frame and fid != self.base_frame:
             self.get_logger().error(
@@ -649,6 +705,9 @@ class ServoNode(TolerantNode):
             self._target_lost_flag = False
             self._track_source = "overhead"
             self._d435_settled = False
+            self._d435_vendor_done = False
+            self._d435_vendor_busy = False
+            self._d435_vendor_failed = False
             self._overhead_t = now_m
             self._last_seen_time = now_m
             self._stamp_sec = now_sec
@@ -670,6 +729,7 @@ class ServoNode(TolerantNode):
         self._d435_full_done = False
         self._d435_fix_done = False
         self._d435_vendor_done = False
+        self._d435_vendor_busy = False
         self._d435_vendor_failed = False
         self._publish_d435_commanding(True)
         self.get_logger().info(
@@ -690,9 +750,11 @@ class ServoNode(TolerantNode):
         with self._lock:
             using_d435 = self._track_source == "overhead"
             settled = self._d435_settled
+        # 到位必须是 MoveL 已经结束，不能把「还在赶路、XY 进了 15mm」当成到位。
+        # 停稳计时也不许在这段路上累计，否则 D405 会在 D435 没走完时就开始带臂晃。
         if self._hybrid and not d405_commands_arm(
             using_d435=using_d435,
-            arrived=settled,
+            arrived=(self._d435_vendor_done and not self._d435_vendor_busy),
             settled=settled,
             move_only=self.d435_move_only,
         ):
@@ -904,6 +966,15 @@ class ServoNode(TolerantNode):
             self._tick_still_lock()
         if self.enable_control and self._d435_oneshot_holds_the_arm():
             return
+        if (
+            self.enable_control
+            and self._hybrid
+            and self._locked
+            and not self.d435_move_only
+        ):
+            # 框心已经对准并停稳。实时预测到此为止，臂钉住，等二阶段精拍一次。
+            self._execute_twist(np.zeros(3, dtype=float), p_cam=None, hold_xy=True)
+            return
 
         # 还没有目标、人又停在锁高下面：只抬一次 Z。已经有 D435 点或腕部点之后
         # 不再插这一步 —— 平移途中改高度会抖，也会把正在走的 XY 掐掉。
@@ -1443,8 +1514,89 @@ class ServoNode(TolerantNode):
         if not ok:
             self._d435_give_up = True
 
+    def _switch_arm_controller(self, activate: bool) -> bool:
+        """停或重新打开关节轨迹控制器。关节状态广播留着，硬件接口不下线。
+
+        用节点里的客户端，不走 ``ros2 service call``：那条命令自己就要一两秒，
+        交回 D405 的预算会被它吃掉。
+        """
+        from controller_manager_msgs.srv import SwitchController
+
+        if self._sw_cli is None:
+            self._sw_cli = self.create_client(
+                SwitchController,
+                "/controller_manager/switch_controller",
+                callback_group=ReentrantCallbackGroup(),
+            )
+        t0 = time.monotonic()
+        while not self._sw_cli.service_is_ready():
+            if time.monotonic() - t0 > 3.0:
+                self.get_logger().error("switch_controller 服务不在")
+                return False
+            time.sleep(0.02)
+        req = SwitchController.Request()
+        if activate:
+            req.activate_controllers = ["rizon_arm_controller"]
+            req.deactivate_controllers = []
+        else:
+            req.activate_controllers = []
+            req.deactivate_controllers = ["rizon_arm_controller"]
+        req.strictness = SwitchController.Request.STRICT
+        req.activate_asap = True
+        req.timeout.sec = 3
+        req.timeout.nanosec = 0
+        fut = self._sw_cli.call_async(req)
+        while not fut.done():
+            if time.monotonic() - t0 > 8.0:
+                self.get_logger().error(
+                    "switch_controller 超时，"
+                    + ("没能重新激活" if activate else "没能停掉")
+                    + " rizon_arm_controller"
+                )
+                return False
+            time.sleep(0.02)
+        try:
+            resp = fut.result()
+        except Exception as exc:
+            self.get_logger().error(f"switch_controller 失败：{exc}")
+            return False
+        if resp is None or not bool(resp.ok):
+            self.get_logger().error(
+                "switch_controller 返回失败，"
+                + ("控制器没回来" if activate else "控制器还占着关节")
+            )
+            return False
+        return True
+
+    def _wait_movel_file(self, seq: int, timeout_s: float, want_done: bool):
+        lib = ros2_root() / "scripts" / "lib"
+        if str(lib) not in sys.path:
+            sys.path.insert(0, str(lib))
+        from flexiv_movel_ipc import read_ack, read_done
+
+        t0 = time.monotonic()
+        while time.monotonic() - t0 < timeout_s:
+            if want_done:
+                got = read_done()
+                if got is not None and int(got[0]) == int(seq):
+                    return bool(got[1]), str(got[2])
+            elif read_ack() == int(seq):
+                return True, ""
+            time.sleep(0.02)
+        return False, ""
+
     def _d435_vendor_movel(self) -> bool:
-        """把 D435 这一点交给厂家 MoveL。推理不关，只是这期间 D405 的点不下发。"""
+        """D435 这一点：进程内厂家 MoveL，不杀控制栈。
+
+        目标 = 当前厂家 TCP + (K0−法兰) 的 ROS XY，``keep_z``。硬件在 MoveL
+        期间不灌关节指令，D405 被 ``d435_commanding`` 挡住。不许退回杀栈
+        （那会冷启动十几秒，而且两套控制会互抢晃臂）。
+        """
+        lib = ros2_root() / "scripts" / "lib"
+        if str(lib) not in sys.path:
+            sys.path.insert(0, str(lib))
+        from flexiv_movel_ipc import write_request
+
         with self._lock:
             goal = None if self._p_feat_base is None else np.asarray(
                 self._p_feat_base, dtype=float
@@ -1452,44 +1604,72 @@ class ServoNode(TolerantNode):
         if goal is None or not np.all(np.isfinite(goal)):
             self.get_logger().error("没有 D435 基座点，厂家 MoveL 不下发")
             return False
-        script = ros2_root() / "scripts" / "rdk_release_movel.py"
+        tcp_now = self._fresh_tcp_world()
-        if not script.is_file():
+        if tcp_now is None:
-            self.get_logger().error(f"找不到 {script}")
+            self.get_logger().error(
+                "没有厂家 tcp_pose（话题 "
+                f"{getattr(self, 'robot_states_topic', '?')} 不新鲜），"
+                "拒绝杀栈凑 MoveL"
+            )
             return False
-        cmd = [
+        T_ee = self._lookup_T(self.base_frame, self.ee_frame)
-            sys.executable,
+        if T_ee is None:
-            str(script),
+            self.get_logger().error("没有法兰 TF，D435 相对位移算不出，不下发")
-            "--xyz",
+            return False
-            f"{float(goal[0])}",
+        tgt = d435_movel_from_tcp(tcp_now, T_ee[:2, 3], goal[:2])
-            f"{float(goal[1])}",
+        if tgt is None:
-            f"{float(goal[2])}",
+            self.get_logger().error("D435 厂家 TCP 加上相对位移算不出，不下发")
-        ]
+            return False
-        self.get_logger().info(
+        seq = int(time.monotonic() * 1000.0) % 1_000_000_000
-            f"D435 改走厂家 MoveL XY=({goal[0]:.3f},{goal[1]:.3f}) "
+        write_request(
-            "—— 先放开 ros2_control 的 RDK，YOLO 不停，工具尖高度保持当前"
+            seq,
+            float(tgt[0]),
+            float(tgt[1]),
+            float(tgt[2]),
+            True,
+            float(D435_VENDOR_MOVEL_VEL_MPS),
         )
-        try:
+        self.get_logger().info(
-            got = subprocess.run(cmd, check=False, timeout=180)
+            f"D435 进程内 MoveL XY=({goal[0]:.3f},{goal[1]:.3f}) "
-        except subprocess.TimeoutExpired:
+            f"厂家TCP现=({tcp_now[0]:.4f},{tcp_now[1]:.4f},{tcp_now[2]:.4f}) "
-            self.get_logger().error("厂家 MoveL 超过 180s 还没回来")
+            f"MoveL=({tgt[0]:.4f},{tgt[1]:.4f},keep_z) "
+            f"vel={D435_VENDOR_MOVEL_VEL_MPS * 1000:.0f}mm/s "
+            "—— 不杀栈；期间不下发 D405"
+        )
+        t0 = time.monotonic()
+        acked, _why = self._wait_movel_file(seq, 2.0, want_done=False)
+        if not acked:
+            self.get_logger().error(
+                "厂家 MoveL 2s 内没 ack —— 硬件没接 /tmp/flexiv_movel_req。"
+                "真机环境重建 install/ 后再跑，不许改走杀栈假装成功"
+            )
             return False
-        if got.returncode != 0:
+        ok, detail = self._wait_movel_file(seq, 90.0, want_done=True)
-            self.get_logger().error(f"厂家 MoveL 失败，退出码 {got.returncode}")
+        dt = time.monotonic() - t0
-            return False
+        self.get_logger().info(
-        return True
+            f"D435 进程内 MoveL {'完成' if ok else '失败'}，耗时 {dt:.2f}s"
+            + (f" {detail}" if detail else "")
+        )
+        return bool(ok)
 
     def _d435_oneshot_holds_locked(self) -> bool:
         # 混合流程这一段只走厂家 MoveL。下面留下的关节折线不再执行：
         # 那条路径会在真机上一段一段启停。
-        if self._d435_vendor_failed:
+        if self._d435_vendor_failed or self._d435_vendor_busy:
             return True
         if not self._d435_vendor_done:
-            self._d435_vendor_done = True
+            # busy 先于调用置位：另一条控制回调不能再开一次 MoveL，也不能提前交回 D405。
-            if not self._d435_vendor_movel():
+            self._d435_vendor_busy = True
+            ok = self._d435_vendor_movel()
+            self._d435_vendor_busy = False
+            if not ok:
                 self._d435_vendor_failed = True
                 self.get_logger().error(
                     "厂家 MoveL 没走成 —— 停在原地，D405 的点继续不下发"
                 )
+            else:
+                self._d435_vendor_done = True
+                self._arrive.reset()
             return True
         return True
 
@@ -1658,7 +1838,12 @@ class ServoNode(TolerantNode):
         """走到 D435 那个点，并且法兰按一阶段的静止判据停稳，才允许下发 D405。"""
         if not self._hybrid:
             return
-        if self._d435_inflight or time.monotonic() < self._d435_settle_until:
+        if (
+            self._d435_inflight
+            or self._d435_vendor_busy
+            or not self._d435_vendor_done
+            or time.monotonic() < self._d435_settle_until
+        ):
             self._arrive.reset()
             return
         with self._lock:
@@ -1690,7 +1875,7 @@ class ServoNode(TolerantNode):
                 self._locked = True
                 self.get_logger().info(
                     f"已到 D435 位置（还差 {dist*1000:.0f}mm）并停稳 "
-                    f"{self.lock_still_sec:.0f}s —— D435 位移测试结束，"
+                    f"{self.d435_handback_sec:.1f}s —— D435 位移测试结束，"
                     "不交 D405、不进二阶段"
                 )
                 self._lock_pub.publish(Bool(data=True))
@@ -1702,7 +1887,7 @@ class ServoNode(TolerantNode):
         self._publish_d435_commanding(False)
         self.get_logger().info(
             f"已到 D435 位置（还差 {dist*1000:.0f}mm）并停稳 "
-            f"{self.lock_still_sec:.0f}s —— 改回下发 D405 框心"
+            f"{self.d435_handback_sec:.1f}s —— 改回下发 D405 框心"
         )
 
     def _tick_still_lock(self) -> None:
@@ -1712,6 +1897,9 @@ class ServoNode(TolerantNode):
         """
         if not self._hybrid or self.d435_move_only:
             return
+        if self._locked:
+            # 框心对准并且停稳这一拍已经抓住。后面的实时点不再解开，也不再带臂。
+            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)
         with self._lock:

src/arm_application/arm_application/stage3_search_node.py

@@ -104,6 +104,18 @@ def tactile_timeout_reason(pairs: Sequence[Any], timeout_s: float) -> str:
         )
     return f"触觉话题断流 >{timeout_s:.1f}s（左右指都无新鲜数据）"
 
+
+def should_lift_air_baseline(
+    z_now_m: float, z_air_m: float, *, tactile_live: bool
+) -> bool:
+    """空载抬升只在触觉已经在发、并且当前低于空载高度时做。
+
+    触觉没数据时先抬到 hover+50mm 再报断流，等于插孔做到一半把臂送回锁高。
+    """
+    if not tactile_live:
+        return False
+    return float(z_now_m) < float(z_air_m) - 0.008
+
 def spiral_points(
     cx: float, cy: float, pitch_mm: float, step_mm: float, r_max_mm: float
 ) -> List[Tuple[float, float]]:
@@ -581,9 +593,21 @@ class Stage3Search(Stage2Locate):
         result: Dict[str, Any] = {"ok": False, "stage": "stage3", "target_hole": self.target_hole}
         try:
             # 基线必须悬空测。二阶段结束在 hover≈触面+1mm，带载会把阈值抬飞。
+            # 触觉没在发就原地中止，不许先抬到锁高再报断流。
             z_now = float(Tee[2, 3])
             z_air = float(self.hover_z) + 0.05
-            if z_now < z_air - 0.008:
+            self._spin(0.05)
+            tac_live = bool(
+                live_tactile(time.monotonic(), (self._tac_l, self._tac_r))
+            )
+            if not tac_live:
+                raise SearchAbort(
+                    tactile_timeout_reason(
+                        (self._tac_l, self._tac_r), float(self.tac_timeout)
+                    ),
+                    code=7,
+                )
+            if should_lift_air_baseline(z_now, z_air, tactile_live=True):
                 self._state("air_baseline")
                 self.get_logger().info(
                     f"抬到 {z_air:.3f}m 测触觉空载基线（当前 Z={z_now:.3f}）"

src/arm_application/launch/pipeline.launch.py

@@ -82,6 +82,9 @@ def _launch_setup(context, *args, **kwargs):
     # 顶到 63N）。默认关；验证对准时用 --no-descent 打开。
     no_descent = _as_bool(LaunchConfiguration("no_descent").perform(context))
     d435_only = _as_bool(LaunchConfiguration("d435_move_only").perform(context))
+    init_z = LaunchConfiguration("init_z_m").perform(context).strip()
+    hover_z = LaunchConfiguration("hover_z_m").perform(context).strip()
+    final_gap = LaunchConfiguration("final_gap_mm").perform(context).strip()
     target_hole = LaunchConfiguration("target_hole").perform(context).strip()
     depth = LaunchConfiguration("insert_depth_mm").perform(context).strip()
     pitch = LaunchConfiguration("screw_pitch_mm").perform(context).strip()
@@ -121,6 +124,9 @@ def _launch_setup(context, *args, **kwargs):
                 "skip_stage2": skip2 or d435_only,
                 "skip_stage3": skip3 or d435_only,
                 "d435_move_only": d435_only,
+                "init_z_m": float(init_z) if init_z else -1.0,
+                "hover_z_m": float(hover_z) if hover_z else -1.0,
+                "final_gap_mm": float(final_gap) if final_gap else -1.0,
                 # -1=按 site 的 stage2.final_descent_enable；0=强制不降（调试）
                 "final_descent_enable": 0 if no_descent else -1,
             }
@@ -132,7 +138,8 @@ def _launch_setup(context, *args, **kwargs):
         LogInfo(msg=f"[pipeline] arm={robot.get('label') or robot.get('sn')} cam={cam}"),
         LogInfo(
             msg=f"[pipeline] hole={target_hole or 'site'} depth={depth or 'site'} "
-            f"pitch={pitch or 'site'} skip1={skip1} skip2={skip2} skip3={skip3}"
+            f"pitch={pitch or 'site'} final_gap={final_gap or 'site'} "
+            f"skip1={skip1} skip2={skip2} skip3={skip3}"
             + (" [d435_only：只测 D435 位移]" if d435_only else "")
             + (" [no_descent：只对准，不轻降]" if no_descent else "")
         ),
@@ -160,6 +167,9 @@ def generate_launch_description() -> LaunchDescription:
             # 只验证精对准、不轻降（见 _launch_setup 里的说明）
             DeclareLaunchArgument("no_descent", default_value="false"),
             DeclareLaunchArgument("d435_move_only", default_value="false"),
+            DeclareLaunchArgument("init_z_m", default_value=""),
+            DeclareLaunchArgument("hover_z_m", default_value=""),
+            DeclareLaunchArgument("final_gap_mm", default_value=""),
             DeclareLaunchArgument("preview", default_value="true"),
             OpaqueFunction(function=_launch_setup),
         ]

src/arm_application/launch/stage2_locate.launch.py

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

src/arm_application/test/test_gauge_fsm.py

@@ -43,6 +43,7 @@ from arm_application.gauge_fsm import (
     points_stable,
     hover_z_guard,
     hover_z_for_tool_clearance,
+    resolve_hover_z,
     ratchet_cycles,
     resolve_next,
     rotation_verdict,
@@ -785,6 +786,21 @@ def test_hover_guard_uses_want_clearance_when_configuring():
     assert hover_z_for_tool_clearance(0.412, 0.245, 0.005) == pytest.approx(fixed)
 
 
+def test_user_hover_is_refused_instead_of_rewritten():
+    """启动参数写了悬停高：不安全就拒绝，不许改成另一个数再往下走。"""
+    used, why = resolve_hover_z(0.632, 0.412, 0.245, 0.002, True)
+    assert used == pytest.approx(0.632)
+    assert "不会改成别的高度" in why
+    # 没写启动参数：仍按几何抬高，不当成拒绝。
+    used_site, why_site = resolve_hover_z(0.632, 0.412, 0.245, 0.002, False)
+    assert why_site == ""
+    assert used_site == pytest.approx(0.412 + 0.245 + 0.002)
+    # 写了且本身安全：原样用。
+    used_ok, why_ok = resolve_hover_z(0.700, 0.412, 0.245, 0.002, True)
+    assert why_ok == ""
+    assert used_ok == pytest.approx(0.700)
+
+
 def test_hover_guard_tolerates_millimetre_level_shortfall():
     """夹在目标间隙附近（差不到 1mm）不算错：工件面 z 本身就有几毫米不确定度。"""
     assert hover_z_guard(0.6580, 0.412, 0.245, 0.002)[1] == ""   # 差 1.0mm 之内

src/arm_application/test/test_overhead_oneshot.py

@@ -31,6 +31,8 @@ from arm_application.overhead_oneshot import (
     map_uv_to_depth,
     D405_MISS_BEFORE_D435_S,
     D435_MOVE_SPEED_SCALE,
+    D435_VENDOR_MOVEL_ACC_MPS2,
+    D435_VENDOR_MOVEL_VEL_MPS,
     D435_NEAR_CRUISE_MPS,
     D435_NEAR_HOP_MAX_M,
     D435_NEAR_LEFTOVER_M,
@@ -52,6 +54,22 @@ from arm_application.overhead_oneshot import (
     oneshot_shot_timed_out,
     oneshot_next_hop_m,
     oneshot_should_submit,
+    cam_xy_bias_axes,
+    decompose_base_xy,
+    hole_pair_span_m,
+    hole_pair_span_ok,
+    oneshot_align_step_m,
+    flange_z_for_gap_m,
+    final_gap_refusal,
+    d435_movel_from_tcp,
+    once_ff_movel_from_tcp,
+    once_ff_tcp_xyz,
+    once_look_ff_xy,
+    ONCE_APPROACH_STEP_M,
+    ONCE_FF_LOOK_PER_M,
+    ONCE_FF_MOVEL_VEL_MPS,
+    ONCE_TRIM_MAX_STEP_M,
+    ONCE_TRIM_REFUSE_M,
     pixel_on_base_plane,
     select_oneshot_det,
     trap_acc_for_hop,
@@ -151,7 +169,7 @@ class TestHandoff:
         assert "d435_commanding" in perc
 
     def test_after_d435_wrist_tracks_box_center_then_hole_without_depth(self):
-        """交回 D405 追框心；对准 1s 后对孔只用像素交到工件面。"""
+        """交回 D405 追框心；本分支单次精拍 k1/k2，孔坐标只交到工件面。"""
         perc = (
             __import__("pathlib").Path(__file__).resolve().parents[1]
             / "arm_application" / "perception_node.py"
@@ -164,12 +182,123 @@ class TestHandoff:
         ).read_text(encoding="utf-8")
         assert "pixel_on_base_plane(" in stage2
         assert "hole_visual_only" in stage2
+        coarse = stage2[stage2.find("# ---- 3) 粗居中") : stage2.find("# ---- 4)")]
+        assert "pixel_on_base_plane" in coarse
+        assert "拒绝用深度图凑框心" in coarse
+        assert "_trim_xy_after_descent" in stage2
+        assert "align_max_iters" in stage2
+        assert "像素交工件面" in stage2
+        assert "det_depth = None if self.hole_visual_only else med_depth" in stage2
+        assert 'self.hole_keypoint_once = _as_bool(stage2.get("hole_keypoint_once")' in stage2
+        assert 'self.hole_once_lock_xy_descend = _as_bool(' in stage2
+        obs = stage2[stage2.find("def _observe_holes"): stage2.find("def _oneshot_align_move")]
+        assert "_goto(" not in obs, "核验拍不许改 XY"
+        assert "residual_remeasured" in obs
+        desc = stage2[stage2.find("# ---- 4.5)"):]
+        desc = desc[:2800]
+        assert "hole_once_lock_xy_descend" in desc
+        assert "if self.hole_keypoint_once:" in desc
+        assert 'tag="降后核验"' in desc
+        assert "_trim_xy_after_descent" in desc
+        assert "target_gap_m=self.observe_gap_m" in desc
+        assert "ONCE_APPROACH_STEP_M" in desc
+        lock_at = stage2.find("# ---- 4.5)")
+        assert lock_at > 0
+        lock_block = stage2[lock_at: stage2.find("elif self.hole_keypoint_once:", lock_at)]
+        assert "hole_once_lock_xy_descend" in lock_block
+        assert "lock_gap" in lock_block
+        assert "_once_ff_diagonal_to_gap" in lock_block
+        assert "self._descend_above_hole" not in lock_block
+        assert "_observe_holes" not in lock_block
+        assert "_trim_xy_after_descent" not in lock_block
+        assert "once_ff_tcp_xyz" in stage2
+        assert "once_ff_movel_from_tcp" in stage2
+        assert "_fresh_tcp_world" in stage2
+        assert "没有厂家 tcp_pose" in stage2
+        assert "write_request" in stage2
+        moved_at = stage2.find("q, Tee, _moved = self._oneshot_align_move")
+        moved_blk = stage2[moved_at: stage2.find("self._state(\"aligned\")", moved_at) + 80]
+        assert "hole_once_lock_xy_descend" in moved_blk
+        assert 'tag="走后核验"' in moved_blk
+        keep = stage2[
+            stage2.find("def _trim_xy_keep_hover_tip"): stage2.find("def _trim_xy_after_descent")
+        ]
+        assert "_tip_offset_from_depth" not in keep, "单次精拍下降后不许重测规尖"
+        assert "悬停冻结" in keep
+        assert "ONCE_TRIM_MAX_STEP_M" in keep
+        assert "hole_pair_span_ok" in keep
+        assert "range(3)" not in keep, "下降后补孔不许再连跳三次"
+        assert "撤回" in keep
+        assert "trim_blend_to_final" in keep
+        assert "斜线贴面" in keep
+        once_fn = stage2[stage2.find("def _oneshot_align_move"): stage2.find("def _trim_xy_keep_hover_tip")]
+        assert "oneshot_align_step_m(" in once_fn
+        assert "0.012 if n_raw > 0.025" not in once_fn
+        assert "from arm_application.overhead_oneshot import decompose_base_xy" in stage2 or (
+            "oneshot_align_step_m" in stage2 and "decompose_base_xy" in stage2
+        )
+        assert "def decompose_base_xy(" not in stage2
+        assert "def oneshot_align_step_m(" not in stage2
         servo = (
             __import__("pathlib").Path(__file__).resolve().parents[1]
             / "arm_application" / "servo_node.py"
         ).read_text(encoding="utf-8")
-        assert "rdk_release_movel.py" in servo
+        hw = (
+            __import__("pathlib").Path(__file__).resolve().parents[3]
+            / "src" / "flexiv_hardware" / "src" / "flexiv_hardware_interface.cpp"
+        ).read_text(encoding="utf-8")
+        assert "movel_euler_deg" in hw
+        assert "Quat2EulerZYX" not in hw
+        assert "sy > 1e-6" in hw
+        goto = (
+            __import__("pathlib").Path(__file__).resolve().parents[3]
+            / "scripts" / "rdk_goto_xyz.py"
+        ).read_text(encoding="utf-8")
+        assert "quat_wxyz_to_euler_xyz_deg" in goto
+        # 工具朝下是绕 X 转 180°。这组角必须还是 180,0,0，不能翻成会让 MoveL 拧一圈的另一组。
+        sys_path = __import__("sys").path
+        scripts = __import__("pathlib").Path(__file__).resolve().parents[3] / "scripts"
+        if str(scripts) not in sys_path:
+            sys_path.insert(0, str(scripts))
+        from rdk_goto_xyz import quat_wxyz_to_euler_xyz_deg
+        euler = quat_wxyz_to_euler_xyz_deg([0.0, 1.0, 0.0, 0.0])
+        assert abs(euler[0]) == pytest.approx(180.0, abs=1e-6)
+        assert abs(euler[1]) == pytest.approx(0.0, abs=1e-6)
+        assert abs(euler[2]) == pytest.approx(0.0, abs=1e-6)
+        movel = servo[servo.find("def _d435_vendor_movel"): servo.find("def _d435_oneshot_holds_locked")]
+        assert "rdk_release_movel.py" not in movel
+        assert "write_request" in movel
+        assert "d435_movel_from_tcp" in movel
+        assert "keep_z" in movel or "True" in movel
+        assert "拒绝杀栈凑 MoveL" in movel
+        assert "D435_VENDOR_MOVEL_VEL_MPS" in movel
+        assert "D435_VENDOR_MOVEL_ACC_MPS2" not in movel
+        assert '"--acc"' not in movel
+        assert D435_VENDOR_MOVEL_VEL_MPS == pytest.approx(0.3)
+        assert D435_VENDOR_MOVEL_ACC_MPS2 == pytest.approx(1.5)
+        rel = (
+            __import__("pathlib").Path(__file__).resolve().parents[3]
+            / "scripts" / "rdk_release_movel.py"
+        ).read_text(encoding="utf-8")
+        assert f"default={D435_VENDOR_MOVEL_VEL_MPS}" in rel
+        assert f"default={D435_VENDOR_MOVEL_ACC_MPS2}" in rel
+        assert '"--acc"' in rel
+        assert 'prim["acc"]' in goto
+        tgt = d435_movel_from_tcp([0.580, -0.460, 0.096], [0.687, -0.113], [0.708, -0.168])
+        assert tgt is not None
+        np.testing.assert_allclose(tgt, [0.580 + 0.021, -0.460 - 0.055, 0.096], atol=1e-9)
+        assert d435_movel_from_tcp(None, [0.0, 0.0], [0.1, 0.1]) is None
+        assert "d435_handback_sec" in servo
+        assert "still_sec=max(0.0, self.d435_handback_sec)" in servo
         assert "wrist_align_sec" in servo
+        # 进程内 MoveL 还在走时不许把 vendor_done 提前置上，停稳计时也不许在这段路上累计。
+        call_at = servo.find("ok = self._d435_vendor_movel()")
+        done_at = servo.find("self._d435_vendor_done = True", call_at)
+        assert call_at > 0 and done_at > call_at
+        assert "arrived=(self._d435_vendor_done and not self._d435_vendor_busy)" in servo
+        assert "or not self._d435_vendor_done" in servo
+        assert "实时预测到此为止" in servo
+        assert "workpiece/detection_overhead" in perc
         # 相机在工件正上方朝下：主点射线落在相机 XY、工件面高度上。
         T = np.eye(4)
         T[1, 1] = -1.0
@@ -182,6 +311,23 @@ class TestHandoff:
         flat = np.eye(4)
         flat[:3, 3] = [0.0, 0.0, 0.50]
         assert pixel_on_base_plane(320, 240, 400, 400, 320, 240, flat, 0.022) is None
+        # 前倾 30°：同一主点，深度偏远的点比平面交点更 −X。爪站在深度点上时
+        # K1.x−规尖.x > 0；深度偏近则反号。这就是两套一阶段站位符号相反的几何。
+        ca, sa = float(np.cos(np.deg2rad(30))), float(np.sin(np.deg2rad(30)))
+        tilt = np.eye(4)
+        tilt[:3, :3] = np.array(
+            [[0.0, sa, -ca], [1.0, 0.0, 0.0], [0.0, -ca, -sa]], dtype=float
+        )
+        tilt[:3, 3] = [0.65, -0.50, 0.19]
+        plane_pt = pixel_on_base_plane(320, 240, 400, 400, 320, 240, tilt, 0.022)
+        assert plane_pt is not None
+        ray = tilt[:3, :3] @ np.array([0.0, 0.0, 1.0])
+        ray = ray / float(np.linalg.norm(ray))
+        far = plane_pt[:3] + 0.020 * ray
+        near = plane_pt[:3] - 0.020 * ray
+        assert far[0] < plane_pt[0] < near[0]
+        assert (plane_pt[0] - far[0]) > 0.0
+        assert (plane_pt[0] - near[0]) < 0.0
         lib = (
             __import__("pathlib").Path(__file__).resolve().parents[3] / "scripts" / "lib"
         )
@@ -239,8 +385,10 @@ class TestHandoff:
 
     def test_whole_path_uses_one_cruise_and_an_arc(self):
         """远处仍 80 mm/s。近处必须另降一档。路径是圆弧，不是对着终点做 P。"""
-        assert D435_MOVE_SPEED_SCALE == 1.0
+        assert D435_MOVE_SPEED_SCALE == pytest.approx(1.5)
-        assert hybrid_cruise_vxy(0.08) == pytest.approx(0.08)
+        assert D435_VENDOR_MOVEL_VEL_MPS == pytest.approx(0.3)
+        assert D435_VENDOR_MOVEL_ACC_MPS2 == pytest.approx(1.5)
+        assert hybrid_cruise_vxy(0.08) == pytest.approx(0.12)
         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)
@@ -368,7 +516,8 @@ class TestHandoff:
         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 cruise == pytest.approx(D435_NEAR_CRUISE_MPS)
+        assert cruise == pytest.approx(0.045)
         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
@@ -406,8 +555,8 @@ class TestHandoff:
         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.12) == pytest.approx(D435_FULL_CRUISE_MPS)
-        assert oneshot_full_cruise_mps(0.20, 0.08) < 0.08
+        assert oneshot_full_cruise_mps(0.20, 0.12) < 0.12
         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
@@ -415,16 +564,18 @@ class TestHandoff:
         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，峰值加速度必须低于旧的弹射档。"""
+        """300 mm @ 60 mm/s 的加减速形状：头尾速度 0，起步 ≥3 s，低于旧弹射档。"""
-        assert D435_FULL_ACC_M == pytest.approx(0.100)
+        assert D435_FULL_ACC_M == pytest.approx(0.150)
-        assert D435_FULL_DEC_M == pytest.approx(0.080)
+        assert D435_FULL_DEC_M == pytest.approx(0.120)
-        assert D435_FULL_CRUISE_MPS == pytest.approx(0.060)
+        assert D435_FULL_CRUISE_MPS == pytest.approx(0.090)
         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.055
-        assert a_now < 0.30 * a_old
+        assert a_now < 0.45 * a_old
-        T, v_peak, t_acc, t_cruise, d_acc, d_dec = smooth_path_params(0.30, 0.06)
+        T, v_peak, t_acc, t_cruise, d_acc, d_dec = smooth_path_params(
+            0.30, 0.06, d_acc=0.100, d_dec=0.080
+        )
         assert v_peak == pytest.approx(0.06)
         assert d_acc == pytest.approx(0.100)
         assert d_dec == pytest.approx(0.080)
@@ -455,7 +606,9 @@ class TestHandoff:
         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)
+        sched, T_move, vp = smooth_joint_schedule(
+            [q0, q1], 0.30, 0.06, d_acc=0.100, d_dec=0.080
+        )
         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)
@@ -672,3 +825,154 @@ class TestStillLock:
         p = np.array([0.50, -0.12])
         assert s.update(p, 0.0, tracking_ok=False) is False
         assert s.update(p, 10.0, tracking_ok=False) is False
+
+
+class TestHoleBiasAxes:
+    def test_look_along_x_right_along_y(self):
+        """相机朝 +X、画面右为 +Y：残差必须拆到这两轴，不许混。"""
+        T = np.eye(4)
+        # 列 = 相机轴在基座：X=+Y，Y=−Z，Z=+X
+        T[:3, :3] = np.array([
+            [0.0, 0.0, 1.0],
+            [1.0, 0.0, 0.0],
+            [0.0, -1.0, 0.0],
+        ], dtype=float)
+        axes = cam_xy_bias_axes(T)
+        assert axes is not None
+        look, right = axes
+        np.testing.assert_allclose(look, [1.0, 0.0], atol=1e-9)
+        np.testing.assert_allclose(right, [0.0, 1.0], atol=1e-9)
+        d = decompose_base_xy([0.010, 0.006], T)
+        assert d is not None
+        assert d["along_look_m"] == pytest.approx(0.010, abs=1e-9)
+        assert d["along_right_m"] == pytest.approx(0.006, abs=1e-9)
+
+    def test_straight_down_has_no_look_axis(self):
+        """光轴竖直朝下时水平投影退化，必须返回 None，不许用 (0,0) 硬拆。"""
+        T = np.eye(4)
+        T[1, 1] = -1.0
+        T[2, 2] = -1.0
+        assert cam_xy_bias_axes(T) is None
+        assert decompose_base_xy([0.01, 0.0], T) is None
+
+    def test_site_wires_keypoint_once_on_this_branch(self):
+        """本分支 site 必须打开单次精拍，否则实验跑的还是多走多拍。"""
+        from pathlib import Path
+
+        import yaml
+
+        site = yaml.safe_load(
+            (Path(__file__).resolve().parents[3] / "config" / "site.yaml").read_text(
+                encoding="utf-8"
+            )
+        )
+        st2 = (site.get("hole_detect") or {}).get("stage2") or {}
+        assert st2.get("hole_keypoint_once") is True
+        assert st2.get("hole_visual_only") is True
+        assert st2.get("hole_once_lock_xy_descend") is True
+        assert st2.get("once_ff_look_per_m") == pytest.approx(ONCE_FF_LOOK_PER_M)
+        servo = (site.get("servo") or {})
+        assert servo.get("d435_handback_sec") == pytest.approx(0.5)
+
+    def test_oneshot_step_covers_half_hole_pair(self):
+        """框心到 k1 约 29mm：必须整步走完，不许再截成 12mm。"""
+        assert oneshot_align_step_m(0.02894) == pytest.approx(0.02894)
+        assert oneshot_align_step_m(0.012) == pytest.approx(0.012)
+        assert oneshot_align_step_m(0.050) == pytest.approx(0.040)
+        assert oneshot_align_step_m(0.050) > 0.012 + 1e-9
+
+    def test_once_trim_pair_window_and_step_cap(self):
+        """下降后补孔：60mm 间距可走；45mm 是检测漂了。一跳上限 3mm。"""
+        assert ONCE_TRIM_MAX_STEP_M == pytest.approx(0.003)
+        assert ONCE_TRIM_REFUSE_M == pytest.approx(0.008)
+        assert hole_pair_span_m([0.50, -0.10], [0.50, -0.16]) == pytest.approx(0.060)
+        assert hole_pair_span_ok(0.060) is True
+        assert hole_pair_span_ok(0.050) is True
+        assert hole_pair_span_ok(0.070) is True
+        assert hole_pair_span_ok(0.045) is False
+        assert hole_pair_span_ok(0.080) is False
+        assert ONCE_APPROACH_STEP_M == pytest.approx(0.020)
+        assert flange_z_for_gap_m(0.223, 0.022, 0.002) == pytest.approx(0.247)
+        assert flange_z_for_gap_m(0.223, 0.022, 0.015) == pytest.approx(0.260)
+
+    def test_final_gap_refusal_uses_tip_clearance_not_flange_z(self):
+        """贴面参数是规尖离面。0 / 比悬停还高 / 法兰出盒必须红。"""
+        tool, surf, hover = 0.2245, 0.022, 0.320
+        z_lo, z_hi = 0.18, 0.65
+        assert final_gap_refusal(0.002, tool, surf, hover, z_lo, z_hi) == ""
+        assert final_gap_refusal(0.008, tool, surf, hover, z_lo, z_hi) == ""
+        assert "≤ 0" in final_gap_refusal(0.0, tool, surf, hover, z_lo, z_hi)
+        assert "未知" in final_gap_refusal(0.008, 0.0, surf, hover, z_lo, z_hi)
+        hover_clear = hover - tool - surf
+        why_up = final_gap_refusal(hover_clear + 0.005, tool, surf, hover, z_lo, z_hi)
+        assert "抬臂" in why_up
+        why_box = final_gap_refusal(0.008, tool, surf, hover, 0.40, z_hi)
+        assert "工作盒" in why_box
+        flange_8 = flange_z_for_gap_m(tool, surf, 0.008)
+        assert flange_8 == pytest.approx(0.2545)
+        assert flange_8 < hover
+
+    def test_final_gap_cli_overrides_site_and_conflicts_with_no_descent(self):
+        """--final-gap 必须接到 launch/节点；和 --no-descent 一起写必须在脚本里红。"""
+        from pathlib import Path
+
+        root = Path(__file__).resolve().parents[1]
+        ws = root
+        while ws != ws.parent and not (ws / "scripts" / "run_all.sh").is_file():
+            ws = ws.parent
+        run_all = (ws / "scripts" / "run_all.sh").read_text(encoding="utf-8")
+        stage2 = (root / "arm_application" / "stage2_locate_node.py").read_text(
+            encoding="utf-8"
+        )
+        launch = (root / "launch" / "pipeline.launch.py").read_text(encoding="utf-8")
+        assert "--final-gap" in run_all
+        assert "final_gap_mm:=${FINAL_GAP}" in run_all
+        assert "同时写了 --final-gap 和 --no-descent" in run_all
+        assert 'self.declare_parameter("final_gap_mm"' in stage2
+        assert "final_gap_user_set" in stage2
+        assert "target_gap_m=lock_gap" in stage2
+        assert '"final_gap_mm": float(final_gap) if final_gap else -1.0' in launch
+        assert 'DeclareLaunchArgument("final_gap_mm"' in launch
+
+    def test_once_look_ff_is_minus_look_times_k_dz(self):
+        """现行 k=0 → ΔXY=(0,0)。k=0.018、look=-X、降 71.5mm 才是 +1.287mm X。"""
+        k0 = float(ONCE_FF_LOOK_PER_M)
+        assert k0 == pytest.approx(0.0)
+        dxy0 = once_look_ff_xy([-1.0, 0.0], 0.0715, k0)
+        assert dxy0 is not None
+        np.testing.assert_allclose(dxy0, [0.0, 0.0], atol=1e-12)
+        np.testing.assert_allclose(
+            once_look_ff_xy([0.0, 0.0], 0.0715, 0.0), [0.0, 0.0], atol=1e-12
+        )
+        assert once_look_ff_xy([-1.0, 0.0], 0.0, 0.0) is None
+        k = 0.018
+        expect_x = k * 0.0715
+        dxy = once_look_ff_xy([-1.0, 0.0], 0.0715, k)
+        assert dxy is not None
+        np.testing.assert_allclose(dxy, [expect_x, 0.0], atol=1e-9)
+        assert expect_x == pytest.approx(0.001287, abs=1e-6)
+        assert once_look_ff_xy([0.0, 0.0], 0.0715, k) is None
+        assert once_look_ff_xy([-1.0, 0.0], 0.0, k) is None
+        packed = once_ff_tcp_xyz(
+            [0.50, -0.40],
+            [-0.001, -0.001],
+            [-1.0, 0.0],
+            0.0715,
+            k0,
+            0.022,
+            0.002,
+        )
+        assert packed is not None
+        tcp, dxy2 = packed
+        assert tcp[2] == pytest.approx(0.024)
+        assert tcp[2] < 0.18
+        np.testing.assert_allclose(tcp[:2], [0.50 - 0.001, -0.40 - 0.001])
+        np.testing.assert_allclose(dxy2, [0.0, 0.0], atol=1e-12)
+        assert ONCE_FF_MOVEL_VEL_MPS == pytest.approx(0.05)
+        dz_refuse = ONCE_TRIM_REFUSE_M / k + 0.01
+        assert once_look_ff_xy([-1.0, 0.0], dz_refuse, k) is not None
+        assert float(np.hypot(*once_look_ff_xy([-1.0, 0.0], dz_refuse, k))) > ONCE_TRIM_REFUSE_M
+        rel = once_ff_movel_from_tcp([0.580, -0.460, 0.096], [0.0, 0.0], 0.0715)
+        assert rel is not None
+        np.testing.assert_allclose(rel, [0.580, -0.460, 0.0245], atol=1e-9)
+        assert once_ff_movel_from_tcp([0.580, -0.460, 0.096], [0.00129, 0.0], 0.0) is None

src/arm_application/test/test_overhead_track.py

@@ -42,7 +42,8 @@ def _track_site(**over) -> dict:
     site["servo"] = {
         "xy_track": "hover",
         "lock_still_sec": 3.0,
-        "wrist_align_sec": 1.0,
+        "d435_handback_sec": 3.0,
+        "wrist_align_sec": 0.5,
         "lock_still_move_m": 0.015,
     }
     site["perception"] = {
@@ -153,7 +154,8 @@ class TestPerceptionServoWiring:
         assert "d405" not in p["overhead_camera_source"]
         assert s["handeye_mode"] == "eye_in_hand"
         assert s["lock_still_sec"] == pytest.approx(3.0)
-        assert s["wrist_align_sec"] == pytest.approx(1.0)
+        assert s["d435_handback_sec"] == pytest.approx(3.0)
+        assert s["wrist_align_sec"] == pytest.approx(0.5)
         assert s["wrist_handoff_conf"] == pytest.approx(0.5)
         assert s["overhead_hold_s"] == pytest.approx(8.0)
 
@@ -169,6 +171,12 @@ class TestPerceptionServoWiring:
         with pytest.raises(RuntimeError, match="lock_still_sec"):
             export_site_env.servo_overrides(site)
 
+    def test_wrist_hybrid_missing_handback_raises(self):
+        site = _track_site()
+        del site["servo"]["d435_handback_sec"]
+        with pytest.raises(RuntimeError, match="d435_handback_sec"):
+            export_site_env.servo_overrides(site)
+
     def test_overhead_lookat_raises(self):
         site = _overhead_ok()
         site["servo"] = {"xy_track": "lookat"}
@@ -230,7 +238,8 @@ class TestRealSiteBranchDefault:
         assert s["handeye_mode"] == "eye_in_hand"
         assert s["handeye_file"].endswith("89_eye_in_hand.yaml")
         assert s["lock_still_sec"] == pytest.approx(3.0)
-        assert s["wrist_align_sec"] == pytest.approx(1.0)
+        assert s["d435_handback_sec"] == pytest.approx(0.5)
+        assert s["wrist_align_sec"] == pytest.approx(0.5)
         assert s["lock_still_move_m"] == pytest.approx(0.015)
         # 负向：跟踪外参不能被换成顶视文件
         assert "eye_to_hand" not in s["handeye_file"]

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, track_lock_z
+from arm_application.pipeline_node import init_z_refusal, stage1_timeout_reason, track_lock_z
 
 
 def test_zero_points_blames_perception_not_calibration():
@@ -54,6 +54,30 @@ def test_sim_still_lifts_when_hover_barely_clears_the_part():
     assert z == pytest.approx(0.707)
 
 
+def test_init_z_refuses_a_value_that_would_enter_the_part():
+    """用户指定的初始高度会钻进工件时必须拒绝，不许悄悄抬高。"""
+    why = init_z_refusal(0.20, 0.2245, 0.022, 0.002, 0.18, 0.65)
+    assert "进入工件" in why
+    assert "不会悄悄抬高" in why
+    assert init_z_refusal(0.320, 0.2245, 0.022, 0.002, 0.18, 0.65) == ""
+    assert "工作盒" in init_z_refusal(0.10, 0.2245, 0.022, 0.002, 0.18, 0.65)
+    assert "未知" in init_z_refusal(0.320, 0.0, 0.022, 0.002, 0.18, 0.65)
+
+
+def test_movel_ipc_roundtrip():
+    lib = __import__("pathlib").Path(__file__).resolve().parents[3] / "scripts" / "lib"
+    import sys
+    if str(lib) not in sys.path:
+        sys.path.insert(0, str(lib))
+    from flexiv_movel_ipc import format_request, parse_done
+
+    line = format_request(7, 0.4, -0.2, 0.32, True, 0.05)
+    assert line.startswith("7 0.400000 -0.200000 0.320000 1 0.0500")
+    assert parse_done("7 ok\n") == (7, True, "")
+    assert parse_done("7 fail mode\n") == (7, False, "mode")
+    assert parse_done("nope\n") is None
+
+
 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)

src/arm_application/test/test_stage2_coarse_guard.py

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

src/arm_application/test/test_stage2_descent_clamp.py

@@ -36,8 +36,13 @@ from __future__ import annotations
 from typing import Any, List, Optional
 
 import numpy as np
+import pytest
 
-from arm_application.stage2_locate_node import Stage2Locate
+from arm_application.overhead_oneshot import ONCE_APPROACH_STEP_M
+from arm_application.stage2_locate_node import (
+    Stage2Locate,
+    contact_armed_by_gap,
+)
 
 # 与真机那次一致的一组数（site.yaml 现行值）
 _HOVER_Z = 0.320
@@ -90,6 +95,7 @@ class _StubStage2:
         self._logger = _StubLogger(self.logs)
         self.final_descent_enable = True
         self.final_gap_m = _FINAL_GAP
+        self.observe_gap_m = 0.015
         self.final_descent_speed = 0.005
         self.vert_tol_rad = 0.01
         self.hover_z = _HOVER_Z
@@ -107,6 +113,7 @@ class _StubStage2:
         self._contact_after = contact_after_steps
         # 记录每一次实际下发的下降步长，用来核对累计行程
         self.steps_mm: List[float] = []
+        self.goto_hold_xy: List[bool] = []
         self.pins = 0
 
     # --- 被测函数会调到的钩子 ---
@@ -129,10 +136,14 @@ class _StubStage2:
 
     def _goto(self, _q: Any, Tee: Any, target: Any, **_kw: Any) -> Any:
         Tee = np.array(Tee, dtype=float)
+        self.goto_hold_xy.append(bool(_kw.get("hold_xy")))
         step_mm = (Tee[2, 3] - float(target[2])) * 1000.0
         # 记录"下降步"（正数）；回退步（负数）不计入下降行程
         if step_mm > 0.0:
             self.steps_mm.append(step_mm)
+        if not _kw.get("hold_xy"):
+            Tee[0, 3] = float(target[0])
+            Tee[1, 3] = float(target[1])
         Tee[2, 3] = float(target[2])
         self._cur_z = float(target[2])
         return True, np.zeros(7), Tee
@@ -259,18 +270,277 @@ def test_zero_tool_length_refuses_to_descend():
 # 真安全机制仍在：FT 接触、goto 失败
 # --------------------------------------------------------------------------- #
 def test_ft_contact_stops_and_backs_off():
-    """腕部力突变必须立即停并回退 —— 这是与测距无关的**物理**兜底。"""
+    """腕部力突变必须立即停并回退 —— 这是与测距无关的**物理**兜底。
-    st = _StubStage2(contact_after_steps=2)
+
+    力从第 8 步（间隙约 21mm）就超门槛，但门控是 ``final_gap+4mm``（6mm）。
+    21mm / 10mm 这一档必须继续降；进到 6mm 才许停，且不许把 2mm 目标走完。
+    """
+    st = _StubStage2(contact_after_steps=8)
     ok, Tee, st = _run(st)
 
     assert ok is True, "接触后停住即算安全收工"
     assert any("腕部力突变" in m for m in st.logs), "碰到工件却没喊"
-    # 只降了 2 步就被拦住（远没走满预算）
+    assert any("不当作碰到" in m for m in st.logs), "21mm 间隙时力超了也要明说不当成碰到"
-    assert sum(st.steps_mm) < _BUDGET * 1000 - 5.0, "力突变后还在继续降"
+    assert sum(st.steps_mm) > 32.0, "力从 21mm 间隙就热，必须穿过 10mm 假接触区"
-    # 回退 1mm：最终位置应高于"最后一步的下发目标"
+    assert sum(st.steps_mm) < _BUDGET * 1000 - 1.0, "力突变后还在继续降到 2mm"
     assert Tee[2, 3] > _HOVER_Z - _BUDGET, "回退没生效"
 
 
+def test_contact_not_armed_while_still_high_above_the_part():
+    """远场 138mm 和近场 10mm / ΔFz≈3N 都不算碰到；6mm 才武装。"""
+    assert contact_armed_by_gap(0.1375, 0.015) is False
+    assert contact_armed_by_gap(0.020, 0.015) is False
+    assert contact_armed_by_gap(0.010, 0.015) is False
+    assert contact_armed_by_gap(0.0095, 0.015) is False
+    assert contact_armed_by_gap(0.006, 0.015) is True
+    assert contact_armed_by_gap(0.005, 0.015) is True
+
+
+def test_lock_xy_descend_holds_xy_and_stops_at_final_gap():
+    """锁 XY 实验：从悬停纯 Z 收到 2mm，XY 不许动，步长仍 20mm。"""
+    import rclpy
+
+    if not rclpy.ok():
+        rclpy.init()
+    st = _StubStage2()
+    Tee0 = np.eye(4)
+    Tee0[0, 3] = 0.512
+    Tee0[1, 3] = -0.401
+    Tee0[2, 3] = _HOVER_Z
+    ok, _q, Tee = Stage2Locate._descend_above_hole(  # type: ignore[arg-type]
+        st, np.zeros(7), Tee0, target_gap_m=_FINAL_GAP, step_m=ONCE_APPROACH_STEP_M
+    )
+    assert ok is True
+    gap = float(Tee[2, 3]) - _TOOL_LEN - _SURFACE_Z
+    assert abs(gap - _FINAL_GAP) < 0.001, f"应停在贴面 2mm，实际 {gap * 1000:.1f}mm"
+    assert Tee[0, 3] == 0.512
+    assert Tee[1, 3] == -0.401
+    assert max(st.steps_mm) <= ONCE_APPROACH_STEP_M * 1000 + 1e-6
+    assert all(st.goto_hold_xy), "下臂每一步都必须 hold_xy"
+
+
+def _look_neg_x_T() -> np.ndarray:
+    T = np.eye(4)
+    T[:3, :3] = np.array(
+        [[0.0, 0.0, -1.0], [1.0, 0.0, 0.0], [0.0, -1.0, 0.0]],
+        dtype=float,
+    )
+    return T
+
+
+def test_once_ff_zero_k_is_cartesian_lock_xy_movel():
+    """k=0 必须下发相对 MoveL，且 ΔXY=0、只降 Z。不许停在悬停高装成功。"""
+    import rclpy
+
+    if not rclpy.ok():
+        rclpy.init()
+    st = _ff_stub_ready()
+    st.once_ff_look_per_m = 0.0
+    st._lookup = lambda *_a, **_k: np.eye(4)  # type: ignore[method-assign]
+    st._fresh_tcp_world = lambda: (0.580, -0.460, 0.096)  # type: ignore[method-assign]
+    got: list = []
+    st._vendor_movel_tcp = (  # type: ignore[method-assign]
+        lambda xyz, _vel: got.append([float(xyz[0]), float(xyz[1]), float(xyz[2])]) or True
+    )
+    Tee = np.eye(4)
+    Tee[0, 3] = 0.595
+    Tee[1, 3] = -0.467
+    Tee[2, 3] = _HOVER_Z
+    ok, _q, _Tee1 = Stage2Locate._once_ff_diagonal_to_gap(  # type: ignore[arg-type]
+        st, np.zeros(7), Tee, target_gap_m=_FINAL_GAP
+    )
+    assert ok is True
+    assert len(got) == 1
+    gap0 = _HOVER_Z - _TOOL_LEN - _SURFACE_Z
+    descent = gap0 - _FINAL_GAP
+    np.testing.assert_allclose(got[0], [0.580, -0.460, 0.096 - descent], atol=1e-9)
+    assert any("笛卡尔锁XY只降Z" in m for m in st.logs)
+
+
+def _ff_stub_ready() -> _StubStage2:
+    st = _StubStage2()
+    st.once_ff_look_per_m = 0.0
+    st._tip_off = np.array([-0.001, -0.001], dtype=float)
+    st.cam_frame = "cam"
+    st.base_frame = "base"
+    st._ws_clipped = False
+    st._lookup = lambda *_a, **_k: _look_neg_x_T()  # type: ignore[method-assign]
+    st._clamp_ws = lambda *_a, **_k: None  # type: ignore[method-assign]
+    st._spin = lambda *_a, **_k: None  # type: ignore[method-assign]
+    st._wait_state = lambda *_a, **_k: (None, None)  # type: ignore[method-assign]
+    st.robot_states_topic = "/Rizon4_063989/flexiv_robot_states"
+    return st
+
+
+def test_once_ff_refuses_without_tcp_pose():
+    """没有厂家 tcp_pose 不许退回 ROS 绝对坐标。"""
+    import rclpy
+
+    if not rclpy.ok():
+        rclpy.init()
+    st = _ff_stub_ready()
+    st._fresh_tcp_world = lambda: None  # type: ignore[method-assign]
+    called: list = []
+    st._vendor_movel_tcp = lambda *_a, **_k: called.append(1) or True  # type: ignore[method-assign]
+    Tee = np.eye(4)
+    Tee[0, 3] = 0.595
+    Tee[1, 3] = -0.467
+    Tee[2, 3] = _HOVER_Z
+    ok, _q, Tee1 = Stage2Locate._once_ff_diagonal_to_gap(  # type: ignore[arg-type]
+        st, np.zeros(7), Tee, target_gap_m=_FINAL_GAP
+    )
+    assert ok is False
+    assert called == []
+    assert Tee1[2, 3] == pytest.approx(_HOVER_Z)
+    assert any("没有厂家 tcp_pose" in m for m in st.logs)
+
+
+def test_once_ff_movel_is_flexiv_tcp_plus_delta():
+    """MoveL 必须是当前厂家 TCP + (ΔXY, −下降)，不是 ROS 规尖绝对坐标。"""
+    import rclpy
+
+    if not rclpy.ok():
+        rclpy.init()
+    st = _ff_stub_ready()
+    st._fresh_tcp_world = lambda: (0.580, -0.460, 0.096)  # type: ignore[method-assign]
+    got: list = []
+    st._vendor_movel_tcp = (  # type: ignore[method-assign]
+        lambda xyz, _vel: got.append([float(xyz[0]), float(xyz[1]), float(xyz[2])]) or True
+    )
+    Tee = np.eye(4)
+    Tee[0, 3] = 0.595
+    Tee[1, 3] = -0.467
+    Tee[2, 3] = _HOVER_Z
+    ok, _q, _Tee1 = Stage2Locate._once_ff_diagonal_to_gap(  # type: ignore[arg-type]
+        st, np.zeros(7), Tee, target_gap_m=_FINAL_GAP
+    )
+    assert ok is True
+    assert len(got) == 1
+    gap0 = _HOVER_Z - _TOOL_LEN - _SURFACE_Z
+    descent = gap0 - _FINAL_GAP
+    np.testing.assert_allclose(got[0], [0.580, -0.460, 0.096 - descent], atol=1e-9)
+    assert got[0][0] == pytest.approx(0.580)
+    assert not np.allclose(got[0][:2], [0.595 - 0.001, -0.467 - 0.001], atol=1e-4)
+
+
+def test_once_ff_custom_final_gap_only_changes_z():
+    """--final-gap 8mm：MoveL XY 仍为 0，下降量必须少 6mm，不许再走贴面 2mm。"""
+    import rclpy
+
+    if not rclpy.ok():
+        rclpy.init()
+    st = _ff_stub_ready()
+    st._fresh_tcp_world = lambda: (0.580, -0.460, 0.096)  # type: ignore[method-assign]
+    got: list = []
+    st._vendor_movel_tcp = (  # type: ignore[method-assign]
+        lambda xyz, _vel: got.append([float(xyz[0]), float(xyz[1]), float(xyz[2])]) or True
+    )
+    Tee = np.eye(4)
+    Tee[0, 3] = 0.595
+    Tee[1, 3] = -0.467
+    Tee[2, 3] = _HOVER_Z
+    gap_custom = 0.008
+    ok, _q, _Tee1 = Stage2Locate._once_ff_diagonal_to_gap(  # type: ignore[arg-type]
+        st, np.zeros(7), Tee, target_gap_m=gap_custom
+    )
+    assert ok is True
+    assert len(got) == 1
+    gap0 = _HOVER_Z - _TOOL_LEN - _SURFACE_Z
+    descent = gap0 - gap_custom
+    descent_2mm = gap0 - _FINAL_GAP
+    np.testing.assert_allclose(got[0], [0.580, -0.460, 0.096 - descent], atol=1e-9)
+    assert got[0][0] == pytest.approx(0.580)
+    assert got[0][1] == pytest.approx(-0.460)
+    assert descent == pytest.approx(descent_2mm - 0.006)
+    assert any("笛卡尔锁XY只降Z" in m for m in st.logs)
+
+
+def test_once_ff_k_018_still_adds_look_xy():
+    """有人把 k 改回 0.018 时，MoveL 必须真的加 +k×下降，不许再吞成绝对坐标。"""
+    import rclpy
+
+    if not rclpy.ok():
+        rclpy.init()
+    st = _ff_stub_ready()
+    st.once_ff_look_per_m = 0.018
+    st._fresh_tcp_world = lambda: (0.580, -0.460, 0.096)  # type: ignore[method-assign]
+    got: list = []
+    st._vendor_movel_tcp = (  # type: ignore[method-assign]
+        lambda xyz, _vel: got.append([float(xyz[0]), float(xyz[1]), float(xyz[2])]) or True
+    )
+    Tee = np.eye(4)
+    Tee[0, 3] = 0.595
+    Tee[1, 3] = -0.467
+    Tee[2, 3] = _HOVER_Z
+    ok, _q, _Tee1 = Stage2Locate._once_ff_diagonal_to_gap(  # type: ignore[arg-type]
+        st, np.zeros(7), Tee, target_gap_m=_FINAL_GAP
+    )
+    assert ok is True
+    assert len(got) == 1
+    gap0 = _HOVER_Z - _TOOL_LEN - _SURFACE_Z
+    descent = gap0 - _FINAL_GAP
+    dxy = 0.018 * descent
+    np.testing.assert_allclose(got[0], [0.580 + dxy, -0.460, 0.096 - descent], atol=1e-9)
+    assert any("视线前馈斜线" in m for m in st.logs)
+
+
+def test_oneshot_approach_stops_at_observe_gap_with_larger_steps():
+    """单次精拍第一段只降到 15mm，步长 20mm（约 3 步），不许 4mm 切到 2mm。"""
+    import rclpy
+
+    if not rclpy.ok():
+        rclpy.init()
+    st = _StubStage2()
+    Tee0 = np.eye(4)
+    Tee0[2, 3] = _HOVER_Z
+    ok, _q, Tee = Stage2Locate._descend_above_hole(  # type: ignore[arg-type]
+        st, np.zeros(7), Tee0, target_gap_m=0.015, step_m=ONCE_APPROACH_STEP_M
+    )
+    assert ok is True
+    gap = float(Tee[2, 3]) - _TOOL_LEN - _SURFACE_Z
+    assert abs(gap - 0.015) < 0.001, f"应停在观察高 15mm，实际 {gap * 1000:.1f}mm"
+    assert max(st.steps_mm) <= ONCE_APPROACH_STEP_M * 1000 + 1e-6
+    assert len(st.steps_mm) <= 3
+    assert abs(sum(st.steps_mm) - (_GAP0 - 0.015) * 1000) < 1.5
+
+
+def test_oneshot_approach_does_not_treat_force_as_contact():
+    """观察高 15mm 仍在接触门控之外，力再热也不许当碰到。"""
+
+    class _AlwaysHot(_StubStage2):
+        def _contact_detected(self, _base_fz: float) -> bool:
+            return True
+
+    import rclpy
+
+    if not rclpy.ok():
+        rclpy.init()
+    st = _AlwaysHot()
+    Tee0 = np.eye(4)
+    Tee0[2, 3] = _HOVER_Z
+    ok, _q, Tee = Stage2Locate._descend_above_hole(  # type: ignore[arg-type]
+        st, np.zeros(7), Tee0, target_gap_m=0.015, step_m=ONCE_APPROACH_STEP_M
+    )
+    assert ok is True
+    gap = float(Tee[2, 3]) - _TOOL_LEN - _SURFACE_Z
+    assert abs(gap - 0.015) < 0.001
+    assert not any("腕部力突变" in m for m in st.logs)
+
+
+def test_ft_drift_far_from_surface_does_not_stop_on_the_first_step():
+    """离面还远时力一直超门槛，也要先降到门控间隙附近才许停。"""
+
+    class _AlwaysHot(_StubStage2):
+        def _contact_detected(self, _base_fz: float) -> bool:
+            return True
+
+    ok, _Tee, st = _run(_AlwaysHot())
+    assert ok is True
+    assert sum(st.steps_mm) >= 40.0, "离面 53mm 时力超门槛也不该第一步就停"
+    assert any("不当作碰到" in m for m in st.logs)
+    assert any("腕部力突变" in m for m in st.logs)
+
+
 def test_every_step_logs_the_ft_force():
     """每一步都要打出 FT 轴向力 —— 这是"停住是碰到还是没跟上"的唯一直接证据。
 

src/arm_application/test/test_stage2_tip_align.py

@@ -442,6 +442,7 @@ class _TrimStub:
     """下降后 XY 修正用到的成员。孔在基座里不动，规尖偏移是常数。"""
 
     tip_vision = True
+    hole_keypoint_once = False
     k1_label = "left"
     target_hole = "k1"
     align_tol = 0.00025
@@ -449,15 +450,27 @@ class _TrimStub:
     vert_tol_rad = 0.02
     base_frame = "world"
     ee_frame = "flange"
+    cam_frame = "cam"
+    image_topic = "/cam/color"
     _tip_off = np.array([0.01, 0.0])
     _tip_samp: list = []
     _tip_len_m = 0.22
+    _ws_clipped = False
+    final_descent_enable = False
+    tool_tip_below_flange_m = 0.223
+    surface_z_m = 0.022
+    final_gap_m = 0.002
+    final_descent_speed = 0.005
 
     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.tip_calls = 0
         self._logger = _StubLogger()
+        self.pair_dy = -0.060
+        self.hole_after_move = None
+        self.pair_dy_after_move = None
 
     def get_logger(self):
         return self._logger
@@ -469,17 +482,42 @@ class _TrimStub:
         return None
 
     def _snap_holes(self, *, skip_center_gate: bool):
+        hole = self.hole
+        pair_dy = self.pair_dy
+        if self.moves:
+            if self.hole_after_move is not None:
+                hole = self.hole_after_move
+            if self.pair_dy_after_move is not None:
+                pair_dy = self.pair_dy_after_move
         res = {
             "status": True,
-            "hole_base": [self.hole.tolist(), (self.hole + np.array([0.0, -0.060, 0.0])).tolist()],
+            "hole_base": [hole.tolist(), (hole + np.array([0.0, pair_dy, 0.0])).tolist()],
             "z_wp": 0.127,
         }
         return None, None, {}, res
 
     def _tip_offset_from_depth(self, *_a, **_kw):
+        self.tip_calls += 1
         if self.tip is None:
             return None, None, "没看到规"
-        return self.tip.copy(), 0.222, "px=(447,226)"
+        off = self.tip.copy()
+        if self.hole_keypoint_once:
+            off = off + np.array([0.018 * (self.tip_calls % 3), 0.0])
+        return off, 0.222, "px=(447,226)"
+
+    def _stamp_bias(self, rec, *, tgt_b, gxy, tag):
+        rec = dict(rec)
+        rec["tag"] = tag
+        rec["err_xy_m"] = [
+            round(float(tgt_b[0]) - float(gxy[0]), 5),
+            round(float(tgt_b[1]) - float(gxy[1]), 5),
+        ]
+        rec["along_look_mm"] = None
+        rec["along_right_mm"] = None
+        return rec
+
+    def _publish_result(self, _payload) -> None:
+        return None
 
     def _goto(self, _q, Tee, target, **_kw):
         Tee = np.array(Tee, float)
@@ -535,3 +573,170 @@ def test_trim_does_not_move_when_the_tip_is_missing():
     assert not st.moves
     assert abs(float(Tee[0, 3]) - 0.50) < 1e-9
     assert final["gauge_source"] == "视觉规尖(已冻结)"
+
+
+def test_once_trim_keeps_hover_tip_when_depth_jumps():
+    """单次精拍：沿用悬停规尖，只跳一次、步长 ≤3mm，剩下的不许再追。
+
+    2026-10-13：两跳合计 4.3mm 把近距检测带崩。5mm 交平面漂只许吃掉 3mm。
+    """
+    frozen = np.array([0.0001, -0.0003])
+    st = _TrimStub(err_xy=(0.0044, -0.0024))
+    st.hole_keypoint_once = True
+    st._tip_off = frozen.copy()
+    st.align_tol = 0.00025
+    _q, Tee, final = _trim(st)
+    assert st.tip_calls == 0, "单次精拍下降后不许再走深度重测规尖"
+    assert abs(float(Tee[2, 3]) - 0.260) < 1e-9, "下降后修正不许改高度"
+    assert len(st.moves) == 1, f"只许一跳，实际 {len(st.moves)} 跳"
+    hop = float(np.hypot(float(st.moves[0][0]) - 0.50, float(st.moves[0][1]) + 0.10))
+    assert hop <= 0.003 + 1e-9, f"一跳 {hop*1000:.2f}mm 超过 3mm"
+    gxy = np.array([float(Tee[0, 3]) + frozen[0], float(Tee[1, 3]) + frozen[1]])
+    err = float(np.hypot(gxy[0] - st.hole[0], gxy[1] - st.hole[1]))
+    assert 0.0015 <= err <= 0.0025, f"3mm 一跳后应剩约 2mm，实际 {err*1000:.2f}mm"
+    assert final["residual_mm"] == pytest.approx(err * 1000.0, abs=0.02)
+    assert "悬停冻结" in final["gauge_source"]
+    assert final["residual_remeasured"] is True
+    assert final.get("trim_reverted") is False
+
+
+def test_once_trim_refuses_without_frozen_tip():
+    """没有悬停冻住的规尖，不许用法兰或乱跳的深度尖去补。"""
+    st = _TrimStub(err_xy=(0.0044, 0.0))
+    st.hole_keypoint_once = True
+    st._tip_off = None
+    _q, Tee, final = _trim(st)
+    assert not st.moves
+    assert st.tip_calls == 0
+    assert abs(float(Tee[0, 3]) - 0.50) < 1e-9
+    assert any("冻住的规尖" in m for m in st._logger.msgs)
+
+
+def test_once_trim_refuses_a_centimeter_scale_hole_shift():
+    """孔 XY 要补到厘米，不是交平面那 4mm，必须停。"""
+    st = _TrimStub(err_xy=(0.014, 0.0))
+    st.hole_keypoint_once = True
+    st._tip_off = np.array([0.0001, -0.0003])
+    _q, Tee, final = _trim(st)
+    assert not st.moves
+    assert final["residual_mm"] == 0.22
+    assert any("不像交平面" in m for m in st._logger.msgs)
+
+
+def test_once_trim_reverts_when_pair_breaks():
+    """一跳后两孔间距从 60mm 掉到 45mm：检测漂了，必须撤回。"""
+    frozen = np.array([0.0001, -0.0003])
+    st = _TrimStub(err_xy=(0.0026, 0.0))
+    st.hole_keypoint_once = True
+    st._tip_off = frozen.copy()
+    st.pair_dy_after_move = -0.045
+    _q, Tee, final = _trim(st)
+    assert len(st.moves) == 2, "补完变差必须再走一跳撤回"
+    assert abs(float(Tee[0, 3]) - 0.50) < 1e-9
+    assert abs(float(Tee[1, 3]) + 0.10) < 1e-9
+    assert final.get("trim_reverted") is True
+    assert any("撤回" in m for m in st._logger.msgs)
+    gxy = np.array([float(Tee[0, 3]) + frozen[0], float(Tee[1, 3]) + frozen[1]])
+    err = float(np.hypot(gxy[0] - st.hole[0], gxy[1] - st.hole[1]))
+    assert abs(err - 0.0025) < 2e-4, "撤回后残差应回到走前那一档"
+    assert final["residual_mm"] == pytest.approx(err * 1000.0, abs=0.05)
+
+
+def test_once_trim_reverts_when_residual_worsens():
+    """一跳后残差变大：停并撤回，终值不许还是走前那份假装没动。"""
+    frozen = np.array([0.0001, -0.0003])
+    st = _TrimStub(err_xy=(0.0026, 0.0))
+    st.hole_keypoint_once = True
+    st._tip_off = frozen.copy()
+    st.hole_after_move = np.array([0.50 + 0.015, -0.10, 0.022])
+    _q, Tee, final = _trim(st)
+    assert len(st.moves) == 2
+    assert abs(float(Tee[0, 3]) - 0.50) < 1e-9
+    assert final.get("trim_reverted") is True
+    assert any("撤回" in m for m in st._logger.msgs)
+    assert final["residual_remeasured"] is True
+    assert float(final["residual_mm"]) < 5.0, "撤回后不许把 15mm 漂检测当终值"
+
+
+def _enable_blend(st: _TrimStub) -> None:
+    """观察高 15mm（法兰 0.260）斜线收到贴面 2mm（法兰 0.247）。"""
+    st.final_descent_enable = True
+    st.tool_tip_below_flange_m = 0.223
+    st.surface_z_m = 0.022
+    st.final_gap_m = 0.002
+    st.final_descent_speed = 0.005
+
+
+def test_once_trim_blend_drops_z_and_xy():
+    """观察高测到的残差和剩余 Z 合成一跳，不许再切 4mm 贴面。"""
+    frozen = np.array([0.0001, -0.0003])
+    st = _TrimStub(err_xy=(0.0044, -0.0024))
+    st.hole_keypoint_once = True
+    st._tip_off = frozen.copy()
+    st.align_tol = 0.00025
+    _enable_blend(st)
+    _q, Tee, final = _trim(st)
+    assert len(st.moves) == 1, f"斜线只许一跳，实际 {len(st.moves)}"
+    assert abs(float(Tee[2, 3]) - 0.247) < 1e-9, "必须收到贴面法兰高"
+    hop_xy = float(np.hypot(float(st.moves[0][0]) - 0.50, float(st.moves[0][1]) + 0.10))
+    assert hop_xy <= 0.003 + 1e-9
+    gxy = np.array([float(Tee[0, 3]) + frozen[0], float(Tee[1, 3]) + frozen[1]])
+    err = float(np.hypot(gxy[0] - st.hole[0], gxy[1] - st.hole[1]))
+    assert 0.0015 <= err <= 0.0025
+    assert final.get("trim_reverted") is False
+
+
+def test_once_trim_blend_z_only_when_xy_already_in_tol():
+    """观察高 XY 已在容差，斜线仍要收 Z，不许停在 15mm。"""
+    frozen = np.array([0.0001, -0.0003])
+    st = _TrimStub(err_xy=(0.0001, -0.0003))
+    st.hole_keypoint_once = True
+    st._tip_off = frozen.copy()
+    _enable_blend(st)
+    _q, Tee, final = _trim(st)
+    assert len(st.moves) == 1
+    assert abs(float(Tee[0, 3]) - 0.50) < 1e-9
+    assert abs(float(Tee[2, 3]) - 0.247) < 1e-9
+    assert final.get("trim_reverted") is False
+
+
+def test_once_trim_blend_refuses_pair_does_not_drop_z():
+    """观察高间距已经坏了，不许斜线贴面。"""
+    st = _TrimStub(err_xy=(0.004, 0.0))
+    st.hole_keypoint_once = True
+    st._tip_off = np.array([0.0001, -0.0003])
+    st.pair_dy = -0.045
+    _enable_blend(st)
+    _q, Tee, final = _trim(st)
+    assert not st.moves
+    assert abs(float(Tee[2, 3]) - 0.260) < 1e-9
+    assert final["residual_mm"] == 0.22
+
+
+def test_once_trim_blend_keeps_pose_when_close_snap_breaks():
+    """贴面复拍间距漂了：近距不可信，观察高算的斜线位姿必须留下。"""
+    frozen = np.array([0.0001, -0.0003])
+    st = _TrimStub(err_xy=(0.0026, 0.0))
+    st.hole_keypoint_once = True
+    st._tip_off = frozen.copy()
+    st.pair_dy_after_move = -0.045
+    _enable_blend(st)
+    _q, Tee, final = _trim(st)
+    assert len(st.moves) == 1, "贴面复拍漂了不许撤回观察高"
+    assert abs(float(Tee[2, 3]) - 0.247) < 1e-9
+    assert abs(float(Tee[0, 3]) - 0.50) > 1e-4, "XY 补偿被撤回了"
+    assert final.get("trim_reverted") is not True
+    assert final.get("close_snap_untrusted") is True
+
+
+def test_once_trim_refuses_when_pair_already_broken():
+    """第一拍间距已经 45mm，检测没立住，不许再动。"""
+    st = _TrimStub(err_xy=(0.004, 0.0))
+    st.hole_keypoint_once = True
+    st._tip_off = np.array([0.0001, -0.0003])
+    st.pair_dy = -0.045
+    _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)

src/arm_application/test/test_stage3_tactile_live.py

@@ -24,6 +24,7 @@ from dataclasses import dataclass
 from arm_application.stage3_search_node import (
     TACTILE_MAX_AGE_S,
     live_tactile,
+    should_lift_air_baseline,
     tactile_timeout_reason,
 )
 
@@ -77,3 +78,10 @@ def test_timeout_reason_still_reports_stall_when_only_stale_frames_exist():
     old = time.monotonic() - 10.0
     msg = tactile_timeout_reason([(old, _Msg(connected=True)), None], 3.0)
     assert "断流" in msg
+
+
+def test_no_tactile_does_not_lift_to_air_baseline():
+    """触觉没数据时不许先抬到 hover+50mm 再报断流。"""
+    assert should_lift_air_baseline(0.385, 0.470, tactile_live=False) is False
+    assert should_lift_air_baseline(0.249, 0.470, tactile_live=True) is True
+    assert should_lift_air_baseline(0.468, 0.470, tactile_live=True) is False

src/flexiv_ros2/flexiv_hardware/include/flexiv_hardware/flexiv_hardware_interface.hpp

@@ -9,6 +9,7 @@
 #ifndef FLEXIV_HARDWARE__FLEXIV_HARDWARE_INTERFACE_HPP_
 #define FLEXIV_HARDWARE__FLEXIV_HARDWARE_INTERFACE_HPP_
 
+#include <chrono>
 #include <memory>
 #include <string>
 #include <vector>
@@ -139,6 +140,16 @@ private:
     // 让 ros2_control 把栈停下来报错，而不是静默地"成功"。
     int64_t last_state_stamp_ns_ = 0;   // 上一帧的机器人时间戳
     int64_t last_state_change_ns_ = 0;  // 上一帧时间戳发生变化的本地时刻
+
+    // D435 粗定位的 MoveL 走已经连上的这份 RDK。write() 里一步一步推进，
+    // 不另开线程，也不在这段里往关节接口灌点。请求文件是 /tmp/flexiv_movel_req。
+    enum class VendorMovelPhase { kIdle, kRunning };
+    VendorMovelPhase vendor_movel_phase_ = VendorMovelPhase::kIdle;
+    long vendor_movel_seq_ = -1;
+    long vendor_movel_done_seq_ = -1;
+    std::chrono::steady_clock::time_point vendor_movel_deadline_ {};
+    bool poll_vendor_movel();
+    void finish_vendor_movel(long seq, bool ok, const std::string& detail);
 };
 
 } /* namespace flexiv_hardware */

src/flexiv_ros2/flexiv_hardware/src/flexiv_hardware_interface.cpp

@@ -7,15 +7,21 @@
  */
 
 #include <algorithm>
+#include <array>
 #include <chrono>
 #include <cmath>
 #include <cstdint>
+#include <cstdio>
 #include <cstdlib>
 #include <exception>
+#include <fstream>
 #include <string>
+#include <variant>
 #include <thread>
 #include <vector>
 
+#include "flexiv/rdk/mode.hpp"
+
 // RDK 网卡白名单的本地核对（见下面 resolve_network_interface_whitelist 的说明）
 #include <arpa/inet.h>
 #include <ifaddrs.h>
@@ -43,6 +49,76 @@ constexpr double kServoJointAcceleration = 1.00;
 // 留了很大余量；宁可晚 0.5 s 报错，也不要在运动中误判把栈停掉。
 constexpr int64_t kStateStaleTimeoutNs = 500'000'000LL;
 
+// 伺服和硬件不在一个进程里。请求写成一行，原子改名，避免读到半行。
+constexpr const char* kMovelReq = "/tmp/flexiv_movel_req";
+constexpr const char* kMovelAck = "/tmp/flexiv_movel_ack";
+constexpr const char* kMovelDone = "/tmp/flexiv_movel_done";
+
+long read_movel_seq()
+{
+    std::ifstream in(kMovelReq);
+    long seq = -1;
+    if (!(in >> seq)) {
+        return -1;
+    }
+    return seq;
+}
+
+// 与 scripts/rdk_goto_xyz.py 的 quat_wxyz_to_euler_xyz_deg 同一套。
+// 不要改用 Eigen eulerAngles：工具朝下接近 180° 时会翻成另一组角，MoveL 会拧一圈。
+std::array<double, 3> movel_euler_deg(double qw, double qx, double qy, double qz)
+{
+    const double n = std::sqrt(qw * qw + qx * qx + qy * qy + qz * qz);
+    if (n < 1e-12) {
+        throw std::runtime_error("tcp 四元数是零");
+    }
+    qw /= n;
+    qx /= n;
+    qy /= n;
+    qz /= n;
+    const double xx = qx * qx;
+    const double yy = qy * qy;
+    const double zz = qz * qz;
+    const double xy = qx * qy;
+    const double xz = qx * qz;
+    const double yz = qy * qz;
+    const double wx = qw * qx;
+    const double wy = qw * qy;
+    const double wz = qw * qz;
+    const double r00 = 1.0 - 2.0 * (yy + zz);
+    const double r10 = 2.0 * (xy + wz);
+    const double r20 = 2.0 * (xz - wy);
+    const double r21 = 2.0 * (yz + wx);
+    const double r22 = 1.0 - 2.0 * (xx + yy);
+    const double r12 = 2.0 * (yz - wx);
+    const double r11 = 1.0 - 2.0 * (xx + zz);
+    const double sy = std::sqrt(r00 * r00 + r10 * r10);
+    double x = 0.0;
+    double y = 0.0;
+    double z = 0.0;
+    if (sy > 1e-6) {
+        x = std::atan2(r21, r22);
+        y = std::atan2(-r20, sy);
+        z = std::atan2(r10, r00);
+    } else {
+        x = std::atan2(-r12, r11);
+        y = std::atan2(-r20, sy);
+        z = 0.0;
+    }
+    constexpr double kRad2Deg = 180.0 / 3.14159265358979323846;
+    return {x * kRad2Deg, y * kRad2Deg, z * kRad2Deg};
+}
+
+void write_movel_line(const char* path, const std::string& line)
+{
+    const std::string tmp = std::string(path) + ".tmp";
+    {
+        std::ofstream out(tmp, std::ios::trunc);
+        out << line << "\n";
+    }
+    std::rename(tmp.c_str(), path);
+}
+
 struct NrtSendGate
 {
     std::vector<double> last_sent;
@@ -573,6 +649,9 @@ hardware_interface::CallbackReturn FlexivHardwareInterface::on_activate(
     }
 
     RCLCPP_INFO(getLogger(), "System successfully started!");
+    // 启动前磁盘上的旧请求不许再执行一遍。新请求用更晚的序号。
+    vendor_movel_done_seq_ = read_movel_seq();
+    vendor_movel_phase_ = VendorMovelPhase::kIdle;
 
     return hardware_interface::CallbackReturn::SUCCESS;
 }
@@ -662,9 +741,112 @@ hardware_interface::return_type FlexivHardwareInterface::read(
     return hardware_interface::return_type::OK;
 }
 
+void FlexivHardwareInterface::finish_vendor_movel(
+    long seq, bool ok, const std::string& detail)
+{
+    std::string clean;
+    clean.reserve(detail.size());
+    for (char c : detail) {
+        clean.push_back(c == '\n' || c == '\r' ? ' ' : c);
+    }
+    if (ok) {
+        write_movel_line(kMovelDone, std::to_string(seq) + " ok");
+    } else {
+        write_movel_line(kMovelDone, std::to_string(seq) + " fail " + clean);
+    }
+    vendor_movel_done_seq_ = seq;
+    vendor_movel_phase_ = VendorMovelPhase::kIdle;
+}
+
+bool FlexivHardwareInterface::poll_vendor_movel()
+{
+    // 这一拍正在走 MoveL：只查 reachedTarget，不往关节接口灌点。
+    // read() 仍每拍更新状态，时间戳看门狗不会把这段误判成断连。
+    if (vendor_movel_phase_ == VendorMovelPhase::kRunning) {
+        try {
+            if (std::chrono::steady_clock::now() > vendor_movel_deadline_) {
+                try {
+                    robot_->SwitchMode(rdk_control_mode_);
+                } catch (const std::exception&) {
+                }
+                finish_vendor_movel(vendor_movel_seq_, false, "timeout");
+                return true;
+            }
+            const auto states = robot_->primitive_states();
+            const auto it = states.find("reachedTarget");
+            const bool reached = it != states.end()
+                && std::holds_alternative<int>(it->second)
+                && std::get<int>(it->second) != 0;
+            if (reached) {
+                robot_->SwitchMode(rdk_control_mode_);
+                finish_vendor_movel(vendor_movel_seq_, true, "");
+            }
+        } catch (const std::exception& e) {
+            try {
+                robot_->SwitchMode(rdk_control_mode_);
+            } catch (const std::exception&) {
+            }
+            finish_vendor_movel(vendor_movel_seq_, false, e.what());
+        }
+        return true;
+    }
+
+    std::ifstream in(kMovelReq);
+    long seq = 0;
+    double x = 0.0;
+    double y = 0.0;
+    double z = 0.0;
+    double vel = 0.0;
+    int keep_z = 0;
+    if (!(in >> seq >> x >> y >> z >> keep_z >> vel)) {
+        return false;
+    }
+    if (seq == vendor_movel_done_seq_) {
+        return false;
+    }
+
+    // 先应答，伺服才能在 1.5s 内分清「新接口没编进去」和「臂还在走」。
+    write_movel_line(kMovelAck, std::to_string(seq));
+    try {
+        const auto pose = robot_->states().tcp_pose;
+        if (keep_z != 0) {
+            z = pose[2];
+        }
+        const auto euler = movel_euler_deg(pose[3], pose[4], pose[5], pose[6]);
+        if (robot_->fault()) {
+            robot_->ClearFault(3);
+        }
+        robot_->SwitchMode(flexiv::rdk::Mode::NRT_PRIMITIVE_EXECUTION);
+        const flexiv::rdk::Coord target(
+            std::array<double, 3> {x, y, z},
+            euler,
+            std::array<std::string, 2> {"WORLD", "WORLD_ORIGIN"});
+        robot_->ExecutePrimitive(
+            "MoveL", {{"target", target}, {"vel", vel}}, true);
+        vendor_movel_seq_ = seq;
+        vendor_movel_phase_ = VendorMovelPhase::kRunning;
+        vendor_movel_deadline_ = std::chrono::steady_clock::now() + std::chrono::seconds(90);
+        RCLCPP_INFO(getLogger(),
+            "厂家 MoveL 已下发 xyz=(%.3f, %.3f, %.3f) keep_z=%d vel=%.3f",
+            x, y, z, keep_z, vel);
+    } catch (const std::exception& e) {
+        try {
+            robot_->SwitchMode(rdk_control_mode_);
+        } catch (const std::exception&) {
+        }
+        finish_vendor_movel(seq, false, e.what());
+    }
+    return true;
+}
+
 hardware_interface::return_type FlexivHardwareInterface::write(
     const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/)
 {
+    // MoveL 期间不能回到关节模式，也不能把旧关节目标再发出去。
+    if (poll_vendor_movel()) {
+        return hardware_interface::return_type::OK;
+    }
+
     // Initialize target vectors to hold position
     std::vector<double> target_pos(robot_->info().DoF);
     std::vector<double> target_vel(robot_->info().DoF);