空回微抬改成一条厂家 MoveL，并按实测抬高降回。默认 Z50 会在还差约 0.5mm 时就报到位，微升降改用 ZFine 和到位等级 1。合到 0 时固件常报 grip=1，手指已在合拢端就继续顺时针，避免最后一转永远不发。快转样条按峰速不超过 0.90 rad/s 排程，避免时间到了还差几十度。

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

config/site.example.yaml

@@ -937,7 +937,13 @@ gauge_assembly:
   # 预绕是为了给 S08 的棘轮留出回转行程：不预绕，拧两圈就贴关节限位。
   extreme_angle_deg: 150.0   # 想转多少度（按工具系逆时针）
   extreme_speed_deg_s: 10.0  # 关节角速度 [deg/s]。2026-10-19 左臂 A7 咬合从 20 降到 10，
-                             # 限位到限位约 34s（原先约 17s）
+                             # 限位到限位约 34s（原先约 17s）。顺时针进孔用这个速度。
+  unwind_open_permille: 400  # 第三步松爪只开到这个千分比。0=合，1000=全开。
+                             # 2026-10-19 从 200 改成 400，只张开这个量。
+  unwind_lift_mm: 3.0       # 第三步张开后只抬的 TCP Z [mm]（0.3cm）。
+                             # 2026-10-19 指令 5mm、ZFine 后实走 +4.86mm，下降 -4.70mm，差 0.16mm。
+                             # 空回到限位后，下降量用这次实测的抬高，不另写一个数。
+                             # 超过 5mm 拒绝。
   extreme_direction: -1      # +1=逆时针 / -1=顺时针（2026-10-19 真机 A7 转反了，关节角增大是顺时针）
   extreme_margin_deg: 3.0    # 距关节限位留的余量 [deg]；撞限位不算失败（按 JOINT_LIMIT 继续）
                              # 但余量太小会让规划器频繁拒收，表现为"偶尔转不到位"

config/site.yaml

@@ -921,7 +921,13 @@ gauge_assembly:
   # 预绕是为了给 S08 的棘轮留出回转行程：不预绕，拧两圈就贴关节限位。
   extreme_angle_deg: 150.0   # 想转多少度（按工具系逆时针）
   extreme_speed_deg_s: 10.0  # 关节角速度 [deg/s]。2026-10-19 左臂 A7 咬合从 20 降到 10，
-                             # 限位到限位约 34s（原先约 17s）
+                             # 限位到限位约 34s（原先约 17s）。顺时针进孔用这个速度。
+  unwind_open_permille: 400  # 第三步松爪只开到这个千分比。0=合，1000=全开。
+                             # 2026-10-19 从 200 改成 400，只张开这个量。
+  unwind_lift_mm: 3.0       # 第三步张开后只抬的 TCP Z [mm]（0.3cm）。
+                             # 2026-10-19 指令 5mm、ZFine 后实走 +4.86mm，下降 -4.70mm，差 0.16mm。
+                             # 空回到限位后，下降量用这次实测的抬高，不另写一个数。
+                             # 超过 5mm 拒绝。
   extreme_direction: -1      # +1=逆时针 / -1=顺时针（2026-10-19 真机 A7 转反了，关节角增大是顺时针）
   extreme_margin_deg: 3.0    # 距关节限位留的余量 [deg]；撞限位不算失败（按 JOINT_LIMIT 继续）
                              # 但余量太小会让规划器频繁拒收，表现为"偶尔转不到位"

docs/HANDBOOK.md

