克隆下来必须带上正在用的视觉权重和本机 site.yaml，否则感知只能指到一台机器上的路径，别人起不来。

顶视迁臂改走厂家 MoveL，D405 框心对准后再对孔；夹爪停在 caught 时 open 不再把相同目标当成已经张开，init 会真的回零。

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

scripts/d435_k0_once.py

@@ -0,0 +1,173 @@
+#!/usr/bin/env python3
+"""拍一张顶视 D435，把 k0 用眼在手外标定变到左臂基座。
+
+stdout 只打一行，给 run_all.sh --d435-only 接厂家 MoveL：
+
+    K0_BASE x y z
+
+x y z 单位米，坐标系是这条臂的基座（与 RDK 的 WORLD 同一套）。
+变换就是 handeye/results/<臂>_d435_eye_to_hand.yaml 的 T_4x4：
+
+    p_基座 = T_cam_to_base · p_相机
+
+k0 的 z 是工件表面，不是工具尖该去的高度。运动脚本会保持当前尖高，只用这里的 xy。
+没有可信 k0 就退出，不用检测框中心冒充。
+"""
+
+from __future__ import annotations
+
+import sys
+import time
+from pathlib import Path
+
+import numpy as np
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import qos_profile_sensor_data
+from sensor_msgs.msg import CameraInfo, Image
+
+ROOT = Path(__file__).resolve().parents[1]
+sys.path.insert(0, str(ROOT / "scripts" / "lib"))
+
+from export_site_env import load_site, perception_overrides  # noqa: E402
+from arm_application.geom_ik import load_handeye_T  # noqa: E402
+from arm_application.overhead_oneshot import (  # noqa: E402
+    depth_raw_to_m,
+    oneshot_k0_base,
+    select_oneshot_det,
+)
+from arm_application.perception_node import _bgr_copy  # noqa: E402
+from arm_application.yolo_detector import build_detector  # noqa: E402
+
+
+def _decode_depth(msg: Image):
+    w, h = int(msg.width), int(msg.height)
+    enc = (msg.encoding or "").lower()
+    if enc in ("16uc1", "mono16"):
+        raw = np.frombuffer(msg.data, dtype=np.uint16)
+        if raw.size < w * h:
+            return None
+        return raw[: w * h].reshape(h, w)
+    if enc == "32fc1":
+        raw = np.frombuffer(msg.data, dtype=np.float32)
+        if raw.size < w * h:
+            return None
+        return raw[: w * h].reshape(h, w)
+    return None
+
+
+class OneShot(Node):
+    def __init__(self, topics: dict) -> None:
+        super().__init__("d435_k0_once")
+        self.color = None
+        self.depth = None
+        self.info = None
+        self.create_subscription(Image, topics["image"], self._on_color, qos_profile_sensor_data)
+        self.create_subscription(Image, topics["depth"], self._on_depth, qos_profile_sensor_data)
+        self.create_subscription(CameraInfo, topics["info"], self._on_info, qos_profile_sensor_data)
+
+    def _on_color(self, msg: Image) -> None:
+        self.color = msg
+
+    def _on_depth(self, msg: Image) -> None:
+        self.depth = msg
+
+    def _on_info(self, msg: CameraInfo) -> None:
+        self.info = msg
+
+    def ready(self) -> bool:
+        return self.color is not None and self.depth is not None and self.info is not None
+
+
+def main() -> int:
+    perc = perception_overrides(load_site())
+    if not perc.get("overhead_oneshot_enable"):
+        print("site 没有接上 D435 一次性（overhead_oneshot_enable）", file=sys.stderr)
+        return 2
+    handeye = str(perc.get("overhead_handeye_file") or "")
+    topics = {
+        "image": str(perc.get("overhead_image_topic") or ""),
+        "depth": str(perc.get("overhead_depth_topic") or ""),
+        "info": str(perc.get("overhead_info_topic") or ""),
+    }
+    if not handeye or not all(topics.values()):
+        print(f"眼在手外话题或标定文件没接上：{handeye} {topics}", file=sys.stderr)
+        return 2
+    T = load_handeye_T(handeye)
+    print(f"手眼 {handeye}", file=sys.stderr)
+    print(f"图 {topics['image']}", file=sys.stderr)
+
+    weights = str(perc.get("weights") or "")
+    detector = build_detector(
+        weights,
+        backend=str(perc.get("backend") or "auto"),
+        conf=float(perc.get("conf") or 0.1),
+        iou=float(perc.get("iou") or 0.45),
+        device=str(perc.get("device") or "0"),
+        imgsz=int(perc.get("imgsz") or 640),
+    )
+
+    rclpy.init()
+    node = OneShot(topics)
+    t0 = time.monotonic()
+    try:
+        while rclpy.ok() and not node.ready():
+            rclpy.spin_once(node, timeout_sec=0.2)
+            if time.monotonic() - t0 > 15.0:
+                print("15s 内没有凑齐彩色图、深度、内参", file=sys.stderr)
+                return 3
+        color, depth, info = node.color, node.depth, node.info
+    finally:
+        node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+    bgr = _bgr_copy(color)
+    dets = detector.detect(bgr)
+    min_conf = float(perc["overhead_oneshot_conf"])
+    det = select_oneshot_det(dets, min_conf)
+    if det is None:
+        print(f"画面里没有 conf≥{min_conf:.2f} 的工件", file=sys.stderr)
+        return 4
+    raw = _decode_depth(depth)
+    depth_m = depth_raw_to_m(raw, depth.encoding or "")
+    if depth_m is None:
+        print("深度无法转成米", file=sys.stderr)
+        return 4
+    k = info.k
+    got = oneshot_k0_base(
+        det,
+        depth_m,
+        fx=float(k[0]),
+        fy=float(k[4]),
+        cx=float(k[2]),
+        cy=float(k[5]),
+        T_cam_to_base=T,
+        min_z_m=float(perc["overhead_min_depth_m"]),
+        max_z_m=float(perc["overhead_max_depth_m"]),
+        kpt_index=int(perc.get("keypoint_index") or 0),
+        kpt_min_conf=float(perc.get("keypoint_min_conf") or 0.30),
+        min_det_conf=min_conf,
+        color_wh=(int(color.width), int(color.height)),
+        depth_wh=(int(depth_m.shape[1]), int(depth_m.shape[0])),
+    )
+    if got is None:
+        print("k0 或深度不可用，不发点", file=sys.stderr)
+        return 4
+    p_base, is_k0 = got
+    if not is_k0:
+        print(
+            f"顶视没有可信 k0，只有框心 ({p_base[0]:.3f},{p_base[1]:.3f},{p_base[2]:.3f})，拒绝",
+            file=sys.stderr,
+        )
+        return 4
+    print(
+        f"k0 基座 ({p_base[0]:.4f}, {p_base[1]:.4f}, {p_base[2]:.4f}) m  conf={float(det.conf):.2f}",
+        file=sys.stderr,
+    )
+    print(f"K0_BASE {p_base[0]:.6f} {p_base[1]:.6f} {p_base[2]:.6f}")
+    return 0
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())

