旋转操作加速版本

build/arm_application/arm_application.egg-info/SOURCES.txt

@@ -40,7 +40,19 @@ arm_application/kalman_filter.py
 arm_application/model_labels.py
 arm_application/move_to_pose_client.py
 arm_application/onnx_workpiece.py
+arm_application/operator_app.py
+arm_application/operator_arm_mesh.py
+arm_application/operator_arm_state.py
+arm_application/operator_camera_view.py
+arm_application/operator_console.py
+arm_application/operator_gripper_mesh.py
+arm_application/operator_gripper_state.py
+arm_application/operator_handeye_board.py
+arm_application/operator_tactile_view.py
+arm_application/operator_teach.py
 arm_application/orisys_tactile.py
+arm_application/overhead_oneshot.py
+arm_application/path_profile.py
 arm_application/perception_node.py
 arm_application/pid_controller.py
 arm_application/pipeline_node.py

build/arm_application/build/lib/arm_application/demo_named_poses.py

@@ -10,8 +10,10 @@ from rclpy.node import Node
 
 from arm_msgs.srv import SetNamedPose
 
+from arm_application.ros_node import TolerantNode
 
-class DemoNamedPoses(Node):
+
+class DemoNamedPoses(TolerantNode):
     def __init__(self) -> None:
         super().__init__('demo_named_poses')
         self.declare_parameter(

build/arm_application/build/lib/arm_application/dh_official.py

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

build/arm_application/build/lib/arm_application/dh_pgea.py

@@ -104,6 +104,44 @@ INIT_STATE_NAME = {
 }
 
 
+def latched_position_break(command: int, current: int, target: int) -> Optional[int]:
+    """命令寄存器已经等于目标、手指却不在目标上时，返回要先写入的中间位置。
+
+    大寰固件：再写一遍与 0x0103 当前值相同的数，不会启动新运动。
+    开爪卡在 caught 时就是这样——命令已是 1000，手指停在夹持位。
+    先把命令改成当前位置（手指本来就在这，不会再夹紧），等读回确认
+    固件已经采样，再写最终目标。只 sleep 几十毫秒就写回旧值，控制周期
+    来不及看到中间值，最终仍是旧命令，手指不动。
+    """
+    cmd = int(max(0, min(1000, command)))
+    cur = int(max(0, min(1000, current)))
+    pos = int(max(0, min(1000, target)))
+    if cmd != pos or abs(cur - pos) <= 25:
+        return None
+    if cur != cmd:
+        return cur
+    return 0 if cmd != 0 else 40
+
+
+def grip_motion_settled(
+    grip_state: int, position: int, target: int, seen_motion: bool
+) -> bool:
+    """这一拍能不能算「这句位置命令已经结束」。
+
+    张开前本来就是 caught、位置还停在夹持位：不能当成张开完成。
+    闭合夹住物体：动过之后停在比目标更开的位置，算完成。
+    """
+    near = abs(int(position) - int(target)) <= 25
+    if grip_state in (S_GRIP_ARRIVED, S_GRIP_DROPPED) and near:
+        return True
+    if grip_state == S_GRIP_CAUGHT and seen_motion:
+        if int(target) + 25 < int(position):
+            return True
+        if near:
+            return True
+    return False
+
+
 def crc16_modbus(data: bytes) -> int:
     """官方 DH CRC16（Modbus，线路上按小端发送）。"""
     crc = 0xFFFF
@@ -175,6 +213,34 @@ WIRING_HINT = (
 )
 
 
+def open_failure_text(port: str, exc: BaseException) -> str:
+    """串口打不开时把「被占用」和「没应答」分开。
+
+    独占锁失败（errno 11）以前和真没上电写成同一句「需要 24 V DC」，
+    界面上看起来像缺自动上电，实际是别的进程还握着这个口。
+    """
+    text = str(exc).lower()
+    errno = getattr(exc, "errno", None)
+    busy = errno in (11, 16) or any(
+        token in text
+        for token in (
+            "temporarily unavailable",
+            "device or resource busy",
+            "could not exclusively lock",
+            "resource busy",
+        )
+    )
+    if busy:
+        return (
+            f"无法独占打开 {port}：{exc}。"
+            "串口已被别的进程占用（夹爪驱动或状态读取）。"
+        )
+    return (
+        f"无法打开 {port}：{exc}。"
+        "需要 24 V DC 供电 + RS485 接线，且当前用户在 dialout 组内。"
+    )
+
+
 class DhPgeaGripper:
     """面向 PGE / PGEA 的线程安全 Modbus RTU 客户端（官方寄存器映射）。"""
 
@@ -219,10 +285,7 @@ class DhPgeaGripper:
                 exclusive=True,
             )
         except Exception as exc:
-            raise DhPgeaError(
-                f"无法打开 {self.port}：{exc}。"
-                "需要 24 V DC 供电 + RS485 接线，且当前用户在 dialout 组内。"
-            ) from exc
+            raise DhPgeaError(open_failure_text(self.port, exc)) from exc
         try:
             self._ser.dtr = True
         except Exception:
@@ -319,17 +382,28 @@ class DhPgeaGripper:
     def initialization(self) -> bool:
         return self.write_register(REG_INIT, INIT_CMD)
 
-    def initialize(self, timeout_s: float = 20.0, poll_s: float = 0.2) -> bool:
-        """写 0xA5，然后等到初始化状态 == 完成（对应官方上电流程）。"""
+    def initialize(
+        self, timeout_s: float = 20.0, poll_s: float = 0.2, *, force: bool = False
+    ) -> bool:
+        """写 0xA5，然后等到初始化状态 == 完成（对应官方上电流程）。
+
+        ``force=False`` 时，已经完成就不再写（节点启动不要每次都让手指找零）。
+        命令行 ``init`` 必须 ``force=True``：状态字已经是完成时，不写 0xA5
+        手指根本不会回零，看起来像初始化失效。
+        """
         state = self.get_init_state()
-        if state == S_INIT_FINISHED:
+        if state == S_INIT_FINISHED and not force:
             return True
         if not self.initialization():
             raise DhPgeaError("初始化写 0x0100=0xA5 失败（无 Modbus 回显）")
         deadline = time.monotonic() + float(timeout_s)
+        # force 时必须先看到状态离开「完成」，否则读到的还是写之前的旧值。
+        saw_busy = not force
         while time.monotonic() < deadline:
             state = self.get_init_state()