@@ -93,7 +93,7 @@ ROS 相对 XY，`keep_z`。硬件在 MoveL 期间不灌关节指令。D435 这
 `tool10120938` 的 TCP 长度（0.226587 m，2026-10-16 下午 RDK 重读）加上 `final_gap_mm` 之上，不按旧 tool1 的 0.223 m。
 贴面之后就停在这个法兰高度，不抬回 `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。顺时针是进孔，逆时针是出孔。夹爪命令失败或状态没到，就停在当前关节。
+然后只转左臂 A7（厂家 `SendJointPosition` 的第 7 轴，模式 `NRT_JOINT_POSITION`）：先逆时针转到软件限位内侧 `extreme_margin_deg`，再顺时针转到另一侧。顺时针到位记为「首轮咬合结束」。接着 `run_gripper.sh --arm <臂号> --cli pos` 加上 `unwind_open_permille`（现为 400，不全开），再用一条厂家 `MoveL` 只抬 TCP 的 Z `unwind_lift_mm`（现为 3，即 0.3 cm；XY 和姿态不动，不拆成关节小跳）。这条和随后的下降都带 `zoneRadius=ZFine`、`targetTolerLevel=1`：厂家默认 `Z50` 和等级 3 会在还差约 0.5 mm 时就报到位，下降和记住的上抬对不上，合爪就不会发。A7 逆时针回到限位。请求速度是接口上限 0.90 rad/s；两端速度为 0 的样条峰速约是平均的 1.5 倍，所以整段按平均约 34 °/s 排程，避免时间到了还差几十度。然后下降刚才实测的那一段（不另写一个下降量），`--cli close`。合爪目标是位置 0；固件在位置先到时写 grip=1（到位），力先到才写 grip=2（夹住）。手指已经在合拢端就继续，还停在张开位则不转。再按 `extreme_speed_deg_s` 顺时针转到限位。前六轴不动。顺时针是进孔，逆时针是出孔。夹爪或这一段 Z 对不上，就停在当前关节。
 只测 D435 这一段位移时加 `--d435-only`：不交 D405、不进二阶段，并且要求先停 `run_real.sh`。
 
 ```bash

scripts/lib/flexiv_movel_ipc.py