scripts/lib/export_site_env.py

@@ -903,6 +903,7 @@ def servo_overrides(data: dict) -> dict:
         "ee_hold_kp", "ee_w_max",
         "lock_hold_sec",
         "lock_still_sec",
+        "wrist_align_sec",
         "lock_still_move_m",
         "a_max", "max_qdot", "damping",
         "vel_lpf_alpha", "dq_window_size", "traj_stream_hz", "exec_horizon_sec",
@@ -934,7 +935,12 @@ def servo_overrides(data: dict) -> dict:
         if "lock_still_sec" not in servo:
             raise RuntimeError(
                 "servo.lock_still_sec 必须显式写"
-                "（D405 跟住后法兰静止多久才进二阶段）。不许用代码默认 3。"
+                "（D435 到位后法兰停稳多久才交回 D405）。不许用代码默认 3。"
+            )
+        if "wrist_align_sec" not in servo:
+            raise RuntimeError(
+                "servo.wrist_align_sec 必须显式写"
+                "（D405 框心对准多久才进对孔）。不许用代码默认 1。"
             )
         if "lock_still_move_m" not in servo:
             raise RuntimeError(
@@ -942,6 +948,7 @@ def servo_overrides(data: dict) -> dict:
                 "（静止判据的法兰 XY 位移上限）。不许用代码默认 15mm。"
             )
         out["lock_still_sec"] = float(servo["lock_still_sec"])
