初步完成平行夹爪旋转入孔-不断的张开转到逆时针极限再闭合转到顺时针极限

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

config/site.example.yaml

@@ -39,7 +39,7 @@ robot:
       # 本臂装了 Orisys 视触觉（只有左臂有）。脚本靠这一项决定要不要拉触觉服务，
       # 而不是靠"SN 等于某个具体值"这种写死的判断（换台臂就会失效）。
       tactile: true
-      # 2026-10-12 示教的工具 TCP（tool + 1012 + 0938）。眼在手上 AX=XB 用这份 TCP。
+      # 2026-10-16 下午 RDK 重读 Studio tool10120938（用户再次更新 TCP）。
       tool_name: "tool10120938"
       camera:
         name: d405_left          # 话题 /d405_left/... 与 91 分开
@@ -68,6 +68,8 @@ robot:
         handeye_workdir: "/home/USER/ros2/handeye/tmp/89"
       handeye:
         # Studio tcp_location（法兰系，米 + qwxyz）。与 rdk Tool.params 必须一致。
+        # 眼在手上：AX=XB 的手是这份 TCP，再折成 cam→法兰写 T_4x4。
+        # 眼在手外：AX=XB 的手同样是这份 TCP（板夹在工具上）；T_4x4 仍是 cam→基座，不折。
         gripper_tool: "tool10120938"
         gripper_tcp_xyz: [0.000537, -0.000264, 0.226587]
         gripper_tcp_qwxyz: [1.0, 0.0, 0.0, 0.0]
@@ -177,7 +179,10 @@ robot:
         #   落在本文件下方注释记录的合理区间 0.204~0.233 内。
         #   复核：python3 scripts/sense_tool_length.py --sn Rizon4-XXXXXX
         #         python3 scripts/lib/real_geometry_check.py --sn Rizon4-XXXXXX
-        # 下臂安全距离按 tool10120938 的 TCP +Z，不再用旧 tool1 的 0.223。
+        # 2026-10-16 下午：RDK Tool.params("tool10120938").tcp_location
+        #   [x,y,z,qw,qx,qy,qz] = [0.000537, -0.000264, 0.226587, 1, 0, 0, 0]
+        #   示教器拟合误差：平均 0.58 / 最大 0.99 / 最小 0.29 mm。
+        # 下臂安全距离用 TCP +Z。上午 0.226274；再往前 0.2245 是 10-12；0.223 是 tool1。
         tool_tip_below_flange_m: 0.226587
     # >>> 仿真那台臂（Gazebo）也登记成 robots[] 里的一台 <<<
     # 为什么必须登记：应用是按"当前臂"的名字去查 frame / joint / 相机话题的
@@ -769,17 +774,25 @@ hole_detect:
     # —— 柔顺运动（夹爪持规，防急加速/急停/过冲把规震脱）：速度放慢 + 平滑刹车 ——
     align_speed: 0.014       # 对准段笛卡尔速度 [m/s]（原0.03，放缓防冲）
     move_speed: 0.022        # 升降/粗居中段速度 [m/s]（原0.05，放缓防震规）
+    # 对小孔关键点：只用像素射线交到 workpiece_surface_z_m，不读深度图。
     hole_visual_only: true
-    # 本分支实验：true=悬停拍一次并走完到孔；main 保存点应保持 false。
+    # 本分支实验（exp/hole-keypoint-once）：true=悬停拍一次，框心→k1 一次走完
+    # （40mm 上限）。main 保存点是 false（多走多拍）。
     hole_keypoint_once: true
-    # true=悬停走完后一条厂家 MoveL 贴面。对照观察高补交时改 false。
+    # true=悬停走完后一条厂家 MoveL 贴面（不补拍）。对照 37b85aa 改回 false。
     hole_once_lock_xy_descend: true
-    # 0=笛卡尔锁 XY 只降 Z。k>0 才沿 -look 加前馈。
+    # 贴面相对悬停：沿 -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]。CLI --final-gap 覆盖。
+    final_gap_mm: 2.0        # 不进孔时的贴面目标：规尖离面 [mm]。笛卡尔锁 XY
+                             # 时法兰 Z = 工具长 + 工件面 + 间隙。CLI --final-gap 覆盖。
+    hole_entry_mm: 1.0       # 规尖停在工件上表面以下 [mm]。2026-10-19 12:20 左臂：
+                             # 4mm 指令在面下 1.6mm 被挡住（厂家 302064 到不了位），
+                             # 流水线接着空等 90s。1.0mm 留在挡住点之上，仍进孔口。
+                             # 0 = 停在 final_gap（面上）。上限 8mm（板厚 12mm）。
     observe_gap_mm: 15.0     # --no-descent 时的观测下降：只降到离面这么高 [mm]，
                              # 方便看规尖偏没偏。必须 ≥ final_gap_mm。
-                             # 左臂89：悬停间隙约 75mm（0.320−0.223−0.022）⇒ 大约下降 60mm。
+                             # 左臂89：悬停间隙约 71.4mm（0.320−0.226587−0.022）⇒ 观测下降约 56.4mm。
     final_descent_speed: 0.005  # 最后轻降速度 [m/s]（很慢，柔顺）
     # ✅ 2026-10-08 定稿：下降量**只由几何决定**（法兰 FK − 工具长 − 工件面 − final_gap），
     #   深度读数只做交叉核对、不再参与任何决策。两段真机事故方向相反、都指向这条：
@@ -792,7 +805,7 @@ hole_detect:
     #     ⇒ 读数往**小**错一次就永远到不了目标。
     #   同一套读数两个方向各错一次、幅度 150mm 量级、随位姿乱跳 —— 它对"能降多少"
     #   和"该不该停"都没有发言权。现在它只在偏差 >10mm 时喊一句"深度测距与几何不符"。
