chore: 框架科学优化前的完整基线
在开始'设备契约层 + 目录重构 + 通止规装配'改造之前，先钉住当前可运行状态， 使后续任何删除/重构都可回退。 本基线包含： - 现有 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 / 模型权重二进制。
This commit is contained in:
arm_dev
Executable
+207
@@ -0,0 +1,207 @@
|
||||
#!/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)
|
||||
Reference in New Issue
Block a user