+        out["wrist_align_sec"] = float(servo["wrist_align_sec"])
         out["lock_still_move_m"] = float(servo["lock_still_move_m"])
         out["overhead_k0_topic"] = "visual_servo/overhead_k0"
         out["wrist_conf_topic"] = "visual_servo/wrist_conf"

scripts/lib/rdk_release.py

@@ -0,0 +1,19 @@
+"""D435 一次性要走厂家 MoveL 时，先认出谁占着 RDK。
+
+`ros2_control_node` 和真机 `stack.launch` 占着唯一的 RDK 连接。
+感知、伺服、本脚本自己不算。误伤它们会把正在推理的进程一起杀掉。
+"""
+
+from __future__ import annotations
+
+
+def is_control_stack_cmd(cmd: str) -> bool:
+    """这条命令行是不是占着 RDK 的真机控制栈。"""
+    text = cmd or ""
+    if "rdk_release_movel" in text:
+        return False
+    if "ros2_control_node" in text:
+        return True
+    if "stack.launch.py" in text and "backend:=real" in text:
+        return True
+    return False

scripts/rdk_goto_xyz.py

@@ -17,6 +17,7 @@
    [x, y, z, qw, qx, qy, qz]，单位米，坐标系是机器人世界系（基座那套 WORLD）。
    位置是当前 Studio 工具的 TCP（现在是 tool10120938 的尖），不是法兰，也不是相机。
 4. 目标只换 x、y、z。姿态沿用现在的朝向，折成厂家 MoveL 要的欧拉角（度，ZYX）。
+   `--keep-z` 时 z 保持当前工具尖：K0 的 z 是工件表面，不能把尖送到那上面。
 5. SwitchMode(NRT_PRIMITIVE_EXECUTION)，ExecutePrimitive("MoveL", ...)。
 6. 轮询 primitive_states()["reachedTarget"]。到了再读一次 tcp_pose，印出还差多少毫米。
 
@@ -134,6 +135,11 @@ def main() -> int:
     parser.add_argument("--vel", type=float, default=0.05, help="TCP 线速度 m/s，默认 0.05")
     parser.add_argument("--max-step-m", type=float, default=0.40, help="离当前点超过这么多米就拒绝，除非 --go")
     parser.add_argument("--go", action="store_true", help="允许超过 --max-step-m")
+    parser.add_argument(
+        "--keep-z",
+        action="store_true",
+        help="XY 用给定点，Z 保持当前工具尖。K0 的 Z 是工件表面，不能把尖送到那上面",
+    )
     parser.add_argument("--timeout", type=float, default=60.0, help="等 reachedTarget 的秒数")
     args = parser.parse_args()
 
@@ -154,6 +160,12 @@ def main() -> int:
             return 0
 
         target_xyz = [float(v) for v in args.xyz]
+        if args.keep_z:
+            print(
+                f"给定点 z={target_xyz[2]:.4f} m 是工件表面，不下发。"
+                f"工具尖高度保持当前 {pose[2]:.4f} m"
+            )
+            target_xyz[2] = float(pose[2])
         dist = math.dist(pose[:3], target_xyz)
         print(f"目标 xyz=({target_xyz[0]:.4f}, {target_xyz[1]:.4f}, {target_xyz[2]:.4f}) m")
         print(f"直线距离 {dist * 1000:.0f} mm    速度 {args.vel:.3f} m/s    姿态保持当前")

scripts/rdk_release_movel.py

