ros_contorl/scripts/rdk_goto_xyz.py

#!/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())