@@ -18,15 +18,34 @@ 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 (
+def format_request(
+    seq: int, x: float, y: float, z: float, keep_z: bool, vel: float,
+    zone: str = "", toler: int = -1,
+) -> str:
+    """一行请求。不带 ``zone`` 时仍是 6 个数，插孔和粗定位保持厂家默认到位。
+
+    微升降要带过渡区和到位等级。厂家默认 ``Z50``（50 mm）加等级 3，
+    几毫米的直线会在还差约 0.5 mm 时就报到位。合法的最细过渡区是 ``ZFine``，
+    最严且仍会检查到位的等级是 1（0 表示不检查）。
+    """
+    if bool(zone) != (int(toler) >= 0):
+        raise ValueError("MoveL 过渡区和到位等级必须一起给")
+    if zone and (zone != "ZFine" or int(toler) != 1):
+        raise ValueError(f"MoveL 微升降只接受 ZFine 和到位等级 1，收到 {zone} {toler}")
+    line = (
         f"{int(seq)} {float(x):.6f} {float(y):.6f} {float(z):.6f} "
-        f"{1 if keep_z else 0} {float(vel):.4f}\n"
+        f"{1 if keep_z else 0} {float(vel):.4f}"
     )
+    if zone:
+        line += f" {zone} {int(toler)}"
+    return line + "\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)
+def write_request(
+    seq: int, x: float, y: float, z: float, keep_z: bool, vel: float,
+    zone: str = "", toler: int = -1,
+) -> None:
+    text = format_request(seq, x, y, z, keep_z, vel, zone, toler)
     tmp = REQ.with_name(REQ.name + ".tmp")
     tmp.write_text(text, encoding="utf-8")
     tmp.replace(REQ)

src/arm_application/arm_application/overhead_oneshot.py

@@ -641,6 +641,24 @@ def d435_movel_from_tcp(tcp_xyz, flange_xy, k0_xy):
     return out
 
 
+def movel_tcp_z_only(tcp_xyz, delta_z_m: float):
+    """厂家 MoveL 目标：当前 TCP 的 XY 原样，只加 ``delta_z_m``。
+
+    一段直线。WORLD 绝对坐标不能另算，只能在厂家 TCP 上加这一段。
+    ``delta_z_m`` 为正是上升，为负是下降。接近 0 或非有限数返回 None。
+    """
+    try:
+        t = np.asarray(tcp_xyz, dtype=float).reshape(3)
+        dz = float(delta_z_m)
+    except (TypeError, ValueError):
+        return None
+    if t.size != 3 or not np.all(np.isfinite(t)) or not np.isfinite(dz):
+        return None
+    if abs(dz) < 1e-6:
+        return None
+    return np.array([float(t[0]), float(t[1]), float(t[2]) + dz], dtype=float)
+
+
 def once_ff_movel_from_tcp(tcp_xyz, dxy, descent_m: float):
     """厂家 MoveL 目标 = 当前 Studio TCP + (ΔXY, −下降) [m]。
 

src/arm_application/arm_application/pipeline_node.py

@@ -31,7 +31,7 @@ from arm_application.gauge_kin import (
     match_limits,
     parse_joint_limits,
 )
-from arm_application.overhead_oneshot import D435_MOVEL_GAVE_UP
+from arm_application.overhead_oneshot import D435_MOVEL_GAVE_UP, movel_tcp_z_only
 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
@@ -49,6 +49,44 @@ from arm_application.stage3_search_node import Stage3Search
 TRACK_CLEARANCE_M = 0.05
 #: 悬停时规尖离面已经不少于这个值 [m]，一阶段就锁在悬停高，对孔完成后再下降。
 TRACK_LOCK_GAP_M = 0.040
+#: ``fill_nrt_limits`` 的关节速度上限 [rad/s]。再快的指令会被钳住，时间到了关节还没到。
+NRT_VMAX_RAD_S = 0.90
+#: 两点、两端速度为 0 的三次样条，峰速是平均速度的 1.5 倍。
+CUBIC_PEAK_OVER_AVG = 1.5
+#: 空回按这个角速度请求。真正排程用 :func:`a7_move_duration_s`，峰速不超过上面的上限。
+A7_MAX_SPEED_DEG_S = math.degrees(NRT_VMAX_RAD_S)
+
+
+def a7_move_duration_s(delta_rad: float, speed_deg_s: float) -> float:
+    """A7 这一段该排多长 [s]。
+
+    平均速度按请求来。峰速若会超过 ``NRT_VMAX_RAD_S``，就把平均速度降到上限的
+    1/1.5，让样条峰速刚好顶到上限。2026-10-19 空回按 52°/s 排 6.6s，峰速被钳住，
+    到位时还差 35°。
+    """
+    dist = abs(float(delta_rad))
+    v_req = math.radians(max(float(speed_deg_s), 1.0))
+    v_avg = min(v_req, NRT_VMAX_RAD_S / CUBIC_PEAK_OVER_AVG)
+    if v_avg <= 1e-6:
+        return 0.8
+    return max(0.8, dist / v_avg)
+
+
+# 合爪位置命令是 0。和 ``pos`` 的到位带同一宽度。
+CLOSE_POS_TOL = 40.0
+
+
+def close_ready(grip: int, pos: float, tol: float = CLOSE_POS_TOL) -> bool:
+    """合爪之后能不能接着顺时针转 A7。
+
+    大寰 ``0x0201``：力先到是 2（夹住），位置先到命令值是 1（到位）。
+    这几次合到 0 时规还在两指间，位置先到，固件写 1，只认 2 就永远不转。
+    手指已经在合拢端（``pos`` 不超过 ``tol``）就继续。还停在张开位则不转。
+    """
+    g = int(grip)
+    if g == 2:
+        return True
+    return g == 1 and float(pos) <= float(tol)
 
 
 def init_z_refusal(
@@ -259,8 +297,9 @@ class AssemblyPipeline(Stage3Search):
         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))
+        delta = float(q_hold[6] - q[6])
+        dur = a7_move_duration_s(delta, speed_deg_s)
+        avg_deg_s = abs(math.degrees(delta)) / dur if dur > 1e-6 else 0.0
         zeros = np.zeros(q.size)
         traj = JointTrajectory()
         traj.header.stamp.sec = 0
@@ -270,7 +309,8 @@ class AssemblyPipeline(Stage3Search):
         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"
+            f"请求 {speed_deg_s:.0f}°/s，样条平均 {avg_deg_s:.0f}°/s、{dur:.1f}s"
+            f"（峰速不超过 {NRT_VMAX_RAD_S:.2f} rad/s），前六轴不动，法兰 Z={z0:.4f}m"
         )
         self._send_action(traj, wait_s=dur + 8.0)
         q2, Tee2 = self._wait_state(5.0)
@@ -298,6 +338,53 @@ class AssemblyPipeline(Stage3Search):
         self._pin(q2)
         return True
 
+    def _shift_flange_z(self, delta_m: float, tag: str) -> Optional[float]:
+        """厂家 MoveL 一段，只改 TCP 的 Z。返回实测 ΔZ [m]；失败返回 None。
+
+        不走关节小跳。姿态用当前 TCP 欧拉角，XY 用当前厂家 TCP，不加别的位移。
+        """
+        self._spin(0.05)
+        tcp0 = self._fresh_tcp_world()
+        q, Tee = self._wait_state(2.0)
+        if tcp0 is None or q is None or Tee is None:
+            self.get_logger().error(f"{tag}: 没有厂家 TCP，拒绝用关节小跳凑 Z")
+            return None
+        target = movel_tcp_z_only(tcp0, delta_m)
+        if target is None:
+            self.get_logger().error(f"{tag}: MoveL 的 Z 目标算不出")
+            return None
+        fx0, fy0, fz0 = float(Tee[0, 3]), float(Tee[1, 3]), float(Tee[2, 3])
+        self.get_logger().info(
+            f"{tag}: 厂家 MoveL 一段 TCP "
+            f"({float(tcp0[0]):.4f},{float(tcp0[1]):.4f},{float(tcp0[2]):.4f})"
+            f" → ({target[0]:.4f},{target[1]:.4f},{target[2]:.4f}) "
+            f"只改 Z {float(delta_m) * 1000:+.2f}mm，vel=10mm/s，"
+            f"过渡区 ZFine，到位等级 1"
+        )
+        # 厂家默认 Z50（50mm）和等级 3 会在还差约 0.5mm 时就报完成。
+        ok = self._vendor_movel_tcp(target, 0.010, zone="ZFine", toler=1)
+        q2, Tee2 = self._wait_state(0.5)
+        self._spin(0.05)
+        tcp1 = self._fresh_tcp_world()
+        if not ok or q2 is None or Tee2 is None or tcp1 is None:
+            self.get_logger().error(f"{tag}: 厂家 MoveL 没完成，中止")
+            return None
+        tcp_xy = math.hypot(float(tcp1[0]) - float(tcp0[0]), float(tcp1[1]) - float(tcp0[1]))
+        dz = float(tcp1[2]) - float(tcp0[2])
+        fl_xy = math.hypot(float(Tee2[0, 3]) - fx0, float(Tee2[1, 3]) - fy0)
+        fl_dz = float(Tee2[2, 3]) - fz0
+        self.get_logger().info(
+            f"{tag}: 厂家TCP ΔXY={tcp_xy * 1000:.2f}mm ΔZ={dz * 1000:+.2f}mm；"
+            f"法兰 ΔXY={fl_xy * 1000:.2f}mm ΔZ={fl_dz * 1000:+.2f}mm"
+        )
+        if tcp_xy > 0.001 or fl_xy > 0.001:
+            self.get_logger().error(
+                f"{tag}: XY 动了 TCP {tcp_xy * 1000:.2f}mm / 法兰 {fl_xy * 1000:.2f}mm，中止"
+            )
+            return None
+        self._pin(q2)
+        return dz
+
     def _arm_short_id(self) -> str:
         """当前臂号。与 ``export_site_env.arm_short_id`` 一样，取 SN 末两位。"""
         short = os.environ.get("SITE_ROBOT_SHORT_ID", "").strip()
@@ -359,7 +446,7 @@ class AssemblyPipeline(Stage3Search):
         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）。"""
+        """``/gripper/states``：``pos`` 要停在目标附近；合爪见 :func:`close_ready`。"""
         box: dict = {"data": None}
 
         def _on_state(msg: Float32MultiArray) -> None:
@@ -382,8 +469,13 @@ class AssemblyPipeline(Stage3Search):
                         f"夹爪已到 pos={pos:.0f}（目标 {value}，不全开）"
                     )
                     return True