@@ -0,0 +1,174 @@
+#!/usr/bin/env python3
+"""D435 这一点用厂家 MoveL 送到工具尖，再把真机控制栈拉回来。
+
+RDK 同时只能有一个连接。混合流程里 `run_real.sh` 已经占着它，
+所以顺序是：停真机栈 → MoveL（`--keep-z --go`）→ 用 `--no-stop` 把栈拉回来。
+
+`--no-stop` 是为了不扫 `install/`：感知和伺服还在跑，扫了会把自己杀掉。
+YOLO 不在这里关。
+"""
+
+from __future__ import annotations
+
+import argparse
+import os
+import signal
+import subprocess
+import sys
+import time
+from pathlib import Path
+
+ROOT = Path(__file__).resolve().parents[1]
+sys.path.insert(0, str(ROOT / "scripts" / "lib"))
+
+from rdk_release import is_control_stack_cmd  # noqa: E402
+
+RDK_PYTHON = os.environ.get("RDK_PYTHON", "/home/cc/rdk_py310/bin/python")
+
+
+def _procs() -> list[tuple[int, str]]:
+    out: list[tuple[int, str]] = []
+    proc = Path("/proc")
+    for entry in proc.iterdir():
+        if not entry.name.isdigit():
+            continue
+        pid = int(entry.name)
+        if pid == os.getpid():
+            continue
+        try:
+            raw = (entry / "cmdline").read_bytes().replace(b"\x00", b" ")
+        except OSError:
+            continue
+        cmd = raw.decode("utf-8", "replace").strip()
+        if cmd:
+            out.append((pid, cmd))
+    return out
+
+
+def _control_pids() -> list[tuple[int, str]]:
+    return [(pid, cmd) for pid, cmd in _procs() if is_control_stack_cmd(cmd)]
+
+
+def _term(pids: list[int]) -> None:
+    for pid in pids:
+        try:
+            os.kill(pid, signal.SIGTERM)
+        except OSError:
+            pass
+    deadline = time.monotonic() + 8.0
+    while time.monotonic() < deadline:
+        alive = []
+        for pid in pids:
+            try:
+                os.kill(pid, 0)
+            except OSError:
+                continue
+            alive.append(pid)
+        if not alive:
+            return
+        time.sleep(0.2)
+        pids = alive
+    for pid in pids:
+        try:
+            os.kill(pid, signal.SIGKILL)
+        except OSError:
+            pass
+
+
+def _service_up() -> bool:
+    try:
+        listed = subprocess.run(
+            ["timeout", "3", "ros2", "service", "list"],
+            capture_output=True,
+            text=True,
+            check=False,
+        )
+    except OSError:
+        return False
+    return "/controller_manager/list_controllers" in (listed.stdout or "")
+
+
+def _controller_active() -> bool:
+    if not _service_up():
+        return False
+    try:
+        call = subprocess.run(
+            [
+                "timeout", "3", "ros2", "service", "call",
+                "/controller_manager/list_controllers",
+                "controller_manager_msgs/srv/ListControllers",
+            ],
+            capture_output=True,
+            text=True,
+            check=False,
+        )
+    except OSError:
+        return False
+    text = (call.stdout or "") + (call.stderr or "")
+    return "name='rizon_arm_controller', state='active'" in text
+
+
+def _wait_controller(up: bool, timeout_s: float) -> bool:
+    deadline = time.monotonic() + timeout_s
+    while time.monotonic() < deadline:
+        active = _controller_active()
+        if active == up:
+            return True
+        time.sleep(0.5)
+    return _controller_active() == up
+
+
+def main() -> int:
+    ap = argparse.ArgumentParser()
+    ap.add_argument("--xyz", nargs=3, type=float, required=True, metavar=("X", "Y", "Z"))
+    ap.add_argument("--arm", default=os.environ.get("SITE_ROBOT_ARM", "89"))
+    ap.add_argument("--vel", type=float, default=0.05)
+    args = ap.parse_args()
+
+    held = _control_pids()
+    if held:
+        print(f"放开 RDK：停 {len(held)} 个真机控制进程（感知不停）")
+        for pid, cmd in held:
+            print(f"  {pid} {cmd[:160]}")
+        _term([pid for pid, _cmd in held])
+        if not _wait_controller(False, 20.0):
+            print("ERROR: 控制器服务还在，MoveL 会和它抢 RDK", file=sys.stderr)
+            return 1
+    else:
+        print("真机控制栈没在跑，直接 MoveL")
+
+    x, y, z = (float(v) for v in args.xyz)
+    movel = [
+        RDK_PYTHON,
+        str(ROOT / "scripts" / "rdk_goto_xyz.py"),
+        "--xyz", f"{x}", f"{y}", f"{z}",
+        "--keep-z", "--go",
+        "--vel", f"{float(args.vel)}",
+    ]
+    print("厂家 MoveL：XY 对准 D435 这一点，工具尖高度保持当前")
+    got = subprocess.run(movel, check=False)
+    if got.returncode != 0:
+        print(f"ERROR: MoveL 退出码 {got.returncode}", file=sys.stderr)
+
+    if held:
+        log_path = Path("/tmp/run_all_real_restart.log")
+        print(f"拉回真机栈（--no-stop，不扫感知） log={log_path}")
+        log_f = log_path.open("ab")
+        subprocess.Popen(
+            ["bash", str(ROOT / "scripts" / "run_real.sh"), "--no-stop", "--arm", str(args.arm)],
+            stdout=log_f,
+            stderr=subprocess.STDOUT,
+            start_new_session=True,
+        )
+        if not _wait_controller(True, 120.0):
+            print(
+                "ERROR: 控制栈没回到 ACTIVE。看 /tmp/run_all_real_restart.log",
+                file=sys.stderr,
+            )
+            return 1
+        print("rizon_arm_controller 已回到 ACTIVE")
+    return 0 if got.returncode == 0 else int(got.returncode)
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())

