#!/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）。 `--keep-z` 时 z 保持当前工具尖：K0 的 z 是工件表面，不能把尖送到那上面。 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( "--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() 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] 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 姿态保持当前") 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())