arm_dev
b903161113
眼在手上改用该 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>
277 lines
8.5 KiB
Python
Executable File
277 lines
8.5 KiB
Python
Executable File
#!/usr/bin/env python3
|
||
"""把 Flexiv 当前工具（TCP + 负载）切换成 Studio 里配置好的名字。
|
||
|
||
必须在 RDK 的 Python（rdk_py310）下运行。机器人必须处于 IDLE；如果
|
||
ros2_control 已经占住了 RDK 连接，请先停掉 run_real.sh。
|
||
|
||
/home/cc/rdk_py310/bin/python ~/ros2/scripts/rdk_switch_tool.py --list
|
||
/home/cc/rdk_py310/bin/python ~/ros2/scripts/rdk_switch_tool.py --name Flange
|
||
/home/cc/rdk_py310/bin/python ~/ros2/scripts/rdk_switch_tool.py # site.yaml robot.tool_name
|
||
"""
|
||
|
||
from __future__ import annotations
|
||
|
||
import argparse
|
||
import sys
|
||
import time
|
||
|
||
|
||
def _single_arm_groups(robot):
|
||
"""RDK 1.9 的 Tool API 是单臂的（没有 JointGroup）。老版本用 groups。"""
|
||
info = robot.info()
|
||
groups = getattr(info, "single_arm_groups", None) or {}
|
||
if isinstance(groups, dict):
|
||
keys = list(groups.keys())
|
||
else:
|
||
keys = list(groups)
|
||
if keys:
|
||
return keys
|
||
try:
|
||
import flexivrdk
|
||
|
||
if hasattr(flexivrdk, "JointGroup"):
|
||
return [flexivrdk.JointGroup.ARM_1]
|
||
except Exception:
|
||
pass
|
||
return [None] # 哨兵：调用 Tool.name()/Switch() 时不带 group
|
||
|
||
|
||
def _tool_name(tool, group):
|
||
if group is None:
|
||
return tool.name()
|
||
return tool.name(group)
|
||
|
||
|
||
def _tool_params(tool, group):
|
||
if group is None:
|
||
return tool.params()
|
||
return tool.params(group)
|
||
|
||
|
||
def _tool_switch(tool, group, target: str) -> None:
|
||
if group is None:
|
||
tool.Switch(target)
|
||
return
|
||
tool.Switch(group, target)
|
||
|
||
|
||
def _group_name(group) -> str:
|
||
if group is None:
|
||
return "单臂"
|
||
try:
|
||
import flexivrdk
|
||
|
||
return str(flexivrdk.kJointGroupNames[group])
|
||
except Exception:
|
||
return str(group)
|
||
|
||
|
||
def _fmt_params(params) -> str:
|
||
mass = getattr(params, "mass", None)
|
||
com = list(getattr(params, "CoM", []) or [])
|
||
tcp = list(getattr(params, "tcp_location", []) or [])
|
||
com_s = ",".join(f"{float(v):.4f}" for v in com[:3]) if com else "?"
|
||
tcp_s = ",".join(f"{float(v):.4f}" for v in tcp[:3]) if tcp else "?"
|
||
return f"mass={mass} kg CoM=[{com_s}] m TCP_xyz=[{tcp_s}] m"
|
||
|
||
|
||
def _ensure_idle(robot, mode) -> None:
|
||
try:
|
||
robot.SwitchMode(mode.IDLE)
|
||
return
|
||
except Exception:
|
||
pass
|
||
if hasattr(robot, "Enable"):
|
||
robot.Enable()
|
||
else:
|
||
robot.ServoOn()
|
||
for _ in range(40):
|
||
if robot.operational():
|
||
break
|
||
time.sleep(0.25)
|
||
if not robot.operational():
|
||
raise RuntimeError(
|
||
"机器人未处于 operational 状态 —— 请在示教器切 AUTO Remote 并使能，松开急停"
|
||
)
|
||
robot.SwitchMode(mode.IDLE)
|
||
|
||
|
||
def _resolve_name(tool, want: str) -> str:
|
||
want = (want or "").strip()
|
||
if not want:
|
||
return ""
|
||
names = list(tool.list() or [])
|
||
if want in names:
|
||
return want
|
||
lower = {n.lower(): n for n in names}
|
||
hit = lower.get(want.lower())
|
||
if hit:
|
||
return hit
|
||
raise SystemExit(
|
||
"工具 %r 不在 Studio 工具池里。\n 可选: %s"
|
||
% (want, ", ".join(names) or "（空）")
|
||
)
|
||
|
||
|
||
def _iface_ipv4s(name: str) -> list:
|
||
"""FastDDS 白名单要的是 IPv4 地址，而不是 enp4s0 这类网卡名。"""
|
||
import socket
|
||
import fcntl
|
||
import struct
|
||
|
||
name = (name or "").strip()
|
||
if not name:
|
||
return []
|
||
if name.count(".") == 3:
|
||
return [name]
|
||
addrs = []
|
||
try:
|
||
s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
||
ifreq = struct.pack("256s", name.encode("utf-8")[:15])
|
||
addrs.append(socket.inet_ntoa(fcntl.ioctl(s.fileno(), 0x8915, ifreq)[20:24]))
|
||
s.close()
|
||
except OSError:
|
||
pass
|
||
try:
|
||
import subprocess
|
||
|
||
out = subprocess.check_output(
|
||
["ip", "-4", "-o", "addr", "show", "dev", name],
|
||
text=True,
|
||
stderr=subprocess.DEVNULL,
|
||
)
|
||
for line in out.splitlines():
|
||
parts = line.split()
|
||
if "inet" in parts:
|
||
cidr = parts[parts.index("inet") + 1]
|
||
addrs.append(cidr.split("/")[0])
|
||
except Exception:
|
||
pass
|
||
# 去重，保持原有顺序
|
||
seen = set()
|
||
uniq = []
|
||
for a in addrs:
|
||
if a and a not in seen:
|
||
seen.add(a)
|
||
uniq.append(a)
|
||
return uniq
|
||
|
||
|
||
def _rdk_whitelist(iface: str) -> list[list[str]]:
|
||
"""只放 IPv4。网卡名进白名单会被 FastDDS 滤掉（见 rdk_helpers）。"""
|
||
sys.path.insert(0, str(__import__("pathlib").Path(__file__).resolve().parent / "lib"))
|
||
from rdk_helpers import rdk_whitelist_lists # noqa: PLC0415
|
||
|
||
return rdk_whitelist_lists(iface)
|
||
|
||
|
||
def connect_rdk(sn: str, iface: str = ""):
|
||
import flexivrdk
|
||
|
||
for listen in _rdk_whitelist(iface):
|
||
print("RDK 白名单:", ", ".join(listen))
|
||
for args in ((sn, listen), (sn, listen, True), (sn, listen, True, False)):
|
||
try:
|
||
return flexivrdk.Robot(*args)
|
||
except TypeError:
|
||
continue
|
||
except RuntimeError as exc:
|
||
msg = str(exc)
|
||
if "whitelist" in msg.lower() or "filtered out" in msg.lower():
|
||
print(" 白名单被拒，试下一个")
|
||
break
|
||
if "Failed to connect" in msg or "failed to connect" in msg.lower():
|
||
print(" 用这个白名单搜索超时")
|
||
break
|
||
raise
|
||
print("不带 iface 过滤再试一次")
|
||
return flexivrdk.Robot(sn)
|
||
|
||
|
||
def main() -> int:
|
||
ap = argparse.ArgumentParser(description="切换 Flexiv Studio 工具 / TCP")
|
||
ap.add_argument("--sn", default="", help="机械臂序列号（默认取 site.yaml）")
|
||
ap.add_argument("--iface", default="", help="RDK 用的网卡白名单（如 enp4s0）")
|
||
ap.add_argument(
|
||
"--name",
|
||
default="",
|
||
help="Studio 工具名（Flange = 不装工具）。留空 = 用 site.yaml 的 robot.tool_name",
|
||
)
|
||
ap.add_argument("--list", action="store_true", help="列出工具后退出")
|
||
args = ap.parse_args()
|
||
|
||
sn = (args.sn or "").strip()
|
||
want = (args.name or "").strip()
|
||
if not sn:
|
||
print("错误：必须提供 --sn（请通过 run_real.sh / rdk_switch_tool.sh 运行）", file=sys.stderr)
|
||
return 2
|
||
|
||
import flexivrdk
|
||
|
||
mode = flexivrdk.Mode
|
||
iface = (args.iface or "").strip()
|
||
print(f"=== RDK 工具 SN={sn} iface={iface or 'auto'} ===")
|
||
robot = None
|
||
try:
|
||
robot = connect_rdk(sn, iface)
|
||
if robot.fault():
|
||
print("检测到故障，正在 ClearFault ...")
|
||
if not robot.ClearFault():
|
||
print("错误：故障无法清除", file=sys.stderr)
|
||
return 1
|
||
tool = flexivrdk.Tool(robot)
|
||
names = list(tool.list() or [])
|
||
groups = _single_arm_groups(robot)
|
||
print("Studio 工具:", ", ".join(names) or "（无）")
|
||
for g in groups:
|
||
cur = _tool_name(tool, g)
|
||
extra = ""
|
||
try:
|
||
extra = " " + _fmt_params(_tool_params(tool, g))
|
||
except Exception:
|
||
pass
|
||
print(f" 当前 [{_group_name(g)}]: {cur}{extra}")
|
||
|
||
if args.list:
|
||
return 0
|
||
if not want:
|
||
print("tool_name 为空 —— 保持当前工具（要切换请在 site.yaml 里设置 robot.tool_name）")
|
||
print("RDK 正常")
|
||
return 0
|
||
|
||
target = _resolve_name(tool, want)
|
||
already = all(_tool_name(tool, g) == target for g in groups)
|
||
if already:
|
||
print(f"已经处于 [{target}]")
|
||
print("RDK 正常")
|
||
return 0
|
||
|
||
print(f"正在切换到 → [{target}]（需要 IDLE）")
|
||
_ensure_idle(robot, mode)
|
||
for g in groups:
|
||
_tool_switch(tool, g, target)
|
||
for g in groups:
|
||
cur = _tool_name(tool, g)
|
||
extra = ""
|
||
try:
|
||
extra = " " + _fmt_params(_tool_params(tool, g))
|
||
except Exception:
|
||
pass
|
||
print(f" 现在 [{_group_name(g)}]: {cur}{extra}")
|
||
if cur != target:
|
||
print(f"错误：切换失败，当前仍是 {cur!r}", file=sys.stderr)
|
||
return 1
|
||
print("RDK 正常")
|
||
return 0
|
||
except Exception as exc:
|
||
print(f"错误：{exc}", file=sys.stderr)
|
||
print(" 请先停掉 run_real.sh（RDK 是独占的）。", file=sys.stderr)
|
||
print(" 示教器：切 AUTO Remote，并使能 Ethernet/RDK。", file=sys.stderr)
|
||
return 1
|
||
finally:
|
||
robot = None
|
||
|
||
|
||
if __name__ == "__main__":
|
||
raise SystemExit(main())
|