scripts/run_all.sh

@@ -21,7 +21,7 @@
 #   bash ~/ros2/scripts/run_all.sh --arm 89 --no-camera          # 相机已在别处拉起了
 #   bash ~/ros2/scripts/run_all.sh --arm 89 --no-tactile         # 不起触觉服务
 #   bash ~/ros2/scripts/run_all.sh --arm 89 --no-descent         # 对准后只降到观测高度（默认离面 15mm）
-#   bash ~/ros2/scripts/run_all.sh --arm 89 --d435-only          # 只测 D435 单次位移，到位后不交 D405、不进二阶段
+#   bash ~/ros2/scripts/run_all.sh --arm 89 --d435-only          # 拍一张 K0，厂家 MoveL 把工具尖 XY 对准它（先停 run_real.sh）
 #
 # 前置（另开一个终端，先跑起来）：
 #   bash ~/ros2/scripts/run_real.sh --arm 89
@@ -86,6 +86,49 @@ fi
 
 echo "=== run_all（三阶段串联） arm=${SITE_ROBOT_ARM:-?} ${SITE_ROBOT_LABEL:-} (${SITE_ROBOT_SN:-?}) ==="
 
+# D435 单次定位不走关节轨迹。拍一张 → K0 用眼在手外 T 变到基座 → 厂家 MoveL。
+# RDK 与 run_real.sh 互斥，所以这条路径要求控制器没开。
+if [[ "${D435_ONLY}" == true ]]; then
+  echo "=== D435 单次定位：厂家 RDK MoveL（先停 run_real.sh）==="
+  if timeout 3 ros2 service list 2>/dev/null | grep -q "^/controller_manager/list_controllers$"; then
+    echo "ERROR: run_real.sh 还占着 RDK，MoveL 会被打回 IDLE。先在那个终端 Ctrl-C。" >&2
+    exit 1
+  fi
+  D435_TOPIC="/d435/color/image_raw"
+  _d435_ok() {
+    timeout 2 ros2 topic list 2>/dev/null | grep -qx "${D435_TOPIC}" || return 1
+    timeout 3 ros2 topic echo --once "${D435_TOPIC}" 2>/dev/null | grep -q "^header:"
+  }
+  if _d435_ok; then
+    echo "=== 顶视 ${D435_TOPIC} 已在发图 ==="
+  else
+    echo "=== 拉起顶视 D435 ==="
+    nohup bash "${ROOT}/scripts/run_camera_d435.sh" --no-preview > /tmp/run_all_d435.log 2>&1 &
+    echo "    d435 pid=$! log=/tmp/run_all_d435.log"
+    d435_ok=false
+    for _i in $(seq 1 24); do
+      if _d435_ok; then d435_ok=true; break; fi
+      sleep 0.5
+    done
+    if [[ "${d435_ok}" != true ]]; then
+      echo "ERROR: ${D435_TOPIC} 仍无图像。看 /tmp/run_all_d435.log" >&2
+      exit 1
+    fi
+  fi
+  echo "=== 拍一张，K0 × 眼在手外 T → 基座 ==="
+  k0_line="$(python3 "${ROOT}/scripts/d435_k0_once.py")"
+  echo "    ${k0_line}"
+  read -r _tag kx ky kz <<<"${k0_line}"
+  if [[ "${_tag}" != "K0_BASE" ]]; then
+    echo "ERROR: 没有得到 K0_BASE，得到：${k0_line}" >&2
+    exit 1
+  fi
+  echo "=== MoveL：XY 对准 K0，工具尖高度保持当前（K0 的 z 是工件表面）==="
+  # --go：不按直线距离拦截。800 mm 是脚本自己的保护，不是机械臂限位。
+  exec /home/cc/rdk_py310/bin/python "${ROOT}/scripts/rdk_goto_xyz.py" \
+    --xyz "${kx}" "${ky}" "${kz}" --keep-z --go --vel 0.05
+fi
+
 # ---- 1) 控制器必须在。判活方式与 run_stage2_locate.sh 保持一致 ----
 echo "=== 等待 rizon_arm_controller ACTIVE（T1: run_real.sh --arm ${SITE_ROBOT_ARM:-?}）==="
 # 先看服务在不在，再 call —— 顺序反过来的话，服务不存在时每次 call 都要耗满