-                if action == "close" and grip == 2:
+                if action == "close" and close_ready(grip, pos):
+                    if grip == 2:
                         self.get_logger().info(f"夹爪已夹住 pos={pos:.0f}")
+                    else:
+                        self.get_logger().info(
+                            f"夹爪已合拢 pos={pos:.0f}（固件报到位）。继续顺时针。"
+                        )
                     return True
         finally:
             self.destroy_subscription(sub)
@@ -421,6 +513,8 @@ class AssemblyPipeline(Stage3Search):
             margin = float(ga["extreme_margin_deg"])
             speed = float(ga["extreme_speed_deg_s"])
             ccw = int(ga["extreme_direction"])
+            lift_mm = float(ga["unwind_lift_mm"])
+            open_permille = int(ga["unwind_open_permille"])
         except (KeyError, TypeError, ValueError) as exc:
             self.get_logger().error(
                 f"site.yaml gauge_assembly 缺 A7 旋转参数（{exc}），拒绝旋转"
@@ -431,6 +525,16 @@ class AssemblyPipeline(Stage3Search):
                 f"extreme_direction 只能是 +1/-1，收到 {ccw}，拒绝旋转"
             )
             return False
+        if not (0.0 < lift_mm <= 5.0):
+            self.get_logger().error(
+                f"unwind_lift_mm 只接受 0–5mm 的微抬，收到 {lift_mm}，拒绝旋转"
+            )
+            return False
+        if not (0 < open_permille <= 1000):
+            self.get_logger().error(
+                f"unwind_open_permille 只接受 1–1000，收到 {open_permille}，拒绝旋转"
+            )
+            return False
         # extreme_direction 是逆时针的符号。第一步出孔，第二步顺时针进孔。
         first = (
             (ccw, "A7 逆时针至极限（出孔）"),
@@ -441,13 +545,32 @@ class AssemblyPipeline(Stage3Search):
                 return False
         self._state("首轮咬合结束")
         self.get_logger().info("首轮咬合结束：A7 已顺时针转到限位。")
-        # 第三步：只张开到 400（不全开），逆时针空回，再夹住顺时针拧。
-        if not self._gripper_cli("pos", "400"):
+        # 第三步：只张开到 unwind_open_permille，微抬 Z，最快逆时针空回，再降回同一段 Z，合爪，慢速顺时针进。
+        if not self._gripper_cli("pos", str(open_permille)):
             return False
+        dz_up = self._shift_flange_z(lift_mm / 1000.0, "空回前微抬")
+        if dz_up is None:
+            return False
+        if dz_up < (lift_mm / 1000.0) * 0.6:
+            self.get_logger().error(
+                f"空回前微抬只做到 {dz_up * 1000:.2f}mm，不到指令 {lift_mm:.1f}mm 的六成，不空转"
+            )
+            return False
+        self.get_logger().info(
+            f"记住上抬 {dz_up * 1000:.2f}mm。空回后按这一段下降，XY 不动。"
+        )
         if not self._a7_to_limit(
-            limits, ccw, margin, speed, "A7 逆时针至极限（松爪回转）"
+            limits, ccw, margin, A7_MAX_SPEED_DEG_S, "A7 逆时针至极限（松爪回转）"
         ):
             return False
+        dz_down = self._shift_flange_z(-dz_up, "空回后降回")
+        if dz_down is None:
+            return False
+        if abs(dz_down + dz_up) > 0.0005:
+            self.get_logger().error(
+                f"下降 {dz_down * 1000:.2f}mm 和记住的上抬 {dz_up * 1000:.2f}mm 不一致，不合爪"
+            )
+            return False
         if not self._gripper_cli("close"):
             return False
         if not self._a7_to_limit(
@@ -455,7 +578,7 @@ class AssemblyPipeline(Stage3Search):
         ):
             return False
         self.get_logger().info(
-            "A7 第三步完成：张开到 400、逆时针回限位、合爪、顺时针到限位。只转了 A7。"
+            f"A7 第三步完成：张开到 {open_permille}、微抬、最快逆时针回限位、降回同一段、合爪、顺时针到限位。"
         )
         return True
 
@@ -687,7 +810,8 @@ class AssemblyPipeline(Stage3Search):
                     self.get_logger().info(
                         "插孔已停在贴面高度，不进搜孔、不抬回悬高。"
                         "接着只转 A7：先逆时针到限位，再顺时针到限位（首轮咬合）。"
-                        "然后平行爪只张开到 400、逆时针回限位、合爪、再顺时针到限位。"
+                        "然后平行爪只张开到指定开度、微抬 Z、最快逆时针回限位、"
+                        "按记住的上抬量降回、合爪、再顺时针到限位。"
                     )
                     if not self._run_a7_seat():
                         self.get_logger().error(

src/arm_application/arm_application/stage2_locate_node.py

@@ -2139,8 +2139,13 @@ class Stage2Locate(TolerantNode):
             )
         return q, Tee, True
 
-    def _vendor_movel_tcp(self, xyz, vel: float) -> bool:
-        """进程内厂家 MoveL：TCP 目标，不杀控制栈。"""
+    def _vendor_movel_tcp(
+        self, xyz, vel: float, zone: str = "", toler: int = -1,
+    ) -> bool:
+        """进程内厂家 MoveL：TCP 目标，不杀控制栈。
+
+        ``zone`` 空着用厂家默认过渡区。微升降传 ``ZFine`` 和到位等级 1。
+        """
         lib = ros2_root() / "scripts" / "lib"
         if str(lib) not in sys.path:
             sys.path.insert(0, str(lib))
@@ -2148,7 +2153,8 @@ class Stage2Locate(TolerantNode):
 
         seq = int(time.monotonic() * 1000.0) % 1_000_000_000
         write_request(
-            seq, float(xyz[0]), float(xyz[1]), float(xyz[2]), False, float(vel)
+            seq, float(xyz[0]), float(xyz[1]), float(xyz[2]), False, float(vel),
+            zone, toler,
         )
         t0 = time.monotonic()
         acked = False

src/arm_application/test/test_overhead_oneshot.py

@@ -66,6 +66,7 @@ from arm_application.overhead_oneshot import (
     hole_entry_refusal,
     insert_target_gap_m,
     d435_movel_from_tcp,
+    movel_tcp_z_only,
     once_ff_movel_from_tcp,
     once_ff_tcp_xyz,
     once_look_ff_xy,
@@ -949,6 +950,18 @@ class TestHoleBiasAxes:
         why_box = hole_entry_refusal(0.004, tool, surf, 0.40, z_hi)
         assert "工作盒" in why_box
 
+    def test_movel_tcp_z_only_keeps_xy(self):
+        """微抬只加厂家 TCP 的 Z。XY 必须是原来那两个数。"""
+        up = movel_tcp_z_only([0.52, -0.37, 0.091], 0.002)
+        assert up is not None
+        assert up[0] == pytest.approx(0.52)
+        assert up[1] == pytest.approx(-0.37)
+        assert up[2] == pytest.approx(0.093)
+        down = movel_tcp_z_only(up, -0.002)
+        assert down is not None
+        assert down[2] == pytest.approx(0.091)
+        assert movel_tcp_z_only(up, 0.0) is None
+
     def test_final_gap_cli_overrides_site_and_conflicts_with_no_descent(self):
         """--final-gap 必须接到 launch/节点；和 --no-descent 一起写必须在脚本里红。"""
         from pathlib import Path

src/arm_application/test/test_pipeline_stage1_diag.py

@@ -11,12 +11,14 @@ ultralytics 早退了"被读成"算法不收敛"，白查半天。这里把这
 
 from __future__ import annotations
 
+import math
 from pathlib import Path
 
 import pytest
 
 from arm_application.overhead_oneshot import D435_MOVEL_GAVE_UP
 from arm_application.pipeline_node import (
+    close_ready,
     hold_at_insert_z,
     init_z_refusal,
     stage1_log_fatal,
@@ -25,6 +27,39 @@ from arm_application.pipeline_node import (
 )
 
 
+def test_close_at_zero_continues_even_when_firmware_says_arrived():
+    """2026-10-19：合到 0 时规还在，固件写 grip=1。只认 grip=2 就永远不顺时针。
+
+    停在张开位（pos=200）仍然不能往下走。
+    """
+    assert close_ready(2, 80) is True
+    assert close_ready(1, 0) is True
+    assert close_ready(1, 40) is True
+    assert close_ready(1, 41) is False
+    assert close_ready(1, 200) is False
+    assert close_ready(0, 0) is False
+    assert close_ready(3, 0) is False
+
+
+def test_fast_a7_duration_keeps_spline_peak_under_the_drive_cap():
+    """空回 342° 不能按 0.90 rad/s 的平均速度排。样条峰速会超上限，到位还差几十度。"""
+    from arm_application.pipeline_node import (
+        A7_MAX_SPEED_DEG_S,
+        CUBIC_PEAK_OVER_AVG,
+        NRT_VMAX_RAD_S,
+        a7_move_duration_s,
+    )
+
+    delta = math.radians(169.9 - (-172.0))
+    dur = a7_move_duration_s(delta, A7_MAX_SPEED_DEG_S)
+    peak = CUBIC_PEAK_OVER_AVG * abs(delta) / dur
+    assert peak == pytest.approx(NRT_VMAX_RAD_S, rel=1e-6)
+    assert dur == pytest.approx(abs(delta) / (NRT_VMAX_RAD_S / CUBIC_PEAK_OVER_AVG))
+    # 10°/s 的峰速本来就低于上限，排程不得被放慢。
+    slow = math.radians(137.6)
+    assert a7_move_duration_s(slow, 10.0) == pytest.approx(abs(slow) / math.radians(10.0))
+
+
 def test_insert_hold_does_not_return_to_hover():
     """贴面后停住。三阶段开局会抬回悬高；没有传感器，不另设一个 Z。"""
     assert hold_at_insert_z({"insert_held": True}, skip3=False) is True
@@ -49,8 +84,11 @@ def test_insert_hold_does_not_return_to_hover():
     seat = pipe[pipe.find("def _run_a7_seat") : pipe.find("def _on_lock")]
     marks = (
         "首轮咬合结束",
-        '_gripper_cli("pos", "400")',
+        '_gripper_cli("pos", str(open_permille))',
+        "空回前微抬",
+        "A7_MAX_SPEED_DEG_S",
         "松爪回转",
+        "空回后降回",
         '_gripper_cli("close")',
         "夹紧再进",
     )
@@ -61,6 +99,15 @@ def test_insert_hold_does_not_return_to_hover():
     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")]
+    wait = pipe[pipe.find("def _wait_gripper") : pipe.find("def _a7_to_limit")]
+    assert "close_ready(" in wait
+    assert 'ga["unwind_open_permille"]' in seat
+    shift = pipe[pipe.find("def _shift_flange_z") : pipe.find("def _arm_short_id")]
+    assert "_vendor_movel_tcp" in shift
+    assert "movel_tcp_z_only" in shift
+    assert 'zone="ZFine"' in shift
+    assert "toler=1" in shift
+    assert "_goto(" not in shift
     # /robot_description 是锁存话题。VOLATILE 订阅会错过已经发出的 URDF。
     sub = pipe[pipe.find('"/robot_description"') : pipe.find('"/robot_description"') + 400]
     assert "TRANSIENT_LOCAL" in sub
@@ -124,6 +171,24 @@ def test_movel_ipc_roundtrip():
 
     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 "ZFine" not in line
+    fine = format_request(8, 0.7, -0.1, 0.02, False, 0.01, "ZFine", 1)
+    assert fine.strip().endswith("ZFine 1")
+    try:
+        format_request(8, 0.7, -0.1, 0.02, False, 0.01, "Z50", 3)
+    except ValueError as exc:
+        assert "ZFine" in str(exc)
+    else:
+        raise AssertionError("默认过渡区 Z50 不许写进微升降请求")
+    cpp = (
+        Path(__file__).resolve().parents[3]
+        / "src/flexiv_ros2/flexiv_hardware/src/flexiv_hardware_interface.cpp"
+    ).read_text(encoding="utf-8")
+    start = cpp.find("VendorMovelPhase::kArming")
+    arming = cpp[start : cpp.find("VendorMovelPhase::kRunning", start)]
+    assert 'movel_args.emplace("zoneRadius"' in arming
+    assert "vendor_movel_zone_.empty()" in arming
+    assert "微升降只接受过渡区 ZFine 和到位等级 1" in cpp
     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

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

@@ -152,6 +152,9 @@ private:
     double vendor_movel_z_ = 0.0;
     double vendor_movel_vel_ = 0.0;
     int vendor_movel_keep_z_ = 0;
+    // 空字符串表示不改厂家默认（Z50 / 到位等级 3）。微升降写成 ZFine 和 1。
+    std::string vendor_movel_zone_;
+    int vendor_movel_toler_ = -1;
     std::chrono::steady_clock::time_point vendor_movel_deadline_ {};
     std::chrono::steady_clock::time_point vendor_movel_arm_until_ {};
     // MoveL 切回关节模式后 JTC 还握着贴面前的悬停关节。钉住的是结束

src/flexiv_ros2/flexiv_hardware/src/flexiv_hardware_interface.cpp

@@ -15,6 +15,7 @@
 #include <cstdlib>
 #include <exception>
 #include <fstream>
+#include <map>
 #include <string>
 #include <variant>
 #include <thread>
@@ -832,16 +833,23 @@ bool FlexivHardwareInterface::poll_vendor_movel()
                     std::array<double, 3> {vendor_movel_x_, vendor_movel_y_, z},
                     euler,
                     std::array<std::string, 2> {"WORLD", "WORLD_ORIGIN"});
-                robot_->ExecutePrimitive(
-                    "MoveL",
-                    {{"target", target}, {"vel", vendor_movel_vel_}},
-                    true);
+                std::map<std::string, flexiv::rdk::FlexivDataTypes> movel_args;
+                movel_args.emplace("target", target);
+                movel_args.emplace("vel", vendor_movel_vel_);
+                if (!vendor_movel_zone_.empty()) {
+                    // 默认 Z50 把几毫米的微升降整段包进过渡区，到位等级 3 再提前收。
+                    movel_args.emplace("zoneRadius", vendor_movel_zone_);
+                    movel_args.emplace("targetTolerLevel", vendor_movel_toler_);
+                }
+                robot_->ExecutePrimitive("MoveL", movel_args, true);
                 vendor_movel_phase_ = VendorMovelPhase::kRunning;
                 vendor_movel_deadline_ = now + std::chrono::seconds(90);
                 RCLCPP_INFO(getLogger(),
-                    "厂家 MoveL 已下发 xyz=(%.3f, %.3f, %.3f) keep_z=%d vel=%.3f",
+                    "厂家 MoveL 已下发 xyz=(%.3f, %.3f, %.3f) keep_z=%d vel=%.3f zone=%s toler=%d",
                     vendor_movel_x_, vendor_movel_y_, z,
-                    vendor_movel_keep_z_, vendor_movel_vel_);
+                    vendor_movel_keep_z_, vendor_movel_vel_,
+                    vendor_movel_zone_.empty() ? "-" : vendor_movel_zone_.c_str(),
+                    vendor_movel_toler_);
             } catch (const std::exception& e) {
                 try {
                     robot_->SwitchMode(rdk_control_mode_);
@@ -923,6 +931,17 @@ bool FlexivHardwareInterface::poll_vendor_movel()
     if (seq == vendor_movel_done_seq_) {
         return false;
     }
+    // 后两段可缺。缺了就用厂家默认。写了就必须是 ZFine 和等级 1，别的直接失败。
+    std::string zone;
+    int toler = -1;
+    if (in >> zone) {
+        if (!(in >> toler) || zone != "ZFine" || toler != 1) {
+            write_movel_line(kMovelAck, std::to_string(seq));
+            finish_vendor_movel(
+                seq, false, "MoveL 微升降只接受过渡区 ZFine 和到位等级 1");
+            return true;
+        }
+    }
 
     // 先应答，伺服才能在 1.5s 内分清「新接口没编进去」和「臂还在走」。
     write_movel_line(kMovelAck, std::to_string(seq));
@@ -940,6 +959,8 @@ bool FlexivHardwareInterface::poll_vendor_movel()
         vendor_movel_z_ = z;
         vendor_movel_vel_ = vel;
         vendor_movel_keep_z_ = keep_z;
+        vendor_movel_zone_ = zone;
+        vendor_movel_toler_ = toler;
         robot_->SwitchMode(flexiv::rdk::Mode::NRT_PRIMITIVE_EXECUTION);
         vendor_movel_phase_ = VendorMovelPhase::kArming;
         vendor_movel_arm_until_ = std::chrono::steady_clock::now()