arm_dev
222 lines
8.7 KiB
Python
222 lines
8.7 KiB
Python
#!/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("--acc", type=float, default=None, help="TCP 加速度 m/s²，交给 SDK acc；不给则用厂家默认")
|
||
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")
|
||
acc_txt = "厂家默认" if args.acc is None else f"{float(args.acc):.3f} m/s²"
|
||
print(
|
||
f"直线距离 {dist * 1000:.0f} mm 速度 {args.vel:.3f} m/s"
|
||
f" 加速度 {acc_txt} 姿态保持当前"
|
||
)
|
||
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)
|
||
prim = {
|
||
"target": flexivrdk.Coord(
|
||
target_xyz,
|
||
euler_deg,
|
||
["WORLD", "WORLD_ORIGIN"],
|
||
),
|
||
"vel": float(args.vel),
|
||
}
|
||
if args.acc is not None:
|
||
prim["acc"] = float(args.acc)
|
||
robot.ExecutePrimitive("MoveL", prim)
|
||
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())
|