arm_dev
6edb8d39ab
在开始'设备契约层 + 目录重构 + 通止规装配'改造之前，先钉住当前可运行状态， 使后续任何删除/重构都可回退。 本基线包含： - 现有 ROS 2 工作区全部源码（arm_application / arm_control / arm_bringup 等） - 官方 flexiv_ros2 栈（含 flexiv_hardware / flexiv_controllers 等符号链接） - 设备契约层 src/arm_interfaces（新增，46 项单测通过） - devices/ 设备选型与品牌清单（新增） - scripts/gen_site_example.py 与由它生成的 config/site.example.yaml 按 .gitignore 排除：build/ / install/ / log/ / config/site.yaml / 模型权重二进制。
208 lines
6.7 KiB
Python
Executable File
208 lines
6.7 KiB
Python
Executable File
#!/usr/bin/env python3
|
||
"""Control Studio device GripperDahuan / GripperDahuanModbus via Flexiv RDK.
|
||
|
||
Must run under RDK Python (rdk_py310). Uses a *lite* RDK instance so it can
|
||
share the robot with run_real.sh (ros2_control holds the normal RDK).
|
||
|
||
The controller talks RS485 itself (its own /dev/ttyUSB0), not the IPC serial.
|
||
|
||
/home/cc/rdk_py310/bin/python ~/ros2/scripts/rdk_gripper.py --list
|
||
/home/cc/rdk_py310/bin/python ~/ros2/scripts/rdk_gripper.py status
|
||
/home/cc/rdk_py310/bin/python ~/ros2/scripts/rdk_gripper.py init
|
||
/home/cc/rdk_py310/bin/python ~/ros2/scripts/rdk_gripper.py open
|
||
/home/cc/rdk_py310/bin/python ~/ros2/scripts/rdk_gripper.py close
|
||
"""
|
||
|
||
from __future__ import annotations
|
||
|
||
import argparse
|
||
import sys
|
||
import time
|
||
from pathlib import Path
|
||
|
||
ROOT = Path(__file__).resolve().parents[1]
|
||
sys.path.insert(0, str(ROOT / "scripts"))
|
||
|
||
from rdk_switch_tool import _iface_ipv4s # noqa: E402
|
||
|
||
|
||
def connect_lite(sn: str, iface: str = ""):
|
||
import flexivrdk
|
||
|
||
ips = _iface_ipv4s(iface)
|
||
if ips:
|
||
try:
|
||
return flexivrdk.Robot(sn, ips, True, True)
|
||
except TypeError:
|
||
pass
|
||
except RuntimeError as exc:
|
||
msg = str(exc).lower()
|
||
if "whitelist" not in msg and "filtered out" not in msg:
|
||
raise
|
||
return flexivrdk.Robot(sn, [], True, True)
|
||
|
||
|
||
def _fmt_params(p) -> str:
|
||
return (
|
||
f"width=[{p.min_width:.4f},{p.max_width:.4f}] m "
|
||
f"vel=[{p.min_vel:.4f},{p.max_vel:.4f}] m/s "
|
||
f"force=[{p.min_force:.1f},{p.max_force:.1f}] N"
|
||
)
|
||
|
||
|
||
def _fmt_states(s) -> str:
|
||
return f"width={s.width:.4f} m force={s.force:.2f} N moving={s.is_moving}"
|
||
|
||
|
||
def _clamp(val: float, lo: float, hi: float) -> float:
|
||
if lo > hi:
|
||
lo, hi = hi, lo
|
||
return max(lo, min(hi, val))
|
||
|
||
|
||
def _move(grip, params, width: float, vel: float, force: float) -> None:
|
||
vmin = float(params.min_vel)
|
||
vmax = float(params.max_vel)
|
||
if vmax <= 0:
|
||
vmax = 0.05
|
||
# Move velocity cannot be 0 (RDK).
|
||
vel = _clamp(vel, max(vmin, 1e-4), vmax)
|
||
force = _clamp(force, float(params.min_force), float(params.max_force))
|
||
width = _clamp(width, float(params.min_width), float(params.max_width))
|
||
print(f"Move width={width:.4f} m vel={vel:.4f} m/s force={force:.1f} N")
|
||
grip.Move(width, vel, force)
|
||
|
||
|
||
def main() -> int:
|
||
ap = argparse.ArgumentParser(description="Flexiv RDK gripper (GripperDahuanModbus)")
|
||
ap.add_argument("--sn", default="")
|
||
ap.add_argument("--iface", default="")
|
||
ap.add_argument("--device", default="", help="Studio Device name")
|
||
ap.add_argument("--force-n", type=float, default=0.0, help="force limit [N]; 0=mid")
|
||
ap.add_argument("--vel", type=float, default=0.0, help="speed [m/s]; 0=half max")
|
||
ap.add_argument(
|
||
"cmd",
|
||
nargs="?",
|
||
default="status",
|
||
choices=["list", "status", "init", "open", "close", "pos", "enable"],
|
||
)
|
||
ap.add_argument("value", nargs="?", default="", help="pos: meters or 0–1000")
|
||
args = ap.parse_args()
|
||
|
||
sys.path.insert(0, str(ROOT / "scripts" / "lib"))
|
||
from export_site_env import load_site, resolve_robot # type: ignore
|
||
|
||
data = load_site()
|
||
robot_cfg = resolve_robot(data)
|
||
g = data.get("gripper") or {}
|
||
sn = (args.sn or robot_cfg.get("sn") or "").strip()
|
||
iface = (args.iface or robot_cfg.get("iface") or "").strip()
|
||
device = (args.device or g.get("device_name") or "GripperDahuanModbus").strip()
|
||
if not sn:
|
||
print("ERROR: robot SN missing", file=sys.stderr)
|
||
return 2
|
||
|
||
import flexivrdk
|
||
|
||
print(f"=== RDK lite gripper SN={sn} device={device} iface={iface or 'auto'} ===")
|
||
robot = connect_lite(sn, iface)
|
||
dev = flexivrdk.Device(robot)
|
||
names = list(dev.list() or [])
|
||
|
||
if args.cmd == "list":
|
||
print("Studio devices:")
|
||
for n in names:
|
||
try:
|
||
en = dev.enabled(n)
|
||
cn = dev.connected(n)
|
||
except Exception:
|
||
en, cn = "?", "?"
|
||
mark = " <--" if n == device else ""
|
||
print(f" {n}: enabled={en} connected={cn}{mark}")
|
||
return 0
|
||
|
||
if device not in names:
|
||
print(
|
||
f"ERROR: device {device!r} not in Studio pool.\n available: {', '.join(names)}",
|
||
file=sys.stderr,
|
||
)
|
||
return 1
|
||
|
||
print(
|
||
f" Studio: enabled={dev.enabled(device)} connected={dev.connected(device)}"
|
||
)
|
||
if not dev.connected(device):
|
||
print(
|
||
"WARN: connected=False — 控制器自己的 /dev/ttyUSB0 没接到爪。\n"
|
||
" USB–RS485 要插 Flexiv 机箱 USB（不是工控机），A/B + 24V 到 PGEA。\n"
|
||
" 或用法兰工具口 485（腕部白绿+/绿-）。"
|
||
)
|
||
|
||
grip = flexivrdk.Gripper(robot)
|
||
if args.cmd in ("enable", "status", "init", "open", "close", "pos"):
|
||
try:
|
||
grip.Enable(device)
|
||
print(f"Enable [{device}] OK")
|
||
except Exception as exc:
|
||
print(f"ERROR: Enable failed: {exc}", file=sys.stderr)
|
||
return 1
|
||
|
||
params = grip.params()
|
||
print(" params:", _fmt_params(params))
|
||
print(" states:", _fmt_states(grip.states()))
|
||
|
||
force = float(args.force_n or g.get("force_n") or 0.0)
|
||
if force <= 0:
|
||
force = 0.5 * (float(params.min_force) + float(params.max_force))
|
||
vel = float(args.vel or g.get("vel_mps") or 0.0)
|
||
if vel <= 0:
|
||
vel = 0.5 * float(params.max_vel)
|
||
|
||
if args.cmd in ("enable", "status"):
|
||
if not dev.connected(device):
|
||
return 2
|
||
return 0
|
||
|
||
if args.cmd == "init":
|
||
print("Init (fingers home, ~10 s) …")
|
||
grip.Init()
|
||
time.sleep(2.0)
|
||
print(" states:", _fmt_states(grip.states()))
|
||
return 0
|
||
|
||
if args.cmd == "open":
|
||
_move(grip, params, float(params.max_width), vel, force)
|
||
time.sleep(0.4)
|
||
print(" states:", _fmt_states(grip.states()))
|
||
return 0
|
||
|
||
if args.cmd == "close":
|
||
_move(grip, params, float(params.min_width), vel, force)
|
||
time.sleep(0.4)
|
||
print(" states:", _fmt_states(grip.states()))
|
||
return 0
|
||
|
||
if args.cmd == "pos":
|
||
if args.value == "":
|
||
print("usage: pos <meters|0-1000>", file=sys.stderr)
|
||
return 2
|
||
raw = float(args.value)
|
||
if raw > 1.5:
|
||
span = float(params.max_width) - float(params.min_width)
|
||
width = float(params.min_width) + (raw / 1000.0) * span
|
||
else:
|
||
width = raw
|
||
_move(grip, params, width, vel, force)
|
||
time.sleep(0.4)
|
||
print(" states:", _fmt_states(grip.states()))
|
||
return 0
|
||
return 0
|
||
|
||
|
||
if __name__ == "__main__":
|
||
try:
|
||
raise SystemExit(main())
|
||
except Exception as exc:
|
||
print(f"ERROR: {exc}", file=sys.stderr)
|
||
raise SystemExit(1)
|