scripts/run_real.sh

@@ -10,10 +10,12 @@ set -eo pipefail
 ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
 FLEXIV_PREFIX="${HOME}/flexiv_install"
 LIST_ONLY=false
+NO_STOP=false
 
 while [[ $# -gt 0 ]]; do
   case "$1" in
     --sn|--arm) export ROBOT_SN="${2:?}"; export ROBOT_ARM="${2}"; shift 2 ;;
+    --no-stop) NO_STOP=true; shift ;;
     --list|-l) LIST_ONLY=true; shift ;;
     -h|--help)
       sed -n '2,8p' "$0"
@@ -54,6 +56,9 @@ echo "=== catalog: ${SITE_ROBOT_CATALOG:-} ==="
 echo "=== handeye: ${SITE_HANDEYE_FILE:-} ==="
 echo "=== camera serial: ${SITE_CAM_SERIAL:-}  gripper: ${SITE_GRIP_MODEL:-} ${SITE_GRIP_PORT:-} ==="
 echo "=== network iface=${NET_IFACE:-auto}  robot=${ROBOT_IP}  pc=${SITE_PC_IP:-} ==="
+if [[ "${NO_STOP}" == true ]]; then
+  echo "=== --no-stop：不扫 install/（感知和伺服还在，扫了会把自己杀掉）==="
+else
 echo "=== stop leftover real stack (RDK exclusive) ==="
 # 以前这里是两行 pkill（ros2_control_node + stack.launch），漏掉了
 # arm_control 那一族**独立存活**的孤儿（adapter / named_pose_server /
@@ -75,6 +80,7 @@ if [ "${#_stale[@]}" != "0" ]; then
   unset _pid
 fi
 unset _stale
+fi
 sleep 1
 # 清掉把 91 的 .100 误绑到 89 MAC 上的陈旧 ARP
 if [[ -n "${NET_IFACE}" ]]; then