把现场确认能用的对孔方案落成保存点:小孔只用视觉关键点交到工件面、多走多拍闭环;D435 走到点后杀栈冷启动再停稳 3 秒才交 D405,避免赶路时带臂晃。

Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
arm_dev
2026-10-13 14:57:54 +08:00
parent f882c4309c
commit 1b264c0654
24 changed files with 906 additions and 118 deletions
+9 -2
View File
@@ -903,6 +903,7 @@ def servo_overrides(data: dict) -> dict:
"ee_hold_kp", "ee_w_max",
"lock_hold_sec",
"lock_still_sec",
"d435_handback_sec",
"wrist_align_sec",
"lock_still_move_m",
"a_max", "max_qdot", "damping",
@@ -934,8 +935,13 @@ def servo_overrides(data: dict) -> dict:
out["overhead_hold_s"] = float(oh["oneshot_hold_s"])
if "lock_still_sec" not in servo:
raise RuntimeError(
"servo.lock_still_sec 必须显式写"
"(D435 到位后法兰停稳多久才交回 D405)。不许用代码默认 3。"
"servo.lock_still_sec 必须显式写。"
"不许用代码默认秒数。"
)
if "d435_handback_sec" not in servo:
raise RuntimeError(
"servo.d435_handback_sec 必须显式写"
"(厂家 MoveL 到位后,再确认多久才交回 D405)。不许用代码默认秒数。"
)
if "wrist_align_sec" not in servo:
raise RuntimeError(
@@ -948,6 +954,7 @@ def servo_overrides(data: dict) -> dict:
"(静止判据的法兰 XY 位移上限)。不许用代码默认 15mm。"
)
out["lock_still_sec"] = float(servo["lock_still_sec"])
out["d435_handback_sec"] = float(servo["d435_handback_sec"])
out["wrist_align_sec"] = float(servo["wrist_align_sec"])
out["lock_still_move_m"] = float(servo["lock_still_move_m"])
out["overhead_k0_topic"] = "visual_servo/overhead_k0"
+66
View File
@@ -0,0 +1,66 @@
"""伺服和 flexiv_hardware 之间的 MoveL 交接。
两边不在一个进程里,硬件接口也没有自己的 ROS 节点。用三个文件:
* ``/tmp/flexiv_movel_req`` 一行 ``seq x y z keep_z vel``
* ``/tmp/flexiv_movel_ack`` 硬件接住请求后立刻写序号
* ``/tmp/flexiv_movel_done`` 一行 ``seq ok`` 或 ``seq fail 原因``
先写临时文件再改名,避免对面读到半行。
"""
from __future__ import annotations
from pathlib import Path
REQ = Path("/tmp/flexiv_movel_req")
ACK = Path("/tmp/flexiv_movel_ack")
DONE = Path("/tmp/flexiv_movel_done")
def format_request(seq: int, x: float, y: float, z: float, keep_z: bool, vel: float) -> str:
return (
f"{int(seq)} {float(x):.6f} {float(y):.6f} {float(z):.6f} "
f"{1 if keep_z else 0} {float(vel):.4f}\n"
)
def write_request(seq: int, x: float, y: float, z: float, keep_z: bool, vel: float) -> None:
text = format_request(seq, x, y, z, keep_z, vel)
tmp = REQ.with_name(REQ.name + ".tmp")
tmp.write_text(text, encoding="utf-8")
tmp.replace(REQ)
def parse_done(text: str):
"""返回 ``(seq, ok, detail)``。行不完整时返回 None。"""
parts = text.strip().split(maxsplit=2)
if len(parts) < 2:
return None
try:
seq = int(parts[0])
except ValueError:
return None
if parts[1] not in ("ok", "fail"):
return None
detail = parts[2] if len(parts) > 2 else ""
return seq, parts[1] == "ok", detail
def read_done():
try:
text = DONE.read_text(encoding="utf-8")
except OSError:
return None
return parse_done(text)
def read_ack() -> int:
try:
text = ACK.read_text(encoding="utf-8").strip()
except OSError:
return -1
try:
return int(text.split()[0])
except (ValueError, IndexError):
return -1