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