左臂换成 tool10120938 后，手眼仍按旧 tool1 和 8×11 板求解，顶视迁臂又按短跳 rest-to-rest，真机上要么整体偏一截，要么中途弹射启停。

眼在手上改用该 TCP 做 AX=XB，再折回相机→法兰；A5 内角点改为 11×8，09-04 / 10-09 旧结果留在 backup。一阶段 D435 迁臂改成一条 FollowJointTrajectory（余弦加减速，近处降速，XY 进门槛后再单独收垂直），跟踪锁高与悬停齐平，避免倾斜相机下先降一截把孔位重投影偏掉。--no-descent 只降到离面 15 mm；跳过三阶段时触觉 USB 打不开不再空等几分钟。 Co-authored-by: Cursor <cursoragent@cursor.com>
2026-10-12 15:04:23 +08:00
parent 7d22f777c8
commit b903161113
45 changed files with 3555 additions and 405 deletions
@@ -0,0 +1,204 @@
+#!/usr/bin/env python3
+"""用厂家 Flexiv RDK 的原语 MoveL，把当前工具尖走到世界系里的一个点。
+
+必须用 RDK 自带的 Python 3.10，不能用 ROS 的 3.11：
+
+    # 先在跑 run_real.sh 的那个终端 Ctrl-C。RDK 同时只能有一个连接。
+    /home/cc/rdk_py310/bin/python ~/ros2/scripts/rdk_goto_xyz.py
+    /home/cc/rdk_py310/bin/python ~/ros2/scripts/rdk_goto_xyz.py --xyz 0.45 -0.30 0.40
+
+不给 --xyz 时只打印当前工具尖坐标，不动臂。
+
+这个脚本做的事，按顺序只有这些：
+
+1. 用网卡上的 IPv4 连上序列号那台臂。网卡名（eth0）不能当白名单。
+2. 清故障、使能，等到 operational。示教器要在 AUTO Remote，急停松开。
+3. 读 states().tcp_pose。这 7 个数是
+   [x, y, z, qw, qx, qy, qz]，单位米，坐标系是机器人世界系（基座那套 WORLD）。
+   位置是当前 Studio 工具的 TCP（现在是 tool10120938 的尖），不是法兰，也不是相机。
+4. 目标只换 x、y、z。姿态沿用现在的朝向，折成厂家 MoveL 要的欧拉角（度，ZYX）。
+5. SwitchMode(NRT_PRIMITIVE_EXECUTION)，ExecutePrimitive("MoveL", ...)。
+6. 轮询 primitive_states()["reachedTarget"]。到了再读一次 tcp_pose，印出还差多少毫米。
+
+厂家示例：flexiv_rdk example_py/basics3_primitive_execution.py 第 (3) 段。
+"""
+
+from __future__ import annotations
+
+import argparse
+import math
+import sys
+import time
+from pathlib import Path
+
+ROOT = Path(__file__).resolve().parents[1]
+sys.path.insert(0, str(ROOT / "scripts" / "lib"))
+
+from rdk_helpers import rdk_whitelist_lists  # noqa: E402
+
+
+def site_robot() -> tuple[str, str]:
+    """读 config/site.yaml 里第一台臂的序列号和这台电脑的机器人网 IP。"""
+    text = (ROOT / "config" / "site.yaml").read_text(encoding="utf-8")
+    sn = ""
+    pc_ip = ""
+    for raw in text.splitlines():
+        line = raw.split("#", 1)[0].strip()
+        if line.startswith("sn:") and not sn:
+            sn = line.split(":", 1)[1].strip().strip('"').strip("'")
+        elif line.startswith("pc_ip:") and not pc_ip:
+            pc_ip = line.split(":", 1)[1].strip().strip('"').strip("'")
+        if sn and pc_ip:
+            break
+    if not sn:
+        raise RuntimeError(f"config/site.yaml 里没有 sn：{ROOT / 'config' / 'site.yaml'}")
+    return sn, pc_ip
+
+
+def quat_wxyz_to_euler_xyz_deg(q) -> list[float]:
+    """[qw,qx,qy,qz] → MoveL 要的 [rot_x, rot_y, rot_z] 度。
+
+    与厂家 example_py/utility.py 的 quat2eulerZYX 相同：
+    scipy 的 from_quat([x,y,z,w]).as_euler('xyz', degrees=True)。
+    这里不 import scipy，避免和 ROS 环境的 scipy 打架。
+    """
+    qw, qx, qy, qz = (float(v) for v in q)
+    n = math.sqrt(qw * qw + qx * qx + qy * qy + qz * qz)
+    if n < 1e-12:
+        raise RuntimeError(f"四元数是零：{q}")
+    qw, qx, qy, qz = qw / n, qx / n, qy / n, qz / n
+    xx, yy, zz = qx * qx, qy * qy, qz * qz
+    xy, xz, yz = qx * qy, qx * qz, qy * qz
+    wx, wy, wz = qw * qx, qw * qy, qw * qz
+    r00 = 1.0 - 2.0 * (yy + zz)
+    r10 = 2.0 * (xy + wz)
+    r20 = 2.0 * (xz - wy)
+    r21 = 2.0 * (yz + wx)
+    r22 = 1.0 - 2.0 * (xx + yy)
+    r01 = 2.0 * (xy - wz)
+    r11 = 1.0 - 2.0 * (xx + zz)
+    r12 = 2.0 * (yz - wx)
+    # R = Rx(x) @ Ry(y) @ Rz(z)
+    sy = math.sqrt(r00 * r00 + r10 * r10)
+    if sy > 1e-6:
+        x = math.atan2(r21, r22)
+        y = math.atan2(-r20, sy)
+        z = math.atan2(r10, r00)
+    else:
+        x = math.atan2(-r12, r11)
+        y = math.atan2(-r20, sy)
+        z = 0.0
+    return [math.degrees(x), math.degrees(y), math.degrees(z)]
+
+
+def connect(flexivrdk, sn: str, pc_ip: str):
+    """白名单只放 IPv4。连不上就换下一组，最后再试不带白名单。"""
+    last = None
+    attempts = []
+    for listen in rdk_whitelist_lists(pc_ip=pc_ip):
+        attempts.append((sn, listen))
+    attempts.append((sn,))
+    for args in attempts:
+        try:
+            print(f"连接 {args}")
+            return flexivrdk.Robot(*args)
+        except Exception as exc:  # noqa: BLE001 — 下一组白名单再试，最后一组失败才抛
+            last = exc
+            print(f"  失败：{exc}")
+    raise RuntimeError(f"RDK 连不上 {sn}：{last}")
+
+
+def wait_operational(robot, timeout_s: float) -> None:
+    if robot.fault():
+        print("有故障，尝试 ClearFault")
+        if not robot.ClearFault():
+            raise RuntimeError("故障清不掉。看示教器。")
+    print("Enable …（示教器：松开急停，AUTO Remote）")
+    robot.Enable()
+    t0 = time.monotonic()
+    while not robot.operational():
+        if time.monotonic() - t0 > timeout_s:
+            raise RuntimeError(f"{timeout_s:.0f}s 内没有 operational")
+        time.sleep(0.2)
+    print("operational")
+
+
+def fmt_pose(pose) -> str:
+    x, y, z = pose[0], pose[1], pose[2]
+    return f"xyz=({x:.4f}, {y:.4f}, {z:.4f}) m   qwxyz=({pose[3]:.4f}, {pose[4]:.4f}, {pose[5]:.4f}, {pose[6]:.4f})"
+
+
+def main() -> int:
+    parser = argparse.ArgumentParser(description="RDK MoveL：工具尖走到世界系的一个点")
+    parser.add_argument("--xyz", nargs=3, type=float, metavar=("X", "Y", "Z"), help="目标点，米，机器人世界系")
+    parser.add_argument("--vel", type=float, default=0.05, help="TCP 线速度 m/s，默认 0.05")
+    parser.add_argument("--max-step-m", type=float, default=0.40, help="离当前点超过这么多米就拒绝，除非 --go")
+    parser.add_argument("--go", action="store_true", help="允许超过 --max-step-m")
+    parser.add_argument("--timeout", type=float, default=60.0, help="等 reachedTarget 的秒数")
+    args = parser.parse_args()
+
+    sn, pc_ip = site_robot()
+    print(f"臂 {sn}    本机 {pc_ip or '（site 里没有 pc_ip）'}")
+
+    import flexivrdk
+
+    robot = connect(flexivrdk, sn, pc_ip)
+    try:
+        wait_operational(robot, args.timeout)
+        pose = list(robot.states().tcp_pose)
+        if len(pose) < 7:
+            raise RuntimeError(f"tcp_pose 不是 7 个数：{pose}")
+        print("当前 TCP（世界系）：", fmt_pose(pose))
+        if args.xyz is None:
+            print("没有 --xyz，只看坐标，不动。")
+            return 0
+
+        target_xyz = [float(v) for v in args.xyz]
+        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    姿态保持当前")
+        if dist > args.max_step_m and not args.go:
+            print(f"超过 {args.max_step_m * 1000:.0f} mm，拒绝。确认无误后加 --go。")
+            return 2
+
+        euler_deg = quat_wxyz_to_euler_xyz_deg(pose[3:7])
+        print(f"MoveL 欧拉角 deg xyz=({euler_deg[0]:.2f}, {euler_deg[1]:.2f}, {euler_deg[2]:.2f})")
+
+        robot.SwitchMode(flexivrdk.Mode.NRT_PRIMITIVE_EXECUTION)
+        robot.ExecutePrimitive(
+            "MoveL",
+            {
+                "target": flexivrdk.Coord(
+                    target_xyz,
+                    euler_deg,
+                    ["WORLD", "WORLD_ORIGIN"],
+                ),
+                "vel": float(args.vel),
+            },
+        )
+        print("MoveL 已下发，等 reachedTarget …")
+        t0 = time.monotonic()
+        while True:
+            states = robot.primitive_states()
+            if states.get("reachedTarget"):
+                break
+            if robot.fault():
+                raise RuntimeError(f"运动中故障：{states}")
+            if time.monotonic() - t0 > args.timeout:
+                raise RuntimeError(f"{args.timeout:.0f}s 内没有 reachedTarget。states={states}")
+            time.sleep(0.2)
+
+        end = list(robot.states().tcp_pose)
+        err_mm = math.dist(end[:3], target_xyz) * 1000.0
+        print("到达 TCP：", fmt_pose(end))
+        print(f"相对目标还差 {err_mm:.1f} mm")
+        return 0
+    finally:
+        try:
+            robot.Stop()
+        except Exception:
+            pass
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())