-            if state == S_INIT_FINISHED:
+            if state != S_INIT_FINISHED:
+                saw_busy = True
+            elif saw_busy:
                 return True
             time.sleep(poll_s)
         raise DhPgeaError(
@@ -345,42 +419,58 @@ class DhPgeaGripper:
         speed_pct = int(max(1, min(100, speed_pct)))
         return self.write_register(REG_SPEED, speed_pct)
 
+    def _wait_command_echo(self, value: int, timeout_s: float = 0.6) -> bool:
+        """等到 0x0103 读回就是刚写进去的值。读不回就不要再写下一笔。"""
+        deadline = time.monotonic() + float(timeout_s)
+        while time.monotonic() < deadline:
+            try:
+                if self.get_target_position(retries=1) == int(value):
+                    return True
+            except Exception:
+                pass
+            time.sleep(0.05)
+        return False
+
     def set_target_position(self, pos: int) -> bool:
         pos = int(max(0, min(1000, pos)))
-        # 大寰固件：0x0103 写入与当前命令相同的值不会启动新运动。
-        # 开/关卡在 caught 时最常见——命令已是 1000，手指却停在夹持位。
         try:
             cmd = self.get_target_position(retries=2)
         except Exception:
             cmd = pos
-        if cmd == pos:
         try:
             cur = self.get_current_position(retries=2)
         except Exception:
             cur = pos
-            nudge = cur + (40 if pos >= cur else -40)
-            nudge = int(max(0, min(1000, nudge)))
-            if nudge == pos:
-                nudge = 0 if pos != 0 else 40
-            self.write_register(REG_POSITION, nudge)
-            time.sleep(0.06)
+        bridge = latched_position_break(cmd, cur, pos)
+        if bridge is not None:
+            if not self.write_register(REG_POSITION, bridge):
+                return False
+            if not self._wait_command_echo(bridge):
+                return False
+            # 读回只说明寄存器改了。再留一个控制周期，避免下一笔把中间值盖掉。
+            time.sleep(0.15)
         return self.write_register(REG_POSITION, pos)
 
     def wait_settle(self, target: int, timeout_s: float = 6.0) -> GripperState:
-        """等到到位/夹住/松开，或超时返回最后一帧。"""
+        """等到到位或夹住，或超时返回最后一帧。
+
+        张开前状态字已经是 caught 时不能立刻返回：那是上一句命令的残留，
+        手指还停在夹持位。
+        """
         t0 = time.monotonic()
         last = None
+        seen_motion = False
         st = self.get_run_states()
         while time.monotonic() - t0 < timeout_s:
             st = self.get_run_states()
-            moved = last is not None and st.position != last
+            if last is not None and st.position != last:
+                seen_motion = True
             last = st.position
-            if st.grip_state == S_GRIP_MOVING or moved:
+            if st.grip_state == S_GRIP_MOVING:
+                seen_motion = True
                 time.sleep(0.08)
                 continue
-            if st.grip_state in (S_GRIP_ARRIVED, S_GRIP_DROPPED) and abs(st.position - target) <= 25:
-                return st
-            if st.grip_state == S_GRIP_CAUGHT:
+            if grip_motion_settled(st.grip_state, st.position, target, seen_motion):
                 return st
             time.sleep(0.08)
         return st
@@ -497,6 +587,36 @@ def _scan_bus(port: str) -> int:
     return 1
 
 
+def ctrl_values(
+    cmd: str,
+    value: str = "",
+    force: int = 50,
+    speed: int = 50,
+) -> Optional[tuple[float, float, float, float]]:
+    """节点已经占着串口时，改发 ``/gripper/ctrl`` 的四个数。
+
+    顺序是初始化、位置千分比、力度、速度。``probe`` / ``status`` 只读，返回 None。
+    """
+    name = str(cmd or "").strip().lower()
+    if name in ("probe", "status"):
+        return None
+    if name == "init":
+        return (1.0, 0.0, float(force), float(speed))
+    if name == "open":
+        pos = 1000
+    elif name == "close":
+        pos = 0
+    elif name in ("pos", "move"):
+        if str(value).strip() == "":
+            raise ValueError("pos 需要 0–1000")
+        pos = int(round(float(value)))
+        if pos < 0 or pos > 1000:
+            raise ValueError("位置只接受 0–1000")
+    else:
+        raise ValueError(f"节点在线时不支持 {name}")
+    return (0.0, float(pos), float(force), float(speed))
+
+
 def _cli(argv: Optional[list[str]] = None) -> int:
     site = _site_defaults()
     ap = argparse.ArgumentParser(
@@ -541,8 +661,8 @@ def _cli(argv: Optional[list[str]] = None) -> int:
             print(st.summary())
             return 0
         if args.cmd == "init":
-            print("正在初始化（手指回零）…")
-            grip.initialize()
+            print("正在初始化（手指回零，已初始化也会再做一次）…")
+            grip.initialize(force=True)
             print(grip.get_run_states().summary())
             return 0
         if args.cmd == "status":
@@ -551,12 +671,25 @@ def _cli(argv: Optional[list[str]] = None) -> int:
         if args.cmd == "open":
             if not grip.open_jaw(args.force, args.speed):
                 raise DhPgeaError("张开写入失败")
-            print(grip.wait_settle(1000).summary())
+            st = grip.wait_settle(1000)
+            print(st.summary())
+            if abs(st.position - 1000) > 25:
+                print(
+                    "张开失败：手指没有离开夹持位。"
+                    "若仍是 grip=caught，先取下工件再执行 init。",
+                    file=sys.stderr,
+                )
+                return 1
             return 0
         if args.cmd == "close":
             if not grip.close_jaw(args.force, args.speed):
                 raise DhPgeaError("闭合写入失败")
-            print(grip.wait_settle(0).summary())
+            st = grip.wait_settle(0)
+            print(st.summary())
+            # 夹住物体时位置停在厚度上，grip=caught 算成功。停在张开位则失败。
+            if st.grip_state != S_GRIP_CAUGHT and abs(st.position) > 25:
+                print("闭合失败：手指没有朝合拢方向离开原位。", file=sys.stderr)
+                return 1
             return 0
         if args.cmd == "pos":
             if args.value == "":
@@ -565,7 +698,15 @@ def _cli(argv: Optional[list[str]] = None) -> int:
             pos = int(float(args.value))
             if not grip.move(pos, args.force, args.speed):
                 raise DhPgeaError("位置写入失败")
-            print(grip.wait_settle(pos).summary())
+            st = grip.wait_settle(pos)
+            print(st.summary())
+            grasped_short = st.grip_state == S_GRIP_CAUGHT and st.position > pos
+            if abs(st.position - pos) > 25 and not grasped_short:
+                print(
+                    f"位置 {pos} 没有到位（仍在 {st.position}）。",
+                    file=sys.stderr,
+                )
+                return 1
             return 0
     except DhPgeaError as exc:
         print(f"错误：{exc}", file=sys.stderr)

build/arm_application/build/lib/arm_application/dh_pgea_node.py

@@ -33,15 +33,7 @@ from arm_application.dh_pgea import (
     GripperState,
     WIRING_HINT,
 )
-
-
-def _param_as_str(node: Node, name: str, default: str = "") -> str:
-    """字符串参数，同时允许命令行用数字覆盖（例如 SN 26610799）。"""
-    node.declare_parameter(name, default, ParameterDescriptor(dynamic_typing=True))
-    val = node.get_parameter(name).value
-    if val is None or val == "":
-        return ""
-    return str(val).strip()
+from arm_application.ros_node import TolerantNode, param_as_str
 
 
 def _as_ctrl_array(msg: Float32MultiArray) -> tuple[float, float, float, float]:
@@ -51,7 +43,7 @@ def _as_ctrl_array(msg: Float32MultiArray) -> tuple[float, float, float, float]:
     return float(data[0]), float(data[1]), float(data[2]), float(data[3])
 
 
-class DhPgeaNode(Node):
+class DhPgeaNode(TolerantNode):
     def __init__(self) -> None:
         super().__init__("dh_pgea")
         self.declare_parameter("port", "/dev/ttyUSB0")
@@ -64,9 +56,9 @@ class DhPgeaNode(Node):
         self.declare_parameter("state_hz", 20.0)
         self.declare_parameter("open_position", 1000)
         self.declare_parameter("close_position", 0)
-        self._model = _param_as_str(self, "model", "PGEA-100-20-W")
-        self._sn = _param_as_str(self, "serial_number", "")
-        self._rts = _param_as_str(self, "rs485_rts", "")
+        self._model = param_as_str(self, "model", "PGEA-100-20-W")
+        self._sn = param_as_str(self, "serial_number", "")
+        self._rts = param_as_str(self, "rs485_rts", "")
 
         self._port = str(self.get_parameter("port").value)
         self._baud = int(self.get_parameter("baud").value)

build/arm_application/build/lib/arm_application/dual_gripper_node.py

@@ -22,6 +22,8 @@ from arm_msgs.msg import GripperCommand as GripperCommandMsg
 from arm_msgs.msg import GripperStatus as GripperStatusMsg
 from arm_msgs.srv import GripperExecute
 
+from arm_application.ros_node import TolerantNode
+
 
 def _site_path() -> Path:
     env = os.environ.get("SITE_CONFIG", "").strip()
@@ -38,7 +40,7 @@ def _as_bool(val, default: bool = True) -> bool:
     return bool(val)
 
 
-class DualGripperNode(Node):
+class DualGripperNode(TolerantNode):
     def __init__(self) -> None:
         super().__init__("dual_gripper_driver")
         self.declare_parameter("config_file", "")

build/arm_application/build/lib/arm_application/gauge_assembly_node.py

@@ -94,6 +94,10 @@ from arm_application.gauge_config import (
 )
 from arm_application.gauge_fsm import (
     DESCEND_STALL_STEPS,
+    ENTRY_HIGH,
+    ROT_AXIS,
+    ROT_LIMIT,
+    ROT_SHORT,
     Event,
     FsmError,
     GaugeContext,
@@ -104,12 +108,16 @@ from arm_application.gauge_fsm import (
     descend_iter_budget,
     descend_needed_m,
     descend_progress,
+    hole_entry_verdict,
     descend_tol_m,
     is_joint_limit_error,
     pixel_stable,
     points_stable,
     ratchet_cycles,
+    ratchet_delta_rad,
+    ratchet_travel_ok,
     relief_tol_m,
+    rotation_verdict,
     screw_stop,
     tangential_jump,
     tool_clearance_m,
@@ -135,16 +143,41 @@ from arm_application.gauge_kin import (
 )
 from arm_application.pipeline_node import AssemblyPipeline
 from arm_application.handeye_realsense import resolve_realsense_topics
-from arm_application.site_io import deep_merge, load_site_dict, ros2_root
+from arm_application.site_io import (
+    deep_merge,
+    load_site_dict,
+    overhead_camera,
+    resolve_eye_to_hand_file,
+    ros2_root,
+)
 # 复用同一份图像解码与轨迹点构造，别在这里再写一份（规则 M2）
 from arm_application.stage2_locate_node import _bgr_copy, _pt as _joint_point
 
 #: 允许的"绕轴验证"误差 [deg]。超过就说明这根关节轴不是工具 Z 轴，
 #: 后面所有以"转多少度"为单位的动作都不成立 —— 必须报错而不是继续。
 AXIS_VERIFY_TOL_DEG = 5.0
+#: 棘轮转腕时距离关节限位保留的余量 [deg]。
+#: 3° 不够用：实测轨迹会**自己**少走 ~5°（比如命令 180° 只到位 175.1°，
+#: 而关节7 的限位正好是 175.06° —— 两者叠在一起，判断"是被限位截住还是
+#: 没走完"就分不清了）。留 8° 让这两种情况都还没有贴住。
+RATCHET_LIMIT_MARGIN_DEG = 8.0
 #: 限制"跟踪丢失 → 粗定位 → 跟踪"的来回次数，防死循环。
 MAX_TRACK_RETRIES = 3
 
+#: ``stage2_locate`` 返回码 → 人话。为什么要这张表：日志里只有 ``code=6`` 时，
+#: 现场只能去翻源码；而每个码对应的**下一步动作完全不同**（垂直化/粗定位/
+#: 检测/对准/工件被推走），写在这里就不用来回翻了。
+_LOCATE_CODE_HINT: Dict[int, str] = {
+    2: "（垂直化或悬停失败：工具轴没竖直、或悬停高度到不了）",
+    3: "（粗定位失败：视野里没框到工件 —— 看相机/光照/权重）",
+    4: "（k1/k2 解不出来：孔被挡、曝光不对、或孔根本没在视野里）",
+    5: "（对准迭代用满仍未收敛：检查手眼/相机安装，或放宽 align_tol_m）",
+    6: "（工件被工具推走：不是手眼/检测的问题，是悬停时工具尖端已经进了工件"
+       " —— 查 gauge_assembly.tool_tip_below_flange_m 与 servo.hole_hover_z_m）",
+    8: "（规尖偏移一直没量到：残差看着达标、但用的是**法兰轴线**不是规尖 —— "
+       "对的可能是法兰、不是规；摆到能看清规尖再跑。看日志『规尖视觉量取失败』）",
+}
+
 
 class GaugeAbort(RuntimeError):
     """阶段内部判定要中止。带上 exit code，方便脚本区分原因。"""
@@ -194,12 +227,11 @@ class GripperClient:
         if math.isfinite(pos):
             self._last_aperture = pos
         self._last_gripped = bool(msg.is_gripped)
-        if self._last_gripped:
-            if not self._ever_gripped:
-                # 记住"夹住时的开口"：之后的掉件判据是**相对它的塌陷量**，
-                # 换一把规、换一个夹持开口都不用改阈值。
-                self._grip_aperture = self._last_aperture
-            self._ever_gripped = True
+        # ⚠️ 这里**故意不**记"夹住时的开口"。曾经记过，代价是一个假 E1：
+        #    本后端的 is_gripped 是**推断值**，夹爪**全开**时也会闪成 True
+        #    （实测 99.29mm，见 dropped 的说明），于是基准被记成 99.29mm、
+        #    塌陷量算成 77mm ⇒ "通止规掉了(E1)"，而规好好地在爪里。
+        #    基准只由 :meth:`adopt_current_grip` 在**明确知道夹住了**的时候记。
 
     def _on_legacy(self, msg: Float32MultiArray) -> None:
         fields = legacy_states_to_fields(list(msg.data))
@@ -210,6 +242,8 @@ class GripperClient:
         if got is not None:
             self._last_gripped = got
             if got:
+                # 老后端只有状态字、没有开口，掉件判据本来就吃状态字（见 dropped），
+                # 所以这里记"曾经夹住过"是对的。
                 self._ever_gripped = True
 
     # -- 查询 -------------------------------------------------------------
@@ -224,6 +258,75 @@ class GripperClient:
         """``True``/``False``/``None``（后端给不出明确答案）。"""
         return self._last_gripped
 
+    def aperture(self) -> Optional[float]:
+        """最近一帧的**开口** [mm]（只有契约后端的 ``position`` 有；没有返回 None）。"""
+        return self._last_aperture
+
+    def grip_aperture(self) -> Optional[float]:
+        """第一次判到"夹住"时的开口 [mm]（掉件判据的基准，见 :meth:`dropped`）。"""
+        return self._grip_aperture
+
+    def collapse_mm(self) -> Optional[float]:
+        """相对夹住时的**开口塌陷量** [mm]（正 = 比夹住时更小）。拿不到返回 None。
+
+        S08/S09 每轮都要"松爪 → 回转 → 夹紧"，日志里必须看得到这个数：
+        它一路变大就说明每轮夹紧都比上一轮更往里挤，或者规在往下滑。
+        """
+        if self._grip_aperture is None or self._last_aperture is None:
+            return None
+        return float(self._grip_aperture) - float(self._last_aperture)
+
+    # -- 开口的"等稳定"与"记基准" ------------------------------------------
+    def settle(self, *, settle_s: float = 1.0, tol_mm: float = 0.05) -> Optional[float]:
+        """等开口**稳定**下来，返回稳定后的开口 [mm]（拿不到读数返回 None）。
+
+        为什么必须有这个（2026-09-24 用日志抓到的）：
+            ``close``/``open`` 服务**返回时动作还没做完**（JTC 轨迹是异步的）。
+            立刻读开口会读到**松手前的旧值**——实测 S08 夹紧后立刻读，读到的还是
+            全开 99.93mm，于是日志与基准全错（"夹住时 99.29mm"）。谁要在动作之后
+            判断开口，都必须先过这里。
+        """
+        spin = getattr(self._node, "_spin", None)
+        deadline = time.monotonic() + max(0.0, settle_s)
+        prev = self._last_aperture
+        stable = 0
+        while True:
+            if spin is not None:
+                spin(0.05)
+            else:  # pragma: no cover - 只有测试桩走这里
+                time.sleep(0.05)
+            now = self._last_aperture
+            if now is not None and prev is not None and abs(float(now) - float(prev)) <= tol_mm:
+                stable += 1
+                if stable >= 2:
+                    return float(now)
+            else:
+                stable = 0
+            prev = now
+            if time.monotonic() >= deadline:
+                return None if now is None else float(now)
+
+    def adopt_current_grip(self, why: str, *, settle_s: float = 1.0) -> Optional[float]:
+        """把**当前**开口记为"夹住时的开口"（掉件判据的基准），并记"曾经夹住过"。
+
+        只在**明确知道爪里夹着规**的时刻调用（S00 自检通过后、以及每一次
+        确认夹紧之后）——它是掉件判据唯一的参照，随手记会让判据失去意义
+        （见 :meth:`dropped` 与 :meth:`_on_contract` 的说明）。
+        """
+        ap = self.settle(settle_s=settle_s)
+        if ap is None:
+            # 拿不到开口（老固件/别的夹爪）也要记"曾经夹住过"：否则 dropped()
+            # 会永远返回 False，那比"会抖的旧判据"更糟（真掉件也看不见）。
+            self._ever_gripped = True
+            self._node.get_logger().warn(
+                f"拿不到开口读数（{why}）—— 掉件判据退回状态字，会抖但不会漏"
+            )
+            return None
+        self._grip_aperture = float(ap)
+        self._ever_gripped = True
+        self._node.get_logger().info(f"夹持基准：开口 {ap:.2f}mm（{why}）")
+        return float(ap)
+
     def dropped(self) -> bool:
         """E1 掉件：**曾经夹住过**，现在报"掉落"。
 
@@ -238,6 +341,9 @@ class GripperClient:
             的指令落到与实际开口同一档，``is_gripped`` 就翻成 False。于是
             **S08 第 2/12 轮直接报 E1**（爪里其实好好的，是判据在抖），
             然后整条流程去等人工"放好通止规"，而仿真里根本没人能做这件事。
+            更坏的是这个推断值在**全开**时也会闪成 True（实测 99.29mm）——
+            拿它当基准就会得到"塌陷 77mm"的假 E1。所以基准只由
+            :meth:`adopt_current_grip` 明确记录（见 :meth:`_on_contract`）。
 
             真正"掉了"的物理表现是**两指合到底**：开口从"工件宽度"塌到几毫米，
             是几十毫米的变化，不会被指令切换带偏。所以判据用
@@ -349,6 +455,12 @@ class GaugeAssembly(AssemblyPipeline):
         # `$?` 判不出成败 —— 自动化里会把"流程失败"记成"成功"。结论文件是唯一
         # 能被脚本/CI 读到的东西，由 run_gauge_assembly.sh 传入并读回。
         self.declare_parameter("result_file", "/tmp/gauge_assembly_result.json")
+        # 眼在手外标定产物的**确切路径**，由 launch 按 site.yaml 派生后传入
+        # （`<臂号>_<相机>_eye_to_hand.yaml`）。为什么不让节点自己拼：
+        # "臂号 = SN 末两位"这条口径只有 export_site_env 一处实现，节点再写一遍
+        # 必然漂移；而漂移的症状是"标定做完了、S03 却找不到文件"，回退到 S04 跟踪，
+        # 看起来像跟踪的问题。留空则退回下面那串候选名（兼容旧命名/仿真）。
+        self.declare_parameter("coarse_eye_to_hand_file", "")
 
         site = load_site_dict()
         # robots[].gauge_assembly 覆盖全局段 —— 与 servo / paths / perception 同一套规矩。
@@ -369,6 +481,13 @@ class GaugeAssembly(AssemblyPipeline):
         holes = int(self.get_parameter("holes_total").value)
         if holes > 0:
             overrides["holes_total"] = holes
+        # 工具长：仿真里由启动脚本传**实测值**（上料脚本刚量的），真机不传（走 site）。
+        # 为什么必须能覆盖：这个值决定悬停高度与 S07 下探预算，而仿真里规会在指间
+        # 下滑到落座位置、下滑量随装夹变化（实测见过 243 / 254mm，差 11mm）。
+        # 差 11mm 的后果见 gauge_fsm.hover_z_guard / drag_verdict 的说明。
+        tool_len = float(self.get_parameter("tool_tip_below_flange_m").value)
+        if tool_len > 0.0:
+            overrides["tool_tip_below_flange_m"] = tool_len
         try:
             self.cfg: GaugeConfig = from_site(site, overrides=overrides or None)
         except GaugeConfigError as exc:
@@ -496,49 +615,30 @@ class GaugeAssembly(AssemblyPipeline):
     def _d435_ns(self, site: Dict[str, Any]) -> str:
         """手外相机的命名空间：``role: overhead*`` 且**属于当前这台臂**的那一个。
 
-        为什么必须按 ``arm`` 过滤：仿真那台也有手外相机（``sim_d435``），
-        只按 role 扫会同时命中两台，而"命中哪一台"取决于字典顺序 ——
-        同一份配置在仿真/真机之间**错而不报**地互串。
-
-        规则（确定性的三级）：
-          1. ``arm`` 与当前臂相同的优先（仿真：sim_d435 / 真机若两台各有相机也适用）；
-          2. 都没有 ``arm``（= 双臂共用的那台老相机）时回落到它；
-          3. 还是多个 → 出声告警并取第一个，让人去配置里区分。
+        规则（三级收敛）的**唯一实现**在 ``site_io.overhead_camera``——手眼标定
+        脚本也用它取相机名。这里只负责把"配置没区分开"这件事**出声**：静默取第一台
+        会让"标定用的相机"和"推理用的相机"悄悄不是同一台。
         """
-        cams = [
-            cfg
-            for cfg in (site.get("cameras") or {}).values()
-            if isinstance(cfg, dict)
-            and str(cfg.get("role") or "").startswith("overhead")
-        ]
-        if not cams:
-            return ""
-        arm = str(self._robot_dict(site).get("arm") or "").strip()
-        mine = [c for c in cams if str(c.get("arm") or "").strip() == arm] if arm else []
-        hits = mine or [c for c in cams if not str(c.get("arm") or "").strip()] or cams
-        if len(hits) > 1:
-            names = ", ".join(str(c.get("name") or "?") for c in hits)
+        cfg, others = overhead_camera(site, self._robot_dict(site))
+        if others:
+            names = ", ".join([str(cfg.get("name") or "?")] + others)
             self.get_logger().warn(
-                f"有 {len(hits)} 台手外相机都符合当前臂（{names}）—— 取第一个，"
+                f"有 {len(others) + 1} 台手外相机都符合当前臂（{names}）—— 取第一个，"
                 "但请给 site.yaml 的 cameras.* 补上 arm 字段区分，否则选哪台看运气"
             )
-        return str(hits[0].get("name") or "").strip("/")
+        return str(cfg.get("name") or "").strip("/")
 
     def _eye_to_hand_file(self) -> Optional[Path]:
         """找手外相机的手眼文件。没有就返回 None（调用方据此给出**怎么生成**）。
 
-        路径按**仓库根**解析而不是当前工作目录：相对 CWD 的路径会在"换个目录
-        启动"时静默失效（文件找不到 → S03 直接降级），而人只会怀疑标定。
+        规则（候选顺序 + 按仓库根解析）只有一份，在 ``site_io.resolve_eye_to_hand_file``。
+        这里只负责把 launch 传入的确切路径和当前手外相机名交给它。
         """
-        root = ros2_root()
-        for rel in (
-            f"handeye/results/{self.cfg.coarse_camera}_eye_to_hand.yaml",
-            "handeye/results/eye_to_hand.yaml",
-        ):
-            cand = root / rel
-            if cand.is_file():
-                return cand
-        return None
+        return resolve_eye_to_hand_file(
+            ros2_root(),
+            explicit=str(self.get_parameter("coarse_eye_to_hand_file").value or ""),
+            camera_name=self.cfg.coarse_camera,
+        )
 
     def _on_robot_description(self, msg: String) -> None:
         self._robot_description = str(msg.data)
@@ -959,6 +1059,13 @@ class GaugeAssembly(AssemblyPipeline):
                 return Event.BOOT_OK
             self._publish_gauge_result({"ok": False, "state": "S00", "failures": blockers})
             return Event.BOOT_FAIL
+        # 记"夹住时的开口"（掉件判据的基准）。**放在这里而不是等到 S08**：
+        #   · 此刻爪里确实夹着规（仿真上料脚本刚装好，真机是人工装好才开始跑）；
+        #   · 基准必须是**明确知道夹住**时记的 —— 本后端的 is_gripped 是全开时
+        #     都会闪 True 的推断值（实测 99.29mm），拿它当基准会造出假 E1
+        #     （见 GripperClient.dropped）。
+        if self.grip is not None:
+            self.grip.adopt_current_grip("S00 自检通过：爪里已装好通止规")
         return Event.BOOT_OK
 
     # =====================================================================
@@ -1140,20 +1247,25 @@ class GaugeAssembly(AssemblyPipeline):
             f"{tag}: 目标 {delta_deg:+.1f}°，实测绕工具 Z {got:+.1f}°"
             f"（转轴偏差 {mis:.1f}°，阈值 {AXIS_VERIFY_TOL_DEG:.1f}°）"
         )
-        if abs(got) < 3.0 and abs(delta_deg) > 10.0:
-            # 完全没动：最可能是撞上关节极限（用户要求的"转到极限角"就是这种情形）
+        verdict = rotation_verdict(delta_deg, got, mis, AXIS_VERIFY_TOL_DEG)
+        if verdict == ROT_LIMIT:
+            # 完全没动、或只走了一部分：都是**关节限位**把这段截住了
+            # （2026-09-24 实测：棘轮回转每轮顶到上限 ±175°，于是下一步反向
+            #  只剩 ~16° 余量）。交给调用方让余量重试（见 _ratchet_step）；
+            #  调用方不处理时 FSM 会按未定义转移报错，不会静默走下去。
             self.get_logger().warn(
-                f"{tag}: 指令 {delta_deg:+.1f}° 但几乎没动 —— 判为已贴关节极限"
+                f"{tag}: 期望 {delta_deg:+.1f}°、实测 {got:+.1f}°"
+                f"（差 {abs(delta_deg) - abs(got):.1f}°）—— 判为**贴到关节限位**"
             )
             return Event.JOINT_LIMIT
-        if mis > AXIS_VERIFY_TOL_DEG:
+        if verdict == ROT_AXIS:
             self.get_logger().error(
                 f"{tag}: 转轴偏差 {mis:.1f}° 超过阈值 —— 关节7 轴与工具 Z 不共线，"
                 "以『度』为单位的动作（S01/S08/S09）都不可信，中止。"
                 "先确认工具系是否垂直向下、有没有装转接件"
             )
             return Event.ABORT
-        if abs(abs(got) - abs(delta_deg)) > max(AXIS_VERIFY_TOL_DEG, 0.1 * abs(delta_deg)):
+        if verdict == ROT_SHORT:
             self.get_logger().error(
                 f"{tag}: 期望 {delta_deg:+.1f}°、实测 {got:+.1f}°，偏差过大，中止"
             )
@@ -1161,6 +1273,146 @@ class GaugeAssembly(AssemblyPipeline):
         self._axis_verified = True
         return fsm_event
 
+    def _ratchet_step(
+        self, target_rad: float, *, tag: str, fsm_event: Event, speed_deg_s: Optional[float],
+        direction: int = 0,
+    ) -> Event:
+        """棘轮的一步"转腕"：行程不够时**张开爪让出余量**再转。
+
+        :param target_rad: 关节 7 的**目标角（绝对值）**，与
+            :meth:`_rotate_joint_and_verify` 同一口径（那边算的就是
+            ``target_rad - q_now``）。**不是增量。**
+        :param direction: 这一步的**工艺转向**：``+1`` 正转、``-1`` 反转、
+            ``0`` 交给"最短路径"。棘轮的四步一律显式给：步长接近 180° 时
+            "最短路径"会被 0.1° 的读数差翻方向（详见
+            :func:`arm_application.gauge_fsm.ratchet_delta_rad`）。
+
+        ⚠️ 为什么要把口径写在这里（2026-09-24 栽过一次，S09 直接 ABORT）：
+            调用方（S08/S09）给的本来就是绝对值（``q[-1] ± screw_step_rad``），
+            而本函数此前把它当**增量**用（``prewind(want_deg=abs(deg(target)))``、
+            让余量时又按 ``q - target`` 算新目标）。于是 S09 第一步被算成
+            "要转 338.9°"（其实是目标 338.9°、增量只有 180°）⇒ 误判"行程不够"
+            ⇒ 让余量那一步算到限位**外面**（``158.9 − 338.9 = −180°``，比
+            −175° 的限位还低 5°）⇒ 报"期望 −338.9°、实测 +26.0°，偏差过大"
+            ⇒ 整条流程在**最不该失败的一步** ABORT（规在爪里好好的、什么坏事都没发生）。
+            两个函数各用一种口径，编译器不会说话，日志还会把"目标"和"增量"都印成
+            一个数 —— 所以口径写进参数名和 docstring。
+
+        为什么需要"让余量"这一步：
+            棘轮要求"一正一反各 180°"，而关节 7 的行程只有 ±175°（3.0543 rad）。
+            拧入（S08）的**回转**那一步每轮都正好顶到上限（实测 +175.1°），
+            于是 S09 要做的**反方向**第一步只剩 ~16° 余量，被限位截住。
+            而这一轮其实什么坏事都没发生。
+
+        为什么"让余量"是免费的：
+            指间**张开**时规在打滑（这正是棘轮成立的原理），所以手腕空转既不会
+            把已经拧进去的螺纹转回去，也不会漏掉进度 —— 只是把行程重新分配。
+
+        让多少：整步（不去算亏欠了多少）。反正张开状态下这一步不产生任何装配
+        效果，整步最省事也最不容易算错；重试时的那一步才是"用力"的那一步。
+
+        两条路：**先算**（限位已知时，提前让余量，干净）＋ **兜底**（真被限位
+        截住时再让，覆盖"限位没读到 / 轨迹自己没走完"这两种情况）。
+
+        两条路**最后都汇到同一次让余量**：只有一次，避免"先算发现不够、
+        让完还不够、再让一次"这种把行程越绕越少的做法。
+        """
+        limits = self._wait_joint_limits()
+        q = None
+        if limits is not None:
+            q, _Tee = self._wait_state(3.0)
+        if q is None:
+            return Event.TIMEOUT
+        # 目标角可能落在限位**外面**（这正是棘轮要解决的问题），先折成"最近一圈"
+        # 的增量（gauge_fsm.ratchet_delta_rad，含完整说明与单测）：
+        # 关节在 +158.9°、目标 +338.9° ⇒ 增量 +180°（而不是 +338.9°）。
+        # **方向按工艺意图给**：步长贴着 180° 时"最短路径"会被读数差翻向（R2.10）。
+        delta_rad = ratchet_delta_rad(target_rad, float(q[-1]), direction)
+        why = ""
+        if limits is not None and abs(math.degrees(delta_rad)) > 1e-6:
+            plan = prewind(
+                q_now_rad=float(q[-1]), want_deg=abs(math.degrees(delta_rad)),
+                direction=1 if delta_rad > 0 else -1,
+                limits=limits, margin_deg=RATCHET_LIMIT_MARGIN_DEG,
+            )
+            if plan.clamped:
+                why = (
+                    f"行程不够（关节7 在 {math.degrees(float(q[-1])):+.1f}°，"
+                    f"这一步要转 {math.degrees(delta_rad):+.1f}°"
+                    f"（目标 {math.degrees(float(target_rad)):+.1f}°），限位 "
+                    f"{math.degrees(limits[0]):+.0f}~{math.degrees(limits[1]):+.0f}°）"
+                )
+        if not why:
+            ev = self._rotate_joint_and_verify(
+                target_rad, tag=tag, fsm_event=fsm_event, speed_deg_s=speed_deg_s
+            )
+            if ev is not Event.JOINT_LIMIT:
+                return ev
+            why = "轨迹被关节限位截住（算着够、实际没走完）"
+        return self._ratchet_rewind_then(
+            delta_rad, tag=tag, fsm_event=fsm_event, speed_deg_s=speed_deg_s, why=why,
+        )
+
+    def _ratchet_rewind_then(
+        self, delta_rad: float, *, tag: str, fsm_event: Event,
+        speed_deg_s: Optional[float], why: str,
+    ) -> Event:
+        """张开爪 → 反向让一步 → 夹紧 → 转原来那一步。
+
+        :param delta_rad: **要转的增量**（不是目标角，见 :meth:`_ratchet_step`）。
+            让余量时按它反着退一步，重试时再按它前进 —— 两次都以**当时的**
+            关节角为基准重新算目标，所以让完余量后关节会回到原处、工具净转
+            ``delta_rad``。把"目标"固定成一开始那个绝对值是错的：让完余量之后
+            那个目标已经在另一圈上了（差 360°）。
+        """
+        if self.grip is None:
+            self.get_logger().error(f"{tag}: {why}，但没有夹爪客户端，无法让余量 → 中止")
+            return Event.ABORT
+        self.get_logger().info(
+            f"{tag}: {why} —— 张开爪让出一步（此刻规在打滑，不会动到螺纹）再转"
+        )
+        if not self.grip.open():
+            self.get_logger().error(f"{tag}: 需要张爪让余量，但开爪失败 → 中止")
+            return Event.ABORT
+        time.sleep(self.cfg.screw_release_wait_s)
+        q, _Tee = self._wait_state(3.0)
+        if q is None:
+            return Event.TIMEOUT
+        ev = self._rotate_joint_and_verify(
+            float(q[-1]) - delta_rad, tag=f"{tag}_rewind",
+            fsm_event=fsm_event, speed_deg_s=speed_deg_s,
+        )
+        if ev is not fsm_event:
+            self.get_logger().error(
+                f"{tag}: 让余量那一步也没走开（{ev}）—— 关节7 的行程真用尽了，中止。\n"
+                f"        排查：site.yaml 的 gauge_assembly.screw_step_deg 太接近关节行程了"
+                f"（Rizon4 的关节7 是 ±175°），棘轮一步要 {math.degrees(abs(delta_rad)):.0f}°，"
+                f"留给'回转'的余量本来就不够。\n"
+                f"        改法：把 screw_step_deg 调小（例如 120°），并保证 "
+                f"screw_max_cycles × screw_step_deg 仍是 **360° 的整数倍** —— "
+                f"累计必须转整数圈，否则规会停在螺纹中间（S09 拧出也就少转几分之一圈）。"
+            )
+            return Event.ABORT
+        if not self.grip.close():
+            self.get_logger().error(f"{tag}: 让完余量后夹紧失败 → 中止")
+            return Event.ABORT
+        # 重试目标**按让完余量后的位置重算**：让余量退了一步，原来那个绝对目标
+        # 现在差了 360°（正好一整圈），照原样下发会让关节反向绕一圈回去。
+        q2, _Tee2 = self._wait_state(3.0)
+        if q2 is None:
+            return Event.TIMEOUT
+        ev = self._rotate_joint_and_verify(
+            float(q2[-1]) + delta_rad, tag=f"{tag}_retry",
+            fsm_event=fsm_event, speed_deg_s=speed_deg_s,
+        )
+        if ev is not fsm_event:
+            self.get_logger().error(
+                f"{tag}: 让完余量重试仍然没走到位（{ev}）—— 中止"
+                f"（不是限位问题，先查是不是有东西卡住/螺纹咬死）"
+            )
+            return Event.ABORT
+        return fsm_event
+
     # =====================================================================
     # S02 夹持确认（预留）
     # =====================================================================
@@ -1200,11 +1452,15 @@ class GaugeAssembly(AssemblyPipeline):
         ns = self.cfg.coarse_camera
         he = self._eye_to_hand_file()
         if he is None:
+            expect = str(
+                self.get_parameter("coarse_eye_to_hand_file").value or ""
+            ).strip() or f"handeye/results/{ns}_eye_to_hand.yaml"
             self.get_logger().warn(
                 "S03 需要手外相机的 eye-to-hand 标定文件，现在没有 —— "
                 f"按兜底逻辑回 S04 跟踪。\n"
-                f"  相机 {ns} 的标定产物应放在 handeye/results/<名>_eye_to_hand.yaml；\n"
-                f"  做法：bash scripts/run_handeye.sh --mode eye_to_hand（标完再跑本流程）"
+                f"  应该生成：{expect}\n"
+                f"  做法：bash scripts/run_handeye.sh --mode eye_to_hand "
+                f"teach/merge/calibrate（标完再跑本流程）"
             )
             return Event.COARSE_FAIL
         if not self._wait_coarse_frame(ns):
@@ -1354,7 +1610,9 @@ class GaugeAssembly(AssemblyPipeline):
             return Event.TIMEOUT
         code, q, Tee, final = self._run_locate(q, Tee)
         if code != 0 or not final:
-            self.get_logger().error(f"S05 精拍定位失败 code={code}")
+            self.get_logger().error(
+                f"S05 精拍定位失败 code={code}{_LOCATE_CODE_HINT.get(code, '')}"
+            )
             return Event.TIMEOUT
         self._snap_final = dict(final)
         self._snap_pose = Tee.copy()
@@ -1535,6 +1793,9 @@ class GaugeAssembly(AssemblyPipeline):
         z0 = float(Tee[2, 3])
         step_m = self.cfg.contact_step_mm / 1000.0
         max_depth_m = self.cfg.contact_max_depth_mm / 1000.0
+        # 台肩落面所需的下压量。在下面几何自检里算出来；**要留到接触判据那里用**
+        # （看 hole_entry_verdict：下压深度远小于它 ⇒ 规没进孔）。
+        needed_m: Optional[float] = None
 
         # ---- 工具几何自检：预算必须是**由几何推出来的**，不能拍 ------------------
         # 判"到底了"的那一下是**台肩落到工件上表面**（不是尖端碰面）。工件上表面
@@ -1663,6 +1924,11 @@ class GaugeAssembly(AssemblyPipeline):
                         f"{ft_ref:+.2f}N 超过 +{self.cfg.contact_axial_delta_n:.1f}N"
                         f"（下压 {abs(float(Tee[2, 3]) - z0) * 1000:.2f}mm）"
                     )
+                    # 下压这么浅就"接触" ⇒ 尖端顶在工件上表面，没进孔。必须在这里
+                    # 拦下：放它进 S08 只会干转几轮、憋住，最后报一句"转轴偏差"
+                    # 把所有人引去查工具系。见 hole_entry_verdict 的说明。
+                    if self._hole_lost_guard(z0, Tee, needed_m, q, "腕部 FT"):
+                        return Event.HOLE_LOST
                     # 必须先卸载再交给 S08 —— 判到接触的这一刻臂上顶着的是**出力上限**
                     # 那个量级的力（实测 200N+，与阈值无关），而规的轴向传力上限
                     # 只有 µ·N≈73N。见 _relieve_after_contact 的完整说明。
@@ -1709,6 +1975,9 @@ class GaugeAssembly(AssemblyPipeline):
                     f"（法向 {fn:.1f}，下压 {abs(float(Tee[2, 3]) - z0) * 1000:.2f}mm）"
                 )
                 # 与腕部 FT 那条同规矩：判到接触时臂上顶着的力远大于阈值，必须先卸载。
+                # 同样先过"是不是根本没进孔"这道闸（见 hole_entry_verdict）。
+                if self._hole_lost_guard(z0, Tee, needed_m, q, "触觉切向"):
+                    return Event.HOLE_LOST
                 self._relieve_after_contact(q, Tee, "触觉切向", ft_ref)
                 return Event.CONTACT_OK
             if fn - self._fn_rest > self.cfg.contact_safe_delta:
@@ -1745,6 +2014,47 @@ class GaugeAssembly(AssemblyPipeline):
         self._retract(q, Tee, dz_m=0.01)
         return Event.TIMEOUT
 
+    def _hole_lost_guard(
+        self,
+        z0: float,
+        Tee: np.ndarray,
+        needed_m: Optional[float],
+        q: np.ndarray,
+        why: str,
+    ) -> bool:
+        """S07 判到接触时：下压深度是不是**远小于**几何预期（= 规没进孔）。
+
+        返回 True 表示"确实没进孔"，调用方要抬臂并返回 ``Event.HOLE_LOST``。
+
+        完整来龙去脉在 ``gauge_fsm.hole_entry_verdict`` 的 docstring 里，这里只
+        留现场要看的那几句：工件在 S08/S09/S10 会被工具推走，而 k2 的孔位是
+        S05 一次性定的 ⇒ 工件一动孔位就失效 ⇒ 规落在孔旁边的平面上。
+        不拦的话会干转 5 轮，最后报一句"转轴偏差…先确认工具系有没有装转接件"，
+        把所有人引去查工具系（实测 2026-09-28）。
+        """
+        if needed_m is None:
+            # 没算出几何预期（工件上表面 z 没标定且 S05 也没给）—— 没有判据就别猜。
+            return False
+        descended_m = abs(float(Tee[2, 3]) - z0)
+        if hole_entry_verdict(descended_m, needed_m) != ENTRY_HIGH:
+            return False
+        self.get_logger().error(
+            f"S07 接触（{why}）发生在下压 {descended_m * 1000:.2f}mm 处，"
+            f"而按几何「台肩落面」需要 {needed_m * 1000:.1f}mm —— 只走了 "
+            f"{descended_m / needed_m * 100.0:.0f}%。\n"
+            "  **规没有进孔，尖端顶在工件上表面**（正常入孔要压到台肩落面那一下）。\n"
+            "  最可能的原因：工件被工具推走了，而本孔的坐标是 **S05 一次性定下**的 ——\n"
+            "  工件一动，那个坐标就失效（S06 的「实测残差」发现不了：它比的是\n"
+            "  「命令位置 vs 实际位置」，不是「规尖 vs 真实孔位」）。\n"
+            "  所以**不要**去查工具系/转接件/力阈值（那几个都是好的）。\n"
+            "  正确处置：把臂退开、确认工件的实际位置；能复位就复位工件，然后\n"
+            "  从 S05 重新拍照定位（重跑流程即会重新定位）。\n"
+            "  若要治根：换孔前复核工件没动（当前只有 S05 阶段有 drag 看门狗）。\n"
+            "  抬起并中止，不带着失效的孔位进 S08。"
+        )
+        self._retract(q, Tee, dz_m=0.01)
+        return True
+
     def _relieve_after_contact(
         self,
         q: np.ndarray,
@@ -1879,6 +2189,29 @@ class GaugeAssembly(AssemblyPipeline):
     # =====================================================================
     # S08 旋转装配循环（棘轮）
     # =====================================================================
+    def _log_regrip(self, tag: str) -> None:
+        """把"这一轮夹紧后的开口"打出来（含相对夹住时的塌陷量）。
+
+        为什么每轮都要打（2026-09-24 实测）：
+            S08/S09 每轮都是"慢转 → 松爪 → 回转 → 夹紧"，而 `close` 在仿真里
+            是"位置控制走到全合 0mm" —— **真机走到力上限就停，仿真会一路挤进规里**。
+            只报一句"通止规掉了(E1)"时，人分不出"真掉了"和"被挤小了"。
+            把开口与塌陷量打出来一眼就能分辨：
+                · 塌陷量逐轮变大、开口还在十几毫米 → 是**被挤**（规还在指间）
+                · 开口塌到接近 0（两指合到底）        → 是**真掉了**
+        """
+        if self.grip is None:
+            return
+        ap = self.grip.settle(settle_s=1.0)   # 服务返回时动作还没做完，等它停
+        if ap is None:
+            return
+        gp = self.grip.grip_aperture()
+        col = self.grip.collapse_mm()
+        detail = ""
+        if gp is not None and col is not None:
+            detail = f"（夹住时 {gp:.2f}mm，塌陷 {col:+.2f}mm）"
+        self.get_logger().info(f"{tag}：开口 {ap:.2f}mm{detail}")
+
     def _step_screw_in(self) -> Event:
         hole = self._hole_name()
         self._state(f"S08_screw_in_{hole}")
@@ -1895,6 +2228,20 @@ class GaugeAssembly(AssemblyPipeline):
             f"停止判据「切向 >+{self.cfg.screw_shear_delta:.0f} "
             f"且法向 >+{self.cfg.screw_normal_delta:.0f}」"
         )
+        # 步长 vs 关节行程：**在这里算一次**，不要等第 3 轮贴限位再炸。
+        # 2026-09-24 实测：180° 的步长配 ±175° 的关节 7，第 3 轮 ABORT，
+        # 日志是一长串"贴限位 → 让余量 → 还是贴限位"，真因（配置）藏在最后。
+        travel = ratchet_travel_ok(
+            self.cfg.screw_step_deg, self._wait_joint_limits(), RATCHET_LIMIT_MARGIN_DEG
+        )
+        self.get_logger().info(
+            f"S08 行程核算：一步棘轮要 {travel.need_deg:.0f}°，"
+            f"关节7 行程 {travel.travel_deg:.0f}°"
+            f"（最大可用步长 {travel.max_step_deg:.0f}°）—— {'放得下' if travel.ok else '放不下'}"
+        )
+        if not travel.ok:
+            self.get_logger().error(f"S08 配置自检不通过（先改配置，别硬跑）：{travel.why}")
+            return Event.ABORT
         for cycle in range(self.cfg.screw_max_cycles):
             q, Tee = self._wait_state(5.0)
             if q is None or Tee is None:
@@ -1909,9 +2256,10 @@ class GaugeAssembly(AssemblyPipeline):
             # --- 四步原子动作，顺序固定（慢转 → 松爪 → 快回转 → 夹紧）---
             # 慢转是**顺时针**：与 S01 的逆时针预绕相反，这样预绕行程就是
             # 这一整段拧入的回转空间（S01 不预绕，转两圈就贴限位）。
-            ev = self._rotate_joint_and_verify(
+            ev = self._ratchet_step(
                 float(q[-1]) - self.cfg.screw_step_rad, tag=f"S08_slow_{cycle}",
                 fsm_event=Event.SCREW_DONE, speed_deg_s=self.cfg.screw_speed_deg_s,
+                direction=-1,   # 顺时针拧入（与 S01 的预绕相反）
             )
             if ev is not Event.SCREW_DONE:
                 return ev
@@ -1924,15 +2272,17 @@ class GaugeAssembly(AssemblyPipeline):
                 q, Tee = self._wait_state(3.0)
                 if q is None:
                     return Event.TIMEOUT
-                ev = self._rotate_joint_and_verify(
+                ev = self._ratchet_step(
                     float(q[-1]) + self.cfg.screw_step_rad, tag=f"S08_back_{cycle}",
                     fsm_event=Event.SCREW_DONE, speed_deg_s=self.cfg.screw_back_speed_deg_s,
+                    direction=+1,   # 回转：与慢转正好相反
                 )
                 if ev is not Event.SCREW_DONE:
                     return ev
                 if not self.grip.close():
                     self.get_logger().error("S08 夹紧失败 —— 下一轮慢转会打滑磨螺纹")
                     return Event.ABORT
+                self._log_regrip(f"S08 第 {cycle + 1} 轮夹紧后")
             else:
                 self.get_logger().error("S08 没有夹爪客户端，无法松爪/夹紧，中止")
                 return Event.ABORT
@@ -1961,9 +2311,17 @@ class GaugeAssembly(AssemblyPipeline):
                 self._progress(f"{hole} 拧入到位（{cycle + 1} 轮）")
                 return Event.SCREW_STOP
             if self.grip is not None and self.grip.dropped():
+                ap = self.grip.aperture()
+                gp = self.grip.grip_aperture()
                 self.get_logger().error(
                     "S08 通止规掉了（E1）—— 回 S01 流程：张开爪、提示人工放好，"
-                    "确认后继续（按看板：该孔记为完成，换下一个孔）"
+                    "确认后继续（按看板：该孔记为完成，换下一个孔）\n"
+                    f"        开口：夹住时 {gp if gp is None else f'{gp:.2f}mm'}"
+                    f" → 现在 {ap if ap is None else f'{ap:.2f}mm'}"
+                    f"（判据：塌陷 ≥ {GripperClient.DROP_COLLAPSE_MM:.1f}mm）\n"
+                    "        分辨真掉还是被挤：开口塌到接近 0 = 两指合到底（真掉）；\n"
+                    "        开口还有十几毫米、只是比夹住时小了几毫米 = 被**挤**"
+                    "（仿真位置控制的「全合」不知道「挡住就停」，见 GripperClient.dropped）"
                 )
                 self._e1_pending = True
                 return Event.GAUGE_DROPPED
@@ -1994,9 +2352,10 @@ class GaugeAssembly(AssemblyPipeline):
             if q is None:
                 return Event.TIMEOUT
             # 反向：慢转是逆时针（与 S08 相反），空手回转是顺时针
-            ev = self._rotate_joint_and_verify(
+            ev = self._ratchet_step(
                 float(q[-1]) + self.cfg.screw_step_rad, tag=f"S09_slow_{cycle}",
                 fsm_event=Event.UNSCREW_DONE, speed_deg_s=self.cfg.screw_speed_deg_s,
+                direction=+1,   # 逆时针拧出
             )
             if ev is not Event.UNSCREW_DONE:
                 return ev
@@ -2008,17 +2367,64 @@ class GaugeAssembly(AssemblyPipeline):
             q, Tee = self._wait_state(3.0)
             if q is None:
                 return Event.TIMEOUT
-            ev = self._rotate_joint_and_verify(
+            ev = self._ratchet_step(
                 float(q[-1]) - self.cfg.screw_step_rad, tag=f"S09_back_{cycle}",
                 fsm_event=Event.UNSCREW_DONE, speed_deg_s=self.cfg.screw_back_speed_deg_s,
+                direction=-1,   # 回转：与慢转相反
             )
             if ev is not Event.UNSCREW_DONE:
                 return ev
             if not self.grip.close():
                 return Event.ABORT
+            self._log_regrip(f"S09 第 {cycle + 1} 轮夹紧后")
+        self._lift_clear_of_hole(hole)
         self._progress(f"{hole} 已拧出")
         return Event.UNSCREW_DONE
 
+    def _lift_clear_of_hole(self, hole: str) -> None:
+        """拧出后**先竖直把规提出孔**，之后才允许任何横向动作。
+
+        **为什么必须有这一步**（2026-09-24 仿真实测，两次栽在同一个地方）：
+          1. 仿真里**没有螺纹几何**：S09 的"拧出"只是让规在孔里转，规**仍然插在
+             孔里**（真机会沿螺纹退出来几个毫米，也没退出多少）。
+          2. 于是紧跟着的横向动作（S10 换孔 / S11 回 home）会把规从孔里**拽**
+             出来 —— 实测抓取锁被拽断、规掉在台面上，而下游只表现为
+             "下一孔 S07 下探 35mm 判不到接触"（工具上已经没规了），
+             日志完全指不到真因。
+          3. 真机也怕这个动作：螺纹刚脱开时横向别住最容易啃伤螺纹。标准做法就是
+             **先竖直退出来，再走 XY**。
+
+        高度提到**跟踪高**（``_track_z()`` = 孔悬停高 + 50mm）而不是写死毫米数：
+        跟踪高是这条流程里"工具已离开工件"的既有定义（``_ensure_clearance``
+        也用它），拿它当判据不用再维护第二套口径。实测：不提出时规尖比工件面
+        **低 30mm**，而 S10 的斜线只往上抬了 29mm —— 差 1mm 就是"拽出孔"和
+        "干净退出"的分界，所以必须是**先后有序**的两个动作，不能靠斜线一起走。
+
+        失败只告警（不失败）：这一刻孔里已经没有规要保护了，硬失败会把一次成功
+        的装配变成失败；后面的 S10/S11 仍会走到安全位。
+        """
+        try:
+            q, Tee = self._wait_state(3.0)
+            if q is None or Tee is None:
+                return
+            tz = self._track_z()
+            z = float(Tee[2, 3])
+            if z >= tz - 0.008:
+                return
+            self.get_logger().info(
+                f"S09 {hole} 拧出后竖直提出：Z {z:.3f} → {tz:.3f}m"
+                "（先离开孔口，再做横向动作）"
+            )
+            _, q2, _Tee2 = 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=f"S09_exit_{hole}", v=self.move_speed, max_hops=8,
+                hold_xy=True, settle_s=0.25,
+            )
+        except Exception as exc:  # noqa: BLE001
+            self.get_logger().warn(f"S09 {hole} 提出孔口失败（继续走 S10/S11）：{exc}")
+
     # =====================================================================
     # S10 换孔 / S11 复位
     # =====================================================================

build/arm_application/build/lib/arm_application/gauge_config.py

@@ -254,7 +254,9 @@ DEFAULTS: Dict[str, Any] = {
     "contact_hold_n": 5.0,
     "contact_safe_delta": 2000.0,
     # S08
-    "screw_step_deg": 180.0,
+    # 步长必须满足 2×step + 2×余量 ≤ 关节7 行程（Rizon4/10 ±175° ⇒ step ≤ 167°）：
+    # 180° 会必然贴限位（棘轮一个来回要 360° > 350°），实测第 3 轮 ABORT。
+    "screw_step_deg": 120.0,
     "screw_speed_deg_s": 30.0,
     "screw_back_speed_deg_s": 120.0,
     "screw_max_cycles": 12,

build/arm_application/build/lib/arm_application/gauge_fsm.py

@@ -79,6 +79,10 @@ class Event(str, Enum):
 
     CONTACT_OK = "contact_ok"
     FORCE_OVER_LIMIT = "force_over_limit"  # 力超安全上限 → 抬起
+    #: S07 判到接触、但**下压深度远小于几何预期** ⇒ 规没进孔，顶在工件上表面。
+    #: 单列一个事件是为了让结论与日志都是实话：复用 FORCE_OVER_LIMIT 会把这种
+    #: 情况报成"力超安全上限"，而它的力根本没超限 —— 又是一句误导。
+    HOLE_LOST = "hole_lost"
 
     SCREW_DONE = "screw_done"
     SCREW_STOP = "screw_stop"            # 力判据满足：切向力>阈值 且 法向力突增
@@ -158,6 +162,7 @@ TRANSITIONS: Dict[Tuple[State, Event], State] = {
     # --- S07 柔顺接触 ---
     (State.CONTACT, Event.CONTACT_OK): State.SCREW_IN,
     (State.CONTACT, Event.FORCE_OVER_LIMIT): State.ABORT,
+    (State.CONTACT, Event.HOLE_LOST): State.ABORT,
     # --- S08 旋转装配循环 ---
     (State.SCREW_IN, Event.SCREW_DONE): State.UNSCREW,
     (State.SCREW_IN, Event.SCREW_STOP): State.UNSCREW,
@@ -406,6 +411,40 @@ def wrench_live(age_s: Optional[float]) -> bool:
     return age_s is not None and float(age_s) <= WRENCH_MAX_AGE_S
 
 
+#: 判定"六个分量全为 0"用的容差 [N]/[Nm]。真机 FT 的噪声量级是 1e-2~1e-1，
+#: 所以 1e-6 足够区分"真的是 0（数据没出来）"与"很小但活的读数"。
+WRENCH_ZERO_EPS: float = 1.0e-6
+
+
+def wrench_all_zero(
+    fx: float,
+    fy: float,
+    fz: float,
+    tx: float,
+    ty: float,
+    tz: float,
+    eps: float = WRENCH_ZERO_EPS,
+) -> bool:
+    """六个分量**同时**为 0（容差 ``eps``）⇒ 这是"数据没出来"的指纹，不是"安静"。
+
+    与 :func:`wrench_live` 的分工（两个都要有，缺一不可）：
+
+    * ``wrench_live`` 只看"够不够新"，**故意不看数值** —— 静止时轴向力本来就接近 0，
+      拿数值判活会把"安静的传感器"误判成掉线（2026-09-24 触觉上踩过）。
+    * 但一个**带着负载**的腕部 FT 不可能六个分量**同时**恰好为 0：重力、装配预紧、
+      传感器零漂至少让某个分量不为 0。所以"全零"是另一种失效 —— **数据源没出数**
+      （话题名对、但内容是空的；或 RDK 侧没启用该传感）。
+
+    2026-10-08 真机实测：应用在用的 ``/<sn>/ft_sensor_wrench`` 就是恒 (0,0,0)，
+    于是"``abs(fz−基线)>2.5N`` 判接触"的兜底**永远触发不了** —— 而日志里一切正常
+    （话题收得到、回调在跑、``wrench_live`` 报"活"），属于最危险的静默失效。
+    改用 ``/<sn>/external_wrench_in_tcp`` 后有数（≈负载 54N）即可恢复兜底。
+
+    所以：``wrench_live`` 通过 **且** ``wrench_all_zero`` 为真 ⇒ 判为"数据源是空的"。
+    """
+    return all(abs(float(v)) <= eps for v in (fx, fy, fz, tx, ty, tz))
+
+
 #: S07 单步下压给 ``_goto`` 的到位容差 = 步长的这个比例。
 #: **必须 < 1，而且只能小不能大**：``_goto`` 的语义是"已经落在容差里就直接返回、
 #: 一点都不动"。容差 ≥ 步长时，每一次"往下走 0.5mm"的请求都被判成"已在容差内"
@@ -513,6 +552,209 @@ def tool_clearance_m(
     return tip_z - float(surface_z_m)
 
 
+def hover_z_for_tool_clearance(
+    surface_z_m: float,
+    tool_tip_below_flange_m: float,
+    clearance_m: float,
+) -> float:
+    """让工具尖端停在工件面上方 ``clearance_m`` 所需的**法兰**高度 [m]。
+
+    就是 ``tool_clearance_m`` 的逆运算 —— 写成一个函数是为了让"该用多高"这个
+    结论只有一个来源（下面 ``hover_z_guard`` 与调用方都用它，不各推一遍）。
+    """
+    return float(surface_z_m) + float(tool_tip_below_flange_m) + float(clearance_m)
+
+
+def hover_z_guard(
+    hover_z_m: float,
+    surface_z_m: float,
+    tool_tip_below_flange_m: float,
+    want_clearance_m: float,
+) -> Tuple[float, str]:
+    """二阶段悬停高度的自检 → ``(该用的悬停高度, 说明; 不用改时说明是空串)``。
+
+    **为什么必须有这条**（2026-09-24 实测栽在这上面）：
+
+        悬停高度是**按相机**定的（腕部相机要离工件面 ~0.17m 才看得清孔），
+        而工具（夹爪 + 通止规）可能比相机长得多 —— 仿真里实测
+        ``tool_tip_below_flange_m=0.245``，而悬停算的是
+        ``0.412 + 0.17 + 0.05 = 0.632`` ⇒ 工具尖端在工件上表面**下面 25mm**。
+
+        后果不在 S07（S07 有自己的几何自检），而在**更早的 S05**：
+        "悬停"这一步本身就把工具捅进工件里了，于是 S05 的横向对准等于
+        拿规尖去拨工件 —— 实测把工件从 x=0.520 拖到 0.4284（**92mm**）。
+        而真值孔位是按工件的**实测**位姿算的（``sim_live_pose``，这是对的），
+        工件一走，孔位就跟着工具走 ⇒ 残差恒定在 13mm、迭代 9 次永不收敛
+        （日志：``[fine 3..8] 残差=13.25/13.19/…/11.79mm``）。
+        现象看起来像"手眼标定坏了 / 检测不认孔"。
+
+    真机同参数同结果：这不是仿真专有，只要"悬停按相机定、工具比相机长"就会这样。
+
+    判据取"明显低于目标间隙"而不是"一毫米都不许低"：配置的悬停与实测的
+    工件面 z 都有几毫米的不确定度，拿 0 当界会把正常配置判成错的。
+    """
+    clear_m = tool_clearance_m(hover_z_m, tool_tip_below_flange_m, surface_z_m)
+    if clear_m >= float(want_clearance_m) - 0.001:
+        return float(hover_z_m), ""
+    need_z = hover_z_for_tool_clearance(surface_z_m, tool_tip_below_flange_m, want_clearance_m)
+    why = (
+        f"悬停高度会让工具尖端钻进工件：法兰 {float(hover_z_m):.4f} − 工具长 "
+        f"{float(tool_tip_below_flange_m) * 1000:.1f}mm = 尖端 "
+        f"{float(hover_z_m) - float(tool_tip_below_flange_m):.4f}，工件上表面 "
+        f"{float(surface_z_m):.4f} → 尖端在面{'下' if clear_m < 0 else '上仅'} "
+        f"{abs(clear_m) * 1000:.1f}mm（要求 ≥ {float(want_clearance_m) * 1000:.1f}mm）。\n"
+        f"        悬停高度通常是按**相机**视距定的，而工具比相机长 —— 两者冲突时必须"
+        f"先保证不碰工件。\n"
+        f"        已按几何抬到 {need_z:.4f}（= {float(surface_z_m):.3f} 上表面 + "
+        f"{float(tool_tip_below_flange_m) * 1000:.0f}mm 工具 + "
+        f"{float(want_clearance_m) * 1000:.1f}mm 间隙）。\n"
+        f"        要固定这个值：改 site.yaml 的 robots[<sn>].servo.hole_hover_z_m = "
+        f"{need_z:.3f}（以及同组件的 ee_z_m ≥ 它 +0.05）。"
+    )
+    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 的说明）：
+#       S05 的对准是"看着孔走"的闭环，而孔位是**工件上**的。工具一旦碰到工件，
+#       闭环追的就不再是孔，而是"自己推着走的工件" —— 残差恒定（本轮 11mm）、
+#       迭代用满，日志报的是"检查手眼/相机安装"，与真因（工具比相机长、
+#       悬停时已经插进工件）差着十万八千里。
+#   取 3mm：工件是刚体，两拍之间它整体挪动 >3mm 只可能是被推/被撞（正常加工
+#   过程里它一动不动）。同一对孔的两拍测量噪声在 0.2mm 量级，离阈值很远。
+SNAP_DRAG_TOL_M: float = 0.003
+#: 单拍就这么大 ⇒ 不必等第二拍确认（刚体不会自己"挪回去"）。
+SNAP_DRAG_HARD_M: float = 0.006
+
+
+#: "已经到位"的容差：避免 ±1e-9 的浮点噪声变成"再转一整圈"。
+_RATCHET_FULL_TURN_EPS: float = 1e-9
+
+
+def ratchet_delta_rad(target_rad: float, q_now_rad: float, direction: int = 0) -> float:
+    """"目标角 − 当前角"，折成**一次**能转到的增量 [rad]。
+
+    **为什么要有这个函数**（2026-09-24 实测：棘轮把整条流程 ABORT 在 S09 第一步）：
+
+        S08/S09 给棘轮的是**关节 7 的目标角（绝对值）**（``q[-1] ± step``），
+        而棘轮此前把它当**增量**用。于是"关节在 +158.9°、目标 +338.9°"被算成
+        "要转 338.9°"（实际增量只有 180°）⇒ 误判行程不够 ⇒ 让余量那一步算到
+        限位外面（``158.9 − 338.9 = −180°``，比 −175° 还低）⇒ 报
+        "期望 −338.9°、实测 +26.0°，偏差过大" ⇒ ABORT。现场什么坏事都没发生。
+
+        差值必须折一圈：目标常常**故意**超出限位（这正是要"让余量"的原因），
+        ``+338.9°`` 与 ``−21.1°`` 是同一个关节位置。
+
+    **为什么还要 direction**（2026-09-24 又一次实测：S08 第 3 轮 ABORT）：
+
+        不给方向时这里取"最短的那条路"，而名义步长**正好是 180°** 时，
+        哪条路更短取决于 0.1° 级的读数差：调用方按自己读到的 ``q`` 算出目标
+        ``−18.0°``，棘轮再用**稍晚一点**读到的 ``q=+162.1°`` 去算差值，
+        ``−180.1°`` 折一圈就成了 ``+179.9°`` —— 方向**翻了个个儿**。
+        于是棘轮朝螺纹的反方向走，接着贴限位、让余量、重试，净位移 0 ⇒ ABORT。
+
+        棘轮的转向是**工艺意图**（S08 顺时针拧入、S09 逆时针拧出），不能交给
+        "谁更短"这种数值巧合决定，所以由调用方明确给出 ``direction``。
+
+        注：``direction=0``（默认）保留旧的"最短路径"语义，给"目标本来就明确
+        在某一侧"的调用方用；棘轮的四个原子步一律显式给方向。
+
+    :param target_rad: 目标角（绝对值，可以是限位外的角度）。
+    :param q_now_rad: 当前关节角。
+    :param direction: 想要的转向：``+1`` 正转、``-1`` 反转、``0`` 取最短路径。
+    :returns: 增量 [rad]。``direction=0`` 时落在 ``(−π, π]``；给了方向时落在
+        ``[0, 2π)``（正转）或 ``(−2π, 0]``（反转）——**一定**沿指定方向，
+        且是能落到目标角的最小那一步。**同一个目标、同一个当前位置**只会有一个
+        增量 —— 棘轮的"让余量"与"重试"都按它算，两次不会互相矛盾。
+    """
+    base = float(target_rad) - float(q_now_rad)
+    two_pi = 2.0 * math.pi
+    if direction == 0:
+        return math.remainder(base, two_pi)
+    # 先折到 [0, 2π)，再按想要的转向取那一侧（与目标角差正好一整圈，是同一个位置）。
+    pos = math.fmod(base, two_pi)
+    if pos < 0.0:
+        pos += two_pi
+    if pos >= two_pi - _RATCHET_FULL_TURN_EPS:
+        pos = 0.0  # 已经（几乎）在目标上：不要为了凑方向白转一整圈
+    if direction > 0:
+        return pos
+    return pos - two_pi if pos > 0.0 else 0.0
+
+
+
+def drag_verdict(
+    mid_now: Tuple[float, float],
+    mid_prev: Optional[Tuple[float, float]],
+    mid_prev2: Optional[Tuple[float, float]] = None,
+    *,
+    tol_m: float = SNAP_DRAG_TOL_M,
+    hard_m: float = SNAP_DRAG_HARD_M,
+) -> Tuple[bool, float]:
+    """两孔中点是否在两拍之间"整体挪动" → ``(是否判定为被推走, 本拍位移 m)``。
+
+    **为什么看"中点"而不是看目标孔**：目标孔是**追**的对象，工具一动它就该动
+    （对准本意如此），看它分不出"正常对准"和"推着工件走"。而两孔**中点**在
+    对准过程中不该动 —— 它只有工件整体平移时才会动。所以中点位移干净地对应
+    "工件被搬动了"。
+
+    **为什么看"两孔中点"而不是单孔**：单孔在相机里会因分辨率/遮挡抖动，且
+    目标孔本来就在被逼近；中点是两者的平均，噪声更小、语义更明确。
+
+    :param mid_now: 本拍两孔中点 (x, y)[m]（基座系）。
+    :param mid_prev: 上一拍中点；``None`` = 还没有上一拍（第一拍不判）。
+    :param mid_prev2: 上上拍中点。用来确认"位移是持续的"（单拍可能是误检）。
+    :returns: ``(判定, 本拍位移 m)``。
+    """
+    if mid_prev is None:
+        return False, 0.0
+    dx = float(mid_now[0]) - float(mid_prev[0])
+    dy = float(mid_now[1]) - float(mid_prev[1])
+    d1 = math.hypot(dx, dy)
+    if d1 >= float(hard_m):
+        return True, d1
+    if d1 < float(tol_m) or mid_prev2 is None:
+        return False, d1
+    px = float(mid_prev[0]) - float(mid_prev2[0])
+    py = float(mid_prev[1]) - float(mid_prev2[1])
+    d2 = math.hypot(px, py)
+    # 第二拍确认：上一拍也挪了（≥ 阈值的一半），且方向一致（点积 > 0）。
+    # 为什么允许方向只"大致一致"：工件被推着走时前后两拍的推动量本来就不同
+    # （工件一让开，工具就少推一点），拿夹角严格相等会漏判。
+    if d2 >= float(tol_m) * 0.5 and (dx * px + dy * py) > 0.0:
+        return True, d1
+    return False, d1
+
+
 def descend_needed_m(
     flange_z_m: float,
     tool_tip_below_flange_m: float,
@@ -555,6 +797,58 @@ def descend_budget_m(needed_m: float) -> float:
     return max(float(needed_m) * DESCEND_BUDGET_FRAC, DESCEND_BUDGET_FLOOR_M)
 
 
+#: ``hole_entry_verdict`` 的结论。
+ENTRY_OK = "ok"        # 接触深度与几何预期相符 ⇒ 规是从孔里下去的、台肩落面
+ENTRY_HIGH = "high"    # 早早就接触 ⇒ 尖端顶在工件**上表面**上，根本没进孔
+
+#: 判"没进孔"的下压深度比例阈值。
+#: 实测两个锚点（2026-09-28，仿真）：
+#:   · 正常入孔：下压 28.6mm / 需要 31.9mm ⇒ 0.90
+#:   · 顶在平面上：下压 0.90mm / 需要 31.9mm ⇒ 0.03
+#: 两者差着一个台肩高度（30mm），取 0.5 时两侧余量都很大（正常那侧要掉到
+#: 一半才会误判）。**不要**为了"更灵敏"把它往 0.9 推 —— 台肩倒角没完全落面、
+#: 板厚公差都会让正常值在 0.85~0.95 之间晃。
+HOLE_ENTRY_MIN_RATIO: float = 0.5
+
+
+def hole_entry_verdict(
+    descended_m: float,
+    needed_m: float,
+    *,
+    min_ratio: float = HOLE_ENTRY_MIN_RATIO,
+) -> str:
+    """S07 判到接触的那一刻：规是**进了孔**、还是**顶在工件上表面**。
+
+    为什么需要它（2026-09-28 实测，同一命令三跑里坏一跑：DONE / ABORT / DONE）：
+      工件在 S08/S09/S10 阶段会被工具推走（仿真里实测被推 20~52mm）。而 k2 的
+      孔位是 **S05 一次性定下**的（``_step_hover_k`` 取 ``_hole_xy``），工件一动
+      它就失效。于是规落在孔**旁边的平面**上，只下压「悬停间隙」那么点就判到接触。
+
+    不拦的代价，实测完整发生过一次：
+      S08 干转 5 轮（切向增量 0.0、法向增量 ±0.0，**一点没进给**）→ 第 6 轮憋住 →
+      报出来的是「转轴偏差 7.6° 超过阈值 —— 关节7 轴与工具 Z 不共线，
+      先确认工具系是否垂直向下、有没有装转接件」。
+
+      **那句话会把所有人引到错误方向**：真因既不是工具系也不是转接件，是孔位失效。
+      而且为这句误导，白烧了 5 轮无效拧入（≈30 秒）和一次憋劲（有把规顶掉的风险）。
+
+    为什么比 ``needed_m`` 而不是比"尖端离面高度"：正常入孔时接触发生在**台肩落面**
+    那一刻 ≈ ``needed_m``；顶在平面上时接触发生在**尖端刚碰到面**那一刻，而台肩还在
+    尖端上方 30mm。两者差着一个台肩高度，所以用比例判最稳（见上面两个实测锚点）。
+
+    纯函数（R5.1）：判错两个方向都贵 —— 把正常的判成"没进孔"会白扔一整个孔；
+    把"没进孔"判成正常，就是上面那 30 秒 + 一次误导。
+
+    ``needed_m <= 0`` 时**不判**（几何本身已经错了，上面那条自检会报），
+    返回 ``ENTRY_OK`` 不在这里抢着报第二个错。
+    """
+    if needed_m <= 0.0:
+        return ENTRY_OK
+    if float(descended_m) < float(needed_m) * float(min_ratio):
+        return ENTRY_HIGH
+    return ENTRY_OK
+
+
 def screw_stop(normal_delta: float, shear_max: float, normal_th: float, shear_th: float) -> bool:
     """S08：旋转装配的停止判据 —— **切向力>阈值 且 法向力突增**。
 
@@ -584,6 +878,115 @@ def ratchet_cycles(total_deg: float, step_deg: float) -> int:
     return int(math.ceil(total_deg / step_deg))
 
 
+@dataclass(frozen=True)
+class RatchetTravel:
+    """棘轮步长与关节行程的**可用性核算**（S08 开头自检，别等到第 3 轮才炸）。"""
+
+    ok: bool
+    step_deg: float          # 配置的一步转角
+    travel_deg: float        # 关节行程（上限 − 下限）
+    need_deg: float          # 一步棘轮真正需要的行程
+    max_step_deg: float      # 行程允许的最大步长
+    why: str                 # 不 ok 时给人看的说明（ok 时为空串）
+
+
+def ratchet_travel_ok(
+    step_deg: float, limits_rad: Optional[Tuple[float, float]], margin_deg: float
+) -> RatchetTravel:
+    """一步棘轮 ``step_deg`` 放得进关节行程吗？
+
+    **为什么需要**：2026-09-24 实测，``screw_step_deg=180`` 配 Rizon4 的
+    关节 7（±175°，行程 350°）在第 3 轮 ABORT —— 慢转那一步要转 180°、
+    回转那一步还要 180°，两次加起来 360° > 350°，关节**必然**有一头贴限位；
+    更糟的是"让余量"也腾不出地方（要腾就得往反向再让一整步，还是 360°）。
+    日志里那一长串"贴限位 → 让余量 → 还是贴限位"就是这么来的。
+
+    判据取 ``2×step + 2×margin ≤ 行程``：
+    - 2×step：棘轮一个来回（慢转 + 回转）要的行程；
+    - 2×margin：两头都要留余量 —— 贴死限位再让余量时**一点空间都没有**，
+      而实测每轮回转还会比指令少走 4~5°（速度/加减速限制），
+      这点亏欠要靠余量吃，否则 7~8 轮后累计偏出限位、再也转不回来。
+
+    :param limits_rad: ``(lower, upper)`` [rad]；``None`` = 拿不到限位，放行
+        （返回 ``ok=True``，理由写清楚：真机靠驱动器报 JOINT_LIMIT 兜底）。
+    """
+    need = 2.0 * float(step_deg) + 2.0 * float(margin_deg)
+    if limits_rad is None:
+        return RatchetTravel(
+            ok=True, step_deg=float(step_deg), travel_deg=float("inf"),
+            need_deg=need, max_step_deg=float("inf"),
+            why="",
+        )
+    travel = math.degrees(float(limits_rad[1]) - float(limits_rad[0]))
+    max_step = 0.5 * (travel - 2.0 * float(margin_deg))
+    if need <= travel:
+        return RatchetTravel(
+            ok=True, step_deg=float(step_deg), travel_deg=travel,
+            need_deg=need, max_step_deg=max_step, why="",
+        )
+    # 给个能直接照着改的步长：往整十度取整（现场习惯填 120/150/180），
+    # 但不许把步长压到 30° 以下 —— 那样拧进/拧出的轮数会多到失控。
+    suggest = max(30.0, math.floor(max_step / 10.0) * 10.0)
+    why = (
+        f"棘轮一步 {step_deg:.0f}° 放不进关节 7 的行程：一步棘轮要 "
+        f"2×{step_deg:.0f}° + 2×{margin_deg:.0f}°(余量) = {need:.0f}°，"
+        f"而行程只有 {travel:.0f}°（上限 {math.degrees(limits_rad[1]):.0f}° − "
+        f"下限 {math.degrees(limits_rad[0]):.0f}°）。\n"
+        f"        改法：site.yaml 的 gauge_assembly.screw_step_deg 改到 "
+        f"{max_step:.0f}° 以内（例如 {suggest:.0f}°），并保证 "
+        f"screw_max_cycles × screw_step_deg 仍是 **360° 的整数倍**（"
+        f"累计必须转整数圈，否则规停在螺纹中间、S09 也就少拧出几分之一圈）。"
+    )
+    return RatchetTravel(
+        ok=False, step_deg=float(step_deg), travel_deg=travel,
+        need_deg=need, max_step_deg=max_step, why=why,
+    )
+
+
+#: ``rotation_verdict`` 的结论。
+ROT_OK = "ok"
+ROT_LIMIT = "limit"   # 转了一部分、方向对、但没走完 ⇒ 最可能是贴到关节限位
+ROT_AXIS = "axis"     # 转轴与工具 Z 不共线 ⇒ 以「度」为单位的一切动作都不成立
+ROT_SHORT = "short"   # 其它偏差（方向不对/过冲）⇒ 真有问题，该停
+
+
+def rotation_verdict(
+    delta_deg: float,
+    got_deg: float,
+    mis_deg: float,
+    tol_deg: float,
+) -> str:
+    """绕工具 Z 转 ``delta_deg``、实测转 ``got_deg`` 的验收判据。
+
+    **纯函数**：这条判据 2026-09-24 把整条 S09 判死过一次
+    （"期望 +180.0°、实测 +16.4°，偏差过大" → ABORT），而真因只是
+    **关节7 的行程被前面的棘轮回转顶到了上限**（±175°，而棘轮一步就要 180°）。
+    判错两个方向都贵：
+      * 把"贴限位"判成"short" ⇒ 白扔一整轮（还可能把规撞歪）；
+      * 把"真有问题"判成"limit" ⇒ 该停的没停，继续拧。
+
+    还有一条**曾经漏判**的：只比绝对值时，"让它 +180、它 -170"会算成
+    10° 误差、落在 10% 容差里 ⇒ 判成功，而实际上一个孔都没拧。所以方向必须比。
+    所以它必须能脱离节点被钉住。
+    """
+    if abs(got_deg) < 3.0 and abs(delta_deg) > 10.0:
+        return ROT_LIMIT          # 几乎没动：与下面的"只走了一部分"同因不同余量
+    if mis_deg > tol_deg:
+        return ROT_AXIS
+    if abs(got_deg) >= 3.0 and abs(delta_deg) >= 3.0 and got_deg * delta_deg < 0:
+        # 方向反了。这条以前**漏判**：老判据只比绝对值
+        # （``abs(abs(got) - abs(delta))``），所以"让它 +180、它 -170"会算 10° 误差
+        # 落在 10% 容差里 → 判成功，而实际上一个孔都没拧。
+        return ROT_SHORT
+    slack = max(tol_deg, 0.1 * abs(delta_deg))
+    short = abs(delta_deg) - abs(got_deg)
+    if short > slack:
+        return ROT_LIMIT          # 方向对、只是没走完 ⇒ 被硬止点截住
+    if short < -slack:
+        return ROT_SHORT          # 过冲：转多了，同样不该当成到位
+    return ROT_OK
+
+
 def is_joint_limit_error(text: str) -> bool:
     """S01：把驱动/规划报的字符串认成"撞到关节极限"。
 
@@ -646,6 +1049,7 @@ EVENT_DOC: Dict[Event, str] = {
     Event.ALIGN_OUT: "对位超差",
     Event.CONTACT_OK: "已接触工件面",
     Event.FORCE_OVER_LIMIT: "力超安全上限",
+    Event.HOLE_LOST: "孔位失效（规没进孔）",
     Event.SCREW_DONE: "达到目标圈数",
     Event.SCREW_STOP: "力判据满足（咬住螺纹）",
     Event.GAUGE_DROPPED: "通止规掉了",

build/arm_application/build/lib/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`` 转了多少度"（带符号）。
 

build/arm_application/build/lib/arm_application/goto_ee_plane.py

@@ -39,6 +39,8 @@ from arm_application.site_io import (
     joint_prefix_default,
 )
 
+from arm_application.ros_node import TolerantNode
+
 
 
 def _load_ee_cfg() -> dict:
@@ -78,7 +80,7 @@ def _pt(q: np.ndarray, qdot: np.ndarray, t_sec: float) -> JointTrajectoryPoint:
     return pt
 
 
-class GotoEePlane(Node):
+class GotoEePlane(TolerantNode):
     def __init__(self, cfg: dict) -> None:
         super().__init__("goto_ee_plane")
         self.cfg = cfg

build/arm_application/build/lib/arm_application/handeye_calibrate.py

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

build/arm_application/build/lib/arm_application/handeye_lib.py

@@ -126,6 +126,39 @@ def Rt_to_T(R: np.ndarray, t: np.ndarray) -> np.ndarray:
     return T
 
 
+def cam_to_base_T(
+    T_handeye: np.ndarray, T_ee: np.ndarray, mode: str
+) -> np.ndarray:
+    """相机系 → 基座系的 4×4。
+
+    * ``eye_in_hand``：``T_handeye`` 是 cam→法兰，再左乘 ``T_ee``（base→法兰）。
+    * ``eye_to_hand``：``T_handeye`` **已经是** cam→基座。再乘 ``T_ee`` 会把
+      固定相机错挂到法兰上 —— 定位总能到附近、永远差一截，看起来像标定不准。
+    """
+    T_he = np.asarray(T_handeye, dtype=float).reshape(4, 4)
+    tag = str(mode or "").strip().lower()
+    if tag == "eye_to_hand":
+        return T_he
+    if tag != "eye_in_hand":
+        raise ValueError(
+            f"手眼模式只能是 eye_in_hand 或 eye_to_hand，收到 {mode!r}"
+        )
+    return np.asarray(T_ee, dtype=float).reshape(4, 4) @ T_he
+
+
+def cam_point_to_base(
+    p_cam: np.ndarray,
+    T_handeye: np.ndarray,
+    T_ee: np.ndarray,
+    mode: str,
+) -> np.ndarray:
+    """工件三维点（相机系）→ 基座系 XYZ。"""
+    T_bc = cam_to_base_T(T_handeye, T_ee, mode)
+    xyz = np.asarray(p_cam, dtype=float).reshape(3)
+    ph = np.array([float(xyz[0]), float(xyz[1]), float(xyz[2]), 1.0], dtype=float)
+    return (T_bc @ ph)[:3]
+
+
 def invert_Rt(R: np.ndarray, t: np.ndarray) -> Tuple[np.ndarray, np.ndarray]:
     Ri = R.T
     ti = -Ri @ t.reshape(3, 1)
@@ -309,6 +342,38 @@ def calibrate_hand_eye(
     return calibrate_hand_eye_tsai(Rg, tg, R_target2cam, t_target2cam)
 
 
+def T_from_flexiv_tcp(xyz_m, qwxyz) -> np.ndarray:
+    """Studio ``tcp_location`` → 法兰→工具 4×4。
+
+    Flexiv 顺序是 ``[x, y, z, q_w, q_x, q_y, q_z]``，四元数是 wxyz，
+    和本仓库其余地方的 xyzw 不一样。只在这里换一次。
+    """
+    xyz = np.asarray(xyz_m, dtype=float).reshape(3)
+    q = np.asarray(qwxyz, dtype=float).reshape(7) if np.asarray(qwxyz).size == 7 else np.asarray(qwxyz, dtype=float).reshape(4)
+    if q.size == 7:
+        qw, qx, qy, qz = (float(q[3]), float(q[4]), float(q[5]), float(q[6]))
+    else:
+        qw, qx, qy, qz = (float(q[0]), float(q[1]), float(q[2]), float(q[3]))
+    R = quat_xyzw_to_R((qx, qy, qz, qw))
+    return Rt_to_T(R, xyz.reshape(3, 1))
+
+
+def cam_to_flange_via_tool(T_cam_to_tool: np.ndarray, T_flange_to_tool: np.ndarray) -> np.ndarray:
+    """AX=XB 在工具 TCP 上解出 cam→工具后，再折成 cam→法兰。
+
+    ``T_flange_to_tool`` 是 Studio TCP：``p_法兰 = T_flange_to_tool · p_工具``。
+    OpenCV 的 ``T_cam_to_tool`` 是 ``p_工具 = T_cam_to_tool · p_相机``。
+    所以 ``T_cam_to_flange = T_flange_to_tool · T_cam_to_tool``。
+
+    乘反了（``T_cam_to_tool · inv(T_ft)``）旋转看着还像，平移会把相机
+    扔到法兰后方约 30 cm。忘了乘则 ``T_4x4`` 停在 TCP 上，沿工具偏 ~225 mm。
+    两种都不报错。
+    """
+    T_ct = np.asarray(T_cam_to_tool, dtype=float).reshape(4, 4)
+    T_ft = np.asarray(T_flange_to_tool, dtype=float).reshape(4, 4)
+    return T_ft @ T_ct
+
+
 def format_transform_yaml(R: np.ndarray, t: np.ndarray, name: str) -> str:
     T = Rt_to_T(R, t)
     lines = [
@@ -321,6 +386,14 @@ def format_transform_yaml(R: np.ndarray, t: np.ndarray, name: str) -> str:
     return '\n'.join(lines) + '\n'
 
 
+def format_matrix_yaml(T: np.ndarray, key: str) -> str:
+    arr = np.asarray(T, dtype=float).reshape(4, 4)
+    lines = [f'{key}:']
+    for row in arr:
+        lines.append('  - [' + ', '.join(f'{v:.8f}' for v in row) + ']')
+    return '\n'.join(lines) + '\n'
+
+
 def is_pose_filled(joints: Sequence[float]) -> bool:
     """若全为零 / NaN 占位则返回 False。"""
     arr = np.asarray(joints, dtype=float)

build/arm_application/build/lib/arm_application/handeye_teach.py

@@ -37,6 +37,8 @@ import rclpy
 from rcl_interfaces.msg import ParameterDescriptor
 from rclpy.node import Node
 
+from arm_application.ros_node import TolerantNode, param_as_str
+
 HELP = """
 ================================================
  手眼示教 TEACH：预览 -> 拖拽 -> 拍照+位姿
@@ -94,14 +96,6 @@ def _setup_matplotlib_font():
     return chosen
 
 
-def _param_as_str(node: Node, name: str, default: str = '') -> str:
-    node.declare_parameter(name, default, ParameterDescriptor(dynamic_typing=True))
-    val = node.get_parameter(name).value
-    if val is None or val == '':
-        return ''
-    return str(val).strip()
-
-
 class RdkBridge:
     """通过 rdk_py310 子进程（Python 3.10 ABI）与 Flexiv RDK 通信。"""
 
@@ -128,6 +122,8 @@ class RdkBridge:
             'PATH': f'{Path(py).parent}:/usr/bin:/bin',
             'LD_LIBRARY_PATH': flexiv_lib,
             'LANG': 'C.UTF-8',
+            'SITE_PC_IP': os.environ.get('SITE_PC_IP', ''),
+            'SITE_NET_IFACE': os.environ.get('SITE_NET_IFACE', ''),
         }
         # 不要继承 conda 的 PYTHONPATH。
         # 重要：stderr 不能用 PIPE —— RDK 日志会填满管道并让桥接进程死锁
@@ -240,7 +236,7 @@ class RdkBridge:
             pass
 
 
-class HandeyeTeach(Node):
+class HandeyeTeach(TolerantNode):
     def __init__(self) -> None:
         super().__init__('handeye_teach')
         self.declare_parameter('output_dir', '/home/cc/ros2/handeye/tmp')
@@ -250,7 +246,7 @@ class HandeyeTeach(Node):
         self.declare_parameter('backend', 'ros')  # ros | rdk
         self.declare_parameter('robot_sn', os.environ.get('SITE_ROBOT_SN', '').strip())
         self.declare_parameter('drag_primitive', 'FloatingJoint')  # or FloatingCartesian
-        self._serial = _param_as_str(self, 'camera_serial', '')
+        self._serial = param_as_str(self, 'camera_serial', '')
         self.declare_parameter('auto_exposure', True)
         self.declare_parameter('ae_limit_us', 1800.0)
         self.declare_parameter('emitter_enabled', False)
@@ -260,6 +256,9 @@ class HandeyeTeach(Node):
         # 设了就只是个"提醒"：采够时提示可以收工，不强制关窗 ——
         # 常常采到一半发现某个角度还没覆盖，强制退出反而要重开。
         self.declare_parameter('target_points', 0)
+        # 必须和 run_handeye.sh 的 TAUGHT 是同一个文件。只写 taught_poses.txt、
+        # 脚本去读 taught_poses_<模式>.txt，merge 就会「文件在、内容空」。
+        self.declare_parameter('poses_file', '')
 
         self.backend = str(self.get_parameter('backend').value).strip().lower()
         self.robot_sn = str(self.get_parameter('robot_sn').value)
@@ -267,8 +266,12 @@ class HandeyeTeach(Node):
 
         self.output_dir = Path(str(self.get_parameter('output_dir').value)).expanduser()
         self.images_dir = self.output_dir / 'images'
-        self.poses_file = self.output_dir / 'taught_poses.txt'
+        _pf = str(self.get_parameter('poses_file').value).strip()
+        self.poses_file = (
+            Path(_pf).expanduser() if _pf else self.output_dir / 'taught_poses.txt'
+        )
         self.yaml_file = self.output_dir / 'taught_waypoint_positions.yaml'
+        self._last_capture_t = 0.0
         self.images_dir.mkdir(parents=True, exist_ok=True)
 
         self._captures: List[dict] = []
@@ -537,6 +540,20 @@ class HandeyeTeach(Node):
             if q is None:
                 self.get_logger().error('读取关节失败')
                 return
+            now = time.monotonic()
+            if now - self._last_capture_t < 0.8:
+                self.get_logger().warn('按太快，忽略（防 Enter 连发）')
+                return
+            if self._captures:
+                last = self._captures[-1]['joints']
+                delta = max(abs(a - b) for a, b in zip(q, last))
+                if delta < 0.015:
+                    self.get_logger().warn(
+                        f'关节几乎没动（Δ={delta:.4f} rad），忽略这次拍照。'
+                        '先拖到另一个姿态再按 Enter。'
+                    )
+                    return
+            self._last_capture_t = now
 
             idx = len(self._captures)
             stamp = datetime.now(timezone.utc).strftime('%Y%m%d_%H%M%S')

build/arm_application/build/lib/arm_application/handeye_waypoints.py

@@ -41,6 +41,8 @@ from arm_application.site_io import joint_prefix_default
 from arm_msgs.action import FollowJointTrajectory
 from arm_msgs.srv import GetArmState
 
+from arm_application.ros_node import TolerantNode
+
 try:
     import cv2
 except Exception:  # noqa: BLE001
@@ -110,7 +112,7 @@ def _image_to_bgr(msg: Image) -> np.ndarray:
     raise RuntimeError(f'不支持的图像编码：{msg.encoding!r}')
 
 
-class HandeyeWaypoints(Node):
+class HandeyeWaypoints(TolerantNode):
     def __init__(self) -> None:
         super().__init__('handeye_waypoints')
         self.declare_parameter('joint_prefix', joint_prefix_default())

build/arm_application/build/lib/arm_application/hole_detect_hough.py

@@ -134,6 +134,67 @@ def _parse_best_bbox(dets, conf_thr: float) -> tuple[int, int, int, int] | None:
     return None
 
 
+def _keypoint_holes(
+    dets,
+    *,
+    conf_thr: float,
+    min_kconf: float,
+    radius_px: float,
+    min_dist_px: float,
+):
+    """pose 模型直接给出的 k1/k2 关键点当孔位。
+
+    现场约定（见 ``pose_detector``）：k0 跟踪、k1/k2 对孔。
+    返回 ``((k1x,k1y),(k2x,k2y),r,r)``，拿不到可信关键点时返回 None
+    （不是 pose 模型 / 关键点置信度不够 / 两点几乎重合），调用方回落到找圆。
+
+    为什么值得优先用关键点：Hough / 亮环那套是在**孔本身的图像特征**上找圆，
+    光照、反光、通止规尖伸进视野都会把圆心拉偏；而关键点是模型在训练分布里
+    直接回归出来的，对这几类干扰天然更稳。
+    """
+    if not dets:
+        return None
+    for det in sorted(dets, key=lambda d: d.conf, reverse=True):
+        if float(det.conf) < conf_thr:
+            continue
+        kpts = getattr(det, "kpts", None)
+        if kpts is None:
+            continue
+        kpts = np.asarray(kpts, dtype=float)
+        if kpts.ndim != 2 or kpts.shape[0] < 3 or kpts.shape[1] < 3:
+            continue
+        k1, k2 = kpts[1].copy(), kpts[2].copy()
+        if not (np.isfinite(k1[:2]).all() and np.isfinite(k2[:2]).all()):
+            continue
+        if float(k1[2]) < min_kconf or float(k2[2]) < min_kconf:
+            logger.info(
+                "k1/k2 关键点置信度不足（%.2f / %.2f < %.2f）—— 回落到找圆",
+                float(k1[2]),
+                float(k2[2]),
+                min_kconf,
+            )
+            continue
+        dist = float(np.hypot(k1[0] - k2[0], k1[1] - k2[1]))
+        if dist < float(min_dist_px):
+            logger.info(
+                "k1/k2 两关键点只差 %.1fpx（< %.1fpx）—— 视为不可信，回落到找圆",
+                dist,
+                min_dist_px,
+            )
+            continue
+        r = float(radius_px)
+        # **刻意不按图像 x 交换左右**。
+        # Hough 路径的 k1/k2 是"画面左孔 / 右孔"（位置定义），但 pose 模型学出来的
+        # k1/k2 是**固定语义**（训练时标注的那个物理孔）。工件只要转个角度，
+        # 位置就会翻面；按 x 强制交换会让 `target_hole=k1` 时左时右地指向两个不同
+        # 的物理孔 —— 那是最难查的一类现场问题。
+        # 下游怎么认这两个点由 `stage2.k1_label` 决定：
+        #   hole_points[0] = 模型的 k1，hole_points[1] = 模型的 k2，
+        #   所以 k1_label="left" ↔ k1 就是模型的 k1（见 stage2_locate_node._snap）。
+        return (float(k1[0]), float(k1[1])), (float(k2[0]), float(k2[1])), r, r
+    return None
+
+
 def _refine_circle_center(
     gray: np.ndarray,
     cx: float,
@@ -466,6 +527,35 @@ def detect_holes(
                 gate_block=True,
             )
 
+        # ---- 首选：pose 模型直接给出 k1/k2 关键点（不必再找圆） ----
+        if bool(cfg.get("keypoint_enable", True)):
+            kp = _keypoint_holes(
+                dets,
+                conf_thr=conf,
+                min_kconf=float(cfg.get("keypoint_min_conf", 0.30)),
+                radius_px=float(cfg.get("keypoint_hole_radius_px", 12.0)),
+                min_dist_px=float(cfg.get("keypoint_min_dist_px", 15.0)),
+            )
+            if kp is not None:
+                (k1, k2, r1, r2) = kp
+                hole_points = [[float(k1[0]), float(k1[1])], [float(k2[0]), float(k2[1])]]
+                hole_radii = [float(r1), float(r2)]
+                dist = float(np.hypot(k1[0] - k2[0], k1[1] - k2[1]))
+                gate = (max_dx, max_dy) if bool(cfg.get("center_gate_draw", True)) else None
+                draw_img = _render_success(
+                    image, bbox, hole_points, hole_radii, dist, gate=gate, label=label
+                )
+                return {
+                    "status": True,
+                    "msg": f"pose 关键点 k1/k2，两孔距离 {dist:.1f}px",
+                    "hole_points": hole_points,
+                    "hole_radii": hole_radii,
+                    "bbox": list(bbox),
+                    "label": label,
+                    "source": "keypoint",
+                    "draw_img": draw_img,
+                }
+
         # ---- 主路径：螺纹嵌件对（亮环签名 + 关于中心孔对称），抗多孔干扰、抗跳变 ----
         if bool(cfg.get("insert_pair_enable", True)):
             gray_full = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) if image.ndim == 3 else image

build/arm_application/build/lib/arm_application/hole_detect_node.py

@@ -25,6 +25,8 @@ from arm_application.site_io import load_site_dict
 from arm_msgs.msg import HoleDetection as HoleDetectionMsg
 from arm_msgs.msg import HoleDetections
 
+from arm_application.ros_node import TolerantNode
+
 
 def _as_bool(val, default: bool = False) -> bool:
     if val is None:
@@ -57,7 +59,7 @@ def _bgr_to_msg(image: np.ndarray, stamp, frame_id: str) -> Image:
     return msg
 
 
-class HoleDetectNode(Node):
+class HoleDetectNode(TolerantNode):
     def __init__(self) -> None:
         super().__init__("hole_detect")
         self.declare_parameter("camera_source", os.environ.get("SITE_CAM_NAME") or "d405")

build/arm_application/build/lib/arm_application/hole_detector.py

@@ -21,7 +21,7 @@ DEFAULT_CONFIG = _PKG_SHARE_FALLBACK / "hole_insert.yaml"
 # 配置齐全时永远走不到这条分支，一旦 site.yaml 缺键就报"模型文件不存在"，
 # 而打印出来的路径看着特别合理，很容易被误判成"文件被人误删了"。
 # 改动这个常量时，顺手跑 ``bash scripts/weights.sh doctor`` 确认没有悬空引用。
-_CANONICAL_HOLE_MODEL = "models/hole_insert/detect_best_1.onnx"
+_CANONICAL_HOLE_MODEL = "models/yolo/gongjian0929.onnx"
 
 
 @dataclass
@@ -143,17 +143,18 @@ class HoleDetector:
     def _load_yolo(self):
         if self._detector is not None:
             return self._detector
-        from arm_application.yolo_detector import YoloDetector, resolve_weights
+        from arm_application.yolo_detector import build_detector, resolve_weights
 
         weights = resolve_weights(
             self.model_path,
             backend=str(self.cfg.get("yolo_backend") or "auto"),
         )
-        self._detector = YoloDetector(
+        # build_detector 会自己识别 pose 模型（k0/k1/k2），不必在这里分支。
+        self._detector = build_detector(
             weights,
-            device=str(self.device),
             conf=float(self.cfg.get("conf", 0.25)),
             iou=float(self.cfg.get("iou", 0.7)),
+            device=str(self.device),
             imgsz=int(self.cfg.get("imgsz", 640)),
         )
         return self._detector

build/arm_application/build/lib/arm_application/kalman_filter.py

@@ -62,6 +62,15 @@ class KalmanFilter3D:
         self._predict(dt)
         self._t = t
 
+    def zero_velocity(self) -> None:
+        """把速度状态清零、位置保持不动。
+
+        用途：观测疑似假跳变（见 ``feature_jump_guard.FeatureJumpGate``）时本次不
+        采信观测，但**不能**让上一次的速度继续把位置外推出去 —— 否则"不动"会变成
+        "按旧速度匀速跑掉"，刚好是我们要避免的那件事。
+        """
+        self.x[3:] = 0.0
+
     def update(self, z: np.ndarray) -> None:
         """用一次三维位置观测做量测更新。"""
         z = np.asarray(z, dtype=float).reshape(3)

build/arm_application/build/lib/arm_application/move_to_pose_client.py

@@ -15,8 +15,10 @@ from geometry_msgs.msg import PoseStamped
 
 from arm_msgs.action import MoveToPose
 
+from arm_application.ros_node import TolerantNode
 
-class MoveToPoseClient(Node):
+
+class MoveToPoseClient(TolerantNode):
     def __init__(self) -> None:
         super().__init__('move_to_pose_client')
         self._client = ActionClient(self, MoveToPose, 'move_to_pose')

build/arm_application/build/lib/arm_application/onnx_workpiece.py

@@ -54,6 +54,21 @@ class OnnxWorkpieceDetector:
         self._iname = inp.name
         shape = [d if isinstance(d, int) and d > 0 else imgsz for d in inp.shape]
         self._in_h, self._in_w = int(shape[-2]), int(shape[-1])
+        # 只认 ultralytics end2end 检测输出 ``[1, N, 6]``，否则**当场报错**。
+        # 为什么必须在这里硬失败：2026-10-08 真机，把一个 ``task=pose`` 的模型
+        # （输出 ``[1, 4+nc+3K, 8400]``）从这条分支解出来，得到的是 12 个垃圾框
+        # ——conf 直接是坐标值（最高 70.87）、类名 ``class_73``、退化框 ``y1=y2=0``。
+        # 而流水线照跑不误，只在二阶段"看不出工件"上体现出来，排查代价极大。
+        # 宁可启动就失败（元规则：静默失效是头号敌人）。
+        outs = self._sess.get_outputs()
+        tail = outs[0].shape[-1] if (outs and outs[0].shape) else None
+        if isinstance(tail, int) and tail != 6:
+            raise RuntimeError(
+                f"{path.name} 的检测输出最后一维是 {tail}，不是 6 —— 它不是"
+                " ultralytics end2end 检测模型（[1,N,6]）。带 k0/k1/k2 关键点的"
+                " pose 模型必须交给 pose_detector / build_detector，否则坐标会被"
+                "当成置信度解出垃圾框，而且不会报错。"
+            )
         self.conf = float(conf)
         self.imgsz = int(imgsz)
         # 类别名：显式传入 > 旁挂标签文件 > ONNX 内嵌 metadata > 占位符。

build/arm_application/build/lib/arm_application/pipeline_node.py

@@ -11,31 +11,116 @@
 from __future__ import annotations
 
 import json
+import math
 import os
 import signal
 import subprocess
 import time
-from typing import Optional
+from pathlib import Path
+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 rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile, ReliabilityPolicy
 from std_msgs.msg import Bool, 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
+from arm_application.stage2_locate_node import _pt
 from arm_application.stage3_search_node import Stage3Search
 
 
-#: 一阶段跟踪高相对二阶段悬停高的**抬升量** [m]。
+#: 悬停时规尖离面不够，跟踪高再抬的量 [m]。
 #:
-#: 为什么要抬：跟踪时通止规已经夹在爪上，规尖比相机更靠下；悬停高是"相机拍清
-#: 孔"的高度，直接在那儿跟踪会让规尖刮到工件。抬 5cm 让它离开工件，二阶段再
-#: 小幅下降（只需 5cm 以内的行程）。
+#: 只在「悬停高上规尖离工件面 < :data:`TRACK_LOCK_GAP_M`」时加上。仿真悬停按
+#: 工具几何只留约 2mm，不抬就会在跟踪时刮到工件。真机 89 悬停 0.320 时规尖
+#: 已经在面上方约 75mm，再抬 50mm 会让对孔前先改一次高度（0.37→0.32）。
 #:
 #: ⚠️ 抽成常量是因为它不只被流程用：``scripts/check_sim_scene.py`` 要用它核对
 #: "跟踪高有没有超出工作盒上界"（仿真里悬停 0.632 + 0.05 = 0.682 > 真机那套
 #: 0.65 上界，就是被这条查出来的）。两处各写一个字面量迟早漂移。
 TRACK_CLEARANCE_M = 0.05
+#: 悬停时规尖离面已经不少于这个值 [m]，一阶段就锁在悬停高，对孔完成后再下降。
+TRACK_LOCK_GAP_M = 0.040
+
+
+def 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 hold_at_insert_z(final, *, skip3: bool) -> bool:
+    """贴面已经按 ``final_gap_mm`` 停下时，不再进三阶段。
+
+    三阶段开局会抬到悬高上方测触觉，再落回 ``hole_hover_z_m``。
+    没有接触传感器。到位高度就是这次的贴面间隙，不另设一个 Z。
+    """
+    if skip3:
+        return True
+    if not isinstance(final, dict):
+        return False
+    return bool(final.get("insert_held"))
+
+
+def track_lock_z(hover_z: float, tool_len_m: float, surface_z_m: float) -> float:
+    """一阶段锁高。
+
+    悬停时规尖离面已经 ≥ ``TRACK_LOCK_GAP_M``：锁在悬停高，跟踪和对孔都不改 Z，
+    对孔完成后再下降。离面不够（仿真悬停只留 2mm）：再抬 ``TRACK_CLEARANCE_M``，
+    否则跟踪会刮到工件。工具长或工件面缺失时抬升 —— 缺数时锁在悬停高等于没检查。
+    """
+    tool = float(tool_len_m)
+    surf = float(surface_z_m)
+    hover = float(hover_z)
+    if tool > 0.0 and surf > 0.0 and (hover - tool - surf) >= TRACK_LOCK_GAP_M:
+        return hover
+    return hover + TRACK_CLEARANCE_M
+
+
+def stage1_log_fatal(log_path: str) -> str:
+    """一阶段日志里已经写明失败时，不许再空等锁定。空串 = 还没到这一步。"""
+    try:
+        text = Path(log_path).read_text(encoding="utf-8", errors="replace")
+    except OSError:
+        return ""
+    if D435_MOVEL_GAVE_UP in text:
+        return (
+            f"D435 {D435_MOVEL_GAVE_UP}（RDK 控制模式没切到 primitive），"
+            f"一阶段中止。不是视野/标定。看 {log_path}"
+        )
+    return ""
 
 
 def stage1_timeout_reason(points_seen: int, timeout_s: float, log_path: str) -> str:
@@ -52,6 +137,9 @@ def stage1_timeout_reason(points_seen: int, timeout_s: float, log_path: str) ->
     以前两种情况共用一句"没锁定（视野里没工件？）"，于是"perception_node
     因为缺依赖早退了"被读成"算法不收敛"，白查半天。
     """
+    fatal = stage1_log_fatal(log_path)
+    if fatal:
+        return fatal
     if points_seen <= 0:
         return (
             f"一阶段 {timeout_s:.0f}s 没锁定：**一个视觉目标点都没收到**"
@@ -79,11 +167,26 @@ class AssemblyPipeline(Stage3Search):
         self.declare_parameter("skip_stage1", False)
         self.declare_parameter("skip_stage2", False)
         self.declare_parameter("skip_stage3", False)  # 默认搜孔+轻压；拧入靠 --depth
+        self.declare_parameter("d435_move_only", False)
+        self.declare_parameter("init_z_m", -1.0)  # >0：启动后先到这个法兰高度
         self.declare_parameter("stage1_timeout_sec", 150.0)
         self.linger3 = 1.2  # 串联结束短保持；单独跑 stage3 仍用 site linger_sec
 
         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)
         # 诊断用：一阶段超时时靠它区分"感知没给点"和"给了点没收敛"。
         # 只计数，不参与控制 —— 它绝不能影响流程本身的判断。
@@ -113,9 +216,11 @@ class AssemblyPipeline(Stage3Search):
         skip1 = bool(self.get_parameter("skip_stage1").value)
         skip2 = bool(self.get_parameter("skip_stage2").value)
         skip3 = bool(self.get_parameter("skip_stage3").value)
+        self.d435_move_only = bool(self.get_parameter("d435_move_only").value)
         self.get_logger().info(
             f"pipeline arm_ee={self.ee_frame} hole={self.target_hole} "
-            f"skip1={skip1} skip2={skip2} skip3={skip3} insert={self.insert_depth_mm:.1f}mm"
+            f"skip1={skip1} skip2={skip2} skip3={skip3} "
+            f"d435_only={self.d435_move_only} insert={self.insert_depth_mm:.1f}mm"
         )
 
     def _state(self, s: str) -> None:
@@ -125,6 +230,113 @@ 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 _run_a7_seat(self) -> bool:
+        """贴面后咬螺纹：A7 先逆时针到限位，再顺时针到限位。不改 Z。"""
+        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
+        # +1 = 关节角增大 = 逆时针。第一步逆时针出孔让出牙，第二步顺时针进孔咬合。
+        steps = (
+            (ccw, "A7 逆时针至极限（出孔）"),
+            (-ccw, "A7 顺时针至极限（进孔）"),
+        )
+        for direction, tag in steps:
+            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, margin)
+            if target is None:
+                self.get_logger().error(f"{tag}: 算不出 A7 限位目标")
+                return False
+            if not self._send_a7_only(float(target[6]), speed, tag):
+                return False
+        self.get_logger().info("A7 两步完成：逆时针到限位，再顺时针到限位。法兰高度未改。")
+        return True
+
     def _on_lock(self, msg: Bool) -> None:
         if bool(msg.data):
             self._locked = True
@@ -163,33 +375,64 @@ class AssemblyPipeline(Stage3Search):
         self._kill_old_stage1()
 
     def _track_z(self) -> float:
-        """跟踪高度 = 孔悬停高 + :data:`TRACK_CLEARANCE_M`，通止规离开工件。"""
-        return float(self.hover_z) + TRACK_CLEARANCE_M
+        """一阶段锁高。写了 ``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:
-        """上一轮轻压可能还在孔里：先纯 Z 抬到跟踪高，禁止 0.23↔0.35 来回。"""
+    def _ensure_clearance(self) -> bool:
+        """没写初始高度：只在低于锁高时抬上去。写了：升或降到那个法兰 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()
-        z = float(Tee[2, 3])
-        if z >= tz - 0.008:
-            return
-        self._state("pre_lift")
-        self.get_logger().info(
-            f"先离开工件/孔口：Z {z:.3f} → {tz:.3f}m（跟踪高=hover+50mm）"
+        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]),
             )
-        _, q, Tee = self._goto(
+            if why:
+                self.get_logger().error(why)
+                return False
+        z = float(Tee[2, 3])
+        if user:
+            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(f"先到初始高度：Z {z:.3f} → {tz:.3f}m")
+        ok, 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
@@ -205,9 +448,14 @@ class AssemblyPipeline(Stage3Search):
             "enable_control:=true",
             "publish_handeye_tf:=false",
             f"ee_z_m:={lock_z:.3f}",
-            "lock_hold_sec:=0.6",
             f"use_sim_time:={str(use_sim).lower()}",
+            f"d435_move_only:={str(self.d435_move_only).lower()}",
         ]
+        if self.d435_move_only:
+            self.get_logger().info(
+                f"启动 D435 位移测试（锁高 {lock_z:.3f}m，到位后不交 D405、不进二阶段）"
+            )
+        else:
             self.get_logger().info(
                 f"启动一阶段跟踪（锁高 {lock_z:.3f}m，锁定后自动交二阶段）"
             )
@@ -221,6 +469,11 @@ class AssemblyPipeline(Stage3Search):
             self._spin(0.1)
             if self._locked:
                 break
+            fatal = stage1_log_fatal(log_path)
+            if fatal:
+                self._stop_stage1()
+                self.get_logger().error(fatal)
+                return 1
             if self._s1.poll() is not None:
                 self.get_logger().error(f"一阶段进程提前退出，看 {log_path}")
                 return 1
@@ -230,6 +483,11 @@ class AssemblyPipeline(Stage3Search):
                 stage1_timeout_reason(self._point_msgs, timeout, log_path)
             )
             return 1
+        if self.d435_move_only:
+            self.get_logger().info(
+                "D435 位移测试结束 — 停跟踪、钉住当前关节，跳过二阶段"
+            )
+        else:
             self.get_logger().info("一阶段已锁定 — 停跟踪、钉住当前关节，直接进二阶段")
         self._stop_stage1()
         time.sleep(0.35)
@@ -242,6 +500,9 @@ class AssemblyPipeline(Stage3Search):
         skip1 = bool(self.get_parameter("skip_stage1").value)
         skip2 = bool(self.get_parameter("skip_stage2").value)
         skip3 = bool(self.get_parameter("skip_stage3").value)
+        if self.d435_move_only:
+            skip2 = True
+            skip3 = True
         if not self._cli.wait_for_server(timeout_sec=5.0):
             self.get_logger().error("JTC action 不在 —— 先开 T1: run_real.sh --arm 89")
             return 2
@@ -252,7 +513,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)
@@ -267,19 +529,54 @@ class AssemblyPipeline(Stage3Search):
                 if code != 0:
                     self.get_logger().error(f"二阶段失败 exit={code}，停在当前位姿")
                     return code
+                stay = hold_at_insert_z(final, skip3=skip3)
+                if stay and final.get("insert_held"):
+                    self.get_logger().info(
+                        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）。"
+                        "没有接触传感器，不另设 Z，不抬回悬高。"
+                    )
+                else:
                     self.get_logger().info(
                         f"二阶段 OK 残差 {final.get('residual_mm', -1)}mm"
-                    + (" — 跳过搜孔，保持悬停" if skip3 else " — 立刻下压搜孔并轻压（不拧入）")
+                        + (
+                            " — 跳过搜孔，停在观测高度（看规尖是否对中；进程退出后臂保持）"
+                            if skip3
+                            else " — 立刻下压搜孔并轻压（不拧入）"
+                        )
                     )
             else:
                 self._state("skip_stage2")
+                stay = hold_at_insert_z(final, skip3=skip3)
 
-            if skip3:
+            if stay:
                 self._state("skip_stage3")
-                self.get_logger().info("跳过搜孔 —— 臂保持在孔上方")
+                if self.d435_move_only:
+                    self.get_logger().info(
+                        "D435 位移测试结束 —— 臂停在当前高度，不交 D405、不对孔；"
+                        "15s 后本进程退出，臂保持不动"
+                    )
+                elif isinstance(final, dict) and final.get("insert_held"):
+                    if q is not None:
+                        self._pin(q)
+                    self.get_logger().info(
+                        "插孔已停在贴面高度，不进搜孔、不抬回悬高。"
+                        "接着只转 A7：先逆时针到限位，再顺时针到限位。"
+                    )
+                    if not self._run_a7_seat():
+                        self.get_logger().error(
+                            "A7 旋转没走完，臂停在当前关节。不抬 Z。"
+                        )
+                else:
+                    self.get_logger().info(
+                        "跳过搜孔 —— 臂停在当前高度。请看规尖与孔是否对中；"
+                        "15s 后本进程退出，臂保持不动"
+                    )
                 if final:
                     self._publish_result(final)
-                self._linger_hold(final if final else {"ok": True}, seconds=0.8)
+                self._linger_hold(final if final else {"ok": True}, seconds=15.0)
                 payload = {"ok": True, "stage": "pipeline", "exit": 0, "skipped": "stage3"}
                 try:
                     payload["stage2"] = json.loads(

build/arm_application/build/lib/arm_application/plane_feature.py

@@ -21,6 +21,11 @@ class PlaneFeature:
     xyz: np.ndarray  # (3,) 相机坐标系下的立体点云质心
     n_inliers: int
     note: str
+    # 这个点的 Z 是不是**借**来的（该像素处读不到自己的深度，从检测框里借了一个）。
+    # 为什么要显式带出来、而不是让下游去认 ``note`` 里的字：借深度意味着该像素的
+    # 深度不可读 ⇒ 绝对位置不足以用来**重定位**（2026-10-08 真机，假 k0 带着借来的
+    # 深度、z 看着很合理 0.27~0.39m，于是"物理量程"守卫也失效）。
+    borrowed: bool = False
 
 
 def _depth_meters(buf: np.ndarray, encoding: str) -> Tuple[np.ndarray, float]:

build/arm_application/build/lib/arm_application/realsense_color_pub.py

@@ -32,16 +32,21 @@ from rclpy.qos import qos_profile_sensor_data
 from sensor_msgs.msg import CameraInfo, Image
 from std_msgs.msg import Float32MultiArray
 
+from arm_application.ros_node import TolerantNode, param_as_str
 
-def _param_as_str(node: Node, name: str, default: str = '') -> str:
-    """字符串参数，同时兼容命令行传数字的情况（例如序列号）。"""
-    node.declare_parameter(
-        name, default, ParameterDescriptor(dynamic_typing=True)
+# RealSense 掉到 480Mbps（或整台消失）时的排查口径。写在这里是为了"只有一处"：
+# 相机节点的报错、preflight 脚本、文档都引用同一套说法，不会各说各的。
+# 为什么 480Mbps 值得专门解释一次（2026-09-28 现场）：
+#   D435 原本协商到 USB 3.2、1280x720@30 跑得好好的，中途
+#   `xioctl(UVCIOC_CTRL_QUERY) ... No such file or directory`（= 设备从总线上消失）
+#   之后重新枚举成了 **USB 2.1**。USB3 线缆/座子里 SuperSpeed 那一对针脚一旦
+#   接触不良，USB2 的针脚还能用 —— 于是设备不会"掉线"，只是**悄悄掉速**，
+#   表现成"相机在，但帧率/分辨率不对"。这时查软件是查不出来的。
+USB_RESEAT_HINT = (
+    '拔插一次、换一根 USB3 线，或换到机箱后面板直连的蓝色口 —— '
+    '只协商到 480Mbps 通常是 SuperSpeed 针脚没接上，不是相机坏了。'
+    '看当前协商速率：lsusb -t（设备后面写 480M 还是 5000M）。'
 )
-    val = node.get_parameter(name).value
-    if val is None or val == '':
-        return ''
-    return str(val).strip()
 
 
 def _fill_camera_info(
@@ -68,23 +73,17 @@ def _fill_camera_info(
 
 
 def _site_yaml_path() -> Path:
-    env = str(os.environ.get("SITE_CONFIG") or "").strip()
-    if env:
-        p = Path(env).expanduser()
-        if p.is_file():
-            return p
-    home = Path.home() / "ros2" / "config" / "site.yaml"
-    if home.is_file():
-        return home
-    return Path("/home/cc/ros2/config/site.yaml")
+    from arm_application.site_io import site_path
+
+    return site_path()
 
 
-class RealsenseColorPub(Node):
+class RealsenseColorPub(TolerantNode):
     def __init__(self) -> None:
         super().__init__('realsense_color_pub')
         self.declare_parameter('camera_name', 'd405')  # 话题命名空间
         self._name = str(self.get_parameter('camera_name').value).strip().strip('/')
-        self._serial = _param_as_str(self, 'camera_serial', '')
+        self._serial = param_as_str(self, 'camera_serial', '')
         self.declare_parameter('width', 1280)
         self.declare_parameter('height', 720)
         self.declare_parameter('fps', 15)
@@ -177,6 +176,10 @@ class RealsenseColorPub(Node):
         self._overlay: Optional[list] = None
         self._overlay_stamp = 0.0
         self._capture_started = False
+        self._cap_lock = threading.Lock()
+        self._cap_event = threading.Event()
+        self._pending = None
+        self._last_frame_mono = 0.0
         self._usb_desc = ''
         self._last_ok = None
         self._site_yaml = _site_yaml_path()
@@ -310,9 +313,12 @@ class RealsenseColorPub(Node):
         self._info_pub.publish(info)
 
         if depth_u16 is not None:
+            self._publish_depth(depth_u16, stamp)
+
+    def _publish_depth(self, depth_u16: np.ndarray, stamp=None) -> None:
         dh, dw = int(depth_u16.shape[0]), int(depth_u16.shape[1])
         dmsg = Image()
-            dmsg.header.stamp = stamp
+        dmsg.header.stamp = stamp or self.get_clock().now().to_msg()
         dmsg.header.frame_id = self._frame_id
         dmsg.height = dh
         dmsg.width = dw
@@ -348,8 +354,8 @@ class RealsenseColorPub(Node):
         if not devices:
             raise RuntimeError(
                 '没有连接 RealSense（多次重试后 USB 仍为空）。'
-                '请把 D405 插到**本机**的 USB3（蓝色）口，拔插一次，'
-                '并关闭 realsense-viewer / 192.168.0.108 上占用该相机的采集端。'
+                '拔插一次；确认没有 realsense-viewer / 其它进程占着这台相机；'
+                + USB_RESEAT_HINT
             )
 
         found = self._pick_device(devices, rs)
@@ -374,9 +380,12 @@ class RealsenseColorPub(Node):
             f'requested {self._width}x{self._height}@{self._fps}Hz'
         )
         if usb2:
+            # 只在 480Mbps 口上才降。实测（2026-09-28）640x480@15 彩色+深度
+            # 可以稳定跑满 15fps；但 1280x720@30 在 USB2 上物理不可能
+            # （SuperSpeed 没协商上）。
             self.get_logger().warn(
-                f'{cam_name} 接在 USB {usb_desc} 上 —— 深度/IR 将保持 '
-                '640x480@15。把 D405 插到 USB3（蓝色）口可获得 848@30。'
+                f'{cam_name} 只协商到 USB {usb_desc}（480Mbps）—— 降到 '
+                f'640x480@15Hz。' + USB_RESEAT_HINT
             )
             self._width, self._height, self._fps = 640, 480, 15
 
@@ -455,7 +464,9 @@ class RealsenseColorPub(Node):
                         trial.enable_stream(
                             rs.stream.infrared, 2, w, h, rs.format.y8, fps
                         )
-                    profile = pipe.start(trial)
+                    # 回调里只留最新一帧。wait_for_frames 会等彩色、深度、红外齐套，
+                    # 场景一动红外对不齐，彩色帧就在这里被丢掉。
+                    profile = pipe.start(trial, self._on_sdk_frame)
                     have_depth = with_depth
                     have_infra = infra_mode != 'none'
                     self._infra_mode = infra_mode
@@ -501,11 +512,18 @@ class RealsenseColorPub(Node):
                 break
         if profile is None:
             raise RuntimeError(
-                f'D405 没有可用的彩色配置。最后的错误：{last_err}'
+                f'{self._name} 没有可用的彩色配置。最后的错误：{last_err}'
             )
 
         self._pipe = pipe
         self._profile = profile
+        try:
+            sensor = profile.get_device().first_color_sensor()
+            if sensor.supports(rs.option.frames_queue_size):
+                # 队列只留最新一张，避免越积越慢，动起来画面还停在旧帧。
+                sensor.set_option(rs.option.frames_queue_size, 1)
+        except Exception:  # noqa: BLE001
+            pass
         self._have_depth = have_depth
         self._have_infra = have_infra
         self._logged_depth = False
@@ -607,7 +625,7 @@ class RealsenseColorPub(Node):
 
         if not getattr(self, '_capture_started', False):
             self._capture_started = True
-            self._thread = threading.Thread(target=self._capture_loop, daemon=True)
+            self._thread = threading.Thread(target=self._publish_loop, daemon=True, name="rs-publish")
             self._thread.start()
 
     def _enable_color_ae_fast(self, profile, rs) -> None:
@@ -700,18 +718,60 @@ class RealsenseColorPub(Node):
             )
             return False
 
-    def _capture_loop(self) -> None:
+    def _on_sdk_frame(self, frame) -> None:
+        """SDK 回调。只拷走彩色并握住这一组帧，立刻返回。
+
+        对齐、深度换算和 ROS 发布若做在这个回调里，下一帧来不及取，
+        动起来时彩色帧号会跳。那些工作放到 ``_publish_loop``。
+        """
+        try:
+            frames = frame.as_frameset()
+            color = frames.get_color_frame()
+            if not color:
+                return
+            bgr = np.array(np.asanyarray(color.get_data()), copy=True)
+            frames.keep()
+            with self._cap_lock:
+                self._pending = (bgr, frames)
+            self._last_frame_mono = time.monotonic()
+            self._cap_event.set()
+        except Exception as exc:  # noqa: BLE001
+            try:
+                self.get_logger().warn(f'收帧：{exc}', throttle_duration_sec=2.0)
+            except Exception:  # noqa: BLE001
+                pass
+
+    def _publish_loop(self) -> None:
         got = False
-        misses = 0
         while self._running and rclpy.ok():
             if self._pipe is None:
                 if not self._reconnect():
                     time.sleep(2.0)
                 continue
+            if not self._cap_event.wait(0.2):
+                silent = self._last_frame_mono and time.monotonic() - self._last_frame_mono > 2.0
+                if silent:
+                    usb = getattr(self, '_usb_desc', '?')
+                    self.get_logger().error(
+                        f'相机超过 2 秒没有新帧（USB={usb}）。正在重连 —— '
+                        '请给 USB 线缆留点余量，别让机械臂把它拽掉。'
+                    )
+                    self._last_frame_mono = 0.0
+                    got = False
+                    self._reconnect()
+                continue
+            self._cap_event.clear()
+            with self._cap_lock:
+                item = self._pending
+                self._pending = None
+            if item is None:
+                continue
+            bgr, frames = item
             try:
-                t_wait0 = time.monotonic()
-                frames = self._pipe.wait_for_frames(timeout_ms=2000)
-                self._t_wait = time.monotonic() - t_wait0
+                t_pub = time.monotonic()
+                # 彩色先发出去。深度对齐再慢，也不能把这一帧彩色拖掉。
+                self._publish_frame(bgr, None)
+                self._t_pub = time.monotonic() - t_pub
                 ir_l = (
                     frames.get_infrared_frame(1)
                     if self._have_infra and self._infra_mode in ('both', 'left')
@@ -722,44 +782,45 @@ class RealsenseColorPub(Node):
                     if self._have_infra and self._infra_mode in ('both', 'right')
                     else None
                 )
+                aligned = frames
                 if self._align is not None:
                     t_al = time.monotonic()
-                    frames = self._align.process(frames)
+                    aligned = self._align.process(frames)
                     self._t_align = time.monotonic() - t_al
+                depth = aligned.get_depth_frame()
                 color = frames.get_color_frame()
             except Exception as exc:  # noqa: BLE001
-                misses += 1
                 text = str(exc)
                 dropped = (
                     'disconnect' in text.lower()
                     or 'ioctl' in text.lower()
                     or 'errno=5' in text.lower()
                 )
-                if dropped or misses >= 4:
+                if dropped:
                     usb = getattr(self, '_usb_desc', '?')
                     self.get_logger().error(
                         f'相机数据流中断（USB={usb}）：{text[:120]}。'
                         '正在重连 —— 请给 USB 线缆留点余量，别让机械臂把它拽掉。'
                     )
                     got = False
-                    misses = 0
+                    self._last_frame_mono = 0.0
                     self._reconnect()
+                else:
+                    self.get_logger().warn(f'整理帧：{exc}', throttle_duration_sec=2.0)
                 continue
-                if misses == 1:
-                    self.get_logger().warn(f'wait_for_frames：{exc}')
-                continue
-            if not color:
-                misses += 1
-                continue
-            misses = 0
-            bgr = np.asanyarray(color.get_data())
-            depth = frames.get_depth_frame()
             depth_u16 = np.asanyarray(depth.get_data()) if depth else None
             if depth_u16 is not None and self._depth_to_mm != 1.0:
                 # 原始设备单位 → 真实 mm（D405 原始为 0.1 mm/单位）
                 depth_u16 = np.clip(
                     depth_u16.astype(np.float32) * self._depth_to_mm, 0, 65535
                 ).astype(np.uint16)
+            if depth_u16 is not None:
+                try:
+                    self._publish_depth(depth_u16)
+                except Exception as exc:  # noqa: BLE001
+                    if not self._running:
+                        break
+                    self.get_logger().warn(f'发布深度失败：{exc}', throttle_duration_sec=2.0)
             self._frame_i += 1
             self._fps_meter(color, depth, depth_u16)
             # 预览相关的工作（CPU 上色、IR 拷贝、四窗格绘制）每 N 帧才做一次
@@ -792,7 +853,6 @@ class RealsenseColorPub(Node):
                     )
             try:
                 t_pub = time.monotonic()
-                self._publish_frame(bgr, depth_u16)
                 if self._ir1_pub is not None and ir_l is not None:
                     self._publish_ir(ir_l, self._ir1_pub)
                 if self._ir2_pub is not None and ir_r is not None:

build/arm_application/build/lib/arm_application/realsense_view.py

@@ -27,6 +27,7 @@ from sensor_msgs.msg import Image
 from std_msgs.msg import Float32MultiArray
 
 from arm_application.realsense_color_pub import _site_yaml_path
+from arm_application.ros_node import TolerantNode
 
 
 def _colorize_depth_u16(d: np.ndarray) -> np.ndarray:
@@ -56,15 +57,22 @@ def _mono_to_bgr(mono: np.ndarray) -> np.ndarray:
     return np.ascontiguousarray(np.stack([mono, mono, mono], axis=-1))
 
 
-class RealsenseView(Node):
+class RealsenseView(TolerantNode):
     def __init__(self) -> None:
         super().__init__('realsense_view')
         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
@@ -101,8 +109,9 @@ class RealsenseView(Node):
             depth=5,
             reliability=ReliabilityPolicy.RELIABLE,
         )
+        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(
@@ -124,6 +133,7 @@ class RealsenseView(Node):
         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）。关闭窗口 = 仅隐藏。'
         )
 
@@ -186,6 +196,9 @@ class RealsenseView(Node):
             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(

build/arm_application/build/lib/arm_application/run_joint_trajectory.py

@@ -12,6 +12,8 @@ from rclpy.node import Node
 from arm_application.site_io import joint_prefix_default
 from arm_msgs.action import FollowJointTrajectory
 
+from arm_application.ros_node import TolerantNode
+
 DEFAULT_JOINTS = [
     'joint1', 'joint2', 'joint3', 'joint4', 'joint5', 'joint6', 'joint7',
 ]
@@ -25,7 +27,7 @@ DEFAULT_POSITIONS = [
 ]
 
 
-class RunJointTrajectory(Node):
+class RunJointTrajectory(TolerantNode):
     def __init__(self) -> None:
         super().__init__('run_joint_trajectory')
         self.declare_parameter('joint_prefix', joint_prefix_default())

build/arm_application/build/lib/arm_application/site_io.py

@@ -80,3 +80,137 @@ def ee_frame_default() -> str:
     sn = os.environ.get("SITE_ROBOT_SN", "").strip()
     return f"{sn}_flange" if sn else ""
 
+
+def overhead_camera(
+    site: dict[str, Any], robot: dict[str, Any] | None = None
+) -> tuple[dict[str, Any], list[str]]:
+    """手外（固定）相机 → ``(相机配置字典, 同样合格但未被选中的相机名)``。
+
+    手外相机 = ``cameras.*`` 里 ``role`` 以 ``overhead`` 开头的那台（眼在手外时
+    相机固定在工位外，不随臂动）。
+
+    **为什么必须有唯一实现**：这份"按 role 找，再按 arm 收敛"的规则有两个调用方——
+    S03 粗定位取命名空间（本模块给节点用）、手眼标定脚本取相机名。两边各写一份的
+    后果不是报错，而是"标定用的相机"和"推理用的相机"悄悄不是同一台，且没人发现。
+    所以规则只写在这里，两边都调它（`export_site_env.overhead_camera` 也走这里）。
+
+    收敛规则（确定性的三级）：
+      1. ``arm`` 与当前臂相同的优先（仿真 ``sim_d435`` 靠这级选中）；
+      2. 都没有 ``arm``（= 双臂共用的那台）时回落到它；
+      3. 还是多个 → 取第一个，并把其余名字放进第二个返回值，让调用方**出声告警**。
+    """
+    cams = [
+        cfg
+        for cfg in (site.get("cameras") or {}).values()
+        if isinstance(cfg, dict)
+        and str(cfg.get("role") or "").strip().lower().startswith("overhead")
+    ]
+    if not cams:
+        return {}, []
+    arm = str((robot or {}).get("arm") or "").strip()
+    mine = [c for c in cams if str(c.get("arm") or "").strip() == arm] if arm else []
+    hits = mine or [c for c in cams if not str(c.get("arm") or "").strip()] or cams
+    chosen = dict(hits[0])
+    others = [str(c.get("name") or "?") for c in hits[1:]]
+    return chosen, others
+
+
+def eye_to_hand_result_name(short: str, camera_name: str) -> str:
+    """眼在手外结果文件的**文件名**（不含目录）。
+
+    外参绑定「这条臂的基座 + 这台相机」：换条臂或换台相机都不通用，
+    所以名字必须同时带上臂号与相机。调用方只许调这里，不许自己拼
+    （launch / export_site_env / S03 找文件都走这一处）。
+    """
+    short = str(short or "").strip()
+    cam = str(camera_name or "").strip().strip("/")
+    if short and cam:
+        return f"{short}_{cam}_eye_to_hand.yaml"
+    if cam:
+        return f"{cam}_eye_to_hand.yaml"
+    if short:
+        return f"{short}_eye_to_hand.yaml"
+    return "eye_to_hand.yaml"
+
+
+def resolve_eye_to_hand_file(
+    root: Path,
+    *,
+    explicit: str = "",
+    camera_name: str = "",
+) -> Path | None:
+    """找手外相机的手眼文件。没有就返回 None。
+
+    路径一律按 ``root`` 解析，不跟当前工作目录走——相对 CWD 的路径会在
+    "换个目录启动"时静默失效（文件找不到 → S03 直接降级），而人只会怀疑标定。
+
+    候选顺序：
+      1. ``explicit`` —— launch 按 site.yaml 派生的确切路径
+         （``<臂号>_<相机>_eye_to_hand.yaml``）；
+      2. ``<相机名>_eye_to_hand.yaml`` —— 旧命名 / 仿真（``sim_d435_...``）；
+      3. ``eye_to_hand.yaml`` —— 再兜底。
+    """
+    candidates: list[Path] = []
+    if explicit:
+        p = Path(explicit).expanduser()
+        candidates.append(p if p.is_absolute() else root / p)
+    cam = str(camera_name or "").strip().strip("/")
+    if cam:
+        candidates.append(root / "handeye" / "results" / f"{cam}_eye_to_hand.yaml")
+    candidates.append(root / "handeye" / "results" / "eye_to_hand.yaml")
+    for cand in candidates:
+        if cand.is_file():
+            return cand
+    return None
+
+
+TRACK_CAMERA_ROLES = ("wrist", "overhead")
+
+
+def track_camera_role(site: dict[str, Any] | None) -> str:
+    """一阶段跟踪用哪台相机：``wrist``（腕部 D405）或 ``overhead``（顶置 D435）。
+
+    写错**当场报错**，不会退回腕部 —— 退回会让人以为在验 D435，其实还在用 D405。
+    缺省 ``wrist``：没写这个键时行为与改之前一致（腕部追踪）。
+    """
+    perc = (site or {}).get("perception")
+    perc = perc if isinstance(perc, dict) else {}
+    raw = perc.get("track_camera")
+    if raw is None or str(raw).strip() == "":
+        return "wrist"
+    role = str(raw).strip().lower()
+    if role not in TRACK_CAMERA_ROLES:
+        raise ValueError(
+            f"perception.track_camera 只能是 wrist 或 overhead，收到 {raw!r}。"
+            "写错不会退回腕部 —— 退回会让人以为在用 D435 其实还在用 D405。"
+        )
+    return role
+
+
+def track_handeye_mode(role: str) -> str:
+    """跟踪相机角色 → 手眼装法。overhead 必须用 cam→基座，不能用 cam→法兰。"""
+    if str(role or "").strip().lower() == "overhead":
+        return "eye_to_hand"
+    return "eye_in_hand"
+
+
+def handeye_static_tf_frames(
+    *,
+    mode: str,
+    ee_frame: str,
+    base_frame: str,
+    cam_frame: str,
+) -> tuple[str, str]:
+    """手眼静态 TF 的 ``(父系, 子系)``。
+
+    两种装法的父子关系**不同**，不能混：
+      * ``eye_in_hand``：法兰 ← 相机（相机跟着臂动）
+      * ``eye_to_hand``：基座 ← 相机（相机固定，不随臂动）
+
+    把眼在手外的文件按"法兰 ← 相机"发出去，会得到一条**看着合理**的错 TF ——
+    定位总能到附近、永远差一截，而人只会去怀疑标定精度。
+    """
+    if str(mode or "").strip().lower() == "eye_to_hand":
+        return str(base_frame), str(cam_frame)
+    return str(ee_frame), str(cam_frame)
+

build/arm_application/build/lib/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]]:
@@ -514,7 +526,8 @@ class Stage3Search(Stage2Locate):
             tgt[2] += dz_m
             self._goto(q, Tee, tgt, verticalize=True, tol_m=0.001,
                        tol_tilt=self.vert_tol_rad * 3.0, tag="retract",
-                       v=0.03, max_hops=3, settle_s=0.2, min_dur=0.6)
+                       v=0.03, max_hops=2, settle_s=0.2, min_dur=0.6,
+                       hold_xy=True, z_up_only=True)
         except Exception:
             pass
 
@@ -581,9 +594,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}）"

build/arm_application/build/lib/arm_application/tactile_node.py

@@ -22,8 +22,10 @@ from arm_application.site_io import load_site_dict, site_path
 from arm_msgs.msg import TactileStatus
 from sensor_msgs.msg import Image
 
+from arm_application.ros_node import TolerantNode
 
-class TactileNode(Node):
+
+class TactileNode(TolerantNode):
     def __init__(self) -> None:
         super().__init__("tactile")
         self.declare_parameter("publish_hz", 20.0)

build/arm_application/build/lib/arm_application/weights_layout.py

@@ -176,6 +176,51 @@ def staging_dir(root: Union[str, Path]) -> Path:
     return Path(root) / STAGING_DIRNAME
 
 
+def infer_group(
+    root: Union[str, Path],
+    source: Union[str, Path],
+    fallback: str = "yolo",
+) -> str:
+    """转换产物该落到**哪个分组**：默认沿用源文件所在的分组。
+
+    为什么不能一律落到 ``yolo``（2026-09-28 真机踩到，代价是一次白转）
+    ----------------------------------------------------------------
+    现场把 ``models/hole_insert/detect_best_1.onnx`` 转 engine，工具默认
+    ``--group yolo``，于是产物落到 ``models/yolo/detect_best_1.engine`` ——
+    而运行时找 engine 的规则是**同级同名**（``yolo_detector.resolve_weights``：
+    ``p.with_suffix(".engine")``，即 site.yaml 指的 ``.onnx`` 旁边）。两边
+    一分家，engine 就等于不存在：现象是"启动即报缺 engine"，而文件明明躺着。
+    这正是本项目最忌讳的**静默失效** —— 工具没报错，位置也"合理"。
+
+    规则：
+      - 源文件已在权重根目录下的某个分组里（例如 ``models/hole_insert/x.onnx``）
+        → 沿用 ``hole_insert``。这样"就地转换"永远落在原地，运行时必然找得到；
+      - 源文件在 ``staging/``、权重根目录之外或根目录正下方 → ``fallback``
+        （默认 ``yolo``）：那才是"新来的模型，还没决定分组"的正常情形。
+
+    Args:
+        root: 权重根目录（``models/``）。
+        source: 输入文件。
+        fallback: 无法沿用时的分组名。
+
+    Returns:
+        分组名（不带路径）。
+    """
+    r = Path(root)
+    try:
+        rel = Path(source).expanduser().resolve().relative_to(r.resolve())
+    except ValueError:
+        # 不在权重根目录下（/tmp、外部磁盘、staging 之类）→ 没有"原地"可言
+        return fallback
+    if len(rel.parts) < 2:
+        # 直接躺在 models/ 正下方：没有分组信息，用默认分组
+        return fallback
+    group = rel.parts[0]
+    if is_staging(r / group) or group.startswith("."):
+        return fallback
+    return group
+
+
 def staging_files(root: Union[str, Path]) -> List[Path]:
     """staging 里的待处理文件。
 

build/arm_application/build/lib/arm_application/yolo_detector.py

@@ -43,6 +43,9 @@ class Detection:
     conf: float
     cls_id: int
     cls_name: str
+    # pose 模型才有：(K, 3) = 每个关键点的 x, y, conf（**原图像素坐标**）。
+    # 检测模型恒为 None —— 下游必须先判 None，别假设它有。
+    kpts: Optional[np.ndarray] = None
 
     @property
     def cx(self) -> float:
@@ -301,12 +304,51 @@ class YoloDetector:
         return seq, dets, (time.monotonic() - t0) * 1000.0
 
 
+def build_detector(
+    weights: Union[str, Path],
+    *,
+    backend: str = "auto",
+    conf: float = 0.25,
+    iou: float = 0.45,
+    device: str = "",
+    imgsz: int = 640,
+):
+    """按权重类型挑检测器：pose 模型走 ``PoseDetector``，否则走 ``YoloDetector``。
+
+    为什么要这个工厂：ultralytics 解不了 pose 的 TensorRT engine（见
+    ``pose_detector`` 模块头），所以 pose 必须换成自解码实现。判断依据是模型
+    是不是 ``task=pose`` —— 直接读旁边 ``.onnx`` 的 metadata，不靠调用方声明。
+
+    返回对象统一有 ``detect()/infer()/warmup()/imgsz/class_names/class_ids``。
+    """
+    path = Path(weights).expanduser()
+    from .pose_detector import PoseDetector, is_pose_model
+
+    if is_pose_model(path):
+        logger.info("检测到 pose 模型 %s → 用 PoseDetector（自解码关键点）", path.name)
+        return PoseDetector(
+            path,
+            conf=conf,
+            iou=iou,
+            device=device or "0",
+            imgsz=imgsz,
+        )
+    return YoloDetector(
+        path,
+        conf=conf,
+        iou=iou,
+        device=device,
+        imgsz=imgsz,
+    )
+
+
 def _yolo_worker_main(q_in, q_out, cfg: dict) -> None:
-    """spawn 出来的子进程：只跑 GPU 上的 YOLO。这里绝不能 import rclpy。"""
+    """spawn 出来的子进程：只跑 GPU 上的检测。这里绝不能 import rclpy。"""
     try:
         path = resolve_weights(cfg["weights"], cfg.get("backend") or "auto")
-        det = YoloDetector(
+        det = build_detector(
             path,
+            backend=cfg.get("backend") or "auto",
             conf=float(cfg.get("conf") or 0.25),
             iou=float(cfg.get("iou") or 0.45),
             device=str(cfg.get("device") or "0"),
@@ -336,7 +378,13 @@ def _yolo_worker_main(q_in, q_out, cfg: dict) -> None:
             dets = det.infer(image, half=bool(cfg.get("half", True)))
             gpu_ms = (time.monotonic() - t0) * 1000.0
             packed = [
-                (d.xyxy.astype(np.float32), float(d.conf), int(d.cls_id), str(d.cls_name))
+                (
+                    d.xyxy.astype(np.float32),
+                    float(d.conf),
+                    int(d.cls_id),
+                    str(d.cls_name),
+                    None if d.kpts is None else np.asarray(d.kpts, dtype=np.float32),
+                )
                 for d in dets
             ]
             _put_drop_old(q_out, ("ok", seq, packed, gpu_ms))
@@ -399,15 +447,20 @@ class YoloInferProcess:
     def alive(self) -> bool:
         return bool(self._p is not None and self._p.is_alive())
 
-    def submit(self, image_bgr: np.ndarray, seq: int = 0) -> None:
+    def submit(self, image_bgr: np.ndarray, seq: int = 0) -> bool:
         """把最新一帧排进队列。如果 worker 正忙就跳过 —— 它在每次推理后都会
-        排空到队列里最新的帧，所以不会跑任何过期数据。"""
+        排空到队列里最新的帧，所以不会跑任何过期数据。
+
+        返回是否真的进队。进不去必须告诉调用方，否则一次性会以为「已经在跑」
+        而空等，把 D405 也卡住。
+        """
         if not self.alive:
             raise RuntimeError("YOLO 工作进程已死亡")
         try:
             self._q_in.put((int(seq), image_bgr), timeout=0.010)
+            return True
         except Exception:
-            pass
+            return False
 
     def wait_result(self, timeout: float = 0.045):
         """返回 ``(seq, list[Detection], gpu_ms)`` 或 ``None``。"""
@@ -444,15 +497,20 @@ class YoloInferProcess:
             return None
         seq, packed = int(msg[1]), msg[2]
         gpu_ms = float(msg[3]) if len(msg) > 3 else 0.0
-        dets = [
+        dets = []
+        for row in packed:
+            # 兼容旧格式（4 元组，无关键点），免得子进程和父进程版本不一致时炸掉。
+            xyxy, conf, cid, name = row[0], row[1], row[2], row[3]
+            kpts = row[4] if len(row) > 4 else None
+            dets.append(
                 Detection(
                     xyxy=np.asarray(xyxy, dtype=float),
                     conf=float(conf),
                     cls_id=int(cid),
                     cls_name=str(name),
+                    kpts=None if kpts is None else np.asarray(kpts, dtype=float),
+                )
             )
-            for xyxy, conf, cid, name in packed
-        ]
         return seq, dets, gpu_ms
 
     def infer(self, image_bgr: np.ndarray, *, half: bool = False, seq: int = 0):

build/arm_application/setup.py

@@ -1 +1 @@
-../../src/arm_application/setup.py
+/home/cc/ros2/src/arm_application/setup.py

build/flexiv_hardware/CMakeFiles/flexiv_hardware.dir/compiler_depend.internal

@@ -22,283 +22,6 @@ CMakeFiles/flexiv_hardware.dir/src/flexiv_hardware_interface.cpp.o
  /home/cc/anaconda3/envs/ros_flexiv/include/class_loader/class_loader/visibility_control.hpp
  /home/cc/anaconda3/envs/ros_flexiv/include/console_bridge/console.h
  /home/cc/anaconda3/envs/ros_flexiv/include/console_bridge/console_bridge_export.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/Cholesky
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/Core
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/Dense
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/Eigen
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/Eigenvalues
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/Geometry
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/Householder
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/IterativeLinearSolvers
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/Jacobi
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/LU
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/OrderingMethods
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/QR
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/SVD
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/Sparse
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/SparseCholesky
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/SparseCore
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/SparseLU
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/SparseQR
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/Version
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Cholesky/InternalHeaderCheck.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Cholesky/LDLT.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Cholesky/LLT.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/ArithmeticSequence.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/Array.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/ArrayBase.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/ArrayWrapper.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/Assign.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/AssignEvaluator.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/BandMatrix.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/Block.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/CommaInitializer.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/ConditionEstimator.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/CoreEvaluators.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/CoreIterators.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/CwiseBinaryOp.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/CwiseNullaryOp.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/CwiseTernaryOp.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/CwiseUnaryOp.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/CwiseUnaryView.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/DenseBase.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/DenseCoeffsBase.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/DenseStorage.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/DeviceWrapper.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/Diagonal.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/DiagonalMatrix.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/DiagonalProduct.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/Dot.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/EigenBase.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/Fill.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/FindCoeff.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/Fuzzy.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/GeneralProduct.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/GenericPacketMath.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/GlobalFunctions.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/IO.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/IndexedView.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/InnerProduct.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/InternalHeaderCheck.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/Inverse.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/Map.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/MapBase.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/MathFunctions.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/MathFunctionsImpl.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/Matrix.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/MatrixBase.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/NestByValue.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/NoAlias.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/NumTraits.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/PartialReduxEvaluator.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/PermutationMatrix.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/PlainObjectBase.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/Product.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/ProductEvaluators.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/Random.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/RandomImpl.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/RealView.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/Redux.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/Ref.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/Replicate.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/Reshaped.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/ReturnByValue.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/Reverse.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/Select.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/SelfAdjointView.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/SelfCwiseBinaryOp.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/SkewSymmetricMatrix3.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/Solve.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/SolveTriangular.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/SolverBase.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/StableNorm.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/StlIterators.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/Stride.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/Swap.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/Transpose.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/Transpositions.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/TriangularMatrix.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/VectorBlock.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/VectorwiseOp.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/Visitor.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/arch/Default/BFloat16.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/arch/Default/ConjHelper.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/arch/Default/GenericPacketMathFunctions.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/arch/Default/GenericPacketMathFunctionsFwd.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/arch/Default/Half.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/arch/Default/Settings.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/arch/SSE/Complex.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/arch/SSE/MathFunctions.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/arch/SSE/PacketMath.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/arch/SSE/Reductions.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/arch/SSE/TypeCasting.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/functors/AssignmentFunctors.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/functors/BinaryFunctors.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/functors/NullaryFunctors.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/functors/StlFunctors.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/functors/TernaryFunctors.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/functors/UnaryFunctors.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/products/GeneralBlockPanelKernel.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/products/GeneralMatrixMatrix.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/products/GeneralMatrixMatrixTriangular.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/products/GeneralMatrixVector.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/products/Parallelizer.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/products/SelfadjointMatrixMatrix.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/products/SelfadjointMatrixVector.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/products/SelfadjointProduct.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/products/SelfadjointRank2Update.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/products/TriangularMatrixMatrix.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/products/TriangularMatrixVector.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/products/TriangularSolverMatrix.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/products/TriangularSolverVector.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/util/Assert.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/util/BlasUtil.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/util/ConfigureVectorization.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/util/Constants.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/util/DisableStupidWarnings.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/util/EmulateArray.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/util/ForwardDeclarations.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/util/IndexedViewHelper.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/util/IntegralConstant.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/util/MKL_support.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/util/Macros.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/util/Memory.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/util/Meta.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/util/MoreMeta.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/util/ReenableStupidWarnings.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/util/ReshapedHelper.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/util/Serializer.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/util/StaticAssert.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/util/SymbolicIndex.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Core/util/XprHelper.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Eigenvalues/ComplexEigenSolver.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Eigenvalues/ComplexSchur.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Eigenvalues/EigenSolver.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Eigenvalues/GeneralizedEigenSolver.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Eigenvalues/GeneralizedSelfAdjointEigenSolver.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Eigenvalues/HessenbergDecomposition.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Eigenvalues/InternalHeaderCheck.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Eigenvalues/MatrixBaseEigenvalues.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Eigenvalues/RealQZ.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Eigenvalues/RealSchur.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Eigenvalues/SelfAdjointEigenSolver.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Eigenvalues/Tridiagonalization.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Geometry/AlignedBox.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Geometry/AngleAxis.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Geometry/EulerAngles.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Geometry/Homogeneous.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Geometry/Hyperplane.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Geometry/InternalHeaderCheck.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Geometry/OrthoMethods.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Geometry/ParametrizedLine.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Geometry/Quaternion.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Geometry/Rotation2D.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Geometry/RotationBase.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Geometry/Scaling.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Geometry/Transform.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Geometry/Translation.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Geometry/Umeyama.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Geometry/arch/Geometry_SIMD.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Householder/BlockHouseholder.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Householder/Householder.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Householder/HouseholderSequence.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Householder/InternalHeaderCheck.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/IterativeLinearSolvers/BasicPreconditioners.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/IterativeLinearSolvers/BiCGSTAB.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/IterativeLinearSolvers/ConjugateGradient.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/IterativeLinearSolvers/IncompleteCholesky.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/IterativeLinearSolvers/IncompleteLUT.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/IterativeLinearSolvers/InternalHeaderCheck.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/IterativeLinearSolvers/IterativeSolverBase.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/IterativeLinearSolvers/LeastSquareConjugateGradient.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/IterativeLinearSolvers/SolveWithGuess.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Jacobi/InternalHeaderCheck.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/Jacobi/Jacobi.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/LU/Determinant.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/LU/FullPivLU.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/LU/InternalHeaderCheck.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/LU/InverseImpl.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/LU/PartialPivLU.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/LU/arch/InverseSize4.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/OrderingMethods/Amd.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/OrderingMethods/Eigen_Colamd.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/OrderingMethods/InternalHeaderCheck.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/OrderingMethods/Ordering.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/QR/ColPivHouseholderQR.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/QR/CompleteOrthogonalDecomposition.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/QR/FullPivHouseholderQR.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/QR/HouseholderQR.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/QR/InternalHeaderCheck.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SVD/BDCSVD.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SVD/InternalHeaderCheck.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SVD/JacobiSVD.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SVD/SVDBase.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SVD/UpperBidiagonalization.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCholesky/InternalHeaderCheck.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCholesky/SimplicialCholesky.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCholesky/SimplicialCholesky_impl.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/AmbiVector.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/CompressedStorage.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/ConservativeSparseSparseProduct.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/InternalHeaderCheck.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/SparseAssign.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/SparseBlock.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/SparseColEtree.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/SparseCompressedBase.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/SparseCwiseBinaryOp.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/SparseCwiseUnaryOp.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/SparseDenseProduct.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/SparseDiagonalProduct.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/SparseDot.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/SparseFuzzy.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/SparseMap.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/SparseMatrix.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/SparseMatrixBase.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/SparsePermutation.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/SparseProduct.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/SparseRedux.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/SparseRef.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/SparseSelfAdjointView.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/SparseSolverBase.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/SparseSparseProductWithPruning.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/SparseTranspose.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/SparseTriangularView.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/SparseUtil.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/SparseVector.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/SparseView.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseCore/TriangularSolver.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseLU/InternalHeaderCheck.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseLU/SparseLU.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseLU/SparseLUImpl.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseLU/SparseLU_Memory.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseLU/SparseLU_Structs.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseLU/SparseLU_SupernodalMatrix.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseLU/SparseLU_Utils.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseLU/SparseLU_column_bmod.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseLU/SparseLU_column_dfs.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseLU/SparseLU_copy_to_ucol.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseLU/SparseLU_heap_relax_snode.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseLU/SparseLU_kernel_bmod.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseLU/SparseLU_panel_bmod.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseLU/SparseLU_panel_dfs.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseLU/SparseLU_pivotL.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseLU/SparseLU_pruneL.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseLU/SparseLU_relax_snode.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseQR/InternalHeaderCheck.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/SparseQR/SparseQR.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/misc/Image.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/misc/InternalHeaderCheck.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/misc/Kernel.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/misc/RealSvd2x2.h
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/plugins/ArrayCwiseBinaryOps.inc
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/plugins/ArrayCwiseUnaryOps.inc
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/plugins/BlockMethods.inc
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/plugins/CommonCwiseBinaryOps.inc
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/plugins/CommonCwiseUnaryOps.inc
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/plugins/IndexedViewMethods.inc
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/plugins/MatrixCwiseBinaryOps.inc
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/plugins/MatrixCwiseUnaryOps.inc
- /home/cc/anaconda3/envs/ros_flexiv/include/eigen3/Eigen/src/plugins/ReshapedMethods.inc
  /home/cc/anaconda3/envs/ros_flexiv/include/hardware_interface/handle.hpp
  /home/cc/anaconda3/envs/ros_flexiv/include/hardware_interface/hardware_info.hpp
  /home/cc/anaconda3/envs/ros_flexiv/include/hardware_interface/macros.hpp
@@ -787,12 +510,10 @@ CMakeFiles/flexiv_hardware.dir/src/flexiv_hardware_interface.cpp.o
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/cctype
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/cerrno
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/chrono
- /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/climits
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/clocale
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/cmath
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/codecvt
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/compare
- /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/complex
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/concepts
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/condition_variable
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/csignal
@@ -872,7 +593,6 @@ CMakeFiles/flexiv_hardware.dir/src/flexiv_hardware_interface.cpp.o
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/utility
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/variant
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/vector
- /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/version
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/x86_64-conda-linux-gnu/bits/atomic_word.h
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/x86_64-conda-linux-gnu/bits/basic_file.h
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/x86_64-conda-linux-gnu/bits/c++allocator.h
@@ -889,17 +609,11 @@ CMakeFiles/flexiv_hardware.dir/src/flexiv_hardware_interface.cpp.o
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/x86_64-conda-linux-gnu/bits/messages_members.h
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/x86_64-conda-linux-gnu/bits/os_defines.h
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/x86_64-conda-linux-gnu/bits/time_members.h
- /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/emmintrin.h
- /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/limits.h
- /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/mm_malloc.h
- /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/mmintrin.h
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/stdalign.h
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/stdarg.h
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/stdbool.h
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/stddef.h
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/stdint.h
- /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/syslimits.h
- /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/xmmintrin.h
  /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/alloca.h
  /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/arpa/inet.h
  /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/asm-generic/bitsperlong.h
@@ -933,15 +647,12 @@ CMakeFiles/flexiv_hardware.dir/src/flexiv_hardware_interface.cpp.o
  /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/bits/iscanonical.h
  /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/bits/libc-header-start.h
  /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/bits/libm-simd-decl-stubs.h
- /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/bits/local_lim.h
  /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/bits/locale.h
  /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/bits/long-double.h
  /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/bits/math-vector.h
  /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/bits/mathcalls-helper-functions.h
  /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/bits/mathcalls-narrow.h
  /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/bits/mathcalls.h
- /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/bits/posix1_lim.h
- /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/bits/posix2_lim.h
  /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/bits/posix_opt.h
  /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/bits/pthread_stack_min-dynamic.h
  /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/bits/pthreadtypes-arch.h
@@ -1014,14 +725,12 @@ CMakeFiles/flexiv_hardware.dir/src/flexiv_hardware_interface.cpp.o
  /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/bits/types/wint_t.h
  /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/bits/typesizes.h
  /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/bits/uintn-identity.h
- /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/bits/uio_lim.h
  /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/bits/unistd_ext.h
  /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/bits/waitflags.h
  /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/bits/waitstatus.h
  /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/bits/wchar.h
  /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/bits/wctype-wchar.h
  /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/bits/wordsize.h
- /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/bits/xopen_lim.h
  /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/ctype.h
  /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/endian.h
  /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/errno.h
@@ -1031,10 +740,8 @@ CMakeFiles/flexiv_hardware.dir/src/flexiv_hardware_interface.cpp.o
  /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/gnu/stubs.h
  /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/ifaddrs.h
  /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/libintl.h
- /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/limits.h
  /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/linux/close_range.h
  /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/linux/errno.h
- /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/linux/limits.h
  /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/linux/posix_types.h
  /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/linux/stddef.h
  /home/cc/anaconda3/envs/ros_flexiv/x86_64-conda-linux-gnu/sysroot/usr/include/locale.h
@@ -1063,7 +770,6 @@ CMakeFiles/flexiv_hardware.dir/src/flexiv_hardware_interface.cpp.o
  /home/cc/flexiv_install/include/flexiv/rdk/data.hpp
  /home/cc/flexiv_install/include/flexiv/rdk/mode.hpp
  /home/cc/flexiv_install/include/flexiv/rdk/robot.hpp
- /home/cc/flexiv_install/include/flexiv/rdk/utility.hpp
  /home/cc/ros2/src/flexiv_hardware/include/flexiv_hardware/flexiv_hardware_interface.hpp
  /home/cc/ros2/src/flexiv_hardware/include/flexiv_hardware/visibility_control.h
 

build/flexiv_hardware/CMakeFiles/flexiv_hardware.dir/src/flexiv_hardware_interface.cpp.o.d

@@ -237,10 +237,16 @@ CMakeFiles/flexiv_hardware.dir/src/flexiv_hardware_interface.cpp.o: \
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/x86_64-conda-linux-gnu/bits/basic_file.h \
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/x86_64-conda-linux-gnu/bits/c++io.h \
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/bits/fstream.tcc \
+ /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/map \
+ /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/bits/stl_tree.h \
+ /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/ext/aligned_buffer.h \
+ /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/bits/node_handle.h \
+ /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/bits/stl_map.h \
+ /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/bits/stl_multimap.h \
+ /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/bits/erase_if.h \
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/variant \
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/bits/enable_special_members.h \
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/bits/monostate.h \
- /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/ext/aligned_buffer.h \
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/thread \
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/bits/std_thread.h \
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/bits/unique_ptr.h \
@@ -328,11 +334,8 @@ CMakeFiles/flexiv_hardware.dir/src/flexiv_hardware_interface.cpp.o: \
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/bits/std_function.h \
  /home/cc/anaconda3/envs/ros_flexiv/include/rclcpp/rclcpp/executors/multi_threaded_executor.hpp \
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/set \
- /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/bits/stl_tree.h \
- /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/bits/node_handle.h \
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/bits/stl_set.h \
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/bits/stl_multiset.h \
- /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/bits/erase_if.h \
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/unordered_map \
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/bits/unordered_map.h \
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/bits/hashtable.h \
@@ -344,9 +347,6 @@ CMakeFiles/flexiv_hardware.dir/src/flexiv_hardware_interface.cpp.o: \
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/list \
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/bits/stl_list.h \
  /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/bits/list.tcc \
- /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/map \
- /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/bits/stl_map.h \
- /home/cc/anaconda3/envs/ros_flexiv/lib/gcc/x86_64-conda-linux-gnu/15.2.0/include/c++/bits/stl_multimap.h \
  /home/cc/anaconda3/envs/ros_flexiv/include/rcl/rcl/guard_condition.h \
  /home/cc/anaconda3/envs/ros_flexiv/include/rcl/rcl/allocator.h \
  /home/cc/anaconda3/envs/ros_flexiv/include/rcutils/rcutils/allocator.h \

build/flexiv_hardware/cmake_args.last

@@ -1 +1 @@
-['-DCMAKE_BUILD_TYPE=Release']
+['-DAMENT_CMAKE_SYMLINK_INSTALL=1']

config/site.example.yaml

@@ -936,8 +936,8 @@ gauge_assembly:
   # ---- S01 回 home 后按工具系逆时针预绕到极限角 ----
   # 预绕是为了给 S08 的棘轮留出回转行程：不预绕，拧两圈就贴关节限位。
   extreme_angle_deg: 150.0   # 想转多少度（按工具系逆时针）
-  extreme_speed_deg_s: 10.0  # 关节角速度 [deg/s]。2026-10-19 左臂 A7 咬合从 20 降到 10，
-                             # 限位到限位约 34s（原先约 17s）。顺时针进孔用这个速度。
+  extreme_speed_deg_s: 10.0  # 规装配预绕的关节角速度 [deg/s]。
+                             # 插孔后的四段 A7 不读这个数，走接口上限 0.90 rad/s。
   unwind_open_permille: 400  # 第三步松爪只开到这个千分比。0=合，1000=全开。
                              # 2026-10-19 从 200 改成 400，只张开这个量。
   unwind_lift_mm: 3.0       # 第三步张开后只抬的 TCP Z [mm]（0.3cm）。

config/site.yaml

@@ -920,8 +920,8 @@ gauge_assembly:
   # ---- S01 回 home 后按工具系逆时针预绕到极限角 ----
   # 预绕是为了给 S08 的棘轮留出回转行程：不预绕，拧两圈就贴关节限位。
   extreme_angle_deg: 150.0   # 想转多少度（按工具系逆时针）
-  extreme_speed_deg_s: 10.0  # 关节角速度 [deg/s]。2026-10-19 左臂 A7 咬合从 20 降到 10，
-                             # 限位到限位约 34s（原先约 17s）。顺时针进孔用这个速度。
+  extreme_speed_deg_s: 10.0  # 规装配预绕的关节角速度 [deg/s]。
+                             # 插孔后的四段 A7 不读这个数，走接口上限 0.90 rad/s。
   unwind_open_permille: 400  # 第三步松爪只开到这个千分比。0=合，1000=全开。
                              # 2026-10-19 从 200 改成 400，只张开这个量。
   unwind_lift_mm: 3.0       # 第三步张开后只抬的 TCP Z [mm]（0.3cm）。

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` 加上 `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 对不上，就停在当前关节。
+然后只转左臂 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 再逆时针回到限位。然后下降刚才实测的那一段（不另写一个下降量），`--cli close`。合爪目标是位置 0；固件在位置先到时写 grip=1（到位），力先到才写 grip=2（夹住）。手指已经在合拢端就继续，还停在张开位则不转。再顺时针转到限位。这四段 A7 都请求接口上限 0.90 rad/s；两端速度为 0 的样条峰速约是平均的 1.5 倍，所以整段按平均约 34 °/s 排程，避免时间到了还差几十度。`extreme_speed_deg_s` 只留给规装配预绕。前六轴不动。顺时针是进孔，逆时针是出孔。夹爪或这一段 Z 对不上，就停在当前关节。
 只测 D435 这一段位移时加 `--d435-only`：不交 D405、不进二阶段，并且要求先停 `run_real.sh`。
 
 ```bash

handeye/poses/backup/89_eye_in_hand_21pts_before_20261016.yaml

@@ -0,0 +1,56 @@
+# 手眼标定示教的关节位姿（7 关节 × N）。由 calibrate + waypoints 共用。
+#
+# 在这里编辑，或者：
+#   bash ~/ros2/scripts/run_handeye.sh teach
+#   bash ~/ros2/scripts/run_handeye.sh merge
+#
+# /** 这样任意节点名都能收到这些参数。
+
+/**:
+  ros__parameters:
+    num_poses: 21
+    # 2026-09-02 通过 handeye_teach 示教（RDK 自由驱动）
+    waypoint_positions: [
+      # pose 0
+      -0.928421, -1.237477, 1.528823, 1.526334, -1.230146, 0.098562, 0.538531,
+      # pose 1
+      -0.970203, -1.315554, 1.553230, 1.669383, -1.407869, 0.125907, 0.578173,
+      # pose 2
+      -0.967819, -1.525572, 1.706057, 1.357062, -1.211051, 0.023232, 0.193541,
+      # pose 3
+      -1.065106, -1.624218, 1.793644, 1.279402, -1.120929, 0.019151, 0.139658,
+      # pose 4
+      -0.386279, -1.551838, 1.563578, 1.552444, -1.775337, -0.341974, 0.957445,
+      # pose 5
+      -0.227779, -1.219414, 1.026882, 0.828801, -0.916809, -0.166985, 0.034154,
+      # pose 6
+      -0.324968, -1.273082, 1.092344, 0.723132, -0.922095, -0.206413, 0.152082,
+      # pose 7
+      -0.421966, -1.309101, 1.143065, 0.606456, -0.906673, -0.219282, 0.237953,
+      # pose 8
+      -0.851598, -1.260600, 1.302895, 1.127033, -0.969809, 0.032856, 0.642673,
+      # pose 9
+      -0.951418, -0.550124, 0.848598, 2.139787, -1.025778, 1.184418, 0.598394,
+      # pose 10
+      -0.834498, -0.635767, 0.970038, 2.058410, -1.035138, 1.032040, 0.738619,
+      # pose 11
+      -0.519528, -0.883321, 1.135035, 1.830274, -1.100901, 0.234065, -0.069068,
+      # pose 12
+      -0.347358, -1.081426, 1.192233, 1.514557, -1.244594, -0.116668, -0.564362,
+      # pose 13
+      -0.364832, -1.102689, 1.224844, 1.520320, -1.265760, -0.136855, -0.582459,
+      # pose 14
+      -1.130796, -1.316186, 1.711089, 1.821409, -1.278003, 0.010885, -1.337210,
+      # pose 15
+      -0.732222, -1.376363, 1.812339, 1.293854, -1.410826, -0.226771, -0.217230,
+      # pose 16
+      -0.885017, -1.493416, 1.894070, 1.379021, -1.520015, -0.257115, 1.025331,
+      # pose 17
+      -0.911769, -1.532695, 1.944123, 1.427787, -1.560513, -0.267425, 1.004398,
+      # pose 18
+      -1.436271, -1.768595, 1.559397, 2.244717, -2.057112, 0.341785, 0.749883,
+      # pose 19
+      -1.511828, -1.759337, 1.620075, 2.131574, -1.946595, 0.328227, 0.584657,
+      # pose 20
+      -1.180482, -1.643241, 1.865851, 1.597699, -1.464982, 0.097605, -0.217780,
+    ]

handeye/poses/backup/89_eye_to_hand_24pts_before_20261016.yaml

@@ -0,0 +1,60 @@
+# 眼在手外示教关节（7 关节 × N）。板夹在 tool10120938 上，顶置 D435 看它。
+#
+# 由 teach → merge 写入，不要手抄眼在手上那份（两种模式的姿态完全不同）。
+#
+#   bash ~/ros2/scripts/run_handeye.sh --arm 89 teach --mode eye_to_hand --points 12
+#   bash ~/ros2/scripts/run_handeye.sh --arm 89 merge --mode eye_to_hand
+
+/**:
+  ros__parameters:
+    num_poses: 24
+    waypoint_positions: [
+      # pose 0
+      -1.241563, -0.359631, 0.346153, 2.172565, -0.179027, 0.929920, -0.778876,
+      # pose 1
+      -1.388832, -0.554698, 0.416075, 1.897617, -0.358341, 0.827495, -0.762302,
+      # pose 2
+      -1.436048, -0.573046, 0.461762, 1.870967, -0.387696, 0.813090, -0.762917,
+      # pose 3
+      -1.614491, -0.519365, 0.693991, 2.156193, -0.499113, 0.968762, 0.073287,
+      # pose 4
+      -2.020087, -0.869088, 1.059166, 2.155478, -1.198473, 0.740567, 0.478840,
+      # pose 5
+      -2.082145, -1.122210, 1.203011, 1.950272, -1.269847, 0.434138, -0.641975,
+      # pose 6
+      -1.983153, -0.886082, 0.999329, 2.324821, -1.272773, 0.754127, 0.313613,
+      # pose 7
+      -2.021479, -0.886609, 1.055679, 2.337716, -1.282773, 0.669319, -0.028355,
+      # pose 8
+      -2.022792, -0.947211, 1.118194, 2.127275, -1.211995, 0.527528, -0.169900,
+      # pose 9
+      -2.004766, -1.065820, 1.183799, 1.901084, -1.217488, 0.380285, -0.312733,
+      # pose 10
+      -1.942863, -1.219617, 1.256733, 1.653113, -1.222302, 0.261818, -0.420706,
+      # pose 11
+      -2.039558, -1.082441, 1.189833, 1.996967, -1.241252, 0.521591, 0.526563,
+      # pose 12
+      -2.098074, -1.006236, 1.538183, 2.118382, -0.737422, 0.019903, 0.648802,
+      # pose 13
+      -2.039609, -1.159222, 1.488501, 1.908579, -1.168793, -0.140451, 0.034889,
+      # pose 14
+      -1.880783, -1.104601, 1.579382, 2.036427, -0.665049, 0.254105, 1.518504,
+      # pose 15
+      -2.015530, -1.535468, 1.711220, 1.836908, -1.707450, -0.400234, -0.708683,
+      # pose 16
+      -1.997672, -1.626362, 1.529443, 2.200206, -1.291013, 0.398664, 1.954208,
+      # pose 17
+      -1.981875, -1.620651, 1.545443, 2.373140, -1.167365, 0.396487, 2.012547,
+      # pose 18
+      -2.179415, -1.511968, 1.578832, 2.401706, -1.326480, -0.051353, 1.137181,
+      # pose 19
+      -2.173463, -1.530413, 1.575534, 2.173673, -1.289673, -0.090117, 0.867214,
+      # pose 20
+      -2.096314, -1.569352, 1.579820, 1.942057, -1.458598, -0.057003, 0.572953,
+      # pose 21
+      -2.129376, -1.566787, 1.903585, 1.962858, -1.454137, -0.335990, -0.148441,
+      # pose 22
+      -2.020280, -1.527165, 1.887130, 1.728957, -1.491867, -0.370585, -0.277940,
+      # pose 23
+      -2.140817, -1.607357, 1.617236, 2.153850, -1.571057, -0.147540, 0.770535,
+    ]

handeye/results/backup/89_d435_eye_to_hand_tool10120938_oldtcp_20261012.yaml

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

handeye/results/backup/89_eye_in_hand_tool10120938_20261012.yaml

@@ -0,0 +1,28 @@
+# 手眼标定结果
+# mode: eye_in_hand
+# camera_source: d405_left
+# views: 21
+# utc: 2026-10-12T03:29:51.243487+00:00
+# board: 11x8 square_m=0.015
+# gripper_tool: tool10120938
+# T_4x4 = cam→法兰（AX=XB 在 TCP 上解，再左乘 T_flange_to_tool）
+# T_cam_to_tool 是原始求解；旧的法兰/tool1 备份在 results/backup/
+# 2026-10-12 第一次写出的 T_4x4 乘序反了（tz≈-296 mm），已按同一份 T_cam_to_tool 重折。
+gripper_tool: tool10120938
+T_flange_to_tool:
+  - [1.00000000, 0.00000000, 0.00000000, -0.00050000]
+  - [0.00000000, 1.00000000, 0.00000000, 0.00670000]
+  - [0.00000000, 0.00000000, 1.00000000, 0.22450000]
+  - [0.00000000, 0.00000000, 0.00000000, 1.00000000]
+T_cam_to_tool:
+  - [-0.03435184, -0.86951953, 0.49270249, -0.05961995]
+  - [0.99932767, -0.03620511, 0.00577974, -0.01523625]
+  - [0.01281276, 0.49256977, 0.87017863, -0.09761517]
+  - [0.00000000, 0.00000000, 0.00000000, 1.00000000]
+# T_cam_to_flange (eye_in_hand，由工具 TCP 折算)
+translation_m: [-0.06011995, -0.00853625, 0.12688483]
+T_4x4:
+  - [-0.03435184, -0.86951953, 0.49270249, -0.06011995]
+  - [0.99932767, -0.03620511, 0.00577974, -0.00853625]
+  - [0.01281276, 0.49256977, 0.87017863, 0.12688483]
+  - [0.00000000, 0.00000000, 0.00000000, 1.00000000]

install/arm_application/lib/arm_application/assembly_pipeline

@@ -1,10 +1,10 @@
 #!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
-# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application==0.1.0','console_scripts','assembly_pipeline'
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','assembly_pipeline'
 import re
 import sys
 
 # for compatibility with easy_install; see #2198
-__requires__ = 'arm-application==0.1.0'
+__requires__ = 'arm-application'
 
 try:
     from importlib.metadata import distribution
@@ -30,4 +30,4 @@ globals().setdefault('load_entry_point', importlib_load_entry_point)
 
 if __name__ == '__main__':
     sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
-    sys.exit(load_entry_point('arm-application==0.1.0', 'console_scripts', 'assembly_pipeline')())
+    sys.exit(load_entry_point('arm-application', 'console_scripts', 'assembly_pipeline')())

install/arm_application/lib/arm_application/demo_named_poses

@@ -1,10 +1,10 @@
 #!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
-# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application==0.1.0','console_scripts','demo_named_poses'
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','demo_named_poses'
 import re
 import sys
 
 # for compatibility with easy_install; see #2198
-__requires__ = 'arm-application==0.1.0'
+__requires__ = 'arm-application'
 
 try:
     from importlib.metadata import distribution
@@ -30,4 +30,4 @@ globals().setdefault('load_entry_point', importlib_load_entry_point)
 
 if __name__ == '__main__':
     sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
-    sys.exit(load_entry_point('arm-application==0.1.0', 'console_scripts', 'demo_named_poses')())
+    sys.exit(load_entry_point('arm-application', 'console_scripts', 'demo_named_poses')())

install/arm_application/lib/arm_application/dh_pgea

@@ -1,10 +1,10 @@
 #!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
-# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application==0.1.0','console_scripts','dh_pgea'
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','dh_pgea'
 import re
 import sys
 
 # for compatibility with easy_install; see #2198
-__requires__ = 'arm-application==0.1.0'
+__requires__ = 'arm-application'
 
 try:
     from importlib.metadata import distribution
@@ -30,4 +30,4 @@ globals().setdefault('load_entry_point', importlib_load_entry_point)
 
 if __name__ == '__main__':
     sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
-    sys.exit(load_entry_point('arm-application==0.1.0', 'console_scripts', 'dh_pgea')())
+    sys.exit(load_entry_point('arm-application', 'console_scripts', 'dh_pgea')())

install/arm_application/lib/arm_application/dh_pgea_cli

@@ -1,10 +1,10 @@
 #!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
-# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application==0.1.0','console_scripts','dh_pgea_cli'
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','dh_pgea_cli'
 import re
 import sys
 
 # for compatibility with easy_install; see #2198
-__requires__ = 'arm-application==0.1.0'
+__requires__ = 'arm-application'
 
 try:
     from importlib.metadata import distribution
@@ -30,4 +30,4 @@ globals().setdefault('load_entry_point', importlib_load_entry_point)
 
 if __name__ == '__main__':
     sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
-    sys.exit(load_entry_point('arm-application==0.1.0', 'console_scripts', 'dh_pgea_cli')())
+    sys.exit(load_entry_point('arm-application', 'console_scripts', 'dh_pgea_cli')())

install/arm_application/lib/arm_application/dual_gripper

@@ -1,10 +1,10 @@
 #!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
-# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application==0.1.0','console_scripts','dual_gripper'
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','dual_gripper'
 import re
 import sys
 
 # for compatibility with easy_install; see #2198
-__requires__ = 'arm-application==0.1.0'
+__requires__ = 'arm-application'
 
 try:
     from importlib.metadata import distribution
@@ -30,4 +30,4 @@ globals().setdefault('load_entry_point', importlib_load_entry_point)
 
 if __name__ == '__main__':
     sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
-    sys.exit(load_entry_point('arm-application==0.1.0', 'console_scripts', 'dual_gripper')())
+    sys.exit(load_entry_point('arm-application', 'console_scripts', 'dual_gripper')())

install/arm_application/lib/arm_application/dual_gripper_cli

@@ -1,10 +1,10 @@
 #!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
-# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application==0.1.0','console_scripts','dual_gripper_cli'
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','dual_gripper_cli'
 import re
 import sys
 
 # for compatibility with easy_install; see #2198
-__requires__ = 'arm-application==0.1.0'
+__requires__ = 'arm-application'
 
 try:
     from importlib.metadata import distribution
@@ -30,4 +30,4 @@ globals().setdefault('load_entry_point', importlib_load_entry_point)
 
 if __name__ == '__main__':
     sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
-    sys.exit(load_entry_point('arm-application==0.1.0', 'console_scripts', 'dual_gripper_cli')())
+    sys.exit(load_entry_point('arm-application', 'console_scripts', 'dual_gripper_cli')())

install/arm_application/lib/arm_application/gauge_assembly

@@ -1,10 +1,10 @@
 #!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
-# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application==0.1.0','console_scripts','gauge_assembly'
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','gauge_assembly'
 import re
 import sys
 
 # for compatibility with easy_install; see #2198
-__requires__ = 'arm-application==0.1.0'
+__requires__ = 'arm-application'
 
 try:
     from importlib.metadata import distribution
@@ -30,4 +30,4 @@ globals().setdefault('load_entry_point', importlib_load_entry_point)
 
 if __name__ == '__main__':
     sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
-    sys.exit(load_entry_point('arm-application==0.1.0', 'console_scripts', 'gauge_assembly')())
+    sys.exit(load_entry_point('arm-application', 'console_scripts', 'gauge_assembly')())

install/arm_application/lib/arm_application/goto_ee_plane

@@ -1,10 +1,10 @@
 #!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
-# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application==0.1.0','console_scripts','goto_ee_plane'
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','goto_ee_plane'
 import re
 import sys
 
 # for compatibility with easy_install; see #2198
-__requires__ = 'arm-application==0.1.0'
+__requires__ = 'arm-application'
 
 try:
     from importlib.metadata import distribution
@@ -30,4 +30,4 @@ globals().setdefault('load_entry_point', importlib_load_entry_point)
 
 if __name__ == '__main__':
     sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
-    sys.exit(load_entry_point('arm-application==0.1.0', 'console_scripts', 'goto_ee_plane')())
+    sys.exit(load_entry_point('arm-application', 'console_scripts', 'goto_ee_plane')())

install/arm_application/lib/arm_application/handeye_calibrate

@@ -1,10 +1,10 @@
 #!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
-# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application==0.1.0','console_scripts','handeye_calibrate'
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','handeye_calibrate'
 import re
 import sys
 
 # for compatibility with easy_install; see #2198
-__requires__ = 'arm-application==0.1.0'
+__requires__ = 'arm-application'
 
 try:
     from importlib.metadata import distribution
@@ -30,4 +30,4 @@ globals().setdefault('load_entry_point', importlib_load_entry_point)
 
 if __name__ == '__main__':
     sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
-    sys.exit(load_entry_point('arm-application==0.1.0', 'console_scripts', 'handeye_calibrate')())
+    sys.exit(load_entry_point('arm-application', 'console_scripts', 'handeye_calibrate')())

install/arm_application/lib/arm_application/handeye_teach

@@ -1,10 +1,10 @@
 #!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
-# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application==0.1.0','console_scripts','handeye_teach'
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','handeye_teach'
 import re
 import sys
 
 # for compatibility with easy_install; see #2198
-__requires__ = 'arm-application==0.1.0'
+__requires__ = 'arm-application'
 
 try:
     from importlib.metadata import distribution
@@ -30,4 +30,4 @@ globals().setdefault('load_entry_point', importlib_load_entry_point)
 
 if __name__ == '__main__':
     sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
-    sys.exit(load_entry_point('arm-application==0.1.0', 'console_scripts', 'handeye_teach')())
+    sys.exit(load_entry_point('arm-application', 'console_scripts', 'handeye_teach')())

install/arm_application/lib/arm_application/handeye_waypoints

@@ -1,10 +1,10 @@
 #!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
-# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application==0.1.0','console_scripts','handeye_waypoints'
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','handeye_waypoints'
 import re
 import sys
 
 # for compatibility with easy_install; see #2198
-__requires__ = 'arm-application==0.1.0'
+__requires__ = 'arm-application'
 
 try:
     from importlib.metadata import distribution
@@ -30,4 +30,4 @@ globals().setdefault('load_entry_point', importlib_load_entry_point)
 
 if __name__ == '__main__':
     sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
-    sys.exit(load_entry_point('arm-application==0.1.0', 'console_scripts', 'handeye_waypoints')())
+    sys.exit(load_entry_point('arm-application', 'console_scripts', 'handeye_waypoints')())

install/arm_application/lib/arm_application/hole_detect

@@ -1,10 +1,10 @@
 #!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
-# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application==0.1.0','console_scripts','hole_detect'
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','hole_detect'
 import re
 import sys
 
 # for compatibility with easy_install; see #2198
-__requires__ = 'arm-application==0.1.0'
+__requires__ = 'arm-application'
 
 try:
     from importlib.metadata import distribution
@@ -30,4 +30,4 @@ globals().setdefault('load_entry_point', importlib_load_entry_point)
 
 if __name__ == '__main__':
     sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
-    sys.exit(load_entry_point('arm-application==0.1.0', 'console_scripts', 'hole_detect')())
+    sys.exit(load_entry_point('arm-application', 'console_scripts', 'hole_detect')())

install/arm_application/lib/arm_application/move_to_pose_client

@@ -1,10 +1,10 @@
 #!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
-# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application==0.1.0','console_scripts','move_to_pose_client'
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','move_to_pose_client'
 import re
 import sys
 
 # for compatibility with easy_install; see #2198
-__requires__ = 'arm-application==0.1.0'
+__requires__ = 'arm-application'
 
 try:
     from importlib.metadata import distribution
@@ -30,4 +30,4 @@ globals().setdefault('load_entry_point', importlib_load_entry_point)
 
 if __name__ == '__main__':
     sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
-    sys.exit(load_entry_point('arm-application==0.1.0', 'console_scripts', 'move_to_pose_client')())
+    sys.exit(load_entry_point('arm-application', 'console_scripts', 'move_to_pose_client')())

install/arm_application/lib/arm_application/perception_node

@@ -1,10 +1,10 @@
 #!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
-# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application==0.1.0','console_scripts','perception_node'
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','perception_node'
 import re
 import sys
 
 # for compatibility with easy_install; see #2198
-__requires__ = 'arm-application==0.1.0'
+__requires__ = 'arm-application'
 
 try:
     from importlib.metadata import distribution
@@ -30,4 +30,4 @@ globals().setdefault('load_entry_point', importlib_load_entry_point)
 
 if __name__ == '__main__':
     sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
-    sys.exit(load_entry_point('arm-application==0.1.0', 'console_scripts', 'perception_node')())
+    sys.exit(load_entry_point('arm-application', 'console_scripts', 'perception_node')())

install/arm_application/lib/arm_application/realsense_color_pub

@@ -1,10 +1,10 @@
 #!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
-# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application==0.1.0','console_scripts','realsense_color_pub'
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','realsense_color_pub'
 import re
 import sys
 
 # for compatibility with easy_install; see #2198
-__requires__ = 'arm-application==0.1.0'
+__requires__ = 'arm-application'
 
 try:
     from importlib.metadata import distribution
@@ -30,4 +30,4 @@ globals().setdefault('load_entry_point', importlib_load_entry_point)
 
 if __name__ == '__main__':
     sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
-    sys.exit(load_entry_point('arm-application==0.1.0', 'console_scripts', 'realsense_color_pub')())
+    sys.exit(load_entry_point('arm-application', 'console_scripts', 'realsense_color_pub')())

install/arm_application/lib/arm_application/realsense_view

@@ -1,10 +1,10 @@
 #!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
-# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application==0.1.0','console_scripts','realsense_view'
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','realsense_view'
 import re
 import sys
 
 # for compatibility with easy_install; see #2198
-__requires__ = 'arm-application==0.1.0'
+__requires__ = 'arm-application'
 
 try:
     from importlib.metadata import distribution
@@ -30,4 +30,4 @@ globals().setdefault('load_entry_point', importlib_load_entry_point)
 
 if __name__ == '__main__':
     sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
-    sys.exit(load_entry_point('arm-application==0.1.0', 'console_scripts', 'realsense_view')())
+    sys.exit(load_entry_point('arm-application', 'console_scripts', 'realsense_view')())

install/arm_application/lib/arm_application/run_joint_trajectory

@@ -1,10 +1,10 @@
 #!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
-# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application==0.1.0','console_scripts','run_joint_trajectory'
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','run_joint_trajectory'
 import re
 import sys
 
 # for compatibility with easy_install; see #2198
-__requires__ = 'arm-application==0.1.0'
+__requires__ = 'arm-application'
 
 try:
     from importlib.metadata import distribution
@@ -30,4 +30,4 @@ globals().setdefault('load_entry_point', importlib_load_entry_point)
 
 if __name__ == '__main__':
     sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
-    sys.exit(load_entry_point('arm-application==0.1.0', 'console_scripts', 'run_joint_trajectory')())
+    sys.exit(load_entry_point('arm-application', 'console_scripts', 'run_joint_trajectory')())

install/arm_application/lib/arm_application/servo_node

@@ -1,10 +1,10 @@
 #!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
-# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application==0.1.0','console_scripts','servo_node'
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','servo_node'
 import re
 import sys
 
 # for compatibility with easy_install; see #2198
-__requires__ = 'arm-application==0.1.0'
+__requires__ = 'arm-application'
 
 try:
     from importlib.metadata import distribution
@@ -30,4 +30,4 @@ globals().setdefault('load_entry_point', importlib_load_entry_point)
 
 if __name__ == '__main__':
     sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
-    sys.exit(load_entry_point('arm-application==0.1.0', 'console_scripts', 'servo_node')())
+    sys.exit(load_entry_point('arm-application', 'console_scripts', 'servo_node')())

install/arm_application/lib/arm_application/stage2_locate

@@ -1,10 +1,10 @@
 #!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
-# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application==0.1.0','console_scripts','stage2_locate'
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','stage2_locate'
 import re
 import sys
 
 # for compatibility with easy_install; see #2198
-__requires__ = 'arm-application==0.1.0'
+__requires__ = 'arm-application'
 
 try:
     from importlib.metadata import distribution
@@ -30,4 +30,4 @@ globals().setdefault('load_entry_point', importlib_load_entry_point)
 
 if __name__ == '__main__':
     sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
-    sys.exit(load_entry_point('arm-application==0.1.0', 'console_scripts', 'stage2_locate')())
+    sys.exit(load_entry_point('arm-application', 'console_scripts', 'stage2_locate')())

install/arm_application/lib/arm_application/stage3_search

@@ -1,10 +1,10 @@
 #!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
-# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application==0.1.0','console_scripts','stage3_search'
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','stage3_search'
 import re
 import sys
 
 # for compatibility with easy_install; see #2198
-__requires__ = 'arm-application==0.1.0'
+__requires__ = 'arm-application'
 
 try:
     from importlib.metadata import distribution
@@ -30,4 +30,4 @@ globals().setdefault('load_entry_point', importlib_load_entry_point)
 
 if __name__ == '__main__':
     sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
-    sys.exit(load_entry_point('arm-application==0.1.0', 'console_scripts', 'stage3_search')())
+    sys.exit(load_entry_point('arm-application', 'console_scripts', 'stage3_search')())

install/arm_application/lib/arm_application/tactile

@@ -1,10 +1,10 @@
 #!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
-# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application==0.1.0','console_scripts','tactile'
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','tactile'
 import re
 import sys
 
 # for compatibility with easy_install; see #2198
-__requires__ = 'arm-application==0.1.0'
+__requires__ = 'arm-application'
 
 try:
     from importlib.metadata import distribution
@@ -30,4 +30,4 @@ globals().setdefault('load_entry_point', importlib_load_entry_point)
 
 if __name__ == '__main__':
     sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
-    sys.exit(load_entry_point('arm-application==0.1.0', 'console_scripts', 'tactile')())
+    sys.exit(load_entry_point('arm-application', 'console_scripts', 'tactile')())

install/arm_application/lib/arm_application/tactile_cli

@@ -1,10 +1,10 @@
 #!/home/cc/anaconda3/envs/ros_flexiv/bin/python3.11
-# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application==0.1.0','console_scripts','tactile_cli'
+# EASY-INSTALL-ENTRY-SCRIPT: 'arm-application','console_scripts','tactile_cli'
 import re
 import sys
 
 # for compatibility with easy_install; see #2198
-__requires__ = 'arm-application==0.1.0'
+__requires__ = 'arm-application'
 
 try:
     from importlib.metadata import distribution
@@ -30,4 +30,4 @@ globals().setdefault('load_entry_point', importlib_load_entry_point)
 
 if __name__ == '__main__':
     sys.argv[0] = re.sub(r'(-script\.pyw?|\.exe)?$', '', sys.argv[0])
-    sys.exit(load_entry_point('arm-application==0.1.0', 'console_scripts', 'tactile_cli')())
+    sys.exit(load_entry_point('arm-application', 'console_scripts', 'tactile_cli')())

install/arm_application/lib/python3.11/site-packages/arm-application.egg-link

@@ -0,0 +1,2 @@
+/home/cc/ros2/build/arm_application
+.

install/arm_application/lib/python3.11/site-packages/arm_application/__init__.py

@@ -1,32 +0,0 @@
-"""arm_application —— Flexiv Rizon + D405 + DH 夹爪的现场节点包。
-
-对外可直接运行的 ROS 程序（见 ``setup.py`` 的 ``console_scripts``）
-------------------------------------------------------------------
-相机：      realsense_color_pub
-感知：      perception_node
-伺服：      servo_node
-手眼：      handeye_teach、handeye_waypoints、handeye_calibrate
-夹爪：      dh_pgea、dh_pgea_cli、dual_gripper、dual_gripper_cli
-入孔检测：  hole_detect
-触觉：      tactile、tactile_cli
-辅助：      goto_ee_plane、demo_named_poses、run_joint_trajectory
-
-可复用的 Python 模块（可以在你自己的节点里 import）
-------------------------------------------------------------------
-``yolo_detector``     YOLO / TensorRT 推理 + ``pick_best`` 挑最优框
-``box_tracker``       像素平面内的匀速模型框预测（丢帧时"滑行"用）
-``plane_feature``     在 YOLO 框内取立体平面的质心
-``dh_pgea``           大寰 PGEA 的官方 Modbus RTU 客户端
-``dh_official``       ``dual_gripper`` 用的 DH Modbus 客户端
-``dual_gripper``      左 PGEA + 右 RGI
-``hole_detector``     yolo / hough / two_stage 三种孔心检测
-``orisys_tactile``    Orisys 视触觉适配器
-``geom_ik``           Rizon 几何雅可比 / DLS / 手眼加载
-``handeye_lib``       棋盘格 + OpenCV ``calibrateHandEye``
-``handeye_realsense`` D405 的话题 / 序列号辅助
-``pid_controller``    带积分分离的 PID
-``kalman_filter``     三维匀速模型卡尔曼滤波
-
-现场操作请走 ``scripts/*.sh`` 和 ``config/site.yaml``，不要直接 import 这些模块。
-接口契约见 ``docs/INTERFACES.md``。
-"""

install/arm_application/lib/python3.11/site-packages/arm_application/box_tracker.py

@@ -1,85 +0,0 @@
-#!/usr/bin/env python3
-"""针对 YOLO ``xyxy`` 框（像素坐标系）的匀速跟踪器。
-
-用于避免工件快速运动时丢失锁定：下一帧的闸门使用*预测*框，
-短暂的 YOLO 漏检则由惯性滑行（coast）维持叠加显示。
-"""
-
-from __future__ import annotations
-
-from typing import Optional, Tuple
-
-import numpy as np
-
-
-def _center(xyxy: np.ndarray) -> np.ndarray:
-    return np.array(
-        [0.5 * (float(xyxy[0]) + float(xyxy[2])), 0.5 * (float(xyxy[1]) + float(xyxy[3]))],
-        dtype=float,
-    )
-
-
-def _wh(xyxy: np.ndarray) -> Tuple[float, float]:
-    return (
-        max(8.0, float(xyxy[2]) - float(xyxy[0])),
-        max(8.0, float(xyxy[3]) - float(xyxy[1])),
-    )
-
-
-class BoxTracker:
-    def __init__(
-        self,
-        *,
-        vmax_px_s: float = 1800.0,
-        v_lpf: float = 0.70,
-        max_dt: float = 0.25,
-    ) -> None:
-        self.vmax_px_s = float(vmax_px_s)
-        self.v_lpf = float(np.clip(v_lpf, 0.05, 1.0))
-        self.max_dt = float(max_dt)
-        self.xyxy: Optional[np.ndarray] = None
-        self.v = np.zeros(2, dtype=float)  # 中心点 px/s
-        self.t: Optional[float] = None
-
-    def reset(self) -> None:
-        self.xyxy = None
-        self.v[:] = 0.0
-        self.t = None
-
-    def predict(self, t: float, img_wh: Optional[Tuple[int, int]] = None) -> Optional[np.ndarray]:
-        if self.xyxy is None:
-            return None
-        out = self.xyxy.copy()
-        if self.t is None:
-            return out
-        dt = float(np.clip(t - self.t, 0.0, self.max_dt))
-        if dt <= 1e-4:
-            return out
-        c = _center(out) + self.v * dt
-        w, h = _wh(out)
-        x1, y1 = c[0] - 0.5 * w, c[1] - 0.5 * h
-        x2, y2 = c[0] + 0.5 * w, c[1] + 0.5 * h
-        if img_wh is not None:
-            iw, ih = float(img_wh[0]), float(img_wh[1])
-            x1 = float(np.clip(x1, 0.0, iw - 2.0))
-            y1 = float(np.clip(y1, 0.0, ih - 2.0))
-            x2 = float(np.clip(x2, x1 + 4.0, iw))
-            y2 = float(np.clip(y2, y1 + 4.0, ih))
-        return np.array([x1, y1, x2, y2], dtype=float)
-
-    def update(self, xyxy: np.ndarray, t: float) -> None:
-        xyxy = np.asarray(xyxy, dtype=float).reshape(4)
-        c = _center(xyxy)
-        if self.xyxy is not None and self.t is not None:
-            dt = float(t - self.t)
-            if dt > 0.28:
-                self.v[:] = 0.0
-            elif dt > 1e-3:
-                inst = (c - _center(self.xyxy)) / dt
-                n = float(np.hypot(inst[0], inst[1]))
-                if n > self.vmax_px_s > 0.0:
-                    inst = inst * (self.vmax_px_s / n)
-                a = self.v_lpf
-                self.v = a * inst + (1.0 - a) * self.v
-        self.xyxy = xyxy.copy()
-        self.t = float(t)

install/arm_application/lib/python3.11/site-packages/arm_application/demo_named_poses.py

@@ -1,80 +0,0 @@
-#!/usr/bin/env python3
-"""演示：通过 ``/set_named_pose`` 循环切换命名位姿（对任何后端都适用）。"""
-
-from __future__ import annotations
-
-import time
-
-import rclpy
-from rclpy.node import Node
-
-from arm_msgs.srv import SetNamedPose
-
-from arm_application.ros_node import TolerantNode
-
-
-class DemoNamedPoses(TolerantNode):
-    def __init__(self) -> None:
-        super().__init__('demo_named_poses')
-        self.declare_parameter(
-            'poses', ['home', 'ready', 'inspect', 'pos_left', 'home']
-        )
-        self.declare_parameter('velocity_scale', 0.5)
-        self.declare_parameter('dwell_sec', 0.3)
-        self.declare_parameter('loops', 1)
-        self.poses = list(
-            self.get_parameter('poses').get_parameter_value().string_array_value
-            or ['home', 'ready', 'inspect', 'home']
-        )
-        self.velocity_scale = float(self.get_parameter('velocity_scale').value)
-        self.dwell_sec = float(self.get_parameter('dwell_sec').value)
-        self.loops = int(self.get_parameter('loops').value)
-        self._cli = self.create_client(SetNamedPose, 'set_named_pose')
-
-    def run(self) -> int:
-        if not self._cli.wait_for_service(timeout_sec=15.0):
-            self.get_logger().error('``/set_named_pose`` 服务不可用')
-            return 1
-        for loop in range(max(self.loops, 1)):
-            self.get_logger().info(f'第 {loop + 1}/{self.loops} 轮')
-            for pose in self.poses:
-                req = SetNamedPose.Request()
-                req.pose_name = pose
-                req.velocity_scale = self.velocity_scale
-                req.acceleration_scale = self.velocity_scale
-                fut = self._cli.call_async(req)
-                deadline = time.monotonic() + 60.0
-                while rclpy.ok() and not fut.done():
-                    rclpy.spin_once(self, timeout_sec=0.05)
-                    if time.monotonic() > deadline:
-                        self.get_logger().error(f'{pose} 调用超时')
-                        return 2
-                res = fut.result()
-                if res is None or not res.success:
-                    self.get_logger().error(
-                        f'{pose} 执行失败：{res.message if res else "无响应"}'
-                    )
-                    return 3
-                self.get_logger().info(f'{pose}：{res.message}')
-                time.sleep(self.dwell_sec)
-        self.get_logger().info('演示结束')
-        return 0
-
-
-def main(args=None) -> None:
-    rclpy.init(args=args)
-    node = DemoNamedPoses()
-    code = 1
-    try:
-        code = node.run()
-    except KeyboardInterrupt:
-        code = 0
-    finally:
-        node.destroy_node()
-        if rclpy.ok():
-            rclpy.shutdown()
-    raise SystemExit(code)
-
-
-if __name__ == '__main__':
-    main()

install/arm_application/lib/python3.11/site-packages/arm_application/dh_official.py

@@ -1,286 +0,0 @@
-"""DH-Robotics 官方 Modbus RTU 控制（非 Flexiv RDK）。
-
-覆盖两类设备：
-- PGE/PGEA 平行夹爪：初始化 0x0100，力 0x0101，位置 0x0103（0~1000‰），速度 0x0104。
-- RGI 旋转夹爪：在平行夹爪寄存器基础上，增加旋转角度 0x0105、旋转速度 0x0107、
-  旋转力 0x0108；反馈增加旋转角度 0x0208、旋转初始化状态 0x020A、旋转状态 0x020B。
-
-说明：
-- RGI 手册示例中负角度使用“反码/one's complement”：+360 -> 0x0168，-360 -> 0xFE97。
-  因此这里默认按 one's complement 编解码；若后续厂家确认为 two's complement，只改
-  `_encode_signed` / `_decode_signed` 两处即可。
-- 写位置/角度寄存器后会立即运动，没有 ChangingTek SDK 的 0x0108 触发寄存器。
-"""
-
-from __future__ import annotations
-
-import threading
-import time
-from typing import Iterable
-
-
-# =============================================================================
-# DH-Robotics Modbus 寄存器映射
-# =============================================================================
-
-# 基础控制寄存器（保持寄存器，功能码 03/06/10）
-REG_INIT = 0x0100                 # 0x01 初始化，0xA5 完全初始化
-REG_FORCE = 0x0101                # 夹爪闭合力，20~100 %
-REG_POSITION = 0x0103             # 手指参考位置，0~1000 千分比
-REG_SPEED = 0x0104                # 手指开合速度，1~100 %
-
-# RGI 旋转控制寄存器
-REG_ROTATION_ANGLE = 0x0105       # 目标旋转角度，有符号角度值
-REG_ROTATION_SPEED = 0x0107       # 旋转速度，1~100 %
-REG_ROTATION_FORCE = 0x0108       # 旋转力/力矩，20~100 %
-
-# 反馈寄存器
-REG_INIT_STATE = 0x0200           # 0 未初始化，1 已初始化，2 初始化中
-REG_GRIPPER_STATE = 0x0201        # 0 运动中，1 已到位，2 已夹到物体，3 物体掉落
-REG_CURRENT_POSITION = 0x0202     # 当前手指位置，千分比
-REG_ROTATION_ANGLE_FB = 0x0208    # 当前旋转角度，有符号角度值
-REG_ROTATION_INIT_STATE = 0x020A  # 0 未初始化，1 已初始化，2 初始化中
-REG_ROTATION_STATE = 0x020B       # 0 运动中，1 已到位，2 受阻，3 曾经受阻
-
-# 配置寄存器
-REG_SAVE_PARAMS = 0x0300          # 写入 1 可把所有参数保存到 flash
-
-
-class DHModbusError(RuntimeError):
-    """DH Modbus 通信或协议错误。"""
-
-
-def _clamp_int(value: float | int, lo: int, hi: int) -> int:
-    return int(max(lo, min(hi, int(round(float(value))))))
-
-
-def _encode_signed(value: float | int) -> int:
-    """把有符号角度值编码为 DH 的反码（one's complement）寄存器值。
-
-    RGI 手册示例：+360 -> 0x0168，-360 -> 0xFE97。
-    """
-    value = _clamp_int(value, -32767, 32767)
-    if value >= 0:
-        return value & 0xFFFF
-    return (~abs(value)) & 0xFFFF
-
-
-def _decode_signed(raw: int) -> int:
-    """把 DH 的反码（one's complement）寄存器值解码为有符号整数。"""
-    raw = int(raw) & 0xFFFF
-    if raw & 0x8000:
-        mag = (~raw) & 0x7FFF
-        return -mag
-    return raw & 0x7FFF
-
-
-class DHModbusClient:
-    """面向 DH-Robotics 夹爪的最小线程安全 Modbus RTU 客户端。"""
-
-    def __init__(
-        self,
-        port: str,
-        slave_id: int = 1,
-        baudrate: int = 115200,
-        timeout: float = 0.2,
-        *,
-        bytesize: int = 8,
-        parity: str = "N",
-        stopbits: int = 1,
-    ) -> None:
-        try:
-            import minimalmodbus
-            import serial
-        except ImportError as exc:  # pragma: no cover - 取决于运行时环境
-            raise DHModbusError(
-                "缺少 minimalmodbus/pyserial。请先安装：pip install minimalmodbus pyserial"
-            ) from exc
-
-        parity_map = {
-            "N": serial.PARITY_NONE,
-            "E": serial.PARITY_EVEN,
-            "O": serial.PARITY_ODD,
-        }
-        self.instrument = minimalmodbus.Instrument(port, int(slave_id))
-        self.instrument.serial.baudrate = int(baudrate)
-        self.instrument.serial.bytesize = int(bytesize)
-        self.instrument.serial.parity = parity_map.get(str(parity).upper(), serial.PARITY_NONE)
-        self.instrument.serial.stopbits = int(stopbits)
-        self.instrument.serial.timeout = float(timeout)
-        self.instrument.mode = minimalmodbus.MODE_RTU
-        self.instrument.clear_buffers_before_each_transaction = True
-        try:
-            self.instrument.serial.exclusive = True
-        except Exception:
-            pass
-        self._lock = threading.Lock()
-        self.port = port
-
-    def close(self) -> None:
-        with self._lock:
-            try:
-                self.instrument.serial.close()
-            except Exception:
-                pass
-
-    # ------------------------------------------------------------------
-    # 底层寄存器读写
-    # ------------------------------------------------------------------
-    def write_register(self, address: int, value: int) -> None:
-        value = _clamp_int(value, 0, 0xFFFF)
-        last_exc: Exception | None = None
-        for attempt in range(3):
-            try:
-                with self._lock:
-                    self.instrument.write_register(address, value, functioncode=6)
-                return
-            except Exception as exc:
-                last_exc = exc
-                time.sleep(0.05 * (attempt + 1))
-        raise DHModbusError(f"写寄存器 0x{address:04X}={value} 失败: {last_exc}") from last_exc
-
-    def read_register(self, address: int) -> int:
-        last_exc: Exception | None = None
-        for attempt in range(3):
-            try:
-                with self._lock:
-                    return int(self.instrument.read_register(address, functioncode=3))
-            except Exception as exc:
-                last_exc = exc
-                time.sleep(0.05 * (attempt + 1))
-        hint = ""
-        text = str(last_exc).lower()
-        if "multiple access" in text or "no data" in text:
-            hint = f"。串口 {getattr(self, 'port', '')} 可能被其它进程占用，先执行: fuser -k {getattr(self, 'port', '/dev/ttyUSB*')}"
-        raise DHModbusError(f"读寄存器 0x{address:04X} 失败: {last_exc}{hint}") from last_exc
-
-    def write_registers(self, address: int, values: Iterable[int]) -> None:
-        vals = [_clamp_int(v, 0, 0xFFFF) for v in values]
-        with self._lock:
-            self.instrument.write_registers(address, vals)
-
-    def read_registers(self, address: int, count: int) -> list[int]:
-        with self._lock:
-            return [int(v) for v in self.instrument.read_registers(address, count, functioncode=3)]
-
-    def write_signed_register(self, address: int, value: float | int) -> None:
-        self.write_register(address, _encode_signed(value))
-
-    def read_signed_register(self, address: int) -> int:
-        return _decode_signed(self.read_register(address))
-
-    # ------------------------------------------------------------------
-    # PGE/PGEA/RGI 通用控制
-    # ------------------------------------------------------------------
-    def initialize(self, *, full: bool = False, rotation: bool = False, timeout: float = 10.0) -> None:
-        """初始化/回零夹爪，并等待其就绪。"""
-        self.write_register(REG_INIT, 0xA5 if full else 0x01)
-        start = time.monotonic()
-        while time.monotonic() - start < timeout:
-            init_state = self.read_register(REG_INIT_STATE)
-            rot_ok = True
-            if rotation:
-                rot_ok = self.read_register(REG_ROTATION_INIT_STATE) == 1
-            if init_state == 1 and rot_ok:
-                return
-            time.sleep(0.1)
-        raise DHModbusError(f"夹爪初始化超时（>{timeout:.1f}s）")
-
-    def save_parameters(self) -> None:
-        self.write_register(REG_SAVE_PARAMS, 1)
-
-    def set_force_pct(self, force_pct: float | int) -> int:
-        value = _clamp_int(force_pct, 20, 100)
-        self.write_register(REG_FORCE, value)
-        return value
-
-    def set_speed_pct(self, speed_pct: float | int) -> int:
-        value = _clamp_int(speed_pct, 1, 100)
-        self.write_register(REG_SPEED, value)
-        return value
-
-    def set_position_per_mille(self, position: float | int) -> int:
-        value = _clamp_int(position, 0, 1000)
-        # 与 dh_pgea.latched_position_break 同一份：命令已是目标时先写当前位置，
-        # 读回之后再写最终值。60ms 后写回旧命令，固件采样不到，手指不动。
-        from arm_application.dh_pgea import latched_position_break
-
-        try:
-            cmd = _clamp_int(self.read_register(REG_POSITION), 0, 1000)
-        except Exception:
-            cmd = value
-        try:
-            cur = self.get_position_per_mille()
-        except Exception:
-            cur = value
-        bridge = latched_position_break(cmd, cur, value)
-        if bridge is not None:
-            self.write_register(REG_POSITION, bridge)
-            deadline = time.monotonic() + 0.6
-            echoed = False
-            while time.monotonic() < deadline:
-                try:
-                    if _clamp_int(self.read_register(REG_POSITION), 0, 1000) == bridge:
-                        echoed = True
-                        break
-                except Exception:
-                    pass
-                time.sleep(0.05)
-            if not echoed:
-                raise DHModbusError(
-                    f"位置命令中间值 {bridge} 没有读回，不再写入最终目标 {value}"
-                )
-            time.sleep(0.15)
-        self.write_register(REG_POSITION, value)
-        return value
-
-    def get_position_per_mille(self) -> int:
-        return _clamp_int(self.read_register(REG_CURRENT_POSITION), 0, 1000)
-
-    def get_gripper_state(self) -> int:
-        return self.read_register(REG_GRIPPER_STATE)
-
-    def get_init_state(self) -> int:
-        return self.read_register(REG_INIT_STATE)
-
-    def hold_current_position(self) -> None:
-        """在没有专用停止寄存器的 DH 夹爪上尽力停止/保持。"""
-        self.set_position_per_mille(self.get_position_per_mille())
-
-    # ------------------------------------------------------------------
-    # RGI 旋转控制
-    # ------------------------------------------------------------------
-    def set_rotation_speed_pct(self, speed_pct: float | int) -> int:
-        value = _clamp_int(speed_pct, 1, 100)
-        self.write_register(REG_ROTATION_SPEED, value)
-        return value
-
-    def set_rotation_force_pct(self, force_pct: float | int) -> int:
-        value = _clamp_int(force_pct, 20, 100)
-        self.write_register(REG_ROTATION_FORCE, value)
-        return value
-
-    def set_rotation_angle_deg(self, angle_deg: float | int) -> int:
-        angle = _clamp_int(angle_deg, -32767, 32767)
-        self.write_signed_register(REG_ROTATION_ANGLE, angle)
-        return angle
-
-    def get_rotation_angle_raw(self) -> int:
-        return self.read_register(REG_ROTATION_ANGLE_FB) & 0xFFFF
-
-    def get_rotation_angle_deg(self) -> int:
-        return self.read_signed_register(REG_ROTATION_ANGLE_FB)
-
-    def get_rotation_init_state(self) -> int:
-        return self.read_register(REG_ROTATION_INIT_STATE)
-
-    def get_rotation_state(self) -> int:
-        return self.read_register(REG_ROTATION_STATE)
-
-    def hold_current_rotation(self) -> None:
-        """重新下发当前反馈角度来停止旋转。"""
-        self.write_register(REG_ROTATION_ANGLE, self.get_rotation_angle_raw())
-
-    def hold_current_all(self) -> None:
-        self.hold_current_position()
-        self.hold_current_rotation()

install/arm_application/lib/python3.11/site-packages/arm_application/dh_pgea.py

@@ -1,725 +0,0 @@
-#!/usr/bin/env python3
-"""DH Robotics PGE / PGEA Modbus RTU 客户端。
-
-移植自官方 C++ 驱动：
-  DH-Robotics/dh_gripper_ros  →  DH_Modbus_Gripper
-  （工厂会按型号名里是否包含 ``PGE`` / ``PGEA`` 映射到本类）
-
-Flexiv RDK 的 ``Gripper`` 驱动不了这个夹爪：它不是 Studio Device。
-改用 RS485（USB–UART）通信，寄存器与 DH 的 ROS 驱动完全一致。
-
-寄存器（保持寄存器，FC03 读 / FC06 写，CRC-16/Modbus 小端）：
-  0x0100  初始化，写 0xA5
-  0x0101  力 20–100 %
-  0x0103  位置 0–1000 ‰
-  0x0104  速度 1–100 %
-  0x0200  初始化状态  0=需要  1=完成  2=进行中
-  0x0201  夹持状态  0=运动中  1=已到位  2=已夹住  3=已掉落
-  0x0202  当前位置
-"""
-
-from __future__ import annotations
-
-import argparse
-import sys
-import threading
-import time
-from dataclasses import dataclass
-from typing import Optional
-
-try:
-    import serial
-except ImportError as exc:  # pragma: no cover
-    raise SystemExit(
-        "缺少 pyserial。在 conda 环境 ros_flexiv 里执行：pip install pyserial"
-    ) from exc
-
-
-# 官方 CRC-16/Modbus（与 dh_modbus_gripper.cpp 使用同一张表）。
-_CRC_TABLE = (
-    0x0000, 0xC0C1, 0xC181, 0x0140, 0xC301, 0x03C0, 0x0280, 0xC241,
-    0xC601, 0x06C0, 0x0780, 0xC741, 0x0500, 0xC5C1, 0xC481, 0x0440,
-    0xCC01, 0x0CC0, 0x0D80, 0xCD41, 0x0F00, 0xCFC1, 0xCE81, 0x0E40,
-    0x0A00, 0xCAC1, 0xCB81, 0x0B40, 0xC901, 0x09C0, 0x0880, 0xC841,
-    0xD801, 0x18C0, 0x1980, 0xD941, 0x1B00, 0xDBC1, 0xDA81, 0x1A40,
-    0x1E00, 0xDEC1, 0xDF81, 0x1F40, 0xDD01, 0x1DC0, 0x1C80, 0xDC41,
-    0x1400, 0xD4C1, 0xD581, 0x1540, 0xD701, 0x17C0, 0x1680, 0xD641,
-    0xD201, 0x12C0, 0x1380, 0xD341, 0x1100, 0xD1C1, 0xD081, 0x1040,
-    0xF001, 0x30C0, 0x3180, 0xF141, 0x3300, 0xF3C1, 0xF281, 0x3240,
-    0x3600, 0xF6C1, 0xF781, 0x3740, 0xF501, 0x35C0, 0x3480, 0xF441,
-    0x3C00, 0xFCC1, 0xFD81, 0x3D40, 0xFF01, 0x3FC0, 0x3E80, 0xFE41,
-    0xFA01, 0x3AC0, 0x3B80, 0xFB41, 0x3900, 0xF9C1, 0xF881, 0x3840,
-    0x2800, 0xE8C1, 0xE981, 0x2940, 0xEB01, 0x2BC0, 0x2A80, 0xEA41,
-    0xEE01, 0x2EC0, 0x2F80, 0xEF41, 0x2D00, 0xEDC1, 0xEC81, 0x2C40,
-    0xE401, 0x24C0, 0x2580, 0xE541, 0x2700, 0xE7C1, 0xE681, 0x2640,
-    0x2200, 0xE2C1, 0xE381, 0x2340, 0xE101, 0x21C0, 0x2080, 0xE041,
-    0xA001, 0x60C0, 0x6180, 0xA141, 0x6300, 0xA3C1, 0xA281, 0x6240,
-    0x6600, 0xA6C1, 0xA781, 0x6740, 0xA501, 0x65C0, 0x6480, 0xA441,
-    0x6C00, 0xACC1, 0xAD81, 0x6D40, 0xAF01, 0x6FC0, 0x6E80, 0xAE41,
-    0xAA01, 0x6AC0, 0x6B80, 0xAB41, 0x6900, 0xA9C1, 0xA881, 0x6840,
-    0x7800, 0xB8C1, 0xB981, 0x7940, 0xBB01, 0x7BC0, 0x7A80, 0xBA41,
-    0xBE01, 0x7EC0, 0x7F80, 0xBF41, 0x7D00, 0xBDC1, 0xBC81, 0x7C40,
-    0xB401, 0x74C0, 0x7580, 0xB541, 0x7700, 0xB7C1, 0xB681, 0x7640,
-    0x7200, 0xB2C1, 0xB381, 0x7340, 0xB101, 0x71C0, 0x7080, 0xB041,
-    0x5000, 0x90C1, 0x9181, 0x5140, 0x9301, 0x53C0, 0x5280, 0x9241,
-    0x9601, 0x56C0, 0x5780, 0x9741, 0x5500, 0x95C1, 0x9481, 0x5440,
-    0x9C01, 0x5CC0, 0x5D80, 0x9D41, 0x5F00, 0x9FC1, 0x9E81, 0x5E40,
-    0x5A00, 0x9AC1, 0x9B81, 0x5B40, 0x9901, 0x59C0, 0x5880, 0x9841,
-    0x8801, 0x48C0, 0x4980, 0x8941, 0x4B00, 0x8BC1, 0x8A81, 0x4A40,
-    0x4E00, 0x8EC1, 0x8F81, 0x4F40, 0x8D01, 0x4DC0, 0x4C80, 0x8C41,
-    0x4400, 0x84C1, 0x8581, 0x4540, 0x8701, 0x47C0, 0x4680, 0x8641,
-    0x8201, 0x42C0, 0x4380, 0x8341, 0x4100, 0x81C1, 0x8081, 0x4040,
-)
-
-REG_INIT = 0x0100
-REG_FORCE = 0x0101
-REG_POSITION = 0x0103
-REG_SPEED = 0x0104
-REG_INIT_STATE = 0x0200
-REG_GRIP_STATE = 0x0201
-REG_CUR_POSITION = 0x0202
-
-INIT_CMD = 0xA5
-
-S_INIT_NOT = 0
-S_INIT_FINISHED = 1
-S_INIT_DOING = 2
-
-S_GRIP_MOVING = 0
-S_GRIP_ARRIVED = 1
-S_GRIP_CAUGHT = 2
-S_GRIP_DROPPED = 3
-
-GRIP_STATE_NAME = {
-    S_GRIP_MOVING: "moving",
-    S_GRIP_ARRIVED: "arrived",
-    S_GRIP_CAUGHT: "caught",
-    S_GRIP_DROPPED: "dropped",
-}
-
-INIT_STATE_NAME = {
-    S_INIT_NOT: "need_init",
-    S_INIT_FINISHED: "initialized",
-    S_INIT_DOING: "initializing",
-}
-
-
-def latched_position_break(command: int, current: int, target: int) -> Optional[int]:
-    """命令寄存器已经等于目标、手指却不在目标上时，返回要先写入的中间位置。
-
-    大寰固件：再写一遍与 0x0103 当前值相同的数，不会启动新运动。
-    开爪卡在 caught 时就是这样——命令已是 1000，手指停在夹持位。
-    先把命令改成当前位置（手指本来就在这，不会再夹紧），等读回确认
-    固件已经采样，再写最终目标。只 sleep 几十毫秒就写回旧值，控制周期
-    来不及看到中间值，最终仍是旧命令，手指不动。
-    """
-    cmd = int(max(0, min(1000, command)))
-    cur = int(max(0, min(1000, current)))
-    pos = int(max(0, min(1000, target)))
-    if cmd != pos or abs(cur - pos) <= 25:
-        return None
-    if cur != cmd:
-        return cur
-    return 0 if cmd != 0 else 40
-
-
-def grip_motion_settled(
-    grip_state: int, position: int, target: int, seen_motion: bool
-) -> bool:
-    """这一拍能不能算「这句位置命令已经结束」。
-
-    张开前本来就是 caught、位置还停在夹持位：不能当成张开完成。
-    闭合夹住物体：动过之后停在比目标更开的位置，算完成。
-    """
-    near = abs(int(position) - int(target)) <= 25
-    if grip_state in (S_GRIP_ARRIVED, S_GRIP_DROPPED) and near:
-        return True
-    if grip_state == S_GRIP_CAUGHT and seen_motion:
-        if int(target) + 25 < int(position):
-            return True
-        if near:
-            return True
-    return False
-
-
-def crc16_modbus(data: bytes) -> int:
-    """官方 DH CRC16（Modbus，线路上按小端发送）。"""
-    crc = 0xFFFF
-    for byte in data:
-        crc = (crc >> 8) ^ _CRC_TABLE[(crc ^ byte) & 0xFF]
-    return crc
-
-
-def _frame(slave: int, func: int, index: int, value: int) -> bytes:
-    body = bytes(
-        [
-            slave & 0xFF,
-            func & 0xFF,
-            (index >> 8) & 0xFF,
-            index & 0xFF,
-            (value >> 8) & 0xFF,
-            value & 0xFF,
-        ]
-    )
-    crc = crc16_modbus(body)
-    return body + bytes([crc & 0xFF, (crc >> 8) & 0xFF])
-
-
-@dataclass
-class GripperState:
-    init_state: int = S_INIT_NOT
-    grip_state: int = S_GRIP_MOVING
-    position: int = 0
-    target_position: int = 0
-    target_force: int = 0
-    stroke_mm: float = 20.0
-
-    @property
-    def initialized(self) -> bool:
-        return self.init_state == S_INIT_FINISHED
-
-    @property
-    def width_mm(self) -> float:
-        return float(self.position) / 1000.0 * float(self.stroke_mm)
-
-    def as_tuple(self) -> tuple[float, float, float, float, float]:
-        return (
-            1.0 if self.initialized else 0.0,
-            float(self.grip_state),
-            float(self.position),
-            float(self.target_position),
-            float(self.target_force),
-        )
-
-    def summary(self) -> str:
-        return (
-            f"init={INIT_STATE_NAME.get(self.init_state, self.init_state)} "
-            f"grip={GRIP_STATE_NAME.get(self.grip_state, self.grip_state)} "
-            f"pos={self.position}/1000 ({self.width_mm:.2f} mm) "
-            f"target={self.target_position} force={self.target_force}%"
-        )
-
-
-class DhPgeaError(RuntimeError):
-    pass
-
-
-WIRING_HINT = (
-    "PC 已发出 Modbus，夹爪 0 字节返回。不是力/速度参数问题。按顺序查：\n"
-    "  1) 夹爪 24 V DC 已上电（指示灯亮）\n"
-    "  2) USB 转接头必须是 RS485（A/B），不能是 TTL 的 TX/RX\n"
-    "  3) A↔A、B↔B；不通就对调 A/B\n"
-    "  4) 先 Ctrl+C 停节点，再：bash ~/ros2/scripts/run_gripper.sh --cli scan"
-)
-
-
-def open_failure_text(port: str, exc: BaseException) -> str:
-    """串口打不开时把「被占用」和「没应答」分开。
-
-    独占锁失败（errno 11）以前和真没上电写成同一句「需要 24 V DC」，
-    界面上看起来像缺自动上电，实际是别的进程还握着这个口。
-    """
-    text = str(exc).lower()
-    errno = getattr(exc, "errno", None)
-    busy = errno in (11, 16) or any(
-        token in text
-        for token in (
-            "temporarily unavailable",
-            "device or resource busy",
-            "could not exclusively lock",
-            "resource busy",
-        )
-    )
-    if busy:
-        return (
-            f"无法独占打开 {port}：{exc}。"
-            "串口已被别的进程占用（夹爪驱动或状态读取）。"
-        )
-    return (
-        f"无法打开 {port}：{exc}。"
-        "需要 24 V DC 供电 + RS485 接线，且当前用户在 dialout 组内。"
-    )
-
-
-class DhPgeaGripper:
-    """面向 PGE / PGEA 的线程安全 Modbus RTU 客户端（官方寄存器映射）。"""
-
-    def __init__(
-        self,
-        port: str = "/dev/ttyUSB0",
-        baudrate: int = 115200,
-        slave_id: int = 1,
-        timeout_s: float = 0.25,
-        stroke_mm: float = 20.0,
-        rs485_rts: str = "",
-    ) -> None:
-        self.port = port
-        self.baudrate = int(baudrate)
-        self.slave_id = int(slave_id)
-        self.timeout_s = float(timeout_s)
-        self.stroke_mm = float(stroke_mm)
-        mode = str(rs485_rts or "").strip().lower()
-        if mode in ("", "off", "none", "false", "0"):
-            mode = ""
-        if mode not in ("", "tx", "tx_inv"):
-            raise DhPgeaError(f"rs485_rts 只能是 ''、tx 或 tx_inv，实际为 {rs485_rts!r}")
-        self.rs485_rts = mode
-        self._ser: Optional[serial.Serial] = None
-        self._lock = threading.Lock()
-
-    def open(self) -> None:
-        if self._ser is not None and self._ser.is_open:
-            return
-        try:
-            self._ser = serial.Serial(
-                port=self.port,
-                baudrate=self.baudrate,
-                bytesize=serial.EIGHTBITS,
-                parity=serial.PARITY_NONE,
-                stopbits=serial.STOPBITS_ONE,
-                timeout=self.timeout_s,
-                write_timeout=self.timeout_s,
-                xonxoff=False,
-                rtscts=False,
-                dsrdtr=False,
-                exclusive=True,
-            )
-        except Exception as exc:
-            raise DhPgeaError(open_failure_text(self.port, exc)) from exc
-        try:
-            self._ser.dtr = True
-        except Exception:
-            pass
-        self._set_tx(False)
-        self._ser.reset_input_buffer()
-        self._ser.reset_output_buffer()
-        time.sleep(0.05)
-
-    def close(self) -> None:
-        if self._ser is not None:
-            try:
-                self._ser.close()
-            except Exception:
-                pass
-            self._ser = None
-
-    def __enter__(self) -> "DhPgeaGripper":
-        self.open()
-        return self
-
-    def __exit__(self, *args) -> None:
-        self.close()
-
-    def write_register(self, index: int, value: int, retries: int = 3) -> bool:
-        """FC06 写，等待回显（对应官方 WriteRegisterFunc）。"""
-        frame = _frame(self.slave_id, 0x06, index, int(value) & 0xFFFF)
-        with self._lock:
-            for _ in range(max(1, retries)):
-                if self._transact(frame, 8) == frame:
-                    return True
-                time.sleep(0.02)
-        return False
-
-    def read_register(self, index: int, retries: int = 8) -> int:
-        """FC03 读一个保持寄存器（对应官方 ReadRegisterFunc）。"""
-        req = _frame(self.slave_id, 0x03, index, 1)
-        last: Optional[bytes] = None
-        with self._lock:
-            for _ in range(max(1, retries)):
-                rsp = self._transact(req, 7)
-                last = rsp
-                if rsp is None or len(rsp) != 7:
-                    time.sleep(0.02)
-                    continue
-                if crc16_modbus(rsp[:-2]) != (rsp[5] | (rsp[6] << 8)):
-                    continue
-                if rsp[0] != (self.slave_id & 0xFF) or rsp[1] != 0x03:
-                    continue
-                return (rsp[3] << 8) | (rsp[4] & 0xFF)
-        hint = f" last={last.hex() if last else '(empty)'}"
-        raise DhPgeaError(
-            f"在 {self.port} 上 Modbus 读 0x{index:04X} 失败 "
-            f"（无有效应答{hint}）。baud={self.baudrate} slave={self.slave_id} "
-            f"rts={self.rs485_rts or 'off'}"
-        )
-
-    def probe(self) -> int:
-        """短读一次初始化状态。夹爪无应答时会抛异常。"""
-        return self.read_register(REG_INIT_STATE, retries=2)
-
-    def _set_tx(self, tx: bool) -> None:
-        if not self.rs485_rts or self._ser is None:
-            return
-        try:
-            if self.rs485_rts == "tx":
-                self._ser.rts = bool(tx)
-            else:
-                self._ser.rts = not bool(tx)
-        except Exception:
-            pass
-
-    def _transact(self, frame: bytes, expect: int) -> Optional[bytes]:
-        if self._ser is None or not self._ser.is_open:
-            raise DhPgeaError("串口未打开")
-        bit_s = 10.0 / max(1, self.baudrate)
-        self._set_tx(True)
-        if self.rs485_rts:
-            time.sleep(2.0 * bit_s)
-        self._ser.reset_input_buffer()
-        self._ser.write(frame)
-        self._ser.flush()
-        if self.rs485_rts:
-            time.sleep(len(frame) * bit_s + 0.0008)
-            self._set_tx(False)
-        deadline = time.monotonic() + self.timeout_s
-        buf = bytearray()
-        while time.monotonic() < deadline and len(buf) < expect:
-            chunk = self._ser.read(expect - len(buf))
-            if chunk:
-                buf.extend(chunk)
-        return bytes(buf) if buf else None
-
-    def initialization(self) -> bool:
-        return self.write_register(REG_INIT, INIT_CMD)
-
-    def initialize(
-        self, timeout_s: float = 20.0, poll_s: float = 0.2, *, force: bool = False
-    ) -> bool:
-        """写 0xA5，然后等到初始化状态 == 完成（对应官方上电流程）。
-
-        ``force=False`` 时，已经完成就不再写（节点启动不要每次都让手指找零）。
-        命令行 ``init`` 必须 ``force=True``：状态字已经是完成时，不写 0xA5
-        手指根本不会回零，看起来像初始化失效。
-        """
-        state = self.get_init_state()
-        if state == S_INIT_FINISHED and not force:
-            return True
-        if not self.initialization():
-            raise DhPgeaError("初始化写 0x0100=0xA5 失败（无 Modbus 回显）")
-        deadline = time.monotonic() + float(timeout_s)
-        # force 时必须先看到状态离开「完成」，否则读到的还是写之前的旧值。
-        saw_busy = not force
-        while time.monotonic() < deadline:
-            state = self.get_init_state()
-            if state != S_INIT_FINISHED:
-                saw_busy = True
-            elif saw_busy:
-                return True
-            time.sleep(poll_s)
-        raise DhPgeaError(
-            f"初始化在 {timeout_s:.0f}s 后超时（state={state}）。"
-            "首次上电时手指本应先回零一次。"
-        )
-
-    def set_target_force(self, force_pct: int) -> bool:
-        force_pct = int(max(20, min(100, force_pct)))
-        return self.write_register(REG_FORCE, force_pct)
-
-    def set_target_speed(self, speed_pct: int) -> bool:
-        speed_pct = int(max(1, min(100, speed_pct)))
-        return self.write_register(REG_SPEED, speed_pct)
-
-    def _wait_command_echo(self, value: int, timeout_s: float = 0.6) -> bool:
-        """等到 0x0103 读回就是刚写进去的值。读不回就不要再写下一笔。"""
-        deadline = time.monotonic() + float(timeout_s)
-        while time.monotonic() < deadline:
-            try:
-                if self.get_target_position(retries=1) == int(value):
-                    return True
-            except Exception:
-                pass
-            time.sleep(0.05)
-        return False
-
-    def set_target_position(self, pos: int) -> bool:
-        pos = int(max(0, min(1000, pos)))
-        try:
-            cmd = self.get_target_position(retries=2)
-        except Exception:
-            cmd = pos
-        try:
-            cur = self.get_current_position(retries=2)
-        except Exception:
-            cur = pos
-        bridge = latched_position_break(cmd, cur, pos)
-        if bridge is not None:
-            if not self.write_register(REG_POSITION, bridge):
-                return False
-            if not self._wait_command_echo(bridge):
-                return False
-            # 读回只说明寄存器改了。再留一个控制周期，避免下一笔把中间值盖掉。
-            time.sleep(0.15)
-        return self.write_register(REG_POSITION, pos)
-
-    def wait_settle(self, target: int, timeout_s: float = 6.0) -> GripperState:
-        """等到到位或夹住，或超时返回最后一帧。
-
-        张开前状态字已经是 caught 时不能立刻返回：那是上一句命令的残留，
-        手指还停在夹持位。
-        """
-        t0 = time.monotonic()
-        last = None
-        seen_motion = False
-        st = self.get_run_states()
-        while time.monotonic() - t0 < timeout_s:
-            st = self.get_run_states()
-            if last is not None and st.position != last:
-                seen_motion = True
-            last = st.position
-            if st.grip_state == S_GRIP_MOVING:
-                seen_motion = True
-                time.sleep(0.08)
-                continue
-            if grip_motion_settled(st.grip_state, st.position, target, seen_motion):
-                return st
-            time.sleep(0.08)
-        return st
-
-    def get_init_state(self, retries: int = 8) -> int:
-        return self.read_register(REG_INIT_STATE, retries=retries)
-
-    def get_grip_state(self, retries: int = 8) -> int:
-        return self.read_register(REG_GRIP_STATE, retries=retries)
-
-    def get_current_position(self, retries: int = 8) -> int:
-        return self.read_register(REG_CUR_POSITION, retries=retries)
-
-    def get_target_position(self, retries: int = 8) -> int:
-        return self.read_register(REG_POSITION, retries=retries)
-
-    def get_target_force(self, retries: int = 8) -> int:
-        return self.read_register(REG_FORCE, retries=retries)
-
-    def get_run_states(self, retries: int = 8) -> GripperState:
-        return GripperState(
-            init_state=self.get_init_state(retries=retries),
-            grip_state=self.get_grip_state(retries=retries),
-            position=self.get_current_position(retries=retries),
-            target_position=self.get_target_position(retries=retries),
-            target_force=self.get_target_force(retries=retries),
-            stroke_mm=self.stroke_mm,
-        )
-
-    def open_jaw(self, force: int = 50, speed: int = 50) -> bool:
-        ok = self.set_target_force(force)
-        ok = self.set_target_speed(speed) and ok
-        return self.set_target_position(1000) and ok
-
-    def close_jaw(self, force: int = 50, speed: int = 50) -> bool:
-        ok = self.set_target_force(force)
-        ok = self.set_target_speed(speed) and ok
-        return self.set_target_position(0) and ok
-
-    def move(self, position: int, force: int = 50, speed: int = 50) -> bool:
-        ok = self.set_target_force(force)
-        ok = self.set_target_speed(speed) and ok
-        return self.set_target_position(position) and ok
-
-
-def _selfcheck_crc() -> None:
-    """官方简短手册示例：设置位置 500。"""
-    frame = _frame(1, 0x06, 0x0103, 500)
-    expect = bytes([0x01, 0x06, 0x01, 0x03, 0x01, 0xF4, 0x78, 0x21])
-    if frame != expect:
-        raise AssertionError(f"CRC 不匹配：{frame.hex()} != {expect.hex()}")
-
-
-def _site_defaults() -> dict:
-    try:
-        from pathlib import Path
-
-        root = Path(__file__).resolve().parents[3]
-        sys.path.insert(0, str(root / "scripts" / "lib"))
-        from export_site_env import load_site  # type: ignore
-
-        g = (load_site().get("gripper") or {})
-        return {
-            "port": str(g.get("port") or "/dev/ttyUSB0"),
-            "baudrate": int(g.get("baud") or 115200),
-            "slave_id": int(g.get("slave_id") or 1),
-            "stroke_mm": float(g.get("stroke_mm") or 20.0),
-            "force": int(g.get("force_pct") or 50),
-            "speed": int(g.get("speed_pct") or 50),
-            "rs485_rts": str(g.get("rs485_rts") or ""),
-        }
-    except Exception:
-        return {
-            "port": "/dev/ttyUSB0",
-            "baudrate": 115200,
-            "slave_id": 1,
-            "stroke_mm": 20.0,
-            "force": 50,
-            "speed": 50,
-            "rs485_rts": "",
-        }
-
-
-def _scan_bus(port: str) -> int:
-    """遍历波特率 × 从站号 × RTS，直到某个组合有应答。"""
-    bauds = (115200, 57600, 38400, 19200, 9600)
-    slaves = (1, 2, 3)
-    rts_modes = ("", "tx", "tx_inv")
-    print(f"扫描 {port}（请先停掉 ROS 节点 —— 该串口是独占的）")
-    for baud in bauds:
-        for slave in slaves:
-            for rts in rts_modes:
-                label = f"baud={baud} slave={slave} rts={rts or 'off'}"
-                g = DhPgeaGripper(
-                    port=port,
-                    baudrate=baud,
-                    slave_id=slave,
-                    timeout_s=0.08,
-                    rs485_rts=rts,
-                )
-                try:
-                    g.open()
-                    state = g.probe()
-                    print(f"OK  {label}  init_state={state}")
-                    print(f"  → site.yaml: baud: {baud}  slave_id: {slave}  rs485_rts: \"{rts}\"")
-                    g.close()
-                    return 0
-                except DhPgeaError:
-                    print(f"  miss {label}")
-                finally:
-                    g.close()
-    print("所有组合都无应答。", file=sys.stderr)
-    print(WIRING_HINT, file=sys.stderr)
-    return 1
-
-
-def ctrl_values(
-    cmd: str,
-    value: str = "",
-    force: int = 50,
-    speed: int = 50,
-) -> Optional[tuple[float, float, float, float]]:
-    """节点已经占着串口时，改发 ``/gripper/ctrl`` 的四个数。
-
-    顺序是初始化、位置千分比、力度、速度。``probe`` / ``status`` 只读，返回 None。
-    """
-    name = str(cmd or "").strip().lower()
-    if name in ("probe", "status"):
-        return None
-    if name == "init":
-        return (1.0, 0.0, float(force), float(speed))
-    if name == "open":
-        pos = 1000
-    elif name == "close":
-        pos = 0
-    elif name in ("pos", "move"):
-        if str(value).strip() == "":
-            raise ValueError("pos 需要 0–1000")
-        pos = int(round(float(value)))
-        if pos < 0 or pos > 1000:
-            raise ValueError("位置只接受 0–1000")
-    else:
-        raise ValueError(f"节点在线时不支持 {name}")
-    return (0.0, float(pos), float(force), float(speed))
-
-
-def _cli(argv: Optional[list[str]] = None) -> int:
-    site = _site_defaults()
-    ap = argparse.ArgumentParser(
-        description="DH PGEA Modbus 命令行工具（官方寄存器映射，不用 Flexiv RDK）"
-    )
-    ap.add_argument("--port", default=site["port"])
-    ap.add_argument("--baud", type=int, default=site["baudrate"])
-    ap.add_argument("--slave", type=int, default=site["slave_id"])
-    ap.add_argument("--force", type=int, default=site["force"])
-    ap.add_argument("--speed", type=int, default=site["speed"])
-    ap.add_argument("--rts", default=site.get("rs485_rts") or "", help="RS485 DE：空 | tx | tx_inv")
-    ap.add_argument(
-        "cmd",
-        choices=["probe", "init", "status", "open", "close", "pos", "selfcheck", "scan"],
-    )
-    ap.add_argument("value", nargs="?", default="", help="'pos' 命令的位置，0–1000")
-    args = ap.parse_args(argv)
-
-    if args.cmd == "selfcheck":
-        _selfcheck_crc()
-        print("CRC 自检通过（01 06 01 03 01 F4 78 21）")
-        return 0
-    if args.cmd == "scan":
-        return _scan_bus(args.port)
-
-    grip = DhPgeaGripper(
-        port=args.port,
-        baudrate=args.baud,
-        slave_id=args.slave,
-        stroke_mm=float(site["stroke_mm"]),
-        rs485_rts=args.rts,
-    )
-    try:
-        grip.open()
-    except DhPgeaError as exc:
-        print(f"错误：{exc}", file=sys.stderr)
-        return 1
-
-    try:
-        if args.cmd == "probe":
-            st = grip.get_run_states(retries=2)
-            print(st.summary())
-            return 0
-        if args.cmd == "init":
-            print("正在初始化（手指回零，已初始化也会再做一次）…")
-            grip.initialize(force=True)
-            print(grip.get_run_states().summary())
-            return 0
-        if args.cmd == "status":
-            print(grip.get_run_states().summary())
-            return 0
-        if args.cmd == "open":
-            if not grip.open_jaw(args.force, args.speed):
-                raise DhPgeaError("张开写入失败")
-            st = grip.wait_settle(1000)
-            print(st.summary())
-            if abs(st.position - 1000) > 25:
-                print(
-                    "张开失败：手指没有离开夹持位。"
-                    "若仍是 grip=caught，先取下工件再执行 init。",
-                    file=sys.stderr,
-                )
-                return 1
-            return 0
-        if args.cmd == "close":
-            if not grip.close_jaw(args.force, args.speed):
-                raise DhPgeaError("闭合写入失败")
-            st = grip.wait_settle(0)
-            print(st.summary())
-            # 夹住物体时位置停在厚度上，grip=caught 算成功。停在张开位则失败。
-            if st.grip_state != S_GRIP_CAUGHT and abs(st.position) > 25:
-                print("闭合失败：手指没有朝合拢方向离开原位。", file=sys.stderr)
-                return 1
-            return 0
-        if args.cmd == "pos":
-            if args.value == "":
-                print("用法：pos <0-1000>", file=sys.stderr)
-                return 2
-            pos = int(float(args.value))
-            if not grip.move(pos, args.force, args.speed):
-                raise DhPgeaError("位置写入失败")
-            st = grip.wait_settle(pos)
-            print(st.summary())
-            grasped_short = st.grip_state == S_GRIP_CAUGHT and st.position > pos
-            if abs(st.position - pos) > 25 and not grasped_short:
-                print(
-                    f"位置 {pos} 没有到位（仍在 {st.position}）。",
-                    file=sys.stderr,
-                )
-                return 1
-            return 0
-    except DhPgeaError as exc:
-        print(f"错误：{exc}", file=sys.stderr)
-        print(WIRING_HINT, file=sys.stderr)
-        return 1
-    finally:
-        grip.close()
-    return 0
-
-
-def main(argv: Optional[list[str]] = None) -> int:
-    return _cli(argv)
-
-
-if __name__ == "__main__":
-    raise SystemExit(main())

install/arm_application/lib/python3.11/site-packages/arm_application/dh_pgea_node.py

@@ -1,246 +0,0 @@
-#!/usr/bin/env python3
-"""DH PGEA 的 ROS 2 节点（官方 Modbus RTU，不用 Flexiv 夹爪）。
-
-话题（布局与 DH-Robotics/dh_gripper_ros 一致）：
-  /gripper/ctrl     std_msgs/Float32MultiArray
-                    data: [initialize, position, force, speed]
-                    initialize>0.5 → 写 0xA5；否则设置力/速度/位置
-  /gripper/states   std_msgs/Float32MultiArray
-                    data: [initialized, grip_state, position, target_pos, target_force]
-  /gripper/joint_states  sensor_msgs/JointState
-
-服务（std_srvs/Trigger）：
-  /gripper/initialize  /gripper/open  /gripper/close
-
-不要设置 Flexiv bringup 的 load_gripper:=true —— 那只是 Flexiv-GN01 用的。
-"""
-
-from __future__ import annotations
-
-import threading
-from typing import Optional
-
-import rclpy
-from rcl_interfaces.msg import ParameterDescriptor
-from rclpy.node import Node
-from sensor_msgs.msg import JointState
-from std_msgs.msg import Float32, Float32MultiArray, MultiArrayDimension, MultiArrayLayout
-from std_srvs.srv import Trigger
-
-from arm_application.dh_pgea import (
-    DhPgeaError,
-    DhPgeaGripper,
-    GripperState,
-    WIRING_HINT,
-)
-from arm_application.ros_node import TolerantNode, param_as_str
-
-
-def _as_ctrl_array(msg: Float32MultiArray) -> tuple[float, float, float, float]:
-    data = list(msg.data)
-    while len(data) < 4:
-        data.append(0.0)
-    return float(data[0]), float(data[1]), float(data[2]), float(data[3])
-
-
-class DhPgeaNode(TolerantNode):
-    def __init__(self) -> None:
-        super().__init__("dh_pgea")
-        self.declare_parameter("port", "/dev/ttyUSB0")
-        self.declare_parameter("baud", 115200)
-        self.declare_parameter("slave_id", 1)
-        self.declare_parameter("stroke_mm", 20.0)
-        self.declare_parameter("force_pct", 50)
-        self.declare_parameter("speed_pct", 50)
-        self.declare_parameter("auto_init", True)
-        self.declare_parameter("state_hz", 20.0)
-        self.declare_parameter("open_position", 1000)
-        self.declare_parameter("close_position", 0)
-        self._model = param_as_str(self, "model", "PGEA-100-20-W")
-        self._sn = param_as_str(self, "serial_number", "")
-        self._rts = param_as_str(self, "rs485_rts", "")
-
-        self._port = str(self.get_parameter("port").value)
-        self._baud = int(self.get_parameter("baud").value)
-        self._slave = int(self.get_parameter("slave_id").value)
-        self._stroke = float(self.get_parameter("stroke_mm").value)
-        self._force = int(self.get_parameter("force_pct").value)
-        self._speed = int(self.get_parameter("speed_pct").value)
-        _auto = self.get_parameter("auto_init").value
-        if isinstance(_auto, str):
-            self._auto_init = _auto.strip().lower() in ("1", "true", "yes")
-        else:
-            self._auto_init = bool(_auto)
-        self._open_pos = int(self.get_parameter("open_position").value)
-        self._close_pos = int(self.get_parameter("close_position").value)
-        hz = float(self.get_parameter("state_hz").value)
-
-        self._grip = DhPgeaGripper(
-            port=self._port,
-            baudrate=self._baud,
-            slave_id=self._slave,
-            stroke_mm=self._stroke,
-            rs485_rts=self._rts,
-        )
-        self._state = GripperState(stroke_mm=self._stroke)
-        self._cmd_lock = threading.Lock()
-
-        self._pub_state = self.create_publisher(Float32MultiArray, "/gripper/states", 10)
-        self._pub_js = self.create_publisher(JointState, "/gripper/joint_states", 10)
-        self.create_subscription(Float32MultiArray, "/gripper/ctrl", self._on_ctrl, 10)
-        self.create_subscription(Float32, "/gripper/set_position", self._on_set_pos, 10)
-
-        self.create_service(Trigger, "/gripper/initialize", self._srv_init)
-        self.create_service(Trigger, "/gripper/open", self._srv_open)
-        self.create_service(Trigger, "/gripper/close", self._srv_close)
-
-        self.get_logger().info(
-            f"DH {self._model} SN={self._sn or '?'}  "
-            f"{self._port} @{self._baud} slave={self._slave} rts={self._rts or 'off'}  "
-            f"（官方 Modbus RTU；未使用 Flexiv RDK 夹爪 API）"
-        )
-        try:
-            self._grip.open()
-        except DhPgeaError as exc:
-            self.get_logger().error(str(exc))
-            raise
-
-        self._online = False
-        try:
-            self._grip.probe()
-            self._online = True
-        except DhPgeaError as exc:
-            self.get_logger().error(str(exc))
-            for line in WIRING_HINT.splitlines():
-                self.get_logger().error(line)
-            self.get_logger().error("会持续重试；在总线恢复应答前 open/close 都会失败")
-
-        if self._auto_init and self._online:
-            try:
-                self._grip.initialize()
-                self.get_logger().info("夹爪已初始化")
-            except DhPgeaError as exc:
-                self.get_logger().error(f"auto_init 失败：{exc}")
-                self._online = False
-
-        period = 1.0 / max(1.0, hz)
-        self._timer = self.create_timer(period, self._on_timer)
-
-    def destroy_node(self) -> bool:
-        try:
-            self._grip.close()
-        except Exception:
-            pass
-        return super().destroy_node()
-
-    def _apply_move(self, position: int, force: Optional[int] = None, speed: Optional[int] = None) -> str:
-        with self._cmd_lock:
-            f = self._force if force is None else int(force)
-            s = self._speed if speed is None else int(speed)
-            if force is not None:
-                self._force = f
-            if speed is not None:
-                self._speed = s
-            if not self._grip.move(int(position), f, s):
-                raise DhPgeaError("移动写入失败")
-        return f"pos={int(position)} force={f}% speed={s}%"
-
-    def _on_ctrl(self, msg: Float32MultiArray) -> None:
-        initialize, position, force, speed = _as_ctrl_array(msg)
-        try:
-            if initialize > 0.5:
-                self._grip.initialize()
-                self.get_logger().info("通过 /gripper/ctrl 触发初始化")
-                return
-            f = int(force) if force > 0 else self._force
-            s = int(speed) if speed > 0 else self._speed
-            info = self._apply_move(int(position), f, s)
-            self.get_logger().info(f"ctrl {info}")
-        except DhPgeaError as exc:
-            self.get_logger().error(str(exc))
-
-    def _on_set_pos(self, msg: Float32) -> None:
-        try:
-            info = self._apply_move(int(msg.data))
-            self.get_logger().info(f"set_position {info}")
-        except DhPgeaError as exc:
-            self.get_logger().error(str(exc))
-
-    def _srv_init(self, _req, resp):
-        try:
-            self._grip.initialize()
-            resp.success = True
-            resp.message = self._grip.get_run_states().summary()
-        except DhPgeaError as exc:
-            resp.success = False
-            resp.message = str(exc)
-        return resp
-
-    def _srv_open(self, _req, resp):
-        try:
-            info = self._apply_move(self._open_pos)
-            resp.success = True
-            resp.message = info
-        except DhPgeaError as exc:
-            resp.success = False
-            resp.message = str(exc)
-        return resp
-
-    def _srv_close(self, _req, resp):
-        try:
-            info = self._apply_move(self._close_pos)
-            resp.success = True
-            resp.message = info
-        except DhPgeaError as exc:
-            resp.success = False
-            resp.message = str(exc)
-        return resp
-
-    def _on_timer(self) -> None:
-        try:
-            st = self._grip.get_run_states(retries=2 if self._online else 1)
-            self._state = st
-            if not self._online:
-                self._online = True
-                self.get_logger().info("夹爪总线已恢复")
-        except DhPgeaError as exc:
-            self._online = False
-            self.get_logger().warn(str(exc), throttle_duration_sec=5.0)
-            return
-
-        arr = Float32MultiArray()
-        arr.layout = MultiArrayLayout(
-            dim=[MultiArrayDimension(label="dh_pgea", size=5, stride=5)],
-            data_offset=0,
-        )
-        arr.data = list(st.as_tuple())
-        self._pub_state.publish(arr)
-
-        js = JointState()
-        js.header.stamp = self.get_clock().now().to_msg()
-        js.name = ["gripper_finger1_joint"]
-        # 官方 ROS 驱动把 0–1000 映射到约 0–0.637 rad；我们这里也发布 mm。
-        js.position = [float(st.width_mm) * 0.001]
-        js.effort = [float(st.target_force)]
-        self._pub_js.publish(js)
-
-
-def main(args=None) -> None:
-    rclpy.init(args=args)
-    node = None
-    try:
-        node = DhPgeaNode()
-        rclpy.spin(node)
-    except KeyboardInterrupt:
-        pass
-    except DhPgeaError:
-        pass
-    finally:
-        if node is not None:
-            node.destroy_node()
-        if rclpy.ok():
-            rclpy.shutdown()
-
-
-if __name__ == "__main__":
-    main()

install/arm_application/lib/python3.11/site-packages/arm_application/dual_gripper.py

@@ -1,803 +0,0 @@
-"""双臂夹爪统一控制架构（DH-Robotics 官方 Modbus RTU，非 Flexiv RDK）。
-
-设计目标：
-- 左臂（Rizon4-063989）：大寰 PGEA 平行夹爪，控制行程/夹持力/速度。
-- 右臂（Rizon4-063991）：大寰 RGI-100-30-O-S-M1 旋转夹爪，控制手指夹持与旋转角度/圈数/速度。
-- 统一接口，便于后续扩展到 ROS2 节点和新项目脚本。
-
-注意：DH 官方协议是“写位置/角度寄存器后立即运动”，没有 ChangingTek SDK 的 0x0108 触发寄存器。
-"""
-
-from __future__ import annotations
-
-import time
-from abc import ABC, abstractmethod
-from dataclasses import dataclass, field
-from enum import Enum, auto
-from pathlib import Path
-from typing import Any
-
-import numpy as np
-
-from arm_application.dh_official import DHModbusClient, DHModbusError
-
-
-class GripperType(Enum):
-    PARALLEL = auto()      # 平行夹爪
-    ROTARY = auto()        # 旋转夹爪
-    VACUUM = auto()        # 真空吸盘（预留）
-
-
-class GripperState(Enum):
-    IDLE = auto()
-    MOVING = auto()
-    GRIPPING = auto()
-    RELEASING = auto()
-    ERROR = auto()
-
-
-@dataclass
-class GripperStatus:
-    """夹爪状态反馈。"""
-
-    state: GripperState = GripperState.IDLE
-    position: float = 0.0           # 平行：mm；旋转：角度 deg
-    force: float = 0.0              # 最近一次命令力/力矩 %
-    velocity: float = 0.0           # 最近一次命令速度 %
-    is_moving: bool = False
-    is_gripped: bool = False        # 是否夹持到物体
-    connected: bool = False
-    initialized: bool = False
-    finger_position_per_mille: float = 0.0
-    rotation_deg: float = 0.0
-    gripper_state: int = -1         # DH 0x0201：0 moving,1 reached,2 caught,3 dropped
-    rotation_state: int = -1        # RGI 0x020B：0 moving,1 reached,2 blocking,3 blocked
-    error_msg: str = ""
-    timestamp: float = field(default_factory=time.time)
-
-
-@dataclass
-class GripperCommand:
-    """夹爪控制指令。"""
-
-    target_position: float | None = None    # 平行：mm；旋转：deg
-    target_force: float | None = None       # 目标力/力矩 %
-    target_velocity: float | None = None    # 目标速度 %
-    wait_complete: bool = True              # 是否等待完成
-    timeout: float = 10.0                   # 超时时间
-
-
-class BaseGripper(ABC):
-    """夹爪基类，定义统一接口。"""
-
-    name: str = "base_gripper"
-    gripper_type: GripperType = GripperType.PARALLEL
-
-    def __init__(self, config: dict[str, Any] | None = None) -> None:
-        self.cfg = config or {}
-        self._connected = False
-        self._status = GripperStatus()
-
-    @abstractmethod
-    def connect(self) -> None: ...
-
-    @abstractmethod
-    def disconnect(self) -> None: ...
-
-    @abstractmethod
-    def execute(self, cmd: GripperCommand) -> GripperStatus: ...
-
-    @abstractmethod
-    def get_status(self) -> GripperStatus: ...
-
-    def is_connected(self) -> bool:
-        return self._connected
-
-    def open(self, **kwargs) -> GripperStatus:
-        raise NotImplementedError
-
-    def close(self, **kwargs) -> GripperStatus:
-        raise NotImplementedError
-
-    def stop(self) -> None:
-        raise NotImplementedError
-
-
-# =============================================================================
-# 大寰平行夹爪（PGE/PGEA 系列）
-# =============================================================================
-
-@dataclass
-class ParallelGripperConfig:
-    """大寰平行夹爪配置。"""
-
-    port: str = "/dev/ttyUSB0"
-    slave_id: int = 1
-    baudrate: int = 115200
-    timeout: float = 0.2
-
-    # PGEA-100-40 行程为 40mm；若实际是 PGEA-100-26，请把 stroke_max 改成 26.0。
-    stroke_min: float = 0.0
-    stroke_max: float = 40.0
-
-    # 默认参数（DH 百分比）
-    default_speed: int = 50
-    default_force: int = 50
-    force_min: int = 20
-    force_max: int = 100
-
-    # 控制行为
-    auto_initialize: bool = True
-    init_timeout: float = 10.0
-    position_tolerance_per_mille: int = 15
-    status_poll_interval: float = 0.05
-
-
-class ParallelGripper(BaseGripper):
-    """大寰 PGE/PGEA 平行夹爪。"""
-
-    name = "dh_pgea_parallel"
-    gripper_type = GripperType.PARALLEL
-
-    def __init__(self, config: ParallelGripperConfig | None = None) -> None:
-        super().__init__()
-        self.cfg = config or ParallelGripperConfig()
-        self._client: DHModbusClient | None = None
-        self._last_position_mm = float(self.cfg.stroke_min)
-        self._last_force = int(self.cfg.default_force)
-        self._last_speed = int(self.cfg.default_speed)
-
-    def connect(self) -> None:
-        if self._client is not None:
-            self.disconnect()
-        self._client = DHModbusClient(
-            self.cfg.port,
-            slave_id=self.cfg.slave_id,
-            baudrate=self.cfg.baudrate,
-            timeout=self.cfg.timeout,
-        )
-        self._connected = True
-        self._status.connected = True
-        self._status.state = GripperState.IDLE
-        self._status.error_msg = ""
-        if self.cfg.auto_initialize:
-            self.initialize()
-
-    def disconnect(self) -> None:
-        if self._client is not None:
-            self._client.close()
-        self._client = None
-        self._connected = False
-        self._status.connected = False
-        self._status.state = GripperState.IDLE
-
-    def initialize(self, full: bool = False) -> None:
-        self._require_client()
-        assert self._client is not None
-        self._client.initialize(full=full, rotation=False, timeout=self.cfg.init_timeout)
-        self._status.initialized = True
-
-    def _require_client(self) -> None:
-        if not self._connected or self._client is None:
-            self._status.state = GripperState.ERROR
-            self._status.error_msg = "平行夹爪未连接"
-            raise DHModbusError(self._status.error_msg)
-
-    def _mm_to_per_mille(self, position_mm: float) -> int:
-        lo = float(self.cfg.stroke_min)
-        hi = float(self.cfg.stroke_max)
-        if hi <= lo:
-            hi = lo + 1.0
-        ratio = (float(position_mm) - lo) / (hi - lo)
-        return int(np.clip(round(ratio * 1000.0), 0, 1000))
-
-    def _per_mille_to_mm(self, position: float | int) -> float:
-        lo = float(self.cfg.stroke_min)
-        hi = float(self.cfg.stroke_max)
-        return lo + (float(position) / 1000.0) * (hi - lo)
-
-    def execute(self, cmd: GripperCommand) -> GripperStatus:
-        try:
-            self._require_client()
-        except DHModbusError:
-            return self._status
-        assert self._client is not None
-
-        position_mm = self._last_position_mm if cmd.target_position is None else float(cmd.target_position)
-        position_mm = float(np.clip(position_mm, self.cfg.stroke_min, self.cfg.stroke_max))
-        speed = int(np.clip(cmd.target_velocity or self.cfg.default_speed, 1, 100))
-        force = int(np.clip(cmd.target_force or self.cfg.default_force, self.cfg.force_min, self.cfg.force_max))
-
-        self._client.set_speed_pct(speed)
-        self._client.set_force_pct(force)
-        target_per_mille = self._client.set_position_per_mille(self._mm_to_per_mille(position_mm))
-
-        self._last_position_mm = position_mm
-        self._last_speed = speed
-        self._last_force = force
-        self._status.state = GripperState.MOVING
-        self._status.error_msg = ""
-
-        if cmd.wait_complete:
-            self._wait_complete(target_per_mille, cmd.timeout)
-        return self.get_status()
-
-    def _wait_complete(self, target_per_mille: int, timeout: float) -> None:
-        assert self._client is not None
-        start = time.monotonic()
-        time.sleep(0.05)
-        while time.monotonic() - start < timeout:
-            state = self._client.get_gripper_state()
-            pos = self._client.get_position_per_mille()
-            self._status.gripper_state = state
-            self._status.finger_position_per_mille = float(pos)
-            self._status.position = self._per_mille_to_mm(pos)
-            self._status.is_moving = state == 0
-            self._status.is_gripped = state == 2
-            if state in (1, 2):
-                self._status.state = GripperState.GRIPPING if state == 2 else GripperState.IDLE
-                return
-            if state == 3:
-                self._status.state = GripperState.ERROR
-                self._status.error_msg = "夹持后物体掉落"
-                return
-            if abs(pos - target_per_mille) <= self.cfg.position_tolerance_per_mille and state != 0:
-                self._status.state = GripperState.IDLE
-                return
-            time.sleep(self.cfg.status_poll_interval)
-        self._status.state = GripperState.ERROR
-        self._status.error_msg = f"平行夹爪运动超时（>{timeout:.1f}s）"
-
-    def get_status(self) -> GripperStatus:
-        if not self._connected or self._client is None:
-            self._status.connected = False
-            return self._status
-        try:
-            pos = self._client.get_position_per_mille()
-            state = self._client.get_gripper_state()
-            init_state = self._client.get_init_state()
-            self._status.connected = True
-            self._status.initialized = init_state == 1
-            self._status.finger_position_per_mille = float(pos)
-            self._status.position = self._per_mille_to_mm(pos)
-            self._status.velocity = float(self._last_speed)
-            self._status.force = float(self._last_force)
-            self._status.gripper_state = state
-            self._status.is_moving = state == 0
-            self._status.is_gripped = state == 2
-            if self._status.state != GripperState.ERROR:
-                self._status.state = GripperState.MOVING if state == 0 else (
-                    GripperState.GRIPPING if state == 2 else GripperState.IDLE
-                )
-            self._status.timestamp = time.time()
-        except Exception as exc:
-            self._status.state = GripperState.ERROR
-            self._status.error_msg = str(exc)
-            self._status.connected = False
-        return self._status
-
-    def open(self, force: int | None = None, speed: int | None = None, **kwargs) -> GripperStatus:
-        # DH 千分比：0‰=闭合，1000‰=张开，对应行程 0mm -> stroke_max。
-        return self.execute(GripperCommand(
-            target_position=float(self.cfg.stroke_max),
-            target_force=force or self.cfg.default_force,
-            target_velocity=speed or self.cfg.default_speed,
-            **kwargs,
-        ))
-
-    def close(self, force: int | None = None, speed: int | None = None, **kwargs) -> GripperStatus:
-        return self.execute(GripperCommand(
-            target_position=float(self.cfg.stroke_min),
-            target_force=force or self.cfg.default_force,
-            target_velocity=speed or self.cfg.default_speed,
-            **kwargs,
-        ))
-
-    def move_to(self, position_mm: float, force: int | None = None, speed: int | None = None,
-                **kwargs) -> GripperStatus:
-        return self.execute(GripperCommand(
-            target_position=position_mm,
-            target_force=force or self.cfg.default_force,
-            target_velocity=speed or self.cfg.default_speed,
-            **kwargs,
-        ))
-
-    def set_force(self, force_pct: int) -> None:
-        self._require_client()
-        assert self._client is not None
-        self._last_force = self._client.set_force_pct(force_pct)
-
-    def set_speed(self, speed_pct: int) -> None:
-        self._require_client()
-        assert self._client is not None
-        self._last_speed = self._client.set_speed_pct(speed_pct)
-
-    def stop(self) -> None:
-        if self._client is not None:
-            self._client.hold_current_position()
-        self._status.state = GripperState.IDLE
-
-
-# =============================================================================
-# 大寰旋转夹爪（RGI-100-30-O-S-M1）
-# =============================================================================
-
-@dataclass
-class RotaryGripperConfig:
-    """大寰 RGI 旋转夹爪配置。"""
-
-    port: str = "/dev/ttyUSB1"
-    slave_id: int = 1
-    baudrate: int = 115200
-    timeout: float = 0.2
-
-    # RGI 旋转角度寄存器是 signed 16bit；为避免 one's-complement 负零歧义，默认限制在 ±32767。
-    angle_min: float = -32767.0
-    angle_max: float = 32767.0
-    max_step_deg: float = 30000.0      # rotate_revolutions 分段旋转的最大单步角度
-
-    # DH 手册：手指位置 0~1000‰。0=闭合，1000=张开。RGI-100 行程约 70mm。
-    finger_open_per_mille: int = 1000
-    finger_close_per_mille: int = 0
-
-    # 默认参数（DH 百分比）
-    default_speed: int = 50            # 旋转速度 %
-    default_torque: int = 50           # 旋转力/力矩 %
-    default_finger_speed: int = 50     # 手指速度 %
-    default_finger_force: int = 50     # 手指夹持力 %
-    torque_min: int = 20
-    torque_max: int = 100
-
-    # 控制行为
-    auto_initialize: bool = True
-    init_timeout: float = 15.0         # RGI 初始化包含旋转找零，时间更长
-    angle_tolerance_deg: float = 1.0
-    status_poll_interval: float = 0.05
-
-
-class RotaryGripper(BaseGripper):
-    """大寰 RGI-100-30-O-S-M1 旋转夹爪。"""
-
-    name = "dh_rgi_rotary"
-    gripper_type = GripperType.ROTARY
-
-    def __init__(self, config: RotaryGripperConfig | None = None) -> None:
-        super().__init__()
-        self.cfg = config or RotaryGripperConfig()
-        self._client: DHModbusClient | None = None
-        self._last_angle = 0.0
-        self._last_torque = int(self.cfg.default_torque)
-        self._last_speed = int(self.cfg.default_speed)
-        self._last_finger_force = int(self.cfg.default_finger_force)
-        self._last_finger_speed = int(self.cfg.default_finger_speed)
-        self._last_finger_per_mille = int(self.cfg.finger_open_per_mille)
-
-    def connect(self) -> None:
-        if self._client is not None:
-            self.disconnect()
-        self._client = DHModbusClient(
-            self.cfg.port,
-            slave_id=self.cfg.slave_id,
-            baudrate=self.cfg.baudrate,
-            timeout=self.cfg.timeout,
-        )
-        self._connected = True
-        self._status.connected = True
-        self._status.state = GripperState.IDLE
-        self._status.error_msg = ""
-        if self.cfg.auto_initialize:
-            self.initialize()
-
-    def disconnect(self) -> None:
-        if self._client is not None:
-            self._client.close()
-        self._client = None
-        self._connected = False
-        self._status.connected = False
-        self._status.state = GripperState.IDLE
-
-    def initialize(self, full: bool = False) -> None:
-        self._require_client()
-        assert self._client is not None
-        self._client.initialize(full=full, rotation=True, timeout=self.cfg.init_timeout)
-        self._status.initialized = True
-
-    def _require_client(self) -> None:
-        if not self._connected or self._client is None:
-            self._status.state = GripperState.ERROR
-            self._status.error_msg = "旋转夹爪未连接"
-            raise DHModbusError(self._status.error_msg)
-
-    def execute(self, cmd: GripperCommand) -> GripperStatus:
-        """执行旋转指令；cmd.target_position 单位为度。"""
-        return self.rotate_to(
-            self._last_angle if cmd.target_position is None else float(cmd.target_position),
-            speed=cmd.target_velocity,
-            torque=cmd.target_force,
-            wait_complete=cmd.wait_complete,
-            timeout=cmd.timeout,
-        )
-
-    def _wait_complete(self, target_angle: float, timeout: float) -> None:
-        assert self._client is not None
-        start = time.monotonic()
-        start_angle = float(self._client.get_rotation_angle_deg())
-        expected = abs(float(target_angle) - start_angle) + 30.0
-        settled = 0
-        last_angle = start_angle
-        time.sleep(0.05)
-        while time.monotonic() - start < timeout:
-            rot_state = self._client.get_rotation_state()
-            angle = float(self._client.get_rotation_angle_deg())
-            self._status.rotation_state = rot_state
-            self._status.rotation_deg = angle
-            self._status.position = angle
-            self._status.is_moving = rot_state == 0
-            if abs(angle - start_angle) > expected:
-                self._client.hold_current_rotation()
-                self._status.state = GripperState.ERROR
-                self._status.error_msg = (
-                    f"旋转超程保护：目标 {target_angle:.1f}°，从 {start_angle:.1f}° 跑到 {angle:.1f}°，已发停止"
-                )
-                return
-            if rot_state in (1, 2):
-                self._status.state = GripperState.IDLE if rot_state == 1 else GripperState.GRIPPING
-                if rot_state == 2:
-                    self._status.error_msg = "旋转受阻（blocking）"
-                return
-            if rot_state == 3:
-                self._status.state = GripperState.ERROR
-                self._status.error_msg = "旋转曾受阻且未到目标位"
-                return
-            if abs(angle - float(target_angle)) <= self.cfg.angle_tolerance_deg:
-                settled += 1
-                if settled >= 3:
-                    self._status.state = GripperState.IDLE
-                    self._status.is_moving = False
-                    return
-            elif abs(angle - last_angle) <= self.cfg.angle_tolerance_deg:
-                settled += 1
-                if settled >= 8 and abs(angle - float(target_angle)) <= 15.0:
-                    self._status.state = GripperState.IDLE
-                    self._status.is_moving = False
-                    return
-            else:
-                settled = 0
-            last_angle = angle
-            time.sleep(self.cfg.status_poll_interval)
-        self._client.hold_current_rotation()
-        self._status.state = GripperState.ERROR
-        self._status.error_msg = f"旋转运动超时（>{timeout:.1f}s），已发停止"
-
-    def get_status(self) -> GripperStatus:
-        if not self._connected or self._client is None:
-            self._status.connected = False
-            return self._status
-        try:
-            finger_pos = self._client.get_position_per_mille()
-            finger_state = self._client.get_gripper_state()
-            init_state = self._client.get_init_state()
-            rot_angle = self._client.get_rotation_angle_deg()
-            rot_state = self._client.get_rotation_state()
-            rot_init = self._client.get_rotation_init_state()
-
-            self._status.connected = True
-            self._status.initialized = init_state == 1 and rot_init == 1
-            self._status.finger_position_per_mille = float(finger_pos)
-            self._status.rotation_deg = float(rot_angle)
-            self._status.position = float(rot_angle)
-            self._status.velocity = float(self._last_speed)
-            self._status.force = float(self._last_torque)
-            self._status.gripper_state = finger_state
-            self._status.rotation_state = rot_state
-            self._status.is_moving = finger_state == 0 or rot_state == 0
-            self._status.is_gripped = finger_state == 2
-            if self._status.state != GripperState.ERROR:
-                if finger_state == 2 or rot_state == 2:
-                    self._status.state = GripperState.GRIPPING
-                elif finger_state == 0 or rot_state == 0:
-                    self._status.state = GripperState.MOVING
-                else:
-                    self._status.state = GripperState.IDLE
-            self._status.timestamp = time.time()
-        except Exception as exc:
-            self._status.state = GripperState.ERROR
-            self._status.error_msg = str(exc)
-            self._status.connected = False
-        return self._status
-
-    # -----------------------------
-    # 手指夹持
-    # -----------------------------
-    def move_fingers(self, position_per_mille: int, force: int | None = None,
-                     speed: int | None = None, wait_complete: bool = True,
-                     timeout: float = 10.0) -> GripperStatus:
-        try:
-            self._require_client()
-        except DHModbusError:
-            return self._status
-        assert self._client is not None
-        target = int(np.clip(position_per_mille, 0, 1000))
-        f = int(np.clip(force or self.cfg.default_finger_force, 20, 100))
-        s = int(np.clip(speed or self.cfg.default_finger_speed, 1, 100))
-        self._client.set_speed_pct(s)
-        self._client.set_force_pct(f)
-        self._client.set_position_per_mille(target)
-        self._last_finger_per_mille = target
-        self._last_finger_force = f
-        self._last_finger_speed = s
-        if wait_complete:
-            start = time.monotonic()
-            time.sleep(0.05)
-            while time.monotonic() - start < timeout:
-                state = self._client.get_gripper_state()
-                pos = self._client.get_position_per_mille()
-                self._status.gripper_state = state
-                self._status.finger_position_per_mille = float(pos)
-                if state in (1, 2):
-                    break
-                if state == 3:
-                    self._status.state = GripperState.ERROR
-                    self._status.error_msg = "夹持后物体掉落"
-                    break
-                time.sleep(self.cfg.status_poll_interval)
-        return self.get_status()
-
-    def open_fingers(self, **kwargs) -> GripperStatus:
-        return self.move_fingers(self.cfg.finger_open_per_mille, **kwargs)
-
-    def close_fingers(self, **kwargs) -> GripperStatus:
-        return self.move_fingers(self.cfg.finger_close_per_mille, **kwargs)
-
-    # -----------------------------
-    # 旋转
-    # -----------------------------
-    def rotate_to(self, angle_deg: float, speed: int | None = None, torque: int | None = None,
-                  wait_complete: bool = True, timeout: float = 10.0, **kwargs) -> GripperStatus:
-        try:
-            self._require_client()
-        except DHModbusError:
-            return self._status
-        assert self._client is not None
-
-        angle = float(np.clip(angle_deg, self.cfg.angle_min, self.cfg.angle_max))
-        s = int(np.clip(speed or self.cfg.default_speed, 1, 100))
-        t = int(np.clip(torque or self.cfg.default_torque, self.cfg.torque_min, self.cfg.torque_max))
-
-        self._client.set_rotation_speed_pct(s)
-        self._client.set_rotation_force_pct(t)
-        target = self._client.set_rotation_angle_deg(angle)
-
-        self._last_angle = float(target)
-        self._last_speed = s
-        self._last_torque = t
-        self._status.state = GripperState.MOVING
-        self._status.error_msg = ""
-
-        if wait_complete:
-            self._wait_complete(float(target), timeout)
-        return self.get_status()
-
-    def rotate_by(self, delta_deg: float, **kwargs) -> GripperStatus:
-        current = self.get_status().rotation_deg
-        return self.rotate_to(current + float(delta_deg), **kwargs)
-
-    def rotate_revolutions(self, revolutions: float, **kwargs) -> GripperStatus:
-        """旋转指定圈数；超过单步角度范围时自动分段，并按实际反馈角度结算。"""
-        remaining = float(revolutions) * 360.0
-        status = self.get_status()
-        max_step = abs(float(self.cfg.max_step_deg)) or 30000.0
-        while abs(remaining) > 1e-6:
-            before = float(self.get_status().rotation_deg)
-            step = float(np.clip(remaining, -max_step, max_step))
-            status = self.rotate_to(before + step, **kwargs)
-            if status.state == GripperState.ERROR:
-                return status
-            after = float(status.rotation_deg)
-            achieved = after - before
-            if abs(achieved) < max(1.0, abs(step) * 0.2):
-                status.state = GripperState.ERROR
-                status.error_msg = (
-                    "旋转未达到预期步进，可能超出角度寄存器范围或反馈异常："
-                    f"step={step:.3f}, before={before:.3f}, after={after:.3f}"
-                )
-                return status
-            remaining -= achieved
-        return status
-
-    def home_rotation(self, **kwargs) -> GripperStatus:
-        return self.rotate_to(0.0, **kwargs)
-
-    def set_rotation_speed(self, speed_pct: int) -> None:
-        self._require_client()
-        assert self._client is not None
-        self._last_speed = self._client.set_rotation_speed_pct(speed_pct)
-
-    def set_rotation_torque(self, torque_pct: int) -> None:
-        self._require_client()
-        assert self._client is not None
-        self._last_torque = self._client.set_rotation_force_pct(torque_pct)
-
-    def set_finger_speed(self, speed_pct: int) -> None:
-        self._require_client()
-        assert self._client is not None
-        self._last_finger_speed = self._client.set_speed_pct(speed_pct)
-
-    def set_finger_force(self, force_pct: int) -> None:
-        self._require_client()
-        assert self._client is not None
-        self._last_finger_force = self._client.set_force_pct(force_pct)
-
-    def open(self, **kwargs) -> GripperStatus:
-        """对于 RGI，open 定义为张开手指；旋转请用 rotate_to/rotate_revolutions。"""
-        return self.open_fingers(**kwargs)
-
-    def close(self, **kwargs) -> GripperStatus:
-        """对于 RGI，close 定义为闭合手指；旋转请用 rotate_to/rotate_revolutions。"""
-        return self.close_fingers(**kwargs)
-
-    def stop(self) -> None:
-        if self._client is not None:
-            self._client.hold_current_all()
-        self._status.state = GripperState.IDLE
-
-
-# =============================================================================
-# 双臂夹爪管理器
-# =============================================================================
-
-@dataclass
-class DualArmGripperConfig:
-    """双臂夹爪配置。
-
-    这里**没有** ``left_arm_sn`` / ``right_arm_sn``：它们曾经存在但从来没有被读过
-    （只写不读的死字段，R5.5），而且默认值是本机的 SN —— 换台臂就会误导读者。
-    哪个侧别是哪台臂，由 ``load_from_site()`` 从 site.yaml 的 ``robots[].arm`` 决定。
-    """
-
-    left_gripper: ParallelGripperConfig = field(default_factory=ParallelGripperConfig)
-    right_gripper: RotaryGripperConfig = field(default_factory=RotaryGripperConfig)
-
-
-class DualArmGripperManager:
-    """双臂夹爪管理器：统一控制左右臂夹爪。"""
-
-    def __init__(self, config: DualArmGripperConfig | None = None) -> None:
-        self.cfg = config or DualArmGripperConfig()
-        self._left: ParallelGripper | None = None
-        self._right: RotaryGripper | None = None
-
-    @property
-    def left(self) -> ParallelGripper:
-        if self._left is None:
-            self._left = ParallelGripper(self.cfg.left_gripper)
-        return self._left
-
-    @property
-    def right(self) -> RotaryGripper:
-        if self._right is None:
-            self._right = RotaryGripper(self.cfg.right_gripper)
-        return self._right
-
-    def connect_all(self) -> None:
-        self.left.connect()
-        self.right.connect()
-
-    def disconnect_all(self) -> None:
-        if self._left:
-            self._left.disconnect()
-        if self._right:
-            self._right.disconnect()
-
-    def initialize_all(self, full: bool = False) -> None:
-        self.left.initialize(full=full)
-        self.right.initialize(full=full)
-
-    def get_all_status(self) -> dict[str, GripperStatus]:
-        return {
-            "left": self.left.get_status(),
-            "right": self.right.get_status(),
-        }
-
-    def emergency_stop(self) -> None:
-        self.left.stop()
-        self.right.stop()
-
-
-def load_dual_arm_gripper_config(path: str | Path) -> DualArmGripperConfig:
-    """从 configs/dual_arm_robot.yaml 加载双臂夹爪配置。"""
-    import yaml
-
-    data = yaml.safe_load(Path(path).read_text(encoding="utf-8")) or {}
-    left = data.get("left_arm", {}) or {}
-    right = data.get("right_arm", {}) or {}
-    grip = data.get("gripper", {}) or {}
-    parallel = grip.get("parallel", {}) or {}
-    rotary = grip.get("rotary", {}) or {}
-
-    left_cfg = ParallelGripperConfig(
-        port=left.get("gripper_port", "/dev/ttyUSB0"),
-        slave_id=int(left.get("gripper_slave_id", 1)),
-        baudrate=int(left.get("gripper_baudrate", 115200)),
-        stroke_min=float(parallel.get("stroke_min", 0.0)),
-        stroke_max=float(parallel.get("stroke_max", 40.0)),
-        default_speed=int(parallel.get("default_speed", 50)),
-        default_force=int(parallel.get("default_force", 50)),
-        force_min=int(parallel.get("force_min", 20)),
-        force_max=int(parallel.get("force_max", 100)),
-        auto_initialize=bool(parallel.get("auto_initialize", True)),
-    )
-    right_cfg = RotaryGripperConfig(
-        port=right.get("gripper_port", "/dev/ttyUSB1"),
-        slave_id=int(right.get("gripper_slave_id", 1)),
-        baudrate=int(right.get("gripper_baudrate", 115200)),
-        angle_min=float(rotary.get("angle_min", -32767.0)),
-        angle_max=float(rotary.get("angle_max", 32767.0)),
-        max_step_deg=float(rotary.get("max_step_deg", 30000.0)),
-        finger_open_per_mille=int(rotary.get("finger_open_per_mille", 1000)),
-        finger_close_per_mille=int(rotary.get("finger_close_per_mille", 0)),
-        default_speed=int(rotary.get("default_speed", 50)),
-        default_torque=int(rotary.get("default_torque", 50)),
-        default_finger_speed=int(rotary.get("default_finger_speed", 50)),
-        default_finger_force=int(rotary.get("default_finger_force", 50)),
-        torque_min=int(rotary.get("torque_min", 20)),
-        torque_max=int(rotary.get("torque_max", 100)),
-        auto_initialize=bool(rotary.get("auto_initialize", True)),
-    )
-    return DualArmGripperConfig(
-        left_gripper=left_cfg,
-        right_gripper=right_cfg,
-    )
-
-
-def load_from_site(data: dict) -> DualArmGripperConfig:
-    """从 ``~/ros2/config/site.yaml`` 构建 ``DualArmGripperConfig``。
-
-    左右靠 ``robots[].arm`` 认，**不靠 SN 里的数字**：``"063989 就是左臂"``
-    是站点数据，写进代码后换个站点就会把左右夹爪搞反
-    （症状是"右臂的旋转爪去开左臂的平行爪"，串口对不上才报错）。
-    """
-    robots = list((data.get("robot") or {}).get("robots") or [])
-    left = next((r for r in robots if str(r.get("arm") or "").lower() == "left"), {})
-    right = next((r for r in robots if str(r.get("arm") or "").lower() == "right"), {})
-    g = data.get("gripper") or {}
-    parallel = g.get("parallel") or {}
-    rotary = g.get("rotary") or {}
-    lg = left.get("gripper") or {}
-    rg = right.get("gripper") or {}
-    stroke = float(lg.get("stroke_mm") or parallel.get("stroke_max") or g.get("stroke_mm") or 20.0)
-    left_cfg = ParallelGripperConfig(
-        port=str(lg.get("port") or "/dev/ttyUSB0"),
-        slave_id=int(lg.get("slave_id") or g.get("slave_id") or 1),
-        baudrate=int(lg.get("baud") or g.get("baud") or 115200),
-        stroke_min=float(parallel.get("stroke_min") or 0.0),
-        stroke_max=stroke,
-        default_speed=int(parallel.get("default_speed") or g.get("speed_pct") or 50),
-        default_force=int(parallel.get("default_force") or g.get("force_pct") or 50),
-        force_min=int(parallel.get("force_min") or 20),
-        force_max=int(parallel.get("force_max") or 100),
-        auto_initialize=bool(lg.get("auto_init", parallel.get("auto_initialize", True))),
-        init_timeout=float(parallel.get("init_timeout") or 10.0),
-    )
-    right_cfg = RotaryGripperConfig(
-        port=str(rg.get("port") or "/dev/ttyUSB1"),
-        slave_id=int(rg.get("slave_id") or 1),
-        baudrate=int(rg.get("baud") or g.get("baud") or 115200),
-        angle_min=float(rotary.get("angle_min") or -32767.0),
-        angle_max=float(rotary.get("angle_max") or 32767.0),
-        max_step_deg=float(rotary.get("max_step_deg") or 30000.0),
-        finger_open_per_mille=int(rotary.get("finger_open_per_mille") or 1000),
-        finger_close_per_mille=int(rotary.get("finger_close_per_mille") or 0),
-        default_speed=int(rotary.get("default_speed") or 50),
-        default_torque=int(rotary.get("default_torque") or 50),
-        default_finger_speed=int(rotary.get("default_finger_speed") or 50),
-        default_finger_force=int(rotary.get("default_finger_force") or 50),
-        torque_min=int(rotary.get("torque_min") or 20),
-        torque_max=int(rotary.get("torque_max") or 100),
-        auto_initialize=bool(rg.get("auto_init", rotary.get("auto_initialize", False))),
-        init_timeout=float(rotary.get("init_timeout") or 15.0),
-    )
-    return DualArmGripperConfig(
-        left_gripper=left_cfg,
-        right_gripper=right_cfg,
-    )

install/arm_application/lib/python3.11/site-packages/arm_application/dual_gripper_cli.py

@@ -1,190 +0,0 @@
-#!/usr/bin/env python3
-"""DH PGEA（左臂）与 RGI（右臂）的命令行工具。从 site.yaml 读取串口。"""
-
-from __future__ import annotations
-
-import argparse
-import os
-import sys
-import time
-from pathlib import Path
-
-import yaml
-
-from arm_application.dual_gripper import DualArmGripperManager, load_from_site
-
-
-def _site_path() -> Path:
-    env = os.environ.get("SITE_CONFIG", "").strip()
-    if env:
-        return Path(env).expanduser()
-    return Path.home() / "ros2" / "config" / "site.yaml"
-
-
-def _print_status(label: str, status) -> None:
-    print(
-        f"   {label}: state={status.state.name} "
-        f"pos={status.position:.2f} finger={status.finger_position_per_mille:.0f}‰ "
-        f"rot={status.rotation_deg:.2f}° moving={status.is_moving} "
-        f"gripped={status.is_gripped} init={status.initialized} "
-        f"err={status.error_msg or '-'}"
-    )
-
-
-def _load_site_exporter():
-    """复用 ``scripts/lib/export_site_env.py`` —— 它才是 --arm 别名的权威定义。
-
-    这里**不自己再抄一份** ``89 -> SN`` 的映射：抄一份就多一处会漂移的地方（M2）。
-    """
-    root = Path(__file__).resolve().parents[3]
-    lib = str(root / "scripts" / "lib")
-    if lib not in sys.path:
-        sys.path.insert(0, lib)
-    import export_site_env  # type: ignore
-
-    return export_site_env
-
-
-def _normalize_arm(raw: str, data: dict) -> str:
-    """把 --arm 的写法归一成 ``left`` / ``right`` / ``both``。
-
-    别名（``89`` 这类）由 ``export_site_env`` 从 site.yaml 的 ``robots[]`` 现算
-    （臂号 = 该臂 SN 末两位）。代码里**不写死** SN，否则换台机器人后会指错臂，
-    而症状是"给右臂发命令，左臂动了"。
-    """
-    t = (raw or "").strip()
-    if t.lower() in ("both", "dual", "all"):
-        return "both"
-    site_env = _load_site_exporter()
-    sn, matched = site_env.resolve_robot_sn(data, t)
-    if not matched:
-        raise SystemExit(
-            f"未知的 --arm {raw!r}：不在 site.yaml 的 robot.robots[] 里。\n"
-            f"可用：\n{site_env.describe_arms(data)}\n"
-            f"也可以直接写 left|right|both。"
-        )
-    for row in (data.get("robot") or {}).get("robots") or []:
-        if str(row.get("sn") or "").strip() != sn:
-            continue
-        arm = str(row.get("arm") or "").strip().lower()
-        if arm in ("left", "right"):
-            return arm
-        raise SystemExit(
-            f"--arm {raw!r} 选中了 {sn}，但 site.yaml 里这台臂没写 arm: left|right，"
-            f"无法判断操作哪个夹爪。请补上这个键。"
-        )
-    raise SystemExit(
-        f"--arm {raw!r} 解析出 {sn}，但它不在 site.yaml 的 robot.robots[] 里，"
-        f"无法判断是哪个夹爪。"
-    )
-
-
-def test_parallel(manager: DualArmGripperManager, args: argparse.Namespace) -> None:
-    gripper = manager.left
-    print(f"=== PGEA 平行夹爪（{gripper.cfg.port}）===")
-    gripper.connect()
-    _print_status("已连接", gripper.get_status())
-    if args.test in ("init", "probe", "all"):
-        if args.test != "probe":
-            gripper.initialize(full=args.init_full)
-            _print_status("初始化", gripper.get_status())
-    if args.test in ("open", "all"):
-        _print_status("张开", gripper.open(force=args.force, speed=args.speed))
-    if args.test in ("close", "all"):
-        _print_status("闭合", gripper.close(force=args.force, speed=args.speed))
-    if args.test in ("move", "pos") and args.pos is not None:
-        _print_status("移动", gripper.move_to(args.pos, force=args.force, speed=args.speed))
-    if args.test in ("status", "probe"):
-        _print_status("状态", gripper.get_status())
-
-
-def test_rotary(manager: DualArmGripperManager, args: argparse.Namespace) -> None:
-    gripper = manager.right
-    print(f"=== RGI 旋转夹爪（{gripper.cfg.port}）===")
-    gripper.connect()
-    _print_status("已连接", gripper.get_status())
-    if args.test in ("init", "all"):
-        gripper.initialize(full=args.init_full)
-        _print_status("初始化", gripper.get_status())
-    if args.test in ("open", "all"):
-        _print_status("张开", gripper.open_fingers(force=args.force, speed=args.speed))
-    if args.test in ("close", "all"):
-        _print_status("闭合", gripper.close_fingers(force=args.force, speed=args.speed))
-    if args.test in ("move", "pos") and args.pos is not None:
-        _print_status("手指", gripper.move_fingers(int(args.pos), force=args.force, speed=args.speed))
-    if args.test in ("rotate", "rotate_to", "all"):
-        if args.revolutions is not None:
-            _print_status(
-                "圈数",
-                gripper.rotate_revolutions(args.revolutions, speed=args.speed, torque=args.force),
-            )
-        else:
-            angle = args.angle if args.angle is not None else 90.0
-            _print_status("旋转", gripper.rotate_to(angle, speed=args.speed, torque=args.force))
-    if args.test == "rotate_by":
-        delta = args.angle if args.angle is not None else 45.0
-        _print_status("相对旋转", gripper.rotate_by(delta, speed=args.speed, torque=args.force))
-    if args.test in ("status", "probe"):
-        _print_status("状态", gripper.get_status())
-
-
-def main() -> int:
-    parser = argparse.ArgumentParser(description="DH PGEA / RGI 命令行工具（串口来自 site.yaml）")
-    parser.add_argument(
-        "--arm",
-        # 默认取当前臂（parse_arm_args.sh / load_site_config.sh 已经把它放进环境）。
-        # **不**兜底成某个具体臂号：那等于"没选臂时也能跑，跑的是哪台看运气"。
-        default=(
-            os.environ.get("SITE_ROBOT_ARM")
-            or os.environ.get("ROBOT_SN")
-            or os.environ.get("SITE_ROBOT_SN")
-            or ""
-        ),
-        help="left|right|both，或该臂的臂号（= SN 末两位，由 site.yaml 决定）",
-    )
-    parser.add_argument(
-        "--test",
-        default="status",
-        help="probe|init|open|close|move|pos|rotate|rotate_by|status",
-    )
-    parser.add_argument("--pos", type=float, default=None, help="PGEA 为 mm；RGI 为手指 0-1000")
-    parser.add_argument("--angle", type=float, default=None)
-    parser.add_argument("--revolutions", type=float, default=None)
-    parser.add_argument("--speed", type=int, default=25)
-    parser.add_argument("--force", type=int, default=33)
-    parser.add_argument("--init-full", action="store_true")
-    parser.add_argument("--site", default="")
-    args = parser.parse_args()
-
-    site = Path(args.site).expanduser() if args.site else _site_path()
-    if not site.is_file():
-        print(f"错误：site.yaml 缺失：{site}", file=sys.stderr)
-        return 1
-    data = yaml.safe_load(site.read_text(encoding="utf-8")) or {}
-    manager = DualArmGripperManager(load_from_site(data))
-    arm = _normalize_arm(str(args.arm), data)
-    print(f"site={site} arm={arm} test={args.test}")
-
-    try:
-        if arm == "left":
-            test_parallel(manager, args)
-        elif arm == "right":
-            test_rotary(manager, args)
-        else:
-            test_parallel(manager, args)
-            test_rotary(manager, args)
-    except KeyboardInterrupt:
-        print("收到中断 —— 停止")
-        manager.emergency_stop()
-        return 130
-    except Exception as exc:
-        print(f"错误：{exc}", file=sys.stderr)
-        return 1
-    finally:
-        manager.disconnect_all()
-        time.sleep(0.05)
-    return 0
-
-
-if __name__ == "__main__":
-    raise SystemExit(main())

install/arm_application/lib/python3.11/site-packages/arm_application/dual_gripper_node.py

@@ -1,320 +0,0 @@
-"""ROS 2 双臂 DH 夹爪节点（左臂 PGEA + 右臂 RGI）。使用 arm_msgs，而非 Flexiv RDK。"""
-
-from __future__ import annotations
-
-import os
-import threading
-from pathlib import Path
-
-import rclpy
-import yaml
-from rclpy.callback_groups import ReentrantCallbackGroup
-from rclpy.executors import MultiThreadedExecutor
-from rclpy.node import Node
-
-from arm_application.dual_gripper import (
-    DualArmGripperManager,
-    GripperState,
-    load_dual_arm_gripper_config,
-    load_from_site,
-)
-from arm_msgs.msg import GripperCommand as GripperCommandMsg
-from arm_msgs.msg import GripperStatus as GripperStatusMsg
-from arm_msgs.srv import GripperExecute
-
-from arm_application.ros_node import TolerantNode
-
-
-def _site_path() -> Path:
-    env = os.environ.get("SITE_CONFIG", "").strip()
-    if env:
-        return Path(env).expanduser()
-    return Path.home() / "ros2" / "config" / "site.yaml"
-
-
-def _as_bool(val, default: bool = True) -> bool:
-    if val is None:
-        return default
-    if isinstance(val, str):
-        return val.strip().lower() in ("1", "true", "yes", "on")
-    return bool(val)
-
-
-class DualGripperNode(TolerantNode):
-    def __init__(self) -> None:
-        super().__init__("dual_gripper_driver")
-        self.declare_parameter("config_file", "")
-        self.declare_parameter("auto_connect", True)
-        self.declare_parameter("allow_offline", True)
-        self.declare_parameter("publish_hz", 10.0)
-        self.declare_parameter("sides", "left,right")
-
-        self._cb_group = ReentrantCallbackGroup()
-        self._locks = {"left": threading.Lock(), "right": threading.Lock()}
-        sides_raw = str(self.get_parameter("sides").value or "left,right")
-        self._sides = [s.strip().lower() for s in sides_raw.split(",") if s.strip()]
-        if not self._sides:
-            self._sides = ["left", "right"]
-
-        cfg_path = self.get_parameter("config_file").get_parameter_value().string_value.strip()
-        if cfg_path:
-            self._manager = DualArmGripperManager(load_dual_arm_gripper_config(cfg_path))
-        else:
-            site = _site_path()
-            data = yaml.safe_load(site.read_text(encoding="utf-8")) or {}
-            self._manager = DualArmGripperManager(load_from_site(data))
-            self.get_logger().info(f"夹爪配置来自 site.yaml：{site}")
-
-        self._connect_errors: dict[str, str] = {}
-        if _as_bool(self.get_parameter("auto_connect").value, True):
-            self._connect_all()
-
-        self._left_pub = self.create_publisher(GripperStatusMsg, "~/left/status", 10)
-        self._right_pub = self.create_publisher(GripperStatusMsg, "~/right/status", 10)
-        if "left" in self._sides:
-            self.create_subscription(
-                GripperCommandMsg,
-                "~/left/command",
-                lambda msg: self._on_command("left", msg),
-                10,
-                callback_group=self._cb_group,
-            )
-            self.create_service(
-                GripperExecute,
-                "~/left/execute",
-                lambda req, resp: self._on_execute("left", req, resp),
-                callback_group=self._cb_group,
-            )
-        if "right" in self._sides:
-            self.create_subscription(
-                GripperCommandMsg,
-                "~/right/command",
-                lambda msg: self._on_command("right", msg),
-                10,
-                callback_group=self._cb_group,
-            )
-            self.create_service(
-                GripperExecute,
-                "~/right/execute",
-                lambda req, resp: self._on_execute("right", req, resp),
-                callback_group=self._cb_group,
-            )
-
-        period = 1.0 / max(1e-3, float(self.get_parameter("publish_hz").value))
-        self.create_timer(period, self._publish_status, callback_group=self._cb_group)
-        self.get_logger().info(
-            f"dual_gripper_driver ready sides={self._sides} "
-            "~/left|right/command status execute"
-        )
-
-    def _connect_all(self) -> None:
-        mapping = (("left", self._manager.left), ("right", self._manager.right))
-        for side, gripper in mapping:
-            if side not in self._sides:
-                continue
-            try:
-                gripper.connect()
-                self.get_logger().info(f"{side} 夹爪已连接：{gripper.cfg.port}")
-            except Exception as exc:
-                self._connect_errors[side] = str(exc)
-                self.get_logger().error(f"{side} 夹爪连接失败：{exc}")
-        if self._connect_errors and not _as_bool(self.get_parameter("allow_offline").value, True):
-            raise RuntimeError(f"夹爪连接失败：{self._connect_errors}")
-
-    def _gripper(self, side: str):
-        return self._manager.left if side == "left" else self._manager.right
-
-    def _execute(self, side: str, msg: GripperCommandMsg):
-        gripper = self._gripper(side)
-        command = (msg.command or "").strip().lower()
-        timeout = float(msg.timeout or 10.0)
-        wait = bool(msg.wait_complete)
-
-        if side in self._connect_errors and not gripper.is_connected():
-            status = gripper.get_status()
-            status.error_msg = self._connect_errors[side]
-            return status
-
-        if command == "connect":
-            gripper.connect()
-            self._connect_errors.pop(side, None)
-            return gripper.get_status()
-        if command == "init":
-            gripper.initialize(full=False)
-            return gripper.get_status()
-        if command == "init_full":
-            gripper.initialize(full=True)
-            return gripper.get_status()
-        if command == "stop":
-            gripper.stop()
-            return gripper.get_status()
-
-        if side == "left":
-            if command == "open":
-                return gripper.open(
-                    force=int(msg.force) if msg.force else None,
-                    speed=int(msg.speed) if msg.speed else None,
-                    wait_complete=wait,
-                    timeout=timeout,
-                )
-            if command == "close":
-                return gripper.close(
-                    force=int(msg.force) if msg.force else None,
-                    speed=int(msg.speed) if msg.speed else None,
-                    wait_complete=wait,
-                    timeout=timeout,
-                )
-            if command == "move":
-                return gripper.move_to(
-                    float(msg.position),
-                    force=int(msg.force) if msg.force else None,
-                    speed=int(msg.speed) if msg.speed else None,
-                    wait_complete=wait,
-                    timeout=timeout,
-                )
-            if command == "set_force":
-                gripper.set_force(int(msg.force))
-                return gripper.get_status()
-            if command == "set_speed":
-                gripper.set_speed(int(msg.speed))
-                return gripper.get_status()
-        else:
-            if command == "open":
-                return gripper.open_fingers(
-                    force=int(msg.force) if msg.force else None,
-                    speed=int(msg.speed) if msg.speed else None,
-                    wait_complete=wait,
-                    timeout=timeout,
-                )
-            if command == "close":
-                return gripper.close_fingers(
-                    force=int(msg.force) if msg.force else None,
-                    speed=int(msg.speed) if msg.speed else None,
-                    wait_complete=wait,
-                    timeout=timeout,
-                )
-            if command == "move":
-                return gripper.move_fingers(
-                    int(msg.position),
-                    force=int(msg.force) if msg.force else None,
-                    speed=int(msg.speed) if msg.speed else None,
-                    wait_complete=wait,
-                    timeout=timeout,
-                )
-            if command in ("rotate", "rotate_to"):
-                return gripper.rotate_to(
-                    float(msg.position),
-                    speed=int(msg.speed) if msg.speed else None,
-                    torque=int(msg.force) if msg.force else None,
-                    wait_complete=wait,
-                    timeout=timeout,
-                )
-            if command == "rotate_by":
-                return gripper.rotate_by(
-                    float(msg.position),
-                    speed=int(msg.speed) if msg.speed else None,
-                    torque=int(msg.force) if msg.force else None,
-                    wait_complete=wait,
-                    timeout=timeout,
-                )
-            if command == "rotate_revolutions":
-                return gripper.rotate_revolutions(
-                    float(msg.position),
-                    speed=int(msg.speed) if msg.speed else None,
-                    torque=int(msg.force) if msg.force else None,
-                    wait_complete=wait,
-                    timeout=timeout,
-                )
-            if command == "home_rotation":
-                return gripper.home_rotation(
-                    speed=int(msg.speed) if msg.speed else None,
-                    torque=int(msg.force) if msg.force else None,
-                    wait_complete=wait,
-                    timeout=timeout,
-                )
-            if command == "set_force":
-                gripper.set_rotation_torque(int(msg.force))
-                return gripper.get_status()
-            if command == "set_speed":
-                gripper.set_rotation_speed(int(msg.speed))
-                return gripper.get_status()
-            if command == "set_finger_force":
-                gripper.set_finger_force(int(msg.force))
-                return gripper.get_status()
-            if command == "set_finger_speed":
-                gripper.set_finger_speed(int(msg.speed))
-                return gripper.get_status()
-
-        status = gripper.get_status()
-        status.state = GripperState.ERROR
-        status.error_msg = f"不支持的命令 {command!r}（side={side}）"
-        return status
-
-    def _on_command(self, side: str, msg: GripperCommandMsg) -> None:
-        with self._locks[side]:
-            status = self._execute(side, msg)
-        if status.state == GripperState.ERROR:
-            self.get_logger().warn(f"{side} 命令 {msg.command!r} 失败：{status.error_msg}")
-
-    def _on_execute(self, side: str, request: GripperExecute.Request, response: GripperExecute.Response):
-        with self._locks[side]:
-            status = self._execute(side, request.command)
-        response.success = status.state != GripperState.ERROR and status.connected
-        response.message = status.error_msg or "OK"
-        response.status = self._to_status_msg(side, status)
-        return response
-
-    def _to_status_msg(self, side: str, status) -> GripperStatusMsg:
-        gripper = self._gripper(side)
-        msg = GripperStatusMsg()
-        msg.header.stamp = self.get_clock().now().to_msg()
-        msg.header.frame_id = f"{side}_gripper"
-        msg.name = gripper.name
-        msg.gripper_type = gripper.gripper_type.name.lower()
-        msg.state = status.state.name
-        msg.connected = bool(status.connected)
-        msg.initialized = bool(status.initialized)
-        msg.is_moving = bool(status.is_moving)
-        msg.is_gripped = bool(status.is_gripped)
-        msg.position = float(status.position)
-        msg.finger_position_per_mille = float(status.finger_position_per_mille)
-        msg.rotation_deg = float(status.rotation_deg)
-        msg.gripper_state = int(status.gripper_state)
-        msg.rotation_state = int(status.rotation_state)
-        msg.velocity = float(status.velocity)
-        msg.force = float(status.force)
-        msg.error_msg = status.error_msg or ""
-        return msg
-
-    def _publish_status(self) -> None:
-        if "left" in self._sides:
-            left_status = self._manager.left.get_status()
-            if "left" in self._connect_errors and not left_status.connected:
-                left_status.error_msg = self._connect_errors["left"]
-            self._left_pub.publish(self._to_status_msg("left", left_status))
-        if "right" in self._sides:
-            right_status = self._manager.right.get_status()
-            if "right" in self._connect_errors and not right_status.connected:
-                right_status.error_msg = self._connect_errors["right"]
-            self._right_pub.publish(self._to_status_msg("right", right_status))
-
-    def destroy_node(self) -> bool:
-        try:
-            self._manager.disconnect_all()
-        finally:
-            return super().destroy_node()
-
-
-def main() -> None:
-    rclpy.init()
-    node = DualGripperNode()
-    executor = MultiThreadedExecutor(num_threads=4)
-    try:
-        rclpy.spin(node, executor=executor)
-    finally:
-        node.destroy_node()
-        rclpy.shutdown()
-
-
-if __name__ == "__main__":
-    main()