-    # 现行几何：悬停 0.320 − 工具长 0.223 − 工件面 0.022 = 75mm 间隙 ⇒ 降 73mm 到 2mm。
+    # 现行几何：悬停 0.320 − 工具长 0.226587 − 工件面 0.022 = 71.4mm 间隙 ⇒ 降 69.4mm 到 2mm。
     #   三道**物理/几何**保险：descent_budget_m（配置上限）+ 腕部 FT 接触检测（停+退 1mm）
     #   + 任一步 _goto 走不到位就停。
     #   缺几何（工具长/工件面没配）时**拒绝下降**并报错（保持悬停），绝不拿测距去赌。
@@ -923,8 +936,9 @@ gauge_assembly:
   # ---- S01 回 home 后按工具系逆时针预绕到极限角 ----
   # 预绕是为了给 S08 的棘轮留出回转行程：不预绕，拧两圈就贴关节限位。
   extreme_angle_deg: 150.0   # 想转多少度（按工具系逆时针）
-  extreme_speed_deg_s: 20.0  # 关节角速度 [deg/s]，慢一点防冲击
-  extreme_direction: 1       # +1=逆时针 / -1=顺时针（改符号，别去改代码）
+  extreme_speed_deg_s: 10.0  # 关节角速度 [deg/s]。2026-10-19 左臂 A7 咬合从 20 降到 10，
+                             # 限位到限位约 34s（原先约 17s）
+  extreme_direction: -1      # +1=逆时针 / -1=顺时针（2026-10-19 真机 A7 转反了，关节角增大是顺时针）
   extreme_margin_deg: 3.0    # 距关节限位留的余量 [deg]；撞限位不算失败（按 JOINT_LIMIT 继续）
                              # 但余量太小会让规划器频繁拒收，表现为"偶尔转不到位"
 

config/site.yaml

@@ -768,8 +768,12 @@ hole_detect:
     # 贴面相对悬停：沿 -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 覆盖。
+    final_gap_mm: 2.0        # 不进孔时的贴面目标：规尖离面 [mm]。笛卡尔锁 XY
+                             # 时法兰 Z = 工具长 + 工件面 + 间隙。CLI --final-gap 覆盖。
+    hole_entry_mm: 1.0       # 规尖停在工件上表面以下 [mm]。2026-10-19 12:20 左臂：
+                             # 4mm 指令在面下 1.6mm 被挡住（厂家 302064 到不了位），
+                             # 流水线接着空等 90s。1.0mm 留在挡住点之上，仍进孔口。
+                             # 0 = 停在 final_gap（面上）。上限 8mm（板厚 12mm）。
     observe_gap_mm: 15.0     # --no-descent 时的观测下降：只降到离面这么高 [mm]，
                              # 方便看规尖偏没偏。必须 ≥ final_gap_mm。
                              # 左臂89：悬停间隙约 71.4mm（0.320−0.226587−0.022）⇒ 观测下降约 56.4mm。
@@ -916,8 +920,9 @@ gauge_assembly:
   # ---- S01 回 home 后按工具系逆时针预绕到极限角 ----
   # 预绕是为了给 S08 的棘轮留出回转行程：不预绕，拧两圈就贴关节限位。
   extreme_angle_deg: 150.0   # 想转多少度（按工具系逆时针）
-  extreme_speed_deg_s: 20.0  # 关节角速度 [deg/s]，慢一点防冲击
-  extreme_direction: 1       # +1=逆时针 / -1=顺时针（改符号，别去改代码）
+  extreme_speed_deg_s: 10.0  # 关节角速度 [deg/s]。2026-10-19 左臂 A7 咬合从 20 降到 10，
+                             # 限位到限位约 34s（原先约 17s）
+  extreme_direction: -1      # +1=逆时针 / -1=顺时针（2026-10-19 真机 A7 转反了，关节角增大是顺时针）
   extreme_margin_deg: 3.0    # 距关节限位留的余量 [deg]；撞限位不算失败（按 JOINT_LIMIT 继续）
                              # 但余量太小会让规划器频繁拒收，表现为"偶尔转不到位"
 

docs/HANDBOOK.md

@@ -79,7 +79,8 @@ ROS 相对 XY，`keep_z`。硬件在 MoveL 期间不灌关节指令。D435 这
 启动高度用 `--init-z`（法兰 Z，米），对孔悬停用 `--hover-z`。两个都是法兰高度。
 下臂贴面用 `--final-gap`（规尖离面，毫米；site 默认 `final_gap_mm` 2）。笛卡尔锁 XY
 时只改 Z：法兰 = 工具长 + 工件面 + 间隙。不能和 `--no-descent` 一起写。
-会让工具尖进入工件、或超出工作盒的值直接拒绝，不会改成另一个高度再往下走。
+`--final-gap` ≤ 0、或超出工作盒的值直接拒绝，不会改成另一个高度再往下走。
+往孔里进只走 `stage2.hole_entry_mm`（正值，上限 8 mm）。
 不写则一阶段锁高仍按 site，对孔悬停仍是 `hole_hover_z_m`（89 为 0.320）。
 框心对准连续 `servo.wrist_align_sec`（0.5 s）才 `/visual_servo/locked`，然后才精拍下臂。
 本分支（`exp/hole-keypoint-once`）二阶段是**单次精拍**：悬停拍一次 k1/k2，
@@ -90,7 +91,9 @@ ROS 相对 XY，`keep_z`。硬件在 MoveL 期间不灌关节指令。D435 这
 `main` 保存点仍是多走多拍。
 孔坐标把关键点像素交到 `workpiece_surface_z_m`，不读深度图。下臂停在
 `tool10120938` 的 TCP 长度（0.226587 m，2026-10-16 下午 RDK 重读）加上 `final_gap_mm` 之上，不按旧 tool1 的 0.223 m。
-贴面之后就停在这个法兰高度，不抬回 `hole_hover_z_m`。没有接触传感器，到位只认 `final_gap_mm`，不另设一个 Z。
+贴面之后就停在这个法兰高度，不抬回 `hole_hover_z_m`。没有接触传感器。
+`stage2.hole_entry_mm`（现为 1）> 0 时，插孔目标改成规尖在工件上表面以下这么深（孔口凹槽），再转 A7；为 0 时仍停在 `final_gap_mm`。进孔超过 8 mm（板厚 12 mm）直接拒绝。2026-10-19 12:20 写成 4 mm 时，臂在面下 1.6 mm 被挡住。
+然后只转左臂 A7（厂家 `SendJointPosition` 的第 7 轴，模式 `NRT_JOINT_POSITION`）：先逆时针转到软件限位内侧 `extreme_margin_deg`，再顺时针转到另一侧。顺时针到位记为「首轮咬合结束」。接着 `run_gripper.sh --arm <臂号> --cli pos 400` 把平行爪只张开到 400（不全开），A7 逆时针回到限位，`--cli close` 夹住规，再顺时针转到限位。前六轴不动，不改法兰 Z。顺时针是进孔，逆时针是出孔。夹爪命令失败或状态没到，就停在当前关节。
 只测 D435 这一段位移时加 `--d435-only`：不交 D405、不进二阶段，并且要求先停 `run_real.sh`。
 
 ```bash

src/arm_application/arm_application/gauge_kin.py

@@ -26,6 +26,9 @@ import numpy as np
 #: 直接拒绝整条轨迹 —— 那不是我们要的"极限角"，是报错。
 DEFAULT_LIMIT_MARGIN_DEG = 3.0
 
+#: 厂家 A7。``Robot::SendJointPosition`` 的关节向量按 A1..A7 排列，A7 是最后一轴。
+A7_INDEX = 6
+
 _JOINT_RE = re.compile(r"<joint\s+name\s*=\s*\"([^\"]+)\"")
 _ATTR_RE = re.compile(r"(lower|upper)\s*=\s*\"([-+0-9.eE]+)\"")
 
@@ -178,6 +181,43 @@ def prewind(
     )
 
 
+def a7_limit_target(
+    q: Sequence[float],
+    limits: Optional[Tuple[float, float]],
+    direction: int,
+    margin_deg: float = DEFAULT_LIMIT_MARGIN_DEG,
+) -> Optional[np.ndarray]:
+    """只把 A7 收到软件限位内侧。前六轴原样拷回，不许在这里改。
+
+    厂家接口是 ``Robot::SendJointPosition``（模式 ``NRT_JOINT_POSITION``）。
+    向量按 A1..A7 排，A7 是下标 ``A7_INDEX``。限位是 ``RobotInfo::q_min/q_max``
+    的那一对，真机上与 URDF ``joint7`` 的 lower/upper 是同一组数。
+
+    ``direction`` 与 ``prewind`` 相同：``+1`` 关节角增大 = 逆时针，目标取上限；
+    ``-1`` 关节角减小 = 顺时针，目标取下限。现场若发现正负反了，改
+    ``site.yaml`` 的 ``extreme_direction``，不要改这里的符号。
+
+    没有限位、方向不是 ±1、或者余量把上下限挤没了，返回 None。
+    调用方必须停，不许猜一个角度发出去。
+    """
+    if direction not in (-1, 1):
+        raise ValueError(f"方向只能是 +1 / -1，收到 {direction}")
+    arr = np.asarray(q, dtype=float).reshape(-1)
+    if arr.size != 7:
+        raise ValueError(f"A7 目标要 7 个关节角，收到 {arr.size}")
+    if limits is None:
+        return None
+    lo = float(limits[0])
+    up = float(limits[1])
+    margin = math.radians(float(margin_deg))
+    if not math.isfinite(lo) or not math.isfinite(up) or up - lo <= 2.0 * margin:
+        return None
+    target = (up - margin) if direction > 0 else (lo + margin)
+    out = arr.copy()
+    out[A7_INDEX] = target
+    return out
+
+
 def rotation_about_axis_deg(R_before: np.ndarray, R_after: np.ndarray, axis: np.ndarray) -> float:
     """从两个旋转矩阵算出"绕 ``axis`` 转了多少度"（带符号）。
 

src/arm_application/arm_application/overhead_oneshot.py

@@ -688,10 +688,60 @@ def hole_pair_span_ok(span_m: float) -> bool:
 
 
 def flange_z_for_gap_m(tip_len_m: float, surface_z_m: float, gap_m: float) -> float:
-    """规尖离面 ``gap_m`` 时的法兰 Z [m]。与轻降几何同一条式子。"""
+    """规尖离面 ``gap_m`` 时的法兰 Z [m]。与轻降几何同一条式子。
+
+    ``gap_m`` 为正是停在面上；为负是规尖进入孔口（上表面以下）。
+    """
     return float(tip_len_m) + float(surface_z_m) + float(gap_m)
 
 
+# 工件板厚 12mm。进孔超过这个值会朝下表面穿，直接拒绝，不改成更浅再走。
+HOLE_ENTRY_MAX_M = 0.008
+
+
+def insert_target_gap_m(final_gap_m: float, hole_entry_m: float) -> float:
+    """插孔停在哪 [m]。``hole_entry_m`` > 0 时规尖在工件上表面以下这么深；否则停在面上 ``final_gap_m``。"""
+    entry = float(hole_entry_m)
+    if entry > 1e-9:
+        return -entry
+    return float(final_gap_m)
+
+
+def hole_entry_refusal(
+    entry_m: float,
+    tool_len_m: float,
+    surface_z_m: float,
+    z_min_m: float,
+    z_max_m: float,
+) -> str:
+    """进孔深度能不能用。空串表示可以（含 0 = 不进孔）。非空就是拒绝原因。"""
+    entry = float(entry_m)
+    if not np.isfinite(entry):
+        return "进孔深度不是有限数，拒绝开跑"
+    if entry < -1e-9:
+        return f"进孔深度 {entry * 1000:.1f}mm < 0，拒绝开跑"
+    if entry <= 1e-9:
+        return ""
+    if entry > HOLE_ENTRY_MAX_M + 1e-9:
+        return (
+            f"进孔深度 {entry * 1000:.1f}mm 超过 {HOLE_ENTRY_MAX_M * 1000:.0f}mm"
+            "（板厚 12mm，不许穿出下表面），拒绝开跑"
+        )
+    tool = float(tool_len_m)
+    surf = float(surface_z_m)
+    if tool <= 0.0 or surf <= 0.0:
+        return "写了进孔深度，但工具长度或工件面未知，无法确认尖端进了多深，拒绝开跑"
+    flange = flange_z_for_gap_m(tool, surf, -entry)
+    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（面上再进 {entry * 1000:.1f}mm）"
+            f"超出工作盒 Z [{z_lo:.3f}, {z_hi:.3f}]，拒绝开跑"
+        )
+    return ""
+
+
 def final_gap_refusal(
     gap_m: float,
     tool_len_m: float,

src/arm_application/arm_application/pipeline_node.py

@@ -11,20 +11,29 @@
 from __future__ import annotations
 
 import json
+import math
 import os
 import signal
 import subprocess
 import time
 from pathlib import Path
-from typing import Optional
+from typing import Optional, Tuple
 
 import numpy as np
 import rclpy
 from geometry_msgs.msg import PointStamped
-from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy
-from std_msgs.msg import Bool, String
+from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile, ReliabilityPolicy
+from std_msgs.msg import Bool, Float32MultiArray, String
+from trajectory_msgs.msg import JointTrajectory
 
+from arm_application.gauge_kin import (
+    a7_limit_target,
+    match_limits,
+    parse_joint_limits,
+)
 from arm_application.overhead_oneshot import D435_MOVEL_GAVE_UP
+from arm_application.site_io import load_site_dict, ros2_root
+from arm_application.stage2_locate_node import _pt
 from arm_application.stage3_search_node import Stage3Search
 
 
@@ -165,6 +174,19 @@ class AssemblyPipeline(Stage3Search):
 
         self._s1: Optional[subprocess.Popen] = None
         self._locked = False
+        self._robot_description: Optional[str] = None
+        # robot_state_publisher 用 TRANSIENT_LOCAL 只发一次。默认 VOLATILE
+        # 订阅在它之后才起来，3s 内一条都收不到，A7 会因“没有限位”拒绝旋转。
+        self.create_subscription(
+            String,
+            "/robot_description",
+            self._on_robot_description,
+            QoSProfile(
+                depth=1,
+                durability=DurabilityPolicy.TRANSIENT_LOCAL,
+                reliability=ReliabilityPolicy.RELIABLE,
+            ),
+        )
         self.create_subscription(Bool, "/visual_servo/locked", self._on_lock, 10)
         # 诊断用：一阶段超时时靠它区分"感知没给点"和"给了点没收敛"。
         # 只计数，不参与控制 —— 它绝不能影响流程本身的判断。
@@ -208,6 +230,235 @@ class AssemblyPipeline(Stage3Search):
         self._state_pub.publish(msg)
         self._pipe_pub.publish(msg)
 
+    def _on_robot_description(self, msg: String) -> None:
+        self._robot_description = str(msg.data)
+
+    def _a7_limits(self, timeout_s: float = 3.0) -> Optional[Tuple[float, float]]:
+        """A7 软件限位。来自 ``/robot_description`` 的 joint7，与厂家 ``q_min/q_max`` 同一对。"""
+        t0 = time.monotonic()
+        while self._robot_description is None and time.monotonic() - t0 < timeout_s:
+            self._spin(0.05)
+        if not self._robot_description:
+            self.get_logger().error("拿不到 /robot_description，A7 没有限位，拒绝旋转")
+            return None
+        got = match_limits(
+            parse_joint_limits(self._robot_description), [self.joint_names[-1]]
+        )
+        if got is None:
+            self.get_logger().error(
+                f"/robot_description 里没有 {self.joint_names[-1]} 的限位，拒绝旋转"
+            )
+        return got
+
+    def _send_a7_only(self, a7_rad: float, speed_deg_s: float, tag: str) -> bool:
+        """只下发 A7。前六轴用这一拍的当前角，厂家 ``SendJointPosition`` 收到的也是这一组。"""
+        q, Tee = self._wait_state(5.0)
+        if q is None or Tee is None or int(q.size) != 7:
+            self.get_logger().error(f"{tag}: 拿不到 7 轴关节，不下发")
+            return False
+        z0 = float(Tee[2, 3])
+        q_hold = q.copy()
+        q_hold[6] = float(a7_rad)
+        delta_deg = abs(math.degrees(float(q_hold[6] - q[6])))
+        dur = max(0.8, delta_deg / max(float(speed_deg_s), 1.0))
+        zeros = np.zeros(q.size)
+        traj = JointTrajectory()
+        traj.header.stamp.sec = 0
+        traj.header.stamp.nanosec = 0
+        traj.joint_names = list(self.joint_names)
+        traj.points = [_pt(q, zeros, 0.2), _pt(q_hold, zeros, 0.2 + dur)]
+        self.get_logger().info(
+            f"{tag}: A7 {math.degrees(float(q[6])):+.1f}° → "
+            f"{math.degrees(float(q_hold[6])):+.1f}°，"
+            f"{speed_deg_s:.0f}°/s，前六轴不动，法兰 Z={z0:.4f}m"
+        )
+        self._send_action(traj, wait_s=dur + 8.0)
+        q2, Tee2 = self._wait_state(5.0)
+        if q2 is None or Tee2 is None:
+            self.get_logger().error(f"{tag}: 走完读不到关节")
+            return False
+        others = float(np.max(np.abs(q2[:6] - q[:6])))
+        dz = abs(float(Tee2[2, 3]) - z0)
+        a7_err = abs(float(q2[6] - q_hold[6]))
+        self.get_logger().info(
+            f"{tag}: 前六轴最大偏差 {others:.4f} rad，"
+            f"法兰 ΔZ={dz * 1000:.1f}mm，A7 残差 {math.degrees(a7_err):.1f}°"
+        )
+        if others > 0.02:
+            self.get_logger().error(f"{tag}: 前六轴动了 {others:.3f} rad，中止")
+            return False
+        if dz > 0.003:
+            self.get_logger().error(f"{tag}: 法兰 Z 变了 {dz * 1000:.1f}mm，中止")
+            return False
+        if a7_err > 0.05:
+            self.get_logger().error(
+                f"{tag}: A7 还差 {math.degrees(a7_err):.1f}°，没到限位内侧"
+            )
+            return False
+        self._pin(q2)
+        return True
+
+    def _arm_short_id(self) -> str:
+        """当前臂号。与 ``export_site_env.arm_short_id`` 一样，取 SN 末两位。"""
+        short = os.environ.get("SITE_ROBOT_SHORT_ID", "").strip()
+        if short:
+            return short
+        sn = str(
+            self.get_parameter("robot_sn").value
+            or os.environ.get("SITE_ROBOT_SN")
+            or ""
+        ).strip()
+        tail = sn.rsplit("-", 1)[-1]
+        if tail.isdigit() and len(tail) >= 2:
+            return tail[-2:]
+        return ""
+
+    def _gripper_cli(self, action: str, value: str = "") -> bool:
+        """左平行爪。``close`` 合拢；``pos`` 走到 0–1000 的指定开度，不全开。
+
+        脚本返回只说明命令已交给 ``/gripper/ctrl``。没在状态里看到目标开度或夹住，
+        就不许接着转 A7。
+        """
+        short = self._arm_short_id()
+        if not short:
+            self.get_logger().error("不知道当前臂号，拒绝开合夹爪")
+            return False
+        if action == "close":
+            extra: list[str] = []
+        elif action == "pos":
+            try:
+                pos = int(value)
+            except (TypeError, ValueError):
+                pos = -1
+            if pos < 0 or pos > 1000:
+                self.get_logger().error(f"夹爪位置只接受 0–1000，收到 {value!r}")
+                return False
+            extra = [str(pos)]
+        else:
+            self.get_logger().error(f"夹爪命令只接受 pos/close，收到 {action}")
+            return False
+        script = ros2_root() / "scripts" / "run_gripper.sh"
+        cmd = ["bash", str(script), "--arm", short, "--cli", action, *extra]
+        self.get_logger().info("夹爪：" + " ".join(cmd))
+        try:
+            proc = subprocess.run(
+                cmd, capture_output=True, text=True, timeout=30, check=False
+            )
+        except subprocess.TimeoutExpired:
+            self.get_logger().error(f"夹爪 {action} 30s 没有返回，A7 不再转")
+            return False
+        text = ((proc.stdout or "") + "\n" + (proc.stderr or "")).strip()
+        if text:
+            tail = " | ".join(text.splitlines()[-4:])
+            self.get_logger().info(f"夹爪脚本：{tail}")
+        if proc.returncode != 0:
+            self.get_logger().error(
+                f"夹爪 {action} 失败 exit={proc.returncode}，A7 不再转"
+            )
+            return False
+        return self._wait_gripper(action, value)
+
+    def _wait_gripper(self, action: str, value: str = "", timeout_s: float = 8.0) -> bool:
+        """``/gripper/states``：``pos`` 要停在目标附近；闭合要报夹住（grip=2）。"""
+        box: dict = {"data": None}
+
+        def _on_state(msg: Float32MultiArray) -> None:
+            box["data"] = list(msg.data)
+
+        sub = self.create_subscription(Float32MultiArray, "/gripper/states", _on_state, 10)
+        t0 = time.monotonic()
+        last = None
+        try:
+            while time.monotonic() - t0 < timeout_s:
+                self._spin(0.05)
+                data = box["data"]
+                if not data or len(data) < 3:
+                    continue
+                grip = int(data[1])
+                pos = float(data[2])
+                last = (grip, pos)
+                if action == "pos" and grip != 0 and abs(pos - float(value)) <= 40.0:
+                    self.get_logger().info(
+                        f"夹爪已到 pos={pos:.0f}（目标 {value}，不全开）"
+                    )
+                    return True
+                if action == "close" and grip == 2:
+                    self.get_logger().info(f"夹爪已夹住 pos={pos:.0f}")
+                    return True
+        finally:
+            self.destroy_subscription(sub)
+        shown = f"grip={last[0]} pos={last[1]:.0f}" if last else "没有 /gripper/states"
+        self.get_logger().error(
+            f"夹爪 {action} 后 {timeout_s:.0f}s 未到位（{shown}），A7 不再转"
+        )
+        return False
+
+    def _a7_to_limit(
+        self, limits, direction: int, margin_deg: float, speed: float, tag: str
+    ) -> bool:
+        """只把 A7 转到这一侧限位内侧。前六轴和法兰 Z 的检查在 ``_send_a7_only``。"""
+        q, _Tee = self._wait_state(5.0)
+        if q is None:
+            self.get_logger().error(f"{tag}: 没有当前关节")
+            return False
+        target = a7_limit_target(q, limits, direction, float(margin_deg))
+        if target is None:
+            self.get_logger().error(f"{tag}: 算不出 A7 限位目标")
+            return False
+        return self._send_a7_only(float(target[6]), speed, tag)
+
+    def _run_a7_seat(self) -> bool:
+        """贴面后咬螺纹。只转 A7，不改法兰位姿。
+
+        前两步是首轮：逆时针到限位，再顺时针到限位。顺时针到位即首轮咬合结束。
+        第三步松爪、逆时针回限位、合爪、再顺时针到限位。
+        """
+        limits = self._a7_limits()
+        if limits is None:
+            return False
+        ga = load_site_dict().get("gauge_assembly") or {}
+        try:
+            margin = float(ga["extreme_margin_deg"])
+            speed = float(ga["extreme_speed_deg_s"])
+            ccw = int(ga["extreme_direction"])
+        except (KeyError, TypeError, ValueError) as exc:
+            self.get_logger().error(
+                f"site.yaml gauge_assembly 缺 A7 旋转参数（{exc}），拒绝旋转"
+            )
+            return False
+        if ccw not in (-1, 1):
+            self.get_logger().error(
+                f"extreme_direction 只能是 +1/-1，收到 {ccw}，拒绝旋转"
+            )
+            return False
+        # extreme_direction 是逆时针的符号。第一步出孔，第二步顺时针进孔。
+        first = (
+            (ccw, "A7 逆时针至极限（出孔）"),
+            (-ccw, "A7 顺时针至极限（进孔）"),
+        )
+        for direction, tag in first:
+            if not self._a7_to_limit(limits, direction, margin, speed, tag):
+                return False
+        self._state("首轮咬合结束")
+        self.get_logger().info("首轮咬合结束：A7 已顺时针转到限位。")
+        # 第三步：只张开到 400（不全开），逆时针空回，再夹住顺时针拧。
+        if not self._gripper_cli("pos", "400"):
+            return False
+        if not self._a7_to_limit(
+            limits, ccw, margin, speed, "A7 逆时针至极限（松爪回转）"
+        ):
+            return False
+        if not self._gripper_cli("close"):
+            return False
+        if not self._a7_to_limit(
+            limits, -ccw, margin, speed, "A7 顺时针至极限（夹紧再进）"
+        ):
+            return False
+        self.get_logger().info(
+            "A7 第三步完成：张开到 400、逆时针回限位、合爪、顺时针到限位。只转了 A7。"
+        )
+        return True
+
     def _on_lock(self, msg: Bool) -> None:
         if bool(msg.data):
             self._locked = True
@@ -406,7 +657,8 @@ class AssemblyPipeline(Stage3Search):
                         f"二阶段 OK 残差 {final.get('residual_mm', -1)}mm — "
                         f"插孔停在法兰 Z={final.get('flange_z_m')}m，"
                         f"规尖离面 {final.get('tip_gap_mm')}mm"
-                        f"（目标 {final.get('commanded_gap_mm')}mm，来自 final_gap_mm）。"
+                        f"（目标 {final.get('commanded_gap_mm')}mm，"
+                        f"来自 {final.get('insert_z_source')}）。"
                         "没有接触传感器，不另设 Z，不抬回悬高。"
                     )
                 else:
@@ -434,7 +686,12 @@ class AssemblyPipeline(Stage3Search):
                         self._pin(q)
                     self.get_logger().info(
                         "插孔已停在贴面高度，不进搜孔、不抬回悬高。"
-                        "15s 后本进程退出，臂保持这个 Z。"
+                        "接着只转 A7：先逆时针到限位，再顺时针到限位（首轮咬合）。"
+                        "然后平行爪只张开到 400、逆时针回限位、合爪、再顺时针到限位。"
+                    )
+                    if not self._run_a7_seat():
+                        self.get_logger().error(
+                            "A7 旋转没走完，臂停在当前关节。不抬 Z。"
                         )
                 else:
                     self.get_logger().info(

src/arm_application/arm_application/stage2_locate_node.py

@@ -63,6 +63,8 @@ from arm_application.overhead_oneshot import (
     decompose_base_xy,
     flange_z_for_gap_m,
     final_gap_refusal,
+    hole_entry_refusal,
+    insert_target_gap_m,
     hole_pair_span_m,
     hole_pair_span_ok,
     once_ff_movel_from_tcp,
@@ -260,6 +262,8 @@ class Stage2Locate(TolerantNode):
             self.final_gap_m = _gap_override_mm / 1000.0
         else:
             self.final_gap_m = float(stage2.get("final_gap_mm") or 1.5) / 1000.0
+        # 缺键 = 不进孔（停在面上）。进孔必须在 site 里写明，不许默认钻进去。
+        self.hole_entry_m = float(stage2.get("hole_entry_mm") or 0.0) / 1000.0
         # 观测下降：对准后降到这个间隙就停，方便看规尖是不是对着孔。
         # 比 final_gap（贴面 2mm）高一截，对偏了也不会把规压上孔沿。
         self.observe_gap_m = float(stage2.get("observe_gap_mm") or 15.0) / 1000.0
@@ -325,7 +329,8 @@ class Stage2Locate(TolerantNode):
                     "（这次没写 --hover-z，沿用 site 的几何抬高）"
                 )
                 self.hover_z = _used
-        # 轻降**最多能降多少**：从悬停高度降到"规尖停在工件面上方 final_gap"为止。
+        self.insert_gap_m = insert_target_gap_m(self.final_gap_m, self.hole_entry_m)
+        # 轻降**最多能降多少**：从悬停高度降到插孔目标（面上 final_gap，或面下 hole_entry）。
         # 这是纯几何量，与深度测量无关 —— 测量的唯一职责是让下降**提前**收工，
         # 绝不允许它把下降量**放大**（2026-09-29 实测就是这么撞的：测量出的
         # gap0=141mm 比几何真值大 ~86mm，于是多降 86mm 把规撞进孔）。
@@ -333,7 +338,7 @@ class Stage2Locate(TolerantNode):
         self.descent_budget_m: Optional[float] = None
         if _tool_len > 0.0 and _surface_z > 0.0:
             self.descent_budget_m = max(
-                0.0, self.hover_z - _tool_len - _surface_z - self.final_gap_m
+                0.0, self.hover_z - _tool_len - _surface_z - self.insert_gap_m
             )
         # 轻降结束后**按几何**算出的真实间隙 [m]，供收尾日志如实汇报（不是把目标值
         # 当结果念一遍 —— 那会让"停在离面 50mm"也显示成"~2.0mm"，R1.1 的静默失效）。
@@ -369,6 +374,17 @@ class Stage2Locate(TolerantNode):
             if _why_gap:
                 self._final_gap_refused = _why_gap
                 self.get_logger().error(_why_gap)
+            else:
+                _why_entry = hole_entry_refusal(
+                    self.hole_entry_m,
+                    _tool_len,
+                    _surface_z,
+                    float(self.ws_min[2]),
+                    float(self.ws_max[2]),
+                )
+                if _why_entry:
+                    self._final_gap_refused = _why_entry
+                    self.get_logger().error(_why_entry)
         self.vert_tol_rad = math.radians(float(stage2.get("verticalize_tol_deg") or 0.8))
         self.settle_sec = float(stage2.get("settle_sec") or 0.45)
         self.snap_frames = int(stage2.get("snap_frames") or 5)
@@ -490,14 +506,17 @@ class Stage2Locate(TolerantNode):
             f"hole_keypoint_once={self.hole_keypoint_once}  "
             f"lock_xy_descend={self.hole_once_lock_xy_descend}  "
             f"ff_look={self.once_ff_look_per_m:.3f}  "
-            f"final_gap={self.final_gap_m * 1000:.1f}mm"
+            f"final_gap={self.final_gap_m * 1000:.1f}mm  "
+            f"hole_entry={self.hole_entry_m * 1000:.1f}mm  "
+            f"插孔间隙={self.insert_gap_m * 1000:.1f}mm"
             + (
-                f"→法兰Z={flange_z_for_gap_m(self.tool_tip_below_flange_m, self.surface_z_m, self.final_gap_m):.4f}m"
+                f"→法兰Z={flange_z_for_gap_m(self.tool_tip_below_flange_m, self.surface_z_m, self.insert_gap_m):.4f}m"
                 if self.tool_tip_below_flange_m > 0.0 and self.surface_z_m > 0.0
                 else ""
             )
             + "\n"
-            f"  工具长={self.tool_tip_below_flange_m:.4f}m（法兰到尖，下臂不得低于面+间隙）"
+            f"  工具长={self.tool_tip_below_flange_m:.4f}m（法兰到尖；"
+            "hole_entry>0 时规尖停在上表面以下）"
         )
         if self.hole_keypoint_once and self.hole_once_lock_xy_descend:
             k = float(self.once_ff_look_per_m)
@@ -3363,7 +3382,7 @@ class Stage2Locate(TolerantNode):
         if self.hole_keypoint_once and self.hole_once_lock_xy_descend:
             # 视线前馈斜线：不经过观察高、不补交。一条 MoveL 收到贴面。
             lock_gap = (
-                self.final_gap_m if self.final_descent_enable else self.observe_gap_m
+                self.insert_gap_m if self.final_descent_enable else self.observe_gap_m
             )
             okd, q, Tee = self._once_ff_diagonal_to_gap(
                 q, Tee, target_gap_m=lock_gap
@@ -3406,8 +3425,9 @@ class Stage2Locate(TolerantNode):
         )
 
         # ---- 5) 锁定保持 ----
-        # 贴面之后就停在这个法兰 Z。没有接触传感器，到位只认 final_gap_mm
-        # （规尖离面）。三阶段开局会抬回悬高，这里标明已经插到位，流水线不再上抬。
+        # 插孔之后就停在这个法兰 Z。没有接触传感器。
+        # hole_entry_mm>0：规尖在上表面以下；否则停在 final_gap（面上）。
+        # 三阶段开局会抬回悬高，这里标明已经插到位，流水线不再上抬。
         if (
             self.descent_final_gap_m is not None
             and self.final_descent_enable
@@ -3417,8 +3437,11 @@ class Stage2Locate(TolerantNode):
             final["insert_held"] = True
             final["flange_z_m"] = round(float(Tee[2, 3]), 4)
             final["tip_gap_mm"] = round(float(self.descent_final_gap_m) * 1000.0, 2)
-            final["commanded_gap_mm"] = round(float(self.final_gap_m) * 1000.0, 2)
-            final["insert_z_source"] = "final_gap_mm"
+            final["commanded_gap_mm"] = round(float(self.insert_gap_m) * 1000.0, 2)
+            final["hole_entry_mm"] = round(float(self.hole_entry_m) * 1000.0, 2)
+            final["insert_z_source"] = (
+                "hole_entry_mm" if self.hole_entry_m > 1e-9 else "final_gap_mm"
+            )
         self._state("hold")
         q_fin = self._q()
         self._pin(q_fin if q_fin is not None else q)
@@ -3426,7 +3449,8 @@ class Stage2Locate(TolerantNode):
             self.get_logger().info(
                 f"=== 二阶段定位完成: {self.target_hole} 残差 {final.get('residual_mm', -1)}mm，"
                 f"法兰 Z={final.get('flange_z_m')}m，规尖离面 {gap_txt}mm"
-                f"（final_gap {self.final_gap_m * 1000:.1f}mm，无接触传感器）。"
+                f"（目标 {self.insert_gap_m * 1000:.1f}mm，"
+                f"hole_entry {self.hole_entry_m * 1000:.1f}mm，无接触传感器）。"
                 "停在这一高度，不抬回悬高。"
                 f"结果: /stage2/result, /tmp/stage2_result.json ==="
             )

src/arm_application/test/test_gauge_kin.py

@@ -14,6 +14,7 @@ import pytest
 
 from arm_application.gauge_kin import (
     DEFAULT_LIMIT_MARGIN_DEG,
+    a7_limit_target,
     axis_misalignment_deg,
     match_limits,
     parse_joint_limits,
@@ -200,6 +201,21 @@ def test_rotation_helpers_reject_zero_axis():
         rotation_about_axis_deg(np.eye(3), _rot_z(10.0), [0.0, 0.0, 0.0])
 
 
+def test_a7_limit_moves_only_the_last_joint():
+    """逆时针到上限、顺时针到下限。前六轴必须原样，没有限位就拒绝。"""
+    q = np.array([0.1, -0.2, 0.3, -0.4, 0.5, -0.6, 0.2])
+    limits = (-3.0543, 3.0543)
+    ccw = a7_limit_target(q, limits, direction=1, margin_deg=3.0)
+    cw = a7_limit_target(q, limits, direction=-1, margin_deg=3.0)
+    assert ccw is not None and cw is not None
+    np.testing.assert_allclose(ccw[:6], q[:6])
+    np.testing.assert_allclose(cw[:6], q[:6])
+    assert ccw[6] == pytest.approx(3.0543 - math.radians(3.0))
+    assert cw[6] == pytest.approx(-3.0543 + math.radians(3.0))
+    assert a7_limit_target(q, None, 1) is None
+    assert a7_limit_target(q, limits, 1, margin_deg=200.0) is None
+
+
 def test_tool_z_and_vertical_down_error():
     """工具 Z 朝世界 -Z = 0°；水平 = 90°；朝上 = 180°。"""
     R_down = np.diag([1.0, -1.0, -1.0])

src/arm_application/test/test_overhead_oneshot.py

@@ -61,7 +61,10 @@ from arm_application.overhead_oneshot import (
     hole_pair_span_ok,
     oneshot_align_step_m,
     flange_z_for_gap_m,
+    HOLE_ENTRY_MAX_M,
     final_gap_refusal,
+    hole_entry_refusal,
+    insert_target_gap_m,
     d435_movel_from_tcp,
     once_ff_movel_from_tcp,
     once_ff_tcp_xyz,
@@ -929,6 +932,23 @@ class TestHoleBiasAxes:
         assert flange_8 == pytest.approx(0.2545)
         assert flange_8 < hover
 
+    def test_hole_entry_goes_below_the_face_and_refuses_through_the_plate(self):
+        """孔口凹槽：进孔深度把规尖放到上表面以下；超过 8mm 或出工作盒必须红。"""
+        tool, surf = 0.226587, 0.022
+        z_lo, z_hi = 0.18, 0.65
+        assert insert_target_gap_m(0.002, 0.0) == pytest.approx(0.002)
+        assert insert_target_gap_m(0.002, 0.004) == pytest.approx(-0.004)
+        assert hole_entry_refusal(0.0, tool, surf, z_lo, z_hi) == ""
+        assert hole_entry_refusal(0.004, tool, surf, z_lo, z_hi) == ""
+        flange = flange_z_for_gap_m(tool, surf, -0.004)
+        assert flange == pytest.approx(tool + surf - 0.004)
+        assert flange > z_lo
+        why_deep = hole_entry_refusal(HOLE_ENTRY_MAX_M + 0.004, tool, surf, z_lo, z_hi)
+        assert "穿出" in why_deep
+        assert "未知" in hole_entry_refusal(0.004, 0.0, surf, z_lo, z_hi)
+        why_box = hole_entry_refusal(0.004, tool, surf, 0.40, z_hi)
+        assert "工作盒" in why_box
+
     def test_final_gap_cli_overrides_site_and_conflicts_with_no_descent(self):
         """--final-gap 必须接到 launch/节点；和 --no-descent 一起写必须在脚本里红。"""
         from pathlib import Path
@@ -1060,6 +1080,11 @@ class TestRetractHold:
         ).read_text(encoding="utf-8")
         assert "stage1_log_fatal(" in pipe
         assert "finish_vendor_movel" in cpp
+        running = cpp[cpp.find("if (vendor_movel_phase_ == VendorMovelPhase::kRunning)") :]
+        running = running[: running.find("reachedTarget")]
+        assert "robot_->fault()" in running
+        assert "目标到不了" in running
+        assert running.find("robot_->fault()") < running.find("> vendor_movel_deadline_")
         fin = cpp[cpp.find("void FlexivHardwareInterface::finish_vendor_movel") :]
         fin = fin[: fin.find("bool FlexivHardwareInterface::poll_vendor_movel")]
         assert "hold_current_joint_commands()" in fin

src/arm_application/test/test_pipeline_stage1_diag.py

@@ -36,13 +36,34 @@ def test_insert_hold_does_not_return_to_hover():
     ).read_text(encoding="utf-8")
     hold = src[src.find("# ---- 5) 锁定保持 ----") : src.find("def _linger_hold")]
     assert 'final["insert_held"] = True' in hold
-    assert 'final["insert_z_source"] = "final_gap_mm"' in hold
+    assert 'final["insert_z_source"]' in hold
+    assert "hole_entry_mm" in hold
+    assert "self.insert_gap_m" in hold
     pipe = (
         Path(__file__).resolve().parents[1] / "arm_application" / "pipeline_node.py"
     ).read_text(encoding="utf-8")
     run = pipe[pipe.find("def run") :]
     assert "hold_at_insert_z(" in run
     assert "不抬回悬高" in run
+    assert "_run_a7_seat(" in run
+    seat = pipe[pipe.find("def _run_a7_seat") : pipe.find("def _on_lock")]
+    marks = (
+        "首轮咬合结束",
+        '_gripper_cli("pos", "400")',
+        "松爪回转",
+        '_gripper_cli("close")',
+        "夹紧再进",
+    )
+    pos = [seat.find(m) for m in marks]
+    assert all(i >= 0 for i in pos)
+    assert pos == sorted(pos)
+    grip = pipe[pipe.find("def _gripper_cli") : pipe.find("def _wait_gripper")]
+    assert "run_gripper.sh" in grip
+    assert '"--cli"' in grip
+    assert "_send_a7_only" in pipe[pipe.find("def _a7_to_limit") : pipe.find("def _run_a7_seat")]
+    # /robot_description 是锁存话题。VOLATILE 订阅会错过已经发出的 URDF。
+    sub = pipe[pipe.find('"/robot_description"') : pipe.find('"/robot_description"') + 400]
+    assert "TRANSIENT_LOCAL" in sub
 
 
 def test_zero_points_blames_perception_not_calibration(tmp_path: Path):

src/flexiv_ros2/flexiv_hardware/src/flexiv_hardware_interface.cpp

@@ -866,6 +866,23 @@ bool FlexivHardwareInterface::poll_vendor_movel()
     }
     if (vendor_movel_phase_ == VendorMovelPhase::kRunning) {
         try {
+            // 目标在孔底外面时厂家约 5s 报 302064，原语不会报到位。
+            // 不在这里收，流水线会干等到 90s deadline。
+            if (robot_->fault()) {
+                RCLCPP_ERROR(getLogger(),
+                    "厂家 MoveL 机器人报故障，停止等待原语到位");
+                try {
+                    robot_->ClearFault(3);
+                } catch (const std::exception&) {
+                }
+                try {
+                    robot_->SwitchMode(rdk_control_mode_);
+                } catch (const std::exception&) {
+                }
+                finish_vendor_movel(
+                    vendor_movel_seq_, false, "MoveL 中机器人报故障，目标到不了");
+                return true;
+            }
             if (std::chrono::steady_clock::now() > vendor_movel_deadline_) {
                 try {
                     robot_->SwitchMode(rdk_control_mode_);