真机落地前的仿真补齐：夹持/腕力/真值位姿，让仿真能验证 S07–S09
这批是 09-24 那轮仿真工作的收尾，一直没提交。内容按目的分四块： 1) 仿真真能「夹住」工件 sim_grasp_lock：给规与夹爪之间加接触锁（默认关，run_gauge_assembly --sim 才开）。 没有它时仿真里规只靠摩擦，S08 棘轮一转就滑出指间，于是把「仿真不真」误读成 「力判据不对」。新增 test_sim_grasp_lock.py。 2) 腕部力传感器进仿真 sim_wrist_ft（+ config/wrist_ft.yaml）：S07 的首选判据是腕部 FT 的轴向突变， 而仿真里以前根本没有腕部力，S07 只能退到触觉兜底，两套判据从没在同一场景下比过。 现在两条通道都在，接触判定可对照。 3) 真值位姿可查（不再靠猜） sim_live_pose / sim_pose_service：把 Gazebo 里刚体的实测位姿发布出来， stage2 的真值路径按**实测**位姿算孔位，而不是按场景文件里的标称位姿。 顺带把真值孔位的推导（truth_holes）独立出来并补测。 4) 工具几何成为可配置项 config/sim_tool_geometry.yaml + stage2 的 tool_tip_below_flange_m 启动参数： 仿真里规会在指间下滑，工具长每次装夹都不同（实测见过 243/254mm）， 悬停高度与 S07 下探预算必须按实测值算，不能再写死。 应用层同时跟进：gauge_fsm / gauge_config 把「悬停时工具尖端入没入工件」的判据 （hover_z_guard）与拖件判定（drag_verdict）收成纯函数并补测；stage3 搜孔、 site 导出、run_gauge_assembly 与若干文档同步。 新增 test_gauge_exit_lift / test_gauge_gripper_client / test_node_handler_collisions。 测试：全仓库 1085 passed。
This commit is contained in:
arm_dev
@@ -94,6 +94,9 @@ from arm_application.gauge_config import (
|
||||
)
|
||||
from arm_application.gauge_fsm import (
|
||||
DESCEND_STALL_STEPS,
|
||||
ROT_AXIS,
|
||||
ROT_LIMIT,
|
||||
ROT_SHORT,
|
||||
Event,
|
||||
FsmError,
|
||||
GaugeContext,
|
||||
@@ -109,9 +112,15 @@ from arm_application.gauge_fsm import (
|
||||
pixel_stable,
|
||||
points_stable,
|
||||
ratchet_cycles,
|
||||
ratchet_delta_rad,
|
||||
ratchet_travel_ok,
|
||||
relief_tol_m,
|
||||
rotation_verdict,
|
||||
screw_stop,
|
||||
tangential_jump,
|
||||
tool_clearance_m,
|
||||
wrench_live,
|
||||
wrench_touch,
|
||||
)
|
||||
from arm_application.gauge_gripper import (
|
||||
CONTRACT_STATUS_TOPIC,
|
||||
@@ -139,9 +148,26 @@ from arm_application.stage2_locate_node import _bgr_copy, _pt as _joint_point
|
||||
#: 允许的"绕轴验证"误差 [deg]。超过就说明这根关节轴不是工具 Z 轴，
|
||||
#: 后面所有以"转多少度"为单位的动作都不成立 —— 必须报错而不是继续。
|
||||
AXIS_VERIFY_TOL_DEG = 5.0
|
||||
#: 棘轮转腕时距离关节限位保留的余量 [deg]。
|
||||
#: 3° 不够用：实测轨迹会**自己**少走 ~5°（比如命令 180° 只到位 175.1°，
|
||||
#: 而关节7 的限位正好是 175.06° —— 两者叠在一起，判断"是被限位截住还是
|
||||
#: 没走完"就分不清了）。留 8° 让这两种情况都还没有贴住。
|
||||
RATCHET_LIMIT_MARGIN_DEG = 8.0
|
||||
#: 限制"跟踪丢失 → 粗定位 → 跟踪"的来回次数，防死循环。
|
||||
MAX_TRACK_RETRIES = 3
|
||||
|
||||
#: ``stage2_locate`` 返回码 → 人话。为什么要这张表：日志里只有 ``code=6`` 时，
|
||||
#: 现场只能去翻源码；而每个码对应的**下一步动作完全不同**（垂直化/粗定位/
|
||||
#: 检测/对准/工件被推走），写在这里就不用来回翻了。
|
||||
_LOCATE_CODE_HINT: Dict[int, str] = {
|
||||
2: "（垂直化或悬停失败：工具轴没竖直、或悬停高度到不了）",
|
||||
3: "（粗定位失败：视野里没框到工件 —— 看相机/光照/权重）",
|
||||
4: "（k1/k2 解不出来：孔被挡、曝光不对、或孔根本没在视野里）",
|
||||
5: "（对准迭代用满仍未收敛：检查手眼/相机安装，或放宽 align_tol_m）",
|
||||
6: "（工件被工具推走：不是手眼/检测的问题，是悬停时工具尖端已经进了工件"
|
||||
" —— 查 gauge_assembly.tool_tip_below_flange_m 与 servo.hole_hover_z_m）",
|
||||
}
|
||||
|
||||
|
||||
class GaugeAbort(RuntimeError):
|
||||
"""阶段内部判定要中止。带上 exit code，方便脚本区分原因。"""
|
||||
@@ -158,6 +184,11 @@ class GripperClient:
|
||||
（真机今天是 Trigger，契约/仿真 是 GripperExecute），应用层不需要知道。
|
||||
"""
|
||||
|
||||
#: 判"掉件"的开口塌陷量 [mm]：被夹住的工件掉了以后，两指会一路合到底，
|
||||
#: 开口从"工件宽度"塌下去几十毫米。为什么不用 ``is_gripped`` 瞬时值 —— 见
|
||||
#: :meth:`dropped` 的说明（那是个推断值，会被 S08 的指令切换抖出假 E1）。
|
||||
DROP_COLLAPSE_MM = 5.0
|
||||
|
||||
def __init__(self, node: Any, backend: str, status_topic: str) -> None:
|
||||
self._node = node
|
||||
self.backend = backend
|
||||
@@ -166,6 +197,10 @@ class GripperClient:
|
||||
self._last_gripped: Optional[bool] = None
|
||||
self._legacy_grip_state: Optional[int] = None
|
||||
self._ever_gripped = False
|
||||
#: 最近一帧的开口 [mm]（契约后端才有）。掉件判据看它。
|
||||
self._last_aperture: Optional[float] = None
|
||||
#: "夹住那一刻"的开口 [mm]。掉件判据是相对它的**塌陷量**，不是绝对值。
|
||||
self._grip_aperture: Optional[float] = None
|
||||
|
||||
if backend == "contract":
|
||||
self._open_cli = node.create_client(GripperExecute, "/gripper/open")
|
||||
@@ -178,9 +213,15 @@ class GripperClient:
|
||||
|
||||
# -- 状态回调 ---------------------------------------------------------
|
||||
def _on_contract(self, msg: GripperStatus) -> None:
|
||||
pos = float(getattr(msg, "position", float("nan")))
|
||||
if math.isfinite(pos):
|
||||
self._last_aperture = pos
|
||||
self._last_gripped = bool(msg.is_gripped)
|
||||
if self._last_gripped:
|
||||
self._ever_gripped = True
|
||||
# ⚠️ 这里**故意不**记"夹住时的开口"。曾经记过，代价是一个假 E1：
|
||||
# 本后端的 is_gripped 是**推断值**，夹爪**全开**时也会闪成 True
|
||||
# （实测 99.29mm，见 dropped 的说明），于是基准被记成 99.29mm、
|
||||
# 塌陷量算成 77mm ⇒ "通止规掉了(E1)"，而规好好地在爪里。
|
||||
# 基准只由 :meth:`adopt_current_grip` 在**明确知道夹住了**的时候记。
|
||||
|
||||
def _on_legacy(self, msg: Float32MultiArray) -> None:
|
||||
fields = legacy_states_to_fields(list(msg.data))
|
||||
@@ -191,6 +232,8 @@ class GripperClient:
|
||||
if got is not None:
|
||||
self._last_gripped = got
|
||||
if got:
|
||||
# 老后端只有状态字、没有开口，掉件判据本来就吃状态字（见 dropped），
|
||||
# 所以这里记"曾经夹住过"是对的。
|
||||
self._ever_gripped = True
|
||||
|
||||
# -- 查询 -------------------------------------------------------------
|
||||
@@ -205,15 +248,109 @@ class GripperClient:
|
||||
"""``True``/``False``/``None``（后端给不出明确答案）。"""
|
||||
return self._last_gripped
|
||||
|
||||
def aperture(self) -> Optional[float]:
|
||||
"""最近一帧的**开口** [mm]（只有契约后端的 ``position`` 有；没有返回 None）。"""
|
||||
return self._last_aperture
|
||||
|
||||
def grip_aperture(self) -> Optional[float]:
|
||||
"""第一次判到"夹住"时的开口 [mm]（掉件判据的基准，见 :meth:`dropped`）。"""
|
||||
return self._grip_aperture
|
||||
|
||||
def collapse_mm(self) -> Optional[float]:
|
||||
"""相对夹住时的**开口塌陷量** [mm]（正 = 比夹住时更小）。拿不到返回 None。
|
||||
|
||||
S08/S09 每轮都要"松爪 → 回转 → 夹紧"，日志里必须看得到这个数：
|
||||
它一路变大就说明每轮夹紧都比上一轮更往里挤，或者规在往下滑。
|
||||
"""
|
||||
if self._grip_aperture is None or self._last_aperture is None:
|
||||
return None
|
||||
return float(self._grip_aperture) - float(self._last_aperture)
|
||||
|
||||
# -- 开口的"等稳定"与"记基准" ------------------------------------------
|
||||
def settle(self, *, settle_s: float = 1.0, tol_mm: float = 0.05) -> Optional[float]:
|
||||
"""等开口**稳定**下来，返回稳定后的开口 [mm]（拿不到读数返回 None）。
|
||||
|
||||
为什么必须有这个（2026-09-24 用日志抓到的）：
|
||||
``close``/``open`` 服务**返回时动作还没做完**（JTC 轨迹是异步的）。
|
||||
立刻读开口会读到**松手前的旧值**——实测 S08 夹紧后立刻读，读到的还是
|
||||
全开 99.93mm，于是日志与基准全错（"夹住时 99.29mm"）。谁要在动作之后
|
||||
判断开口，都必须先过这里。
|
||||
"""
|
||||
spin = getattr(self._node, "_spin", None)
|
||||
deadline = time.monotonic() + max(0.0, settle_s)
|
||||
prev = self._last_aperture
|
||||
stable = 0
|
||||
while True:
|
||||
if spin is not None:
|
||||
spin(0.05)
|
||||
else: # pragma: no cover - 只有测试桩走这里
|
||||
time.sleep(0.05)
|
||||
now = self._last_aperture
|
||||
if now is not None and prev is not None and abs(float(now) - float(prev)) <= tol_mm:
|
||||
stable += 1
|
||||
if stable >= 2:
|
||||
return float(now)
|
||||
else:
|
||||
stable = 0
|
||||
prev = now
|
||||
if time.monotonic() >= deadline:
|
||||
return None if now is None else float(now)
|
||||
|
||||
def adopt_current_grip(self, why: str, *, settle_s: float = 1.0) -> Optional[float]:
|
||||
"""把**当前**开口记为"夹住时的开口"（掉件判据的基准），并记"曾经夹住过"。
|
||||
|
||||
只在**明确知道爪里夹着规**的时刻调用（S00 自检通过后、以及每一次
|
||||
确认夹紧之后）——它是掉件判据唯一的参照，随手记会让判据失去意义
|
||||
（见 :meth:`dropped` 与 :meth:`_on_contract` 的说明）。
|
||||
"""
|
||||
ap = self.settle(settle_s=settle_s)
|
||||
if ap is None:
|
||||
# 拿不到开口（老固件/别的夹爪）也要记"曾经夹住过"：否则 dropped()
|
||||
# 会永远返回 False，那比"会抖的旧判据"更糟（真掉件也看不见）。
|
||||
self._ever_gripped = True
|
||||
self._node.get_logger().warn(
|
||||
f"拿不到开口读数（{why}）—— 掉件判据退回状态字，会抖但不会漏"
|
||||
)
|
||||
return None
|
||||
self._grip_aperture = float(ap)
|
||||
self._ever_gripped = True
|
||||
self._node.get_logger().info(f"夹持基准：开口 {ap:.2f}mm（{why}）")
|
||||
return float(ap)
|
||||
|
||||
def dropped(self) -> bool:
|
||||
"""E1 掉件：**曾经夹住过**，现在报"掉落"。
|
||||
|
||||
装配前爪里本来就空，那时状态字也可能是"掉落/无物"，直接判会一开场就
|
||||
报 E1（``is_dropped_from_state`` 里解释了这一点）。
|
||||
|
||||
**契约后端的判据是"开口塌陷"，不是 ``is_gripped`` 的瞬时翻转**
|
||||
（2026-09-24 实测踩到，值得完整读）：
|
||||
|
||||
仿真里 ``is_gripped`` 是**推断值**（"指令到了合拢侧 + 实际开口比指令
|
||||
大出容差"）。S08 会在"合到某个开口 / 全合"之间发指令 —— 只要某一帧
|
||||
的指令落到与实际开口同一档，``is_gripped`` 就翻成 False。于是
|
||||
**S08 第 2/12 轮直接报 E1**（爪里其实好好的，是判据在抖），
|
||||
然后整条流程去等人工"放好通止规"，而仿真里根本没人能做这件事。
|
||||
更坏的是这个推断值在**全开**时也会闪成 True（实测 99.29mm）——
|
||||
拿它当基准就会得到"塌陷 77mm"的假 E1。所以基准只由
|
||||
:meth:`adopt_current_grip` 明确记录（见 :meth:`_on_contract`）。
|
||||
|
||||
真正"掉了"的物理表现是**两指合到底**：开口从"工件宽度"塌到几毫米，
|
||||
是几十毫米的变化，不会被指令切换带偏。所以判据用
|
||||
``夹住时的开口 − 当前开口 ≥ DROP_COLLAPSE_MM``。
|
||||
"""
|
||||
if not self._ever_gripped:
|
||||
return False
|
||||
if self.backend == "legacy" and self._legacy_grip_state is not None:
|
||||
return is_dropped_from_state(self._legacy_grip_state, self._ever_gripped)
|
||||
return self._ever_gripped and self._last_gripped is False
|
||||
if self._grip_aperture is None or self._last_aperture is None:
|
||||
# 没有开口读数（老固件/别的夹爪不给 position）：退回旧判据。
|
||||
# 它确实会抖，但比"永远判不了掉件"好 —— 而且这种情况本身会
|
||||
# 在 S00 的自检里被 `position` 相关检查点到。
|
||||
return self._last_gripped is False
|
||||
return (
|
||||
float(self._grip_aperture) - float(self._last_aperture)
|
||||
) >= self.DROP_COLLAPSE_MM
|
||||
|
||||
# -- 动作 -------------------------------------------------------------
|
||||
def _call(
|
||||
@@ -310,12 +447,31 @@ class GaugeAssembly(AssemblyPipeline):
|
||||
self.declare_parameter("result_file", "/tmp/gauge_assembly_result.json")
|
||||
|
||||
site = load_site_dict()
|
||||
# robots[].gauge_assembly 覆盖全局段 —— 与 servo / paths / perception 同一套规矩。
|
||||
# 为什么必须有这条（2026-09-24）：几何类参数是**按台子**的。
|
||||
# `workpiece_surface_z_m` 在仿真里是 0.412（仿真臂站在地面、台面 0.40m 高），
|
||||
# 真机是 0.012（基座与台面同高）。写死全局值的话，S07 的
|
||||
# "悬停时工具尖端离面多高"自检会算错 —— 而现象是"一路下压判不到接触"，
|
||||
# 看起来像力阈值标歪了，排查会从阈值开始走偏。
|
||||
_robot = self._robot_dict(site)
|
||||
if isinstance(_robot.get("gauge_assembly"), dict):
|
||||
site = dict(site)
|
||||
site["gauge_assembly"] = deep_merge(
|
||||
site.get("gauge_assembly") or {}, _robot["gauge_assembly"]
|
||||
)
|
||||
overrides: Dict[str, Any] = {}
|
||||
if bool(self.get_parameter("dry_run").value):
|
||||
overrides["dry_run"] = True
|
||||
holes = int(self.get_parameter("holes_total").value)
|
||||
if holes > 0:
|
||||
overrides["holes_total"] = holes
|
||||
# 工具长：仿真里由启动脚本传**实测值**（上料脚本刚量的），真机不传（走 site）。
|
||||
# 为什么必须能覆盖：这个值决定悬停高度与 S07 下探预算，而仿真里规会在指间
|
||||
# 下滑到落座位置、下滑量随装夹变化（实测见过 243 / 254mm，差 11mm）。
|
||||
# 差 11mm 的后果见 gauge_fsm.hover_z_guard / drag_verdict 的说明。
|
||||
tool_len = float(self.get_parameter("tool_tip_below_flange_m").value)
|
||||
if tool_len > 0.0:
|
||||
overrides["tool_tip_below_flange_m"] = tool_len
|
||||
try:
|
||||
self.cfg: GaugeConfig = from_site(site, overrides=overrides or None)
|
||||
except GaugeConfigError as exc:
|
||||
@@ -906,6 +1062,13 @@ class GaugeAssembly(AssemblyPipeline):
|
||||
return Event.BOOT_OK
|
||||
self._publish_gauge_result({"ok": False, "state": "S00", "failures": blockers})
|
||||
return Event.BOOT_FAIL
|
||||
# 记"夹住时的开口"（掉件判据的基准）。**放在这里而不是等到 S08**：
|
||||
# · 此刻爪里确实夹着规（仿真上料脚本刚装好，真机是人工装好才开始跑）；
|
||||
# · 基准必须是**明确知道夹住**时记的 —— 本后端的 is_gripped 是全开时
|
||||
# 都会闪 True 的推断值（实测 99.29mm），拿它当基准会造出假 E1
|
||||
# （见 GripperClient.dropped）。
|
||||
if self.grip is not None:
|
||||
self.grip.adopt_current_grip("S00 自检通过：爪里已装好通止规")
|
||||
return Event.BOOT_OK
|
||||
|
||||
# =====================================================================
|
||||
@@ -1087,20 +1250,25 @@ class GaugeAssembly(AssemblyPipeline):
|
||||
f"{tag}: 目标 {delta_deg:+.1f}°，实测绕工具 Z {got:+.1f}°"
|
||||
f"（转轴偏差 {mis:.1f}°，阈值 {AXIS_VERIFY_TOL_DEG:.1f}°）"
|
||||
)
|
||||
if abs(got) < 3.0 and abs(delta_deg) > 10.0:
|
||||
# 完全没动：最可能是撞上关节极限（用户要求的"转到极限角"就是这种情形）
|
||||
verdict = rotation_verdict(delta_deg, got, mis, AXIS_VERIFY_TOL_DEG)
|
||||
if verdict == ROT_LIMIT:
|
||||
# 完全没动、或只走了一部分：都是**关节限位**把这段截住了
|
||||
# （2026-09-24 实测：棘轮回转每轮顶到上限 ±175°，于是下一步反向
|
||||
# 只剩 ~16° 余量）。交给调用方让余量重试（见 _ratchet_step）；
|
||||
# 调用方不处理时 FSM 会按未定义转移报错，不会静默走下去。
|
||||
self.get_logger().warn(
|
||||
f"{tag}: 指令 {delta_deg:+.1f}° 但几乎没动 —— 判为已贴关节极限"
|
||||
f"{tag}: 期望 {delta_deg:+.1f}°、实测 {got:+.1f}°"
|
||||
f"（差 {abs(delta_deg) - abs(got):.1f}°）—— 判为**贴到关节限位**"
|
||||
)
|
||||
return Event.JOINT_LIMIT
|
||||
if mis > AXIS_VERIFY_TOL_DEG:
|
||||
if verdict == ROT_AXIS:
|
||||
self.get_logger().error(
|
||||
f"{tag}: 转轴偏差 {mis:.1f}° 超过阈值 —— 关节7 轴与工具 Z 不共线，"
|
||||
"以『度』为单位的动作（S01/S08/S09）都不可信，中止。"
|
||||
"先确认工具系是否垂直向下、有没有装转接件"
|
||||
)
|
||||
return Event.ABORT
|
||||
if abs(abs(got) - abs(delta_deg)) > max(AXIS_VERIFY_TOL_DEG, 0.1 * abs(delta_deg)):
|
||||
if verdict == ROT_SHORT:
|
||||
self.get_logger().error(
|
||||
f"{tag}: 期望 {delta_deg:+.1f}°、实测 {got:+.1f}°，偏差过大，中止"
|
||||
)
|
||||
@@ -1108,6 +1276,146 @@ class GaugeAssembly(AssemblyPipeline):
|
||||
self._axis_verified = True
|
||||
return fsm_event
|
||||
|
||||
def _ratchet_step(
|
||||
self, target_rad: float, *, tag: str, fsm_event: Event, speed_deg_s: Optional[float],
|
||||
direction: int = 0,
|
||||
) -> Event:
|
||||
"""棘轮的一步"转腕"：行程不够时**张开爪让出余量**再转。
|
||||
|
||||
:param target_rad: 关节 7 的**目标角（绝对值）**，与
|
||||
:meth:`_rotate_joint_and_verify` 同一口径（那边算的就是
|
||||
``target_rad - q_now``）。**不是增量。**
|
||||
:param direction: 这一步的**工艺转向**：``+1`` 正转、``-1`` 反转、
|
||||
``0`` 交给"最短路径"。棘轮的四步一律显式给：步长接近 180° 时
|
||||
"最短路径"会被 0.1° 的读数差翻方向（详见
|
||||
:func:`arm_application.gauge_fsm.ratchet_delta_rad`）。
|
||||
|
||||
⚠️ 为什么要把口径写在这里（2026-09-24 栽过一次，S09 直接 ABORT）：
|
||||
调用方（S08/S09）给的本来就是绝对值（``q[-1] ± screw_step_rad``），
|
||||
而本函数此前把它当**增量**用（``prewind(want_deg=abs(deg(target)))``、
|
||||
让余量时又按 ``q - target`` 算新目标）。于是 S09 第一步被算成
|
||||
"要转 338.9°"（其实是目标 338.9°、增量只有 180°）⇒ 误判"行程不够"
|
||||
⇒ 让余量那一步算到限位**外面**（``158.9 − 338.9 = −180°``，比
|
||||
−175° 的限位还低 5°）⇒ 报"期望 −338.9°、实测 +26.0°，偏差过大"
|
||||
⇒ 整条流程在**最不该失败的一步** ABORT（规在爪里好好的、什么坏事都没发生）。
|
||||
两个函数各用一种口径，编译器不会说话，日志还会把"目标"和"增量"都印成
|
||||
一个数 —— 所以口径写进参数名和 docstring。
|
||||
|
||||
为什么需要"让余量"这一步：
|
||||
棘轮要求"一正一反各 180°"，而关节 7 的行程只有 ±175°（3.0543 rad）。
|
||||
拧入（S08）的**回转**那一步每轮都正好顶到上限（实测 +175.1°），
|
||||
于是 S09 要做的**反方向**第一步只剩 ~16° 余量，被限位截住。
|
||||
而这一轮其实什么坏事都没发生。
|
||||
|
||||
为什么"让余量"是免费的：
|
||||
指间**张开**时规在打滑（这正是棘轮成立的原理），所以手腕空转既不会
|
||||
把已经拧进去的螺纹转回去，也不会漏掉进度 —— 只是把行程重新分配。
|
||||
|
||||
让多少：整步（不去算亏欠了多少）。反正张开状态下这一步不产生任何装配
|
||||
效果，整步最省事也最不容易算错；重试时的那一步才是"用力"的那一步。
|
||||
|
||||
两条路：**先算**（限位已知时，提前让余量，干净）＋ **兜底**（真被限位
|
||||
截住时再让，覆盖"限位没读到 / 轨迹自己没走完"这两种情况）。
|
||||
|
||||
两条路**最后都汇到同一次让余量**：只有一次，避免"先算发现不够、
|
||||
让完还不够、再让一次"这种把行程越绕越少的做法。
|
||||
"""
|
||||
limits = self._wait_joint_limits()
|
||||
q = None
|
||||
if limits is not None:
|
||||
q, _Tee = self._wait_state(3.0)
|
||||
if q is None:
|
||||
return Event.TIMEOUT
|
||||
# 目标角可能落在限位**外面**（这正是棘轮要解决的问题），先折成"最近一圈"
|
||||
# 的增量（gauge_fsm.ratchet_delta_rad，含完整说明与单测）：
|
||||
# 关节在 +158.9°、目标 +338.9° ⇒ 增量 +180°（而不是 +338.9°）。
|
||||
# **方向按工艺意图给**：步长贴着 180° 时"最短路径"会被读数差翻向（R2.10）。
|
||||
delta_rad = ratchet_delta_rad(target_rad, float(q[-1]), direction)
|
||||
why = ""
|
||||
if limits is not None and abs(math.degrees(delta_rad)) > 1e-6:
|
||||
plan = prewind(
|
||||
q_now_rad=float(q[-1]), want_deg=abs(math.degrees(delta_rad)),
|
||||
direction=1 if delta_rad > 0 else -1,
|
||||
limits=limits, margin_deg=RATCHET_LIMIT_MARGIN_DEG,
|
||||
)
|
||||
if plan.clamped:
|
||||
why = (
|
||||
f"行程不够（关节7 在 {math.degrees(float(q[-1])):+.1f}°，"
|
||||
f"这一步要转 {math.degrees(delta_rad):+.1f}°"
|
||||
f"（目标 {math.degrees(float(target_rad)):+.1f}°），限位 "
|
||||
f"{math.degrees(limits[0]):+.0f}~{math.degrees(limits[1]):+.0f}°）"
|
||||
)
|
||||
if not why:
|
||||
ev = self._rotate_joint_and_verify(
|
||||
target_rad, tag=tag, fsm_event=fsm_event, speed_deg_s=speed_deg_s
|
||||
)
|
||||
if ev is not Event.JOINT_LIMIT:
|
||||
return ev
|
||||
why = "轨迹被关节限位截住（算着够、实际没走完）"
|
||||
return self._ratchet_rewind_then(
|
||||
delta_rad, tag=tag, fsm_event=fsm_event, speed_deg_s=speed_deg_s, why=why,
|
||||
)
|
||||
|
||||
def _ratchet_rewind_then(
|
||||
self, delta_rad: float, *, tag: str, fsm_event: Event,
|
||||
speed_deg_s: Optional[float], why: str,
|
||||
) -> Event:
|
||||
"""张开爪 → 反向让一步 → 夹紧 → 转原来那一步。
|
||||
|
||||
:param delta_rad: **要转的增量**（不是目标角，见 :meth:`_ratchet_step`）。
|
||||
让余量时按它反着退一步，重试时再按它前进 —— 两次都以**当时的**
|
||||
关节角为基准重新算目标，所以让完余量后关节会回到原处、工具净转
|
||||
``delta_rad``。把"目标"固定成一开始那个绝对值是错的：让完余量之后
|
||||
那个目标已经在另一圈上了（差 360°）。
|
||||
"""
|
||||
if self.grip is None:
|
||||
self.get_logger().error(f"{tag}: {why}，但没有夹爪客户端，无法让余量 → 中止")
|
||||
return Event.ABORT
|
||||
self.get_logger().info(
|
||||
f"{tag}: {why} —— 张开爪让出一步（此刻规在打滑，不会动到螺纹）再转"
|
||||
)
|
||||
if not self.grip.open():
|
||||
self.get_logger().error(f"{tag}: 需要张爪让余量，但开爪失败 → 中止")
|
||||
return Event.ABORT
|
||||
time.sleep(self.cfg.screw_release_wait_s)
|
||||
q, _Tee = self._wait_state(3.0)
|
||||
if q is None:
|
||||
return Event.TIMEOUT
|
||||
ev = self._rotate_joint_and_verify(
|
||||
float(q[-1]) - delta_rad, tag=f"{tag}_rewind",
|
||||
fsm_event=fsm_event, speed_deg_s=speed_deg_s,
|
||||
)
|
||||
if ev is not fsm_event:
|
||||
self.get_logger().error(
|
||||
f"{tag}: 让余量那一步也没走开（{ev}）—— 关节7 的行程真用尽了，中止。\n"
|
||||
f" 排查：site.yaml 的 gauge_assembly.screw_step_deg 太接近关节行程了"
|
||||
f"（Rizon4 的关节7 是 ±175°），棘轮一步要 {math.degrees(abs(delta_rad)):.0f}°，"
|
||||
f"留给'回转'的余量本来就不够。\n"
|
||||
f" 改法：把 screw_step_deg 调小（例如 120°），并保证 "
|
||||
f"screw_max_cycles × screw_step_deg 仍是 **360° 的整数倍** —— "
|
||||
f"累计必须转整数圈，否则规会停在螺纹中间（S09 拧出也就少转几分之一圈）。"
|
||||
)
|
||||
return Event.ABORT
|
||||
if not self.grip.close():
|
||||
self.get_logger().error(f"{tag}: 让完余量后夹紧失败 → 中止")
|
||||
return Event.ABORT
|
||||
# 重试目标**按让完余量后的位置重算**：让余量退了一步，原来那个绝对目标
|
||||
# 现在差了 360°（正好一整圈），照原样下发会让关节反向绕一圈回去。
|
||||
q2, _Tee2 = self._wait_state(3.0)
|
||||
if q2 is None:
|
||||
return Event.TIMEOUT
|
||||
ev = self._rotate_joint_and_verify(
|
||||
float(q2[-1]) + delta_rad, tag=f"{tag}_retry",
|
||||
fsm_event=fsm_event, speed_deg_s=speed_deg_s,
|
||||
)
|
||||
if ev is not fsm_event:
|
||||
self.get_logger().error(
|
||||
f"{tag}: 让完余量重试仍然没走到位（{ev}）—— 中止"
|
||||
f"（不是限位问题，先查是不是有东西卡住/螺纹咬死）"
|
||||
)
|
||||
return Event.ABORT
|
||||
return fsm_event
|
||||
|
||||
# =====================================================================
|
||||
# S02 夹持确认（预留）
|
||||
# =====================================================================
|
||||
@@ -1301,7 +1609,9 @@ class GaugeAssembly(AssemblyPipeline):
|
||||
return Event.TIMEOUT
|
||||
code, q, Tee, final = self._run_locate(q, Tee)
|
||||
if code != 0 or not final:
|
||||
self.get_logger().error(f"S05 精拍定位失败 code={code}")
|
||||
self.get_logger().error(
|
||||
f"S05 精拍定位失败 code={code}{_LOCATE_CODE_HINT.get(code, '')}"
|
||||
)
|
||||
return Event.TIMEOUT
|
||||
self._snap_final = dict(final)
|
||||
self._snap_pose = Tee.copy()
|
||||
@@ -1440,6 +1750,21 @@ class GaugeAssembly(AssemblyPipeline):
|
||||
return (0.0, 0.0)
|
||||
return (float(getattr(best, "fshearx", 0.0)), float(getattr(best, "fsheary", 0.0)))
|
||||
|
||||
def _wrench_axial(self) -> Optional[float]:
|
||||
"""当前腕部 FT 的轴向力 [N]；一帧都没收到过时返回 None。
|
||||
|
||||
轴向 = ``force.z``。这个口径对真机与仿真**都成立**：
|
||||
真机 ``/<sn>/ft_sensor_wrench`` 是法兰系，z 是法兰轴；
|
||||
仿真传感器挂在 joint7 上取 child(link7) 系，z 也是工具轴。
|
||||
工具锁垂直向下时，两者都等于"竖直方向的力"。
|
||||
"""
|
||||
return getattr(self, "_ft_fz", None)
|
||||
|
||||
def _wrench_age(self) -> Optional[float]:
|
||||
"""腕部 FT 最近一帧的年龄 [s]；一帧都没收到过时返回 None。"""
|
||||
t = getattr(self, "_t_ft", None)
|
||||
return None if t is None else (time.monotonic() - float(t))
|
||||
|
||||
def _step_contact(self) -> Event:
|
||||
self._state("S07_contact")
|
||||
if self.cfg.dry_run:
|
||||
@@ -1452,8 +1777,16 @@ class GaugeAssembly(AssemblyPipeline):
|
||||
# 基线必须在**还没碰工件**的时候测。S06 结束时悬停在工件上方，
|
||||
# 正好是测基线的时刻（带载测会把阈值抬飞）。
|
||||
ref = self._baseline(samples=8)
|
||||
# 腕部 FT 的轴向基线：与触觉基线同一时刻采（这时还没碰工件）。
|
||||
# 腕部 FT 在夹持摩擦的**上游**，所以它是 S07 的首选判据；触觉只是兜底
|
||||
#（通止规靠夹持摩擦抱住，轴向力传不到指垫，见 gauge_fsm.wrench_touch）。
|
||||
ft_ref = self._wrench_axial()
|
||||
ft_age = self._wrench_age()
|
||||
ft_alive = wrench_live(ft_age)
|
||||
self.get_logger().info(
|
||||
f"S07 基线：切向 ({ref[0]:.1f}, {ref[1]:.1f})、法向 {self._fn_rest:.1f}"
|
||||
f"；腕部轴向 {ft_ref if ft_ref is not None else float('nan'):+.2f}N"
|
||||
f"（{'活' if ft_alive else '**没有数据**'}，话题 {self.ft_wrench_topic}）"
|
||||
)
|
||||
|
||||
z0 = float(Tee[2, 3])
|
||||
@@ -1565,23 +1898,75 @@ class GaugeAssembly(AssemblyPipeline):
|
||||
z_prev = z_now
|
||||
if steps % 5 == 0:
|
||||
now_dbg = self._shear_axes()
|
||||
ax_dbg = self._wrench_axial()
|
||||
self.get_logger().info(
|
||||
f"S07 已下压 {abs(z_now - z0) * 1000:.2f}mm"
|
||||
f"（{steps}/{budget} 步）切向 ({now_dbg[0]:.1f}, {now_dbg[1]:.1f})"
|
||||
f" 腕部轴向 "
|
||||
f"{ax_dbg - ft_ref if ax_dbg is not None else float('nan'):+.1f}N"
|
||||
)
|
||||
|
||||
# ---- 判据一（首选）：腕部 FT 轴向突变 --------------------------
|
||||
# 腕部 FT 在夹持摩擦的**上游**（工件→规→夹爪→法兰→腕部关节），
|
||||
# 所以工具在爪里打不打滑都测得准。触觉做不到这点：
|
||||
# 轴向力要传过"指垫↔规"的摩擦面，上限只有 µ·N≈23N，而台肩落面
|
||||
# 一步就产生 25N —— 必然打滑，读数停在 0（而它是对的）。
|
||||
# 完整因果链见 gauge_fsm.wrench_touch 与 docs/GAUGE_ASSEMBLY.md §6。
|
||||
ft_now = self._wrench_axial()
|
||||
if ft_alive and ft_now is not None and ft_ref is not None:
|
||||
if wrench_touch(ft_now, ft_ref, self.cfg.contact_axial_delta_n):
|
||||
self.get_logger().info(
|
||||
f"S07 接触（腕部 FT）：轴向 {ft_now:+.2f}N 相对基线 "
|
||||
f"{ft_ref:+.2f}N 超过 +{self.cfg.contact_axial_delta_n:.1f}N"
|
||||
f"（下压 {abs(float(Tee[2, 3]) - z0) * 1000:.2f}mm）"
|
||||
)
|
||||
# 必须先卸载再交给 S08 —— 判到接触的这一刻臂上顶着的是**出力上限**
|
||||
# 那个量级的力（实测 200N+，与阈值无关），而规的轴向传力上限
|
||||
# 只有 µ·N≈73N。见 _relieve_after_contact 的完整说明。
|
||||
self._relieve_after_contact(q, Tee, "腕部 FT", ft_ref)
|
||||
return Event.CONTACT_OK
|
||||
elif steps == 1:
|
||||
# 只报一次，别每步刷屏。降级到触觉兜底时阈值口径要按现场重新标定。
|
||||
self.get_logger().warn(
|
||||
f"S07 腕部 FT 没有数据（话题 {self.ft_wrench_topic}，"
|
||||
f"年龄 {ft_age if ft_age is not None else float('nan'):.1f}s）——"
|
||||
"降级为只看触觉切向突变。\n"
|
||||
" ⚠️ 触觉是**靠夹持摩擦**传力的，轴向力上限只有 µ·N（≈23N 量级），"
|
||||
"台肩落面那一下可能传不过去、读数停在 0。\n"
|
||||
" 真机上查：真机 bringup 无条件起 flexiv_robot_states_broadcaster，"
|
||||
"话题是 /<sn>/ft_sensor_wrench；收不到就是话题名/网络的问题。\n"
|
||||
" 仿真上查：devices/sim/gazebo/config/wrist_ft.yaml 的 topic 与"
|
||||
" arm.sim.sensors.xacro 的 wrist_ft_topic 必须一致（都是 "
|
||||
"/sim/ft_sensor_wrench）。"
|
||||
)
|
||||
|
||||
try:
|
||||
fn, _fn_max, _shear, _contact = self._tac_fresh(self.tac_timeout)
|
||||
except Exception as exc: # noqa: BLE001
|
||||
self.get_logger().error(f"S07 触觉断流，无法判断接触：{exc}")
|
||||
self._retract(q, Tee, dz_m=0.01)
|
||||
return Event.TIMEOUT
|
||||
# 腕部 FT 活着时，触觉断流不影响判接触（两个来源互相独立）。
|
||||
# 只有两个来源都没了才真的判不了。
|
||||
if not ft_alive:
|
||||
self.get_logger().error(
|
||||
f"S07 腕部 FT 与触觉**都没有**数据，无法判断接触：{exc}\n"
|
||||
" 这不是阈值问题，是数据源问题：先让其中一个活过来。"
|
||||
)
|
||||
self._retract(q, Tee, dz_m=0.01)
|
||||
return Event.TIMEOUT
|
||||
self.get_logger().warn(
|
||||
f"S07 触觉断流（{exc}），但腕部 FT 活着，继续下压"
|
||||
)
|
||||
fn, _fn_max, _shear, _contact = 0.0, 0.0, (0.0, 0.0), False
|
||||
|
||||
# ---- 判据二（兜底）：触觉切向突变 ------------------------------
|
||||
now = self._shear_axes()
|
||||
if tangential_jump(now, ref, self.cfg.contact_tangential_delta):
|
||||
self.get_logger().info(
|
||||
f"S07 接触：切向 {now[0]:.1f}/{now[1]:.1f} 相对基线 "
|
||||
f"S07 接触（触觉切向）：切向 {now[0]:.1f}/{now[1]:.1f} 相对基线 "
|
||||
f"{ref[0]:.1f}/{ref[1]:.1f} 超过 +{self.cfg.contact_tangential_delta:.0f}"
|
||||
f"（法向 {fn:.1f}，下压 {abs(float(Tee[2, 3]) - z0) * 1000:.2f}mm）"
|
||||
)
|
||||
# 与腕部 FT 那条同规矩：判到接触时臂上顶着的力远大于阈值，必须先卸载。
|
||||
self._relieve_after_contact(q, Tee, "触觉切向", ft_ref)
|
||||
return Event.CONTACT_OK
|
||||
if fn - self._fn_rest > self.cfg.contact_safe_delta:
|
||||
self.get_logger().error(
|
||||
@@ -1590,14 +1975,132 @@ class GaugeAssembly(AssemblyPipeline):
|
||||
)
|
||||
self._retract(q, Tee, dz_m=0.01)
|
||||
return Event.FORCE_OVER_LIMIT
|
||||
# 腕部 FT 的独立安全上限：它测的是**真实轴向力**，所以这里能给出
|
||||
# 一个物理量级的兜底（触觉那个 contact_safe_delta 是 au，量纲都对不上）。
|
||||
# 取触觉安全上限的同样口径：一旦超过就说明"压得太狠了"，与"接触了没"
|
||||
# 无关 —— 接触判据应该早就返回了，能走到这里说明判据失灵。
|
||||
if ft_alive and ft_now is not None and ft_ref is not None:
|
||||
if abs(ft_now - ft_ref) > self.cfg.contact_safe_delta:
|
||||
self.get_logger().error(
|
||||
f"S07 腕部轴向力超安全上限：{ft_now:+.2f}N - 基线 {ft_ref:+.2f}N "
|
||||
f"= {abs(ft_now - ft_ref):.1f}N > +{self.cfg.contact_safe_delta:.0f}N，"
|
||||
"抬起并中止。\n"
|
||||
" 能走到这里说明接触判据没生效（那么大的力早该判到接触）——"
|
||||
"先查 contact_axial_delta_n 是不是被调得太大了。"
|
||||
)
|
||||
self._retract(q, Tee, dz_m=0.01)
|
||||
return Event.FORCE_OVER_LIMIT
|
||||
self.get_logger().error(
|
||||
f"S07 下探 {max_depth_m * 1000:.1f}mm 都没触发接触判据。\n"
|
||||
" 常见原因：① 切向阈值太大（先看上面打印的基线/实测值再调）；"
|
||||
"② 孔位偏了，规擦着孔壁下去但没接触面；③ 触觉通道不对"
|
||||
" 排查顺序（按物理可信度，别从阈值开始调）：\n"
|
||||
" ① **先看上面每 5 步打印的「腕部轴向」**：它一直≈0 → 是数据源/几何问题，\n"
|
||||
" 不是阈值问题（传感器没数据、悬停几何错、孔位偏了没碰到面）；\n"
|
||||
" ② 腕部轴向有涨但没到阈值 → 才轮到调 contact_axial_delta_n；\n"
|
||||
" ③ 只有触觉兜底时：轴向力要经夹持摩擦传递，上限 µ·N≈23N，\n"
|
||||
" 台肩落面那一下可能传不过来 —— 这时调阈值是白费，要修的是夹持。"
|
||||
)
|
||||
self._retract(q, Tee, dz_m=0.01)
|
||||
return Event.TIMEOUT
|
||||
|
||||
def _relieve_after_contact(
|
||||
self,
|
||||
q: np.ndarray,
|
||||
Tee: np.ndarray,
|
||||
why: str,
|
||||
axial_ref: Optional[float] = None,
|
||||
) -> None:
|
||||
"""判到接触之后的**卸载回退**：把下压量放掉，再交给 S08。
|
||||
|
||||
为什么必须有这一步（2026-09-24 用一整轮排查换来的，值得完整读）：
|
||||
|
||||
位置控制的臂撞上硬面时，**不会**在"刚碰到"的地方停住 —— 它会一直顶到
|
||||
自己的出力上限。仿真实测：指令 0.5mm 的一步压在工件上，腕部 FT 直接跳到
|
||||
**+244N**，而臂这一步只走了 0.09mm（日志里那句"S07 单步未完全到位"）。
|
||||
也就是说"判到接触"的那一刻，臂上是顶着一个**大得多的力**的，
|
||||
这个力跟 contact_axial_delta_n 没关系（阈值只决定"什么时候承认接触"）。
|
||||
|
||||
而通止规是**靠夹持摩擦**抱住的：轴向上能传的力上限 = µ·N，两指合计
|
||||
约 73N（µ=0.8、握力 46N）。244N 远超它 ⇒ 规被轴向力直接顶出爪外。
|
||||
本次就是这样：S07 判到接触 → 进 S08 一转 → 规掉到地上 → 报 E1，
|
||||
而 E1 的日志只说"通止规掉了"，看不到"是顶着 244N 进去的"。
|
||||
|
||||
所以这里回退 contact_relief_mm，把力放回基线附近；S08 只带着"规自身的
|
||||
重量 + 一点点残余"进孔 —— 那才是螺纹/棘轮真正需要的预压。
|
||||
|
||||
回退量刻意**不做闭环**（不"退到力小了为止"）：判据闭环会在"接触刚度很大"
|
||||
时来回震荡，而这里要的只是"把下压量放掉"。放不干净就报出来（见下），
|
||||
让现场去改夹持（那是机械问题，不是参数问题）。
|
||||
"""
|
||||
dz = float(self.cfg.contact_relief_mm) / 1000.0
|
||||
ft_before = self._wrench_axial()
|
||||
if dz <= 0.0:
|
||||
self.get_logger().warn(
|
||||
f"S07 判到接触（{why}）但 contact_relief_mm=0 —— **不卸载**。\n"
|
||||
" 位置控制的臂此刻顶着的力可能远大于判据阈值（实测 200N+），"
|
||||
"而规靠夹持摩擦抱住、轴向上限只有 µ·N（≈73N）。"
|
||||
"S08 一转就可能掉件（E1），且日志里看不到真因。"
|
||||
)
|
||||
return
|
||||
target = np.array([float(Tee[0, 3]), float(Tee[1, 3]), float(Tee[2, 3]) + dz])
|
||||
# ⚠️ 到位容差**必须远小于回退量**，不能写死一个与它同量级的常数。
|
||||
# 实测（2026-09-24）：回退 0.50mm 而 tol_m 也写 0.0005 ⇒ 目标 z 是
|
||||
# `Tee[2,3] + 0.0005`，浮点回减后 dist 会比 0.0005 略小 ⇒ `_goto` 的
|
||||
# 入口判据 `dist < tol_m` 当场成立、**一步没走就 return True**。
|
||||
# 于是"卸载"变成一次空操作，而下面的残余力判据比的是"回退前 vs 回退后"
|
||||
# 两个一模一样的读数 ⇒ 报"残余 0.0N，已放干净"（**假通过**）。
|
||||
# 后果：S08 带着 70N 的轴向力去转 → 掉件(E1)，日志里只有"规掉了"。
|
||||
# 规则本身抽成 gauge_fsm.relief_tol_m 并有单测钉住（tol 必须 < 回退量）。
|
||||
tol = relief_tol_m(dz)
|
||||
ok = self._goto(
|
||||
q, Tee, target, verticalize=True, tol_m=tol,
|
||||
tol_tilt=self.vert_tol_rad * 2.5, tag="S07_relief",
|
||||
v=min(0.02, self.cfg.contact_speed_m_s), max_hops=3,
|
||||
settle_s=max(0.1, self.settle3 * 0.5), min_dur=0.3,
|
||||
)[0]
|
||||
time.sleep(0.2)
|
||||
ft_after = self._wrench_axial()
|
||||
msg = (
|
||||
f"S07 接触后卸载：回退 {dz * 1000:.2f}mm，腕部轴向 "
|
||||
f"{ft_before if ft_before is not None else float('nan'):+.1f}N → "
|
||||
f"{ft_after if ft_after is not None else float('nan'):+.1f}N"
|
||||
)
|
||||
if not ok:
|
||||
self.get_logger().warn(
|
||||
msg + "\n ⚠ 卸载回退**没走到位**（上面有 _goto 的告警）。"
|
||||
"残余力判据因此不可信 —— 这一路压进去的力仍留在规上。"
|
||||
)
|
||||
if ft_after is not None and ft_before is not None:
|
||||
# 残余按**基线**（还没碰工件时的轴向力）算，不能按"回退前后之差"算：
|
||||
# 回退本身要是没生效，前后读数相同 ⇒ 差值 0 ⇒ 假报"放干净"。
|
||||
# 基线是"完全没有接触"的状态，这才是"卸载干不干净"的参照。
|
||||
residual = (
|
||||
abs(float(ft_after) - float(axial_ref))
|
||||
if axial_ref is not None
|
||||
else abs(float(ft_after) - float(ft_before))
|
||||
)
|
||||
ref_txt = (
|
||||
f"基线 {float(axial_ref):+.1f}N" if axial_ref is not None else "无基线"
|
||||
)
|
||||
if residual > float(self.cfg.contact_hold_n):
|
||||
self.get_logger().warn(
|
||||
msg + f"（残余 {residual:.1f}N（相对{ref_txt}）"
|
||||
f" > {self.cfg.contact_hold_n:.1f}N）\n"
|
||||
" ⚠ 卸载没放干净：这一路压进去的力**超过了夹持摩擦能承受的量**"
|
||||
"（上限 µ·N，两指合计 ≈73N）。\n"
|
||||
" 接着 S08 一转，规会从爪里被推出去（现象是「通止规掉了」E1，"
|
||||
"看不到真因）。\n"
|
||||
" 两条真修法：① 降峰值 —— 调小 contact_step_mm（步长越小峰值越小）"
|
||||
"或加大 contact_relief_mm；② 加握力 —— 改夹爪合拢力度/指垫摩擦。\n"
|
||||
" **不要**去调 contact_axial_delta_n：它只管「什么时候承认接触」，"
|
||||
"管不了「判到的时候已经顶了多大」。"
|
||||
)
|
||||
else:
|
||||
self.get_logger().info(
|
||||
msg + f"（残余 {residual:.1f}N（相对{ref_txt}），已放干净）"
|
||||
)
|
||||
else:
|
||||
self.get_logger().info(msg + "（腕部 FT 没数据，无法确认卸载效果）")
|
||||
|
||||
def _baseline(self, samples: int) -> Tuple[float, float]:
|
||||
"""静止基线的两路切向均值（顺手把法向基线记到 ``self._fn_rest``）。
|
||||
|
||||
@@ -1633,6 +2136,29 @@ class GaugeAssembly(AssemblyPipeline):
|
||||
# =====================================================================
|
||||
# S08 旋转装配循环（棘轮）
|
||||
# =====================================================================
|
||||
def _log_regrip(self, tag: str) -> None:
|
||||
"""把"这一轮夹紧后的开口"打出来（含相对夹住时的塌陷量）。
|
||||
|
||||
为什么每轮都要打（2026-09-24 实测）：
|
||||
S08/S09 每轮都是"慢转 → 松爪 → 回转 → 夹紧"，而 `close` 在仿真里
|
||||
是"位置控制走到全合 0mm" —— **真机走到力上限就停，仿真会一路挤进规里**。
|
||||
只报一句"通止规掉了(E1)"时，人分不出"真掉了"和"被挤小了"。
|
||||
把开口与塌陷量打出来一眼就能分辨：
|
||||
· 塌陷量逐轮变大、开口还在十几毫米 → 是**被挤**（规还在指间）
|
||||
· 开口塌到接近 0（两指合到底） → 是**真掉了**
|
||||
"""
|
||||
if self.grip is None:
|
||||
return
|
||||
ap = self.grip.settle(settle_s=1.0) # 服务返回时动作还没做完，等它停
|
||||
if ap is None:
|
||||
return
|
||||
gp = self.grip.grip_aperture()
|
||||
col = self.grip.collapse_mm()
|
||||
detail = ""
|
||||
if gp is not None and col is not None:
|
||||
detail = f"（夹住时 {gp:.2f}mm，塌陷 {col:+.2f}mm）"
|
||||
self.get_logger().info(f"{tag}：开口 {ap:.2f}mm{detail}")
|
||||
|
||||
def _step_screw_in(self) -> Event:
|
||||
hole = self._hole_name()
|
||||
self._state(f"S08_screw_in_{hole}")
|
||||
@@ -1649,6 +2175,20 @@ class GaugeAssembly(AssemblyPipeline):
|
||||
f"停止判据「切向 >+{self.cfg.screw_shear_delta:.0f} "
|
||||
f"且法向 >+{self.cfg.screw_normal_delta:.0f}」"
|
||||
)
|
||||
# 步长 vs 关节行程：**在这里算一次**，不要等第 3 轮贴限位再炸。
|
||||
# 2026-09-24 实测：180° 的步长配 ±175° 的关节 7，第 3 轮 ABORT，
|
||||
# 日志是一长串"贴限位 → 让余量 → 还是贴限位"，真因（配置）藏在最后。
|
||||
travel = ratchet_travel_ok(
|
||||
self.cfg.screw_step_deg, self._wait_joint_limits(), RATCHET_LIMIT_MARGIN_DEG
|
||||
)
|
||||
self.get_logger().info(
|
||||
f"S08 行程核算：一步棘轮要 {travel.need_deg:.0f}°，"
|
||||
f"关节7 行程 {travel.travel_deg:.0f}°"
|
||||
f"（最大可用步长 {travel.max_step_deg:.0f}°）—— {'放得下' if travel.ok else '放不下'}"
|
||||
)
|
||||
if not travel.ok:
|
||||
self.get_logger().error(f"S08 配置自检不通过（先改配置，别硬跑）：{travel.why}")
|
||||
return Event.ABORT
|
||||
for cycle in range(self.cfg.screw_max_cycles):
|
||||
q, Tee = self._wait_state(5.0)
|
||||
if q is None or Tee is None:
|
||||
@@ -1663,9 +2203,10 @@ class GaugeAssembly(AssemblyPipeline):
|
||||
# --- 四步原子动作，顺序固定（慢转 → 松爪 → 快回转 → 夹紧）---
|
||||
# 慢转是**顺时针**：与 S01 的逆时针预绕相反，这样预绕行程就是
|
||||
# 这一整段拧入的回转空间（S01 不预绕，转两圈就贴限位）。
|
||||
ev = self._rotate_joint_and_verify(
|
||||
ev = self._ratchet_step(
|
||||
float(q[-1]) - self.cfg.screw_step_rad, tag=f"S08_slow_{cycle}",
|
||||
fsm_event=Event.SCREW_DONE, speed_deg_s=self.cfg.screw_speed_deg_s,
|
||||
direction=-1, # 顺时针拧入（与 S01 的预绕相反）
|
||||
)
|
||||
if ev is not Event.SCREW_DONE:
|
||||
return ev
|
||||
@@ -1678,15 +2219,17 @@ class GaugeAssembly(AssemblyPipeline):
|
||||
q, Tee = self._wait_state(3.0)
|
||||
if q is None:
|
||||
return Event.TIMEOUT
|
||||
ev = self._rotate_joint_and_verify(
|
||||
ev = self._ratchet_step(
|
||||
float(q[-1]) + self.cfg.screw_step_rad, tag=f"S08_back_{cycle}",
|
||||
fsm_event=Event.SCREW_DONE, speed_deg_s=self.cfg.screw_back_speed_deg_s,
|
||||
direction=+1, # 回转：与慢转正好相反
|
||||
)
|
||||
if ev is not Event.SCREW_DONE:
|
||||
return ev
|
||||
if not self.grip.close():
|
||||
self.get_logger().error("S08 夹紧失败 —— 下一轮慢转会打滑磨螺纹")
|
||||
return Event.ABORT
|
||||
self._log_regrip(f"S08 第 {cycle + 1} 轮夹紧后")
|
||||
else:
|
||||
self.get_logger().error("S08 没有夹爪客户端，无法松爪/夹紧，中止")
|
||||
return Event.ABORT
|
||||
@@ -1715,9 +2258,17 @@ class GaugeAssembly(AssemblyPipeline):
|
||||
self._progress(f"{hole} 拧入到位（{cycle + 1} 轮）")
|
||||
return Event.SCREW_STOP
|
||||
if self.grip is not None and self.grip.dropped():
|
||||
ap = self.grip.aperture()
|
||||
gp = self.grip.grip_aperture()
|
||||
self.get_logger().error(
|
||||
"S08 通止规掉了（E1）—— 回 S01 流程：张开爪、提示人工放好，"
|
||||
"确认后继续（按看板：该孔记为完成，换下一个孔）"
|
||||
"确认后继续（按看板：该孔记为完成，换下一个孔）\n"
|
||||
f" 开口：夹住时 {gp if gp is None else f'{gp:.2f}mm'}"
|
||||
f" → 现在 {ap if ap is None else f'{ap:.2f}mm'}"
|
||||
f"（判据：塌陷 ≥ {GripperClient.DROP_COLLAPSE_MM:.1f}mm）\n"
|
||||
" 分辨真掉还是被挤：开口塌到接近 0 = 两指合到底（真掉）；\n"
|
||||
" 开口还有十几毫米、只是比夹住时小了几毫米 = 被**挤**"
|
||||
"（仿真位置控制的「全合」不知道「挡住就停」，见 GripperClient.dropped）"
|
||||
)
|
||||
self._e1_pending = True
|
||||
return Event.GAUGE_DROPPED
|
||||
@@ -1748,9 +2299,10 @@ class GaugeAssembly(AssemblyPipeline):
|
||||
if q is None:
|
||||
return Event.TIMEOUT
|
||||
# 反向：慢转是逆时针（与 S08 相反），空手回转是顺时针
|
||||
ev = self._rotate_joint_and_verify(
|
||||
ev = self._ratchet_step(
|
||||
float(q[-1]) + self.cfg.screw_step_rad, tag=f"S09_slow_{cycle}",
|
||||
fsm_event=Event.UNSCREW_DONE, speed_deg_s=self.cfg.screw_speed_deg_s,
|
||||
direction=+1, # 逆时针拧出
|
||||
)
|
||||
if ev is not Event.UNSCREW_DONE:
|
||||
return ev
|
||||
@@ -1762,17 +2314,64 @@ class GaugeAssembly(AssemblyPipeline):
|
||||
q, Tee = self._wait_state(3.0)
|
||||
if q is None:
|
||||
return Event.TIMEOUT
|
||||
ev = self._rotate_joint_and_verify(
|
||||
ev = self._ratchet_step(
|
||||
float(q[-1]) - self.cfg.screw_step_rad, tag=f"S09_back_{cycle}",
|
||||
fsm_event=Event.UNSCREW_DONE, speed_deg_s=self.cfg.screw_back_speed_deg_s,
|
||||
direction=-1, # 回转：与慢转相反
|
||||
)
|
||||
if ev is not Event.UNSCREW_DONE:
|
||||
return ev
|
||||
if not self.grip.close():
|
||||
return Event.ABORT
|
||||
self._log_regrip(f"S09 第 {cycle + 1} 轮夹紧后")
|
||||
self._lift_clear_of_hole(hole)
|
||||
self._progress(f"{hole} 已拧出")
|
||||
return Event.UNSCREW_DONE
|
||||
|
||||
def _lift_clear_of_hole(self, hole: str) -> None:
|
||||
"""拧出后**先竖直把规提出孔**，之后才允许任何横向动作。
|
||||
|
||||
**为什么必须有这一步**（2026-09-24 仿真实测，两次栽在同一个地方）：
|
||||
1. 仿真里**没有螺纹几何**：S09 的"拧出"只是让规在孔里转，规**仍然插在
|
||||
孔里**（真机会沿螺纹退出来几个毫米，也没退出多少）。
|
||||
2. 于是紧跟着的横向动作（S10 换孔 / S11 回 home）会把规从孔里**拽**
|
||||
出来 —— 实测抓取锁被拽断、规掉在台面上，而下游只表现为
|
||||
"下一孔 S07 下探 35mm 判不到接触"（工具上已经没规了），
|
||||
日志完全指不到真因。
|
||||
3. 真机也怕这个动作：螺纹刚脱开时横向别住最容易啃伤螺纹。标准做法就是
|
||||
**先竖直退出来，再走 XY**。
|
||||
|
||||
高度提到**跟踪高**（``_track_z()`` = 孔悬停高 + 50mm）而不是写死毫米数：
|
||||
跟踪高是这条流程里"工具已离开工件"的既有定义（``_ensure_clearance``
|
||||
也用它），拿它当判据不用再维护第二套口径。实测：不提出时规尖比工件面
|
||||
**低 30mm**，而 S10 的斜线只往上抬了 29mm —— 差 1mm 就是"拽出孔"和
|
||||
"干净退出"的分界，所以必须是**先后有序**的两个动作，不能靠斜线一起走。
|
||||
|
||||
失败只告警（不失败）：这一刻孔里已经没有规要保护了，硬失败会把一次成功
|
||||
的装配变成失败；后面的 S10/S11 仍会走到安全位。
|
||||
"""
|
||||
try:
|
||||
q, Tee = self._wait_state(3.0)
|
||||
if q is None or Tee is None:
|
||||
return
|
||||
tz = self._track_z()
|
||||
z = float(Tee[2, 3])
|
||||
if z >= tz - 0.008:
|
||||
return
|
||||
self.get_logger().info(
|
||||
f"S09 {hole} 拧出后竖直提出：Z {z:.3f} → {tz:.3f}m"
|
||||
"（先离开孔口，再做横向动作）"
|
||||
)
|
||||
_, q2, _Tee2 = self._goto(
|
||||
q, Tee,
|
||||
np.array([float(Tee[0, 3]), float(Tee[1, 3]), tz]),
|
||||
verticalize=True, tol_m=0.003, tol_tilt=self.vert_tol_rad * 2.0,
|
||||
tag=f"S09_exit_{hole}", v=self.move_speed, max_hops=8,
|
||||
hold_xy=True, settle_s=0.25,
|
||||
)
|
||||
except Exception as exc: # noqa: BLE001
|
||||
self.get_logger().warn(f"S09 {hole} 提出孔口失败（继续走 S10/S11）：{exc}")
|
||||
|
||||
# =====================================================================
|
||||
# S10 换孔 / S11 复位
|
||||
# =====================================================================
|
||||
|
||||
@@ -53,6 +53,9 @@ _UNIT_SUFFIX: Dict[str, str] = {
|
||||
"px": "_px",
|
||||
"pct": "_pct",
|
||||
"au": "_delta",
|
||||
# 腕部 FT 的**真牛顿**。与 au 分开一个后缀是刻意的：两个口径混用不会报错，
|
||||
# 只会"判据不触发"或"一碰就炸"，看起来像算法问题（见 site.yaml 的注释）。
|
||||
"N": "_n",
|
||||
}
|
||||
|
||||
#: 显式无量纲的键（布尔、计数、比例、枚举）。**必须在这里列出来** ——
|
||||
@@ -99,6 +102,9 @@ _TYPES: Dict[str, type] = {
|
||||
"tool_tip_below_flange_m": float,
|
||||
"workpiece_surface_z_m": float,
|
||||
"contact_tangential_delta": float,
|
||||
"contact_axial_delta_n": float,
|
||||
"contact_relief_mm": float,
|
||||
"contact_hold_n": float,
|
||||
"contact_safe_delta": float,
|
||||
"screw_step_deg": float,
|
||||
"screw_speed_deg_s": float,
|
||||
@@ -142,6 +148,12 @@ _UNITS: Dict[str, str] = {
|
||||
"tool_tip_below_flange_m": "m",
|
||||
"workpiece_surface_z_m": "m",
|
||||
"contact_tangential_delta": "au",
|
||||
# 腕部 FT 是**真牛顿**（与触觉的 au 不是一个口径，所以键名也不带 taut 前缀）
|
||||
"contact_axial_delta_n": "N",
|
||||
# 卸载回退是**长度**（与 step_mm 同一把尺）
|
||||
"contact_relief_mm": "mm",
|
||||
# 残余轴向力是**牛顿**（腕部 FT 口径，不是触觉的 au）
|
||||
"contact_hold_n": "N",
|
||||
"contact_safe_delta": "au",
|
||||
"screw_step_deg": "deg",
|
||||
"screw_speed_deg_s": "deg/s",
|
||||
@@ -209,9 +221,42 @@ DEFAULTS: Dict[str, Any] = {
|
||||
"tool_tip_below_flange_m": 0.243,
|
||||
"workpiece_surface_z_m": 0.0, # 0 = 未知，运行期用 stage2 实测孔位兜底
|
||||
"contact_tangential_delta": 300.0,
|
||||
# 腕部 FT 的轴向力阈值 [N]。**这是 S07 的首选判据**，触觉只是兜底。
|
||||
#
|
||||
# 为什么默认是 8N 这么小：这是"接触了没有"的判据，不是"压到位了没有"。
|
||||
# 台肩一落面，接触刚度 5e4 N/m 意味着过压 0.1mm 就有 5N、0.2mm 有 10N ——
|
||||
# 所以 8N 对应约 0.16mm 的过压，**在打滑之前**就能判到（打滑上限由
|
||||
# µ·N 决定：µ=0.25、N≈46N → 两指合计 ~23N ≈ 0.46mm）。留了 3 倍余量。
|
||||
# 现场标定法：空载慢慢压到台肩落面，看日志里"腕部轴向"从基线涨到多少，
|
||||
# 取那个值的 1/3 作为阈值（要能区分"擦到孔壁"和"台肩落面"）。
|
||||
"contact_axial_delta_n": 8.0,
|
||||
# 接触到之后的**卸载回退** [mm]。
|
||||
#
|
||||
# 为什么必须有这一步（2026-09-24 一整轮排查换来的）：
|
||||
# 位置控制的臂在"撞上硬面"时会把力顶到自己的**出力上限**才停住 ——
|
||||
# 仿真实测：一步 0.5mm 的指令压在工件上，腕部 FT 读数直接跳到 **+244N**，
|
||||
# 而臂这一步根本没走完（"实测只走了 +0.09mm"）。这不是仿真特有的：
|
||||
# 接触刚度越高、步长越大，峰值力就越大，而位置环**不限制**它。
|
||||
#
|
||||
# 规是**靠夹持摩擦**抱住的，轴向能传的力上限是 µ·N（两指合计 ~73N@µ=0.8）。
|
||||
# 244N 远超这个上限 ⇒ 规被轴向力**顶出爪外**（本次就是：S08 一转，
|
||||
# 规直接掉到地上，报 E1）。所以"判到接触"之后不能就那样顶着，
|
||||
# 必须先把下压量放掉，再交给 S08 拧入。
|
||||
#
|
||||
# 0.5mm 的来历：判到接触时的**真实**过压量通常只有 0.0x mm（刚度 5e4 N/m
|
||||
# 时，10N 对应 0.2mm），回退 0.5mm 足够把力放干净，而规尖仍在孔里
|
||||
# （通端穿板 18mm），不会脱出。
|
||||
"contact_relief_mm": 0.5,
|
||||
# 卸载后允许的残余轴向力 [N]。
|
||||
# 超了不失败（可能只是回退量不够），但要**大声报**：它说明"夹持摩擦力不够
|
||||
# 承受这次接触"，接着 S08 一转就会掉件。调法只有两条 —— 降峰值（调小
|
||||
# contact_step_mm / contact_relief_mm）或加握力（改夹爪合拢力度/指垫）。
|
||||
"contact_hold_n": 5.0,
|
||||
"contact_safe_delta": 2000.0,
|
||||
# S08
|
||||
"screw_step_deg": 180.0,
|
||||
# 步长必须满足 2×step + 2×余量 ≤ 关节7 行程（Rizon4/10 ±175° ⇒ step ≤ 167°）：
|
||||
# 180° 会必然贴限位（棘轮一个来回要 360° > 350°），实测第 3 轮 ABORT。
|
||||
"screw_step_deg": 120.0,
|
||||
"screw_speed_deg_s": 30.0,
|
||||
"screw_back_speed_deg_s": 120.0,
|
||||
"screw_max_cycles": 12,
|
||||
@@ -259,6 +304,9 @@ class GaugeConfig:
|
||||
tool_tip_below_flange_m: float
|
||||
workpiece_surface_z_m: float
|
||||
contact_tangential_delta: float
|
||||
contact_axial_delta_n: float
|
||||
contact_relief_mm: float
|
||||
contact_hold_n: float
|
||||
contact_safe_delta: float
|
||||
screw_step_deg: float
|
||||
screw_speed_deg_s: float
|
||||
@@ -310,8 +358,14 @@ class GaugeConfig:
|
||||
# 悬停高度**不在这里**：它取自 servo.hole_hover_z_m（stage2 那一层），
|
||||
# 这里只印本段的容差 —— 一段配置里出现"另一个段的键"会让人改错地方。
|
||||
f" S06 对位容差 {self.align_tol_mm:.1f}mm（悬停高取自 servo.hole_hover_z_m）",
|
||||
f" S07 切向突变阈值 +{self.contact_tangential_delta:.0f}（两通道任一，现场标定），"
|
||||
f" S07 腕部轴向突变阈值 +{self.contact_axial_delta_n:.0f}N（**首选判据**，"
|
||||
"腕部 FT 在夹持摩擦的上游，工具打滑也测得准）,"
|
||||
f"切向突变阈值 +{self.contact_tangential_delta:.0f}au（触觉兜底，两通道任一，现场标定），"
|
||||
f"安全上限 +{self.contact_safe_delta:.0f}，最多下探 {self.contact_max_depth_mm:.0f}mm",
|
||||
f" S07 接触后卸载：回退 {self.contact_relief_mm:.2f}mm，"
|
||||
f"残余轴向力要求 ≤{self.contact_hold_n:.1f}N"
|
||||
"（位置控制的臂撞上硬面会把力顶到出力上限再停住，实测能到 200N+；"
|
||||
"规靠夹持摩擦抱住，轴向上限只有 µ·N≈73N，顶着不放 S08 一转就掉件）",
|
||||
f" S07 工具几何：尖端在法兰下方 {self.tool_tip_below_flange_m * 1000:.1f}mm，"
|
||||
f"工件上表面 z={self.workpiece_surface_z_m:.3f}m"
|
||||
"（启动时会按这两个值 + servo.hole_hover_z_m 算「悬停时工具到底入没入工件」，"
|
||||
@@ -457,6 +511,20 @@ def _sanity(cfg: GaugeConfig) -> None:
|
||||
)
|
||||
if cfg.snap_tol_px <= 0.0:
|
||||
raise GaugeConfigError(f"gauge_assembly.snap_tol_px 必须为正，收到 {cfg.snap_tol_px}")
|
||||
if cfg.contact_relief_mm < 0.0:
|
||||
raise GaugeConfigError(
|
||||
f"gauge_assembly.contact_relief_mm 不能为负（0 = 接触到之后不卸载），"
|
||||
f"收到 {cfg.contact_relief_mm}。\n"
|
||||
" 它为什么值得一条检查：位置控制的臂撞上硬面会把轴向下压顶到**出力上限**"
|
||||
"才停住（实测 200N+），而规靠夹持摩擦抱住、轴向上限只有 µ·N（≈73N）。"
|
||||
"卸载这一步省掉，S08 一转就掉件（报 E1）—— 而那时的日志只会说"
|
||||
"「通止规掉了」，看不到真因。"
|
||||
)
|
||||
if cfg.contact_hold_n <= 0.0:
|
||||
raise GaugeConfigError(
|
||||
f"gauge_assembly.contact_hold_n 必须为正（卸载后的残余轴向力上限 [N]），"
|
||||
f"收到 {cfg.contact_hold_n}。0 会让「卸载干不干净」永远判不过。"
|
||||
)
|
||||
if cfg.snap_tol_mm <= 0.0:
|
||||
raise GaugeConfigError(f"gauge_assembly.snap_tol_mm 必须为正，收到 {cfg.snap_tol_mm}")
|
||||
if cfg.snap_stable_feature not in SNAP_STABLE_FEATURES:
|
||||
|
||||
@@ -358,6 +358,54 @@ def tangential_jump(
|
||||
return any(abs(n - r) >= delta for n, r in zip(shear_now, shear_ref))
|
||||
|
||||
|
||||
def wrench_touch(
|
||||
axial_now: float, axial_ref: float, delta: float
|
||||
) -> bool:
|
||||
"""S07（**首选判据**）：腕部 FT 的轴向力相对基线突变 → 判为接触到工件。
|
||||
|
||||
为什么这是首选、而不是手指触觉（2026-09-24 用一整轮排查换来的结论）：
|
||||
|
||||
通止规是**靠夹持摩擦抱住**的工具。轴向力要传到手指，必须过"指垫↔规"的
|
||||
摩擦面，而摩擦能传的上限是 µ·N：
|
||||
夹持段 µ=0.25、实测握力 N≈46N → 单指 ~11.5N、两指 ~23N
|
||||
可台肩落面那一下，接触刚度 5e4 N/m 意味着**过压 0.5mm 就是 25N** ——
|
||||
一步的步进量就足以让规在爪里打滑。**于是手指触觉物理上读不到那个力**
|
||||
（读数停在 0，而它是对的），阈值怎么调都不对：调小被噪声骗，调大永远
|
||||
达不到。当时确实是照"阈值调高了"去改的，白费一轮。
|
||||
|
||||
腕部 FT 在这条力路的**上游**：工件→规→夹爪→法兰→腕部关节。
|
||||
工具打不打滑都要经过它，所以它测得到真实的轴向力。
|
||||
|
||||
判据用**绝对增量**而不是带符号压入量，理由与 ``tangential_jump`` 相同：
|
||||
传感器坐标系的正负号方向取决于安装，赌错了就是"整段流程静默失效"
|
||||
（下发正常、日志正常、就是停不住）。取绝对值只会更早停一点点，
|
||||
这个方向的错是安全的 —— 与 ``stage2_locate_node._contact_detected``
|
||||
的既有口径也一致（那里也是 ``abs(fz - baseline)``）。
|
||||
|
||||
:param axial_now: 当前轴向力读数 [N]。
|
||||
:param axial_ref: 静止（还没碰工件）时的轴向基线 [N]。
|
||||
:param delta: 触发阈值（相对基线的增量）[N]。
|
||||
"""
|
||||
return abs(float(axial_now) - float(axial_ref)) >= float(delta)
|
||||
|
||||
|
||||
#: S07 腕部 FT 判据需要多"新"的数据才算活着 [s]。
|
||||
#: 真机 broadcaster 与仿真传感器都是 100Hz，0.5s 相当于容许掉 50 帧 ——
|
||||
#: 只用来排除"根本没在发"，不是用来做实时性判据的。
|
||||
WRENCH_MAX_AGE_S: float = 0.5
|
||||
|
||||
|
||||
def wrench_live(age_s: Optional[float]) -> bool:
|
||||
"""腕部 FT 是否有活的数据（够新）。
|
||||
|
||||
``age_s`` 为 None 表示**一帧都没收到过**（话题名写错、传感器没起来）。
|
||||
注意"收到过但数值为 0"**是活的** —— 静止时轴向力本来就接近 0。
|
||||
这个区别很关键：拿数值大小判活，会让"安静的传感器"被误判成掉线，
|
||||
而那正是 2026-09-24 在触觉上踩过的同一个坑（见 stage3 的 ``live_tactile``）。
|
||||
"""
|
||||
return age_s is not None and float(age_s) <= WRENCH_MAX_AGE_S
|
||||
|
||||
|
||||
#: S07 单步下压给 ``_goto`` 的到位容差 = 步长的这个比例。
|
||||
#: **必须 < 1，而且只能小不能大**：``_goto`` 的语义是"已经落在容差里就直接返回、
|
||||
#: 一点都不动"。容差 ≥ 步长时，每一次"往下走 0.5mm"的请求都被判成"已在容差内"
|
||||
@@ -368,10 +416,34 @@ DESCEND_TOL_FRAC: float = 0.4
|
||||
DESCEND_TOL_FLOOR_M: float = 2.0e-5
|
||||
#: 判定"这一步真的往下走了"的位移下限，占步长的比例。
|
||||
DESCEND_PROGRESS_FRAC: float = 0.2
|
||||
#: 卸载回退的到位容差占**回退量**的比例（见 :func:`relief_tol_m`）。
|
||||
RELIEF_TOL_FRAC: float = 0.2
|
||||
#: 允许连续多少步毫无位移后中止（防"没接触也一直转"的静默自旋）。
|
||||
DESCEND_STALL_STEPS: int = 5
|
||||
|
||||
|
||||
def relief_tol_m(dz_m: float) -> float:
|
||||
"""S07 判到接触后**卸载回退**该给 ``_goto`` 的到位容差 [m]。
|
||||
|
||||
为什么单独立一条（2026-09-24 实测踩到，代价是一轮假通过）：
|
||||
|
||||
回退量 0.50mm，而 ``_goto`` 的容差被写死成 0.0005 m（**同量级**）。
|
||||
目标 z 是 ``Tee[2,3] + 0.0005``，浮点回减后 ``dist`` 会比 0.0005 略小
|
||||
⇒ ``_goto`` 的入口判据 ``dist < tol_m`` 当场成立 ⇒ **一步没走就返回 True**。
|
||||
于是"卸载"变成空操作，而残余力判据比的是"回退前 vs 回退后"两个**相同**的
|
||||
读数 ⇒ 报"残余 0.0N，已放干净"（假通过）。
|
||||
接着 S08 带着 ~70N 的轴向力去转（夹持摩擦上限 µ·N 两指合计 ≈73N）
|
||||
→ 掉件(E1)，而 E1 的日志只说"规掉了"。
|
||||
|
||||
返回 ``回退量的 20%``，再夹到 ``[DESCEND_TOL_FLOOR_M, 回退量的一半]`` ——
|
||||
后者保证**容差永远严格小于回退量**，`_goto` 不可能在入口就判"已到位"。
|
||||
"""
|
||||
dz = float(dz_m)
|
||||
if dz <= 0.0:
|
||||
raise ValueError(f"卸载回退量必须为正，收到 {dz_m}")
|
||||
return min(max(DESCEND_TOL_FLOOR_M, dz * RELIEF_TOL_FRAC), dz * 0.5)
|
||||
|
||||
|
||||
def descend_tol_m(step_m: float) -> float:
|
||||
"""S07 单步下压该给 ``_goto`` 的到位容差 [m]。
|
||||
|
||||
@@ -441,6 +513,181 @@ def tool_clearance_m(
|
||||
return tip_z - float(surface_z_m)
|
||||
|
||||
|
||||
def hover_z_for_tool_clearance(
|
||||
surface_z_m: float,
|
||||
tool_tip_below_flange_m: float,
|
||||
clearance_m: float,
|
||||
) -> float:
|
||||
"""让工具尖端停在工件面上方 ``clearance_m`` 所需的**法兰**高度 [m]。
|
||||
|
||||
就是 ``tool_clearance_m`` 的逆运算 —— 写成一个函数是为了让"该用多高"这个
|
||||
结论只有一个来源（下面 ``hover_z_guard`` 与调用方都用它，不各推一遍）。
|
||||
"""
|
||||
return float(surface_z_m) + float(tool_tip_below_flange_m) + float(clearance_m)
|
||||
|
||||
|
||||
def hover_z_guard(
|
||||
hover_z_m: float,
|
||||
surface_z_m: float,
|
||||
tool_tip_below_flange_m: float,
|
||||
want_clearance_m: float,
|
||||
) -> Tuple[float, str]:
|
||||
"""二阶段悬停高度的自检 → ``(该用的悬停高度, 说明; 不用改时说明是空串)``。
|
||||
|
||||
**为什么必须有这条**（2026-09-24 实测栽在这上面）：
|
||||
|
||||
悬停高度是**按相机**定的（腕部相机要离工件面 ~0.17m 才看得清孔），
|
||||
而工具（夹爪 + 通止规）可能比相机长得多 —— 仿真里实测
|
||||
``tool_tip_below_flange_m=0.245``，而悬停算的是
|
||||
``0.412 + 0.17 + 0.05 = 0.632`` ⇒ 工具尖端在工件上表面**下面 25mm**。
|
||||
|
||||
后果不在 S07（S07 有自己的几何自检），而在**更早的 S05**：
|
||||
"悬停"这一步本身就把工具捅进工件里了，于是 S05 的横向对准等于
|
||||
拿规尖去拨工件 —— 实测把工件从 x=0.520 拖到 0.4284（**92mm**）。
|
||||
而真值孔位是按工件的**实测**位姿算的（``sim_live_pose``，这是对的），
|
||||
工件一走，孔位就跟着工具走 ⇒ 残差恒定在 13mm、迭代 9 次永不收敛
|
||||
（日志：``[fine 3..8] 残差=13.25/13.19/…/11.79mm``）。
|
||||
现象看起来像"手眼标定坏了 / 检测不认孔"。
|
||||
|
||||
真机同参数同结果：这不是仿真专有，只要"悬停按相机定、工具比相机长"就会这样。
|
||||
|
||||
判据取"明显低于目标间隙"而不是"一毫米都不许低"：配置的悬停与实测的
|
||||
工件面 z 都有几毫米的不确定度，拿 0 当界会把正常配置判成错的。
|
||||
"""
|
||||
clear_m = tool_clearance_m(hover_z_m, tool_tip_below_flange_m, surface_z_m)
|
||||
if clear_m >= float(want_clearance_m) - 0.001:
|
||||
return float(hover_z_m), ""
|
||||
need_z = hover_z_for_tool_clearance(surface_z_m, tool_tip_below_flange_m, want_clearance_m)
|
||||
why = (
|
||||
f"悬停高度会让工具尖端钻进工件：法兰 {float(hover_z_m):.4f} − 工具长 "
|
||||
f"{float(tool_tip_below_flange_m) * 1000:.1f}mm = 尖端 "
|
||||
f"{float(hover_z_m) - float(tool_tip_below_flange_m):.4f}，工件上表面 "
|
||||
f"{float(surface_z_m):.4f} → 尖端在面{'下' if clear_m < 0 else '上仅'} "
|
||||
f"{abs(clear_m) * 1000:.1f}mm（要求 ≥ {float(want_clearance_m) * 1000:.1f}mm）。\n"
|
||||
f" 悬停高度通常是按**相机**视距定的，而工具比相机长 —— 两者冲突时必须"
|
||||
f"先保证不碰工件。\n"
|
||||
f" 已按几何抬到 {need_z:.4f}（= {float(surface_z_m):.3f} 上表面 + "
|
||||
f"{float(tool_tip_below_flange_m) * 1000:.0f}mm 工具 + "
|
||||
f"{float(want_clearance_m) * 1000:.1f}mm 间隙）。\n"
|
||||
f" 要固定这个值：改 site.yaml 的 robots[<sn>].servo.hole_hover_z_m = "
|
||||
f"{need_z:.3f}（以及同组件的 ee_z_m ≥ 它 +0.05）。"
|
||||
)
|
||||
return need_z, why
|
||||
|
||||
|
||||
#: S05 精对位时判"工件被工具推走"的两级阈值 [m]。
|
||||
#
|
||||
# 为什么需要这条判据（2026-09-24 又栽了一次，见下面 drag_verdict 的说明）：
|
||||
# S05 的对准是"看着孔走"的闭环，而孔位是**工件上**的。工具一旦碰到工件，
|
||||
# 闭环追的就不再是孔，而是"自己推着走的工件" —— 残差恒定（本轮 11mm）、
|
||||
# 迭代用满，日志报的是"检查手眼/相机安装"，与真因（工具比相机长、
|
||||
# 悬停时已经插进工件）差着十万八千里。
|
||||
# 取 3mm：工件是刚体，两拍之间它整体挪动 >3mm 只可能是被推/被撞（正常加工
|
||||
# 过程里它一动不动）。同一对孔的两拍测量噪声在 0.2mm 量级，离阈值很远。
|
||||
SNAP_DRAG_TOL_M: float = 0.003
|
||||
#: 单拍就这么大 ⇒ 不必等第二拍确认（刚体不会自己"挪回去"）。
|
||||
SNAP_DRAG_HARD_M: float = 0.006
|
||||
|
||||
|
||||
#: "已经到位"的容差：避免 ±1e-9 的浮点噪声变成"再转一整圈"。
|
||||
_RATCHET_FULL_TURN_EPS: float = 1e-9
|
||||
|
||||
|
||||
def ratchet_delta_rad(target_rad: float, q_now_rad: float, direction: int = 0) -> float:
|
||||
""""目标角 − 当前角"，折成**一次**能转到的增量 [rad]。
|
||||
|
||||
**为什么要有这个函数**（2026-09-24 实测：棘轮把整条流程 ABORT 在 S09 第一步）：
|
||||
|
||||
S08/S09 给棘轮的是**关节 7 的目标角（绝对值）**（``q[-1] ± step``），
|
||||
而棘轮此前把它当**增量**用。于是"关节在 +158.9°、目标 +338.9°"被算成
|
||||
"要转 338.9°"（实际增量只有 180°）⇒ 误判行程不够 ⇒ 让余量那一步算到
|
||||
限位外面（``158.9 − 338.9 = −180°``，比 −175° 还低）⇒ 报
|
||||
"期望 −338.9°、实测 +26.0°，偏差过大" ⇒ ABORT。现场什么坏事都没发生。
|
||||
|
||||
差值必须折一圈：目标常常**故意**超出限位（这正是要"让余量"的原因），
|
||||
``+338.9°`` 与 ``−21.1°`` 是同一个关节位置。
|
||||
|
||||
**为什么还要 direction**（2026-09-24 又一次实测：S08 第 3 轮 ABORT）：
|
||||
|
||||
不给方向时这里取"最短的那条路"，而名义步长**正好是 180°** 时，
|
||||
哪条路更短取决于 0.1° 级的读数差：调用方按自己读到的 ``q`` 算出目标
|
||||
``−18.0°``，棘轮再用**稍晚一点**读到的 ``q=+162.1°`` 去算差值，
|
||||
``−180.1°`` 折一圈就成了 ``+179.9°`` —— 方向**翻了个个儿**。
|
||||
于是棘轮朝螺纹的反方向走，接着贴限位、让余量、重试，净位移 0 ⇒ ABORT。
|
||||
|
||||
棘轮的转向是**工艺意图**（S08 顺时针拧入、S09 逆时针拧出），不能交给
|
||||
"谁更短"这种数值巧合决定，所以由调用方明确给出 ``direction``。
|
||||
|
||||
注：``direction=0``（默认）保留旧的"最短路径"语义，给"目标本来就明确
|
||||
在某一侧"的调用方用；棘轮的四个原子步一律显式给方向。
|
||||
|
||||
:param target_rad: 目标角（绝对值，可以是限位外的角度）。
|
||||
:param q_now_rad: 当前关节角。
|
||||
:param direction: 想要的转向：``+1`` 正转、``-1`` 反转、``0`` 取最短路径。
|
||||
:returns: 增量 [rad]。``direction=0`` 时落在 ``(−π, π]``；给了方向时落在
|
||||
``[0, 2π)``（正转）或 ``(−2π, 0]``（反转）——**一定**沿指定方向，
|
||||
且是能落到目标角的最小那一步。**同一个目标、同一个当前位置**只会有一个
|
||||
增量 —— 棘轮的"让余量"与"重试"都按它算，两次不会互相矛盾。
|
||||
"""
|
||||
base = float(target_rad) - float(q_now_rad)
|
||||
two_pi = 2.0 * math.pi
|
||||
if direction == 0:
|
||||
return math.remainder(base, two_pi)
|
||||
# 先折到 [0, 2π)，再按想要的转向取那一侧（与目标角差正好一整圈，是同一个位置）。
|
||||
pos = math.fmod(base, two_pi)
|
||||
if pos < 0.0:
|
||||
pos += two_pi
|
||||
if pos >= two_pi - _RATCHET_FULL_TURN_EPS:
|
||||
pos = 0.0 # 已经（几乎）在目标上：不要为了凑方向白转一整圈
|
||||
if direction > 0:
|
||||
return pos
|
||||
return pos - two_pi if pos > 0.0 else 0.0
|
||||
|
||||
|
||||
|
||||
def drag_verdict(
|
||||
mid_now: Tuple[float, float],
|
||||
mid_prev: Optional[Tuple[float, float]],
|
||||
mid_prev2: Optional[Tuple[float, float]] = None,
|
||||
*,
|
||||
tol_m: float = SNAP_DRAG_TOL_M,
|
||||
hard_m: float = SNAP_DRAG_HARD_M,
|
||||
) -> Tuple[bool, float]:
|
||||
"""两孔中点是否在两拍之间"整体挪动" → ``(是否判定为被推走, 本拍位移 m)``。
|
||||
|
||||
**为什么看"中点"而不是看目标孔**：目标孔是**追**的对象，工具一动它就该动
|
||||
（对准本意如此），看它分不出"正常对准"和"推着工件走"。而两孔**中点**在
|
||||
对准过程中不该动 —— 它只有工件整体平移时才会动。所以中点位移干净地对应
|
||||
"工件被搬动了"。
|
||||
|
||||
**为什么看"两孔中点"而不是单孔**：单孔在相机里会因分辨率/遮挡抖动，且
|
||||
目标孔本来就在被逼近；中点是两者的平均，噪声更小、语义更明确。
|
||||
|
||||
:param mid_now: 本拍两孔中点 (x, y)[m]（基座系）。
|
||||
:param mid_prev: 上一拍中点；``None`` = 还没有上一拍（第一拍不判）。
|
||||
:param mid_prev2: 上上拍中点。用来确认"位移是持续的"（单拍可能是误检）。
|
||||
:returns: ``(判定, 本拍位移 m)``。
|
||||
"""
|
||||
if mid_prev is None:
|
||||
return False, 0.0
|
||||
dx = float(mid_now[0]) - float(mid_prev[0])
|
||||
dy = float(mid_now[1]) - float(mid_prev[1])
|
||||
d1 = math.hypot(dx, dy)
|
||||
if d1 >= float(hard_m):
|
||||
return True, d1
|
||||
if d1 < float(tol_m) or mid_prev2 is None:
|
||||
return False, d1
|
||||
px = float(mid_prev[0]) - float(mid_prev2[0])
|
||||
py = float(mid_prev[1]) - float(mid_prev2[1])
|
||||
d2 = math.hypot(px, py)
|
||||
# 第二拍确认：上一拍也挪了（≥ 阈值的一半），且方向一致（点积 > 0）。
|
||||
# 为什么允许方向只"大致一致"：工件被推着走时前后两拍的推动量本来就不同
|
||||
# （工件一让开，工具就少推一点），拿夹角严格相等会漏判。
|
||||
if d2 >= float(tol_m) * 0.5 and (dx * px + dy * py) > 0.0:
|
||||
return True, d1
|
||||
return False, d1
|
||||
|
||||
|
||||
def descend_needed_m(
|
||||
flange_z_m: float,
|
||||
tool_tip_below_flange_m: float,
|
||||
@@ -512,6 +759,115 @@ def ratchet_cycles(total_deg: float, step_deg: float) -> int:
|
||||
return int(math.ceil(total_deg / step_deg))
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class RatchetTravel:
|
||||
"""棘轮步长与关节行程的**可用性核算**（S08 开头自检，别等到第 3 轮才炸）。"""
|
||||
|
||||
ok: bool
|
||||
step_deg: float # 配置的一步转角
|
||||
travel_deg: float # 关节行程（上限 − 下限）
|
||||
need_deg: float # 一步棘轮真正需要的行程
|
||||
max_step_deg: float # 行程允许的最大步长
|
||||
why: str # 不 ok 时给人看的说明（ok 时为空串）
|
||||
|
||||
|
||||
def ratchet_travel_ok(
|
||||
step_deg: float, limits_rad: Optional[Tuple[float, float]], margin_deg: float
|
||||
) -> RatchetTravel:
|
||||
"""一步棘轮 ``step_deg`` 放得进关节行程吗？
|
||||
|
||||
**为什么需要**：2026-09-24 实测，``screw_step_deg=180`` 配 Rizon4 的
|
||||
关节 7（±175°，行程 350°）在第 3 轮 ABORT —— 慢转那一步要转 180°、
|
||||
回转那一步还要 180°，两次加起来 360° > 350°，关节**必然**有一头贴限位；
|
||||
更糟的是"让余量"也腾不出地方（要腾就得往反向再让一整步，还是 360°）。
|
||||
日志里那一长串"贴限位 → 让余量 → 还是贴限位"就是这么来的。
|
||||
|
||||
判据取 ``2×step + 2×margin ≤ 行程``：
|
||||
- 2×step：棘轮一个来回（慢转 + 回转）要的行程；
|
||||
- 2×margin：两头都要留余量 —— 贴死限位再让余量时**一点空间都没有**，
|
||||
而实测每轮回转还会比指令少走 4~5°（速度/加减速限制），
|
||||
这点亏欠要靠余量吃，否则 7~8 轮后累计偏出限位、再也转不回来。
|
||||
|
||||
:param limits_rad: ``(lower, upper)`` [rad]；``None`` = 拿不到限位，放行
|
||||
（返回 ``ok=True``，理由写清楚：真机靠驱动器报 JOINT_LIMIT 兜底）。
|
||||
"""
|
||||
need = 2.0 * float(step_deg) + 2.0 * float(margin_deg)
|
||||
if limits_rad is None:
|
||||
return RatchetTravel(
|
||||
ok=True, step_deg=float(step_deg), travel_deg=float("inf"),
|
||||
need_deg=need, max_step_deg=float("inf"),
|
||||
why="",
|
||||
)
|
||||
travel = math.degrees(float(limits_rad[1]) - float(limits_rad[0]))
|
||||
max_step = 0.5 * (travel - 2.0 * float(margin_deg))
|
||||
if need <= travel:
|
||||
return RatchetTravel(
|
||||
ok=True, step_deg=float(step_deg), travel_deg=travel,
|
||||
need_deg=need, max_step_deg=max_step, why="",
|
||||
)
|
||||
# 给个能直接照着改的步长：往整十度取整（现场习惯填 120/150/180），
|
||||
# 但不许把步长压到 30° 以下 —— 那样拧进/拧出的轮数会多到失控。
|
||||
suggest = max(30.0, math.floor(max_step / 10.0) * 10.0)
|
||||
why = (
|
||||
f"棘轮一步 {step_deg:.0f}° 放不进关节 7 的行程：一步棘轮要 "
|
||||
f"2×{step_deg:.0f}° + 2×{margin_deg:.0f}°(余量) = {need:.0f}°，"
|
||||
f"而行程只有 {travel:.0f}°（上限 {math.degrees(limits_rad[1]):.0f}° − "
|
||||
f"下限 {math.degrees(limits_rad[0]):.0f}°）。\n"
|
||||
f" 改法：site.yaml 的 gauge_assembly.screw_step_deg 改到 "
|
||||
f"{max_step:.0f}° 以内（例如 {suggest:.0f}°），并保证 "
|
||||
f"screw_max_cycles × screw_step_deg 仍是 **360° 的整数倍**（"
|
||||
f"累计必须转整数圈，否则规停在螺纹中间、S09 也就少拧出几分之一圈）。"
|
||||
)
|
||||
return RatchetTravel(
|
||||
ok=False, step_deg=float(step_deg), travel_deg=travel,
|
||||
need_deg=need, max_step_deg=max_step, why=why,
|
||||
)
|
||||
|
||||
|
||||
#: ``rotation_verdict`` 的结论。
|
||||
ROT_OK = "ok"
|
||||
ROT_LIMIT = "limit" # 转了一部分、方向对、但没走完 ⇒ 最可能是贴到关节限位
|
||||
ROT_AXIS = "axis" # 转轴与工具 Z 不共线 ⇒ 以「度」为单位的一切动作都不成立
|
||||
ROT_SHORT = "short" # 其它偏差（方向不对/过冲）⇒ 真有问题，该停
|
||||
|
||||
|
||||
def rotation_verdict(
|
||||
delta_deg: float,
|
||||
got_deg: float,
|
||||
mis_deg: float,
|
||||
tol_deg: float,
|
||||
) -> str:
|
||||
"""绕工具 Z 转 ``delta_deg``、实测转 ``got_deg`` 的验收判据。
|
||||
|
||||
**纯函数**：这条判据 2026-09-24 把整条 S09 判死过一次
|
||||
（"期望 +180.0°、实测 +16.4°，偏差过大" → ABORT），而真因只是
|
||||
**关节7 的行程被前面的棘轮回转顶到了上限**（±175°，而棘轮一步就要 180°）。
|
||||
判错两个方向都贵：
|
||||
* 把"贴限位"判成"short" ⇒ 白扔一整轮（还可能把规撞歪）；
|
||||
* 把"真有问题"判成"limit" ⇒ 该停的没停，继续拧。
|
||||
|
||||
还有一条**曾经漏判**的：只比绝对值时，"让它 +180、它 -170"会算成
|
||||
10° 误差、落在 10% 容差里 ⇒ 判成功，而实际上一个孔都没拧。所以方向必须比。
|
||||
所以它必须能脱离节点被钉住。
|
||||
"""
|
||||
if abs(got_deg) < 3.0 and abs(delta_deg) > 10.0:
|
||||
return ROT_LIMIT # 几乎没动：与下面的"只走了一部分"同因不同余量
|
||||
if mis_deg > tol_deg:
|
||||
return ROT_AXIS
|
||||
if abs(got_deg) >= 3.0 and abs(delta_deg) >= 3.0 and got_deg * delta_deg < 0:
|
||||
# 方向反了。这条以前**漏判**：老判据只比绝对值
|
||||
# （``abs(abs(got) - abs(delta))``），所以"让它 +180、它 -170"会算 10° 误差
|
||||
# 落在 10% 容差里 → 判成功，而实际上一个孔都没拧。
|
||||
return ROT_SHORT
|
||||
slack = max(tol_deg, 0.1 * abs(delta_deg))
|
||||
short = abs(delta_deg) - abs(got_deg)
|
||||
if short > slack:
|
||||
return ROT_LIMIT # 方向对、只是没走完 ⇒ 被硬止点截住
|
||||
if short < -slack:
|
||||
return ROT_SHORT # 过冲：转多了，同样不该当成到位
|
||||
return ROT_OK
|
||||
|
||||
|
||||
def is_joint_limit_error(text: str) -> bool:
|
||||
"""S01：把驱动/规划报的字符串认成"撞到关节极限"。
|
||||
|
||||
|
||||
@@ -247,7 +247,9 @@ class Stage3Search(Stage2Locate):
|
||||
sn_us = self.prefix.rstrip("_").replace("-", "_")
|
||||
self.ft_topic = str(s3.get("ft_topic") or f"/{sn_us}/flexiv_robot_states")
|
||||
if self.ft_watchdog and RobotStates is not None:
|
||||
self.create_subscription(RobotStates, self.ft_topic, self._on_ft, 10)
|
||||
self.create_subscription(
|
||||
RobotStates, self.ft_topic, self._on_robot_states, 10
|
||||
)
|
||||
|
||||
self._state_pub = self.create_publisher(String, "/stage3/state", 10)
|
||||
self._result_pub = self.create_publisher(String, "/stage3/result", 10)
|
||||
@@ -271,7 +273,18 @@ class Stage3Search(Stage2Locate):
|
||||
def _on_tac_r(self, msg: Any) -> None:
|
||||
self._tac_r = (time.monotonic(), msg)
|
||||
|
||||
def _on_ft(self, msg: Any) -> None:
|
||||
def _on_robot_states(self, msg: Any) -> None:
|
||||
"""缓存 ``flexiv_msgs/RobotStates``（腕部力看门狗的输入）。
|
||||
|
||||
⚠️ 这个方法名**必须**与 ``Stage2Locate`` 里那个腕部 FT 的回调区分开。
|
||||
2026-09-24 踩到：两者原先都叫 ``_on_ft``，子类的定义**静默覆盖**了父类的，
|
||||
而两个订阅都在 ``__init__`` 里建好了 —— 于是：
|
||||
Stage2Locate 建的 WrenchStamped 订阅 → 实际进了这个 RobotStates 缓存；
|
||||
``_ft_fz`` 永远不写 → S07 打"腕部 FT 没有数据"（而话题数据是好的）。
|
||||
真机上更阴：两个话题都存在，``self._ft`` 里 RobotStates 与 WrenchStamped
|
||||
交替被覆盖 → ``_ft_mag()`` 时有时无 → 看门狗时灵时不灵。
|
||||
这类"同名覆盖 + 不报错"的坑，只要名字区分开就根除。
|
||||
"""
|
||||
self._ft = (time.monotonic(), msg)
|
||||
|
||||
def _tac_fresh(self, timeout_s: float = 1.0) -> Tuple[float, float, float, bool]:
|
||||
|
||||
@@ -27,7 +27,11 @@ from arm_application.gauge_config import (
|
||||
missing_entries,
|
||||
unit_violations,
|
||||
)
|
||||
from arm_application.gauge_fsm import descend_iter_budget, descend_tol_m
|
||||
from arm_application.gauge_fsm import (
|
||||
descend_iter_budget,
|
||||
descend_tol_m,
|
||||
ratchet_travel_ok,
|
||||
)
|
||||
|
||||
|
||||
# =============================================================================
|
||||
@@ -185,7 +189,7 @@ def test_describe_prints_effective_values_and_marks_calibration_needed():
|
||||
text = "\n".join(cfg.describe())
|
||||
# 实际生效的值（不是"我读了 site.yaml"）
|
||||
assert "S01 预绕 150.0°" in text
|
||||
assert "S08 棘轮 180°×12 轮 = 累计 2160°" in text
|
||||
assert "S08 棘轮 120°×12 轮 = 累计 1440°" in text
|
||||
# 必须提示这些阈值要现场标定，否则默认值会被当成"调好的参数"
|
||||
assert "现场标定" in text
|
||||
# 稳定判据的口径要写出来，避免下次有人改成"相邻帧之差"
|
||||
@@ -265,3 +269,50 @@ def test_shipped_site_yaml_s07_step_cannot_deadlock():
|
||||
assert descend_iter_budget(depth_m, step_m) >= depth_m / step_m
|
||||
# 顺带钉住"出厂配置的派生值印得出来"（describe 里会打印容差）
|
||||
assert "S07" in "\n".join(cfg.describe())
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# 出厂配置的 S08：棘轮步长必须放得进**支持的所有臂**的关节7 行程
|
||||
# =============================================================================
|
||||
def test_shipped_site_yaml_ratchet_step_fits_every_supported_arm():
|
||||
"""**回归锚点**（2026-09-24 在仿真里踩到：S08 第 3 轮 ABORT）。
|
||||
|
||||
`screw_step_deg: 180` 配关节 7 的 ±175°（行程 350°）时，棘轮一个来回要
|
||||
2×180°=360° > 350° —— 关节**必然**有一头贴限位，"让余量"也腾不出空间
|
||||
（腾一步还是 360°）。日志表现是一长串"贴限位 → 让余量 → 还是贴限位"，
|
||||
真因（配置）藏在最后一行，白烧 3 轮节拍。
|
||||
|
||||
这条测试盯**出厂 site.yaml 的真实取值**，并且拿 `flexiv_description` 里
|
||||
**每台支持臂**的 joint7 限位去算（不是写死 350°）：以后加一台行程更小的
|
||||
臂，这里会自动报红。余量取 `gauge_assembly_node.RATCHET_LIMIT_MARGIN_DEG`
|
||||
的同一个口径 —— 两处不一致时，测试会红，而不是现场崩。
|
||||
"""
|
||||
import math
|
||||
|
||||
import yaml
|
||||
|
||||
root = Path(__file__).resolve().parents[3]
|
||||
doc = yaml.safe_load((root / "config" / "site.yaml").read_text(encoding="utf-8"))
|
||||
step = float(doc["gauge_assembly"]["screw_step_deg"])
|
||||
|
||||
margin = 8.0 # 与 gauge_assembly_node.RATCHET_LIMIT_MARGIN_DEG 同口径
|
||||
from arm_application.gauge_assembly_node import RATCHET_LIMIT_MARGIN_DEG
|
||||
|
||||
assert margin == RATCHET_LIMIT_MARGIN_DEG, "余量口径变了，测试要跟着改"
|
||||
|
||||
limit_files = sorted((root / "src" / "flexiv_description" / "config").glob("*/joint_limits.yaml"))
|
||||
assert limit_files, "找不到任何臂的 joint_limits.yaml —— 路径变了？"
|
||||
for path in limit_files:
|
||||
doc_l = yaml.safe_load(path.read_text(encoding="utf-8")) or {}
|
||||
joint7 = (doc_l.get("joint_limits") or {}).get("joint7") or doc_l.get("joint7")
|
||||
assert joint7, f"{path} 里没有 joint7 限位"
|
||||
limits = (float(joint7["lower"]), float(joint7["upper"]))
|
||||
verdict = ratchet_travel_ok(step, limits, margin)
|
||||
assert verdict.ok, f"{path.name}: 出厂配置的棘轮步长放不下 —— {verdict.why}"
|
||||
assert verdict.max_step_deg > 0.0
|
||||
|
||||
# 累计转角必须是整数圈：否则规停在螺纹中间，S09 拧出也少转几分之一圈
|
||||
cycles = int(doc["gauge_assembly"]["screw_max_cycles"])
|
||||
assert math.isclose(step * cycles % 360.0, 0.0, abs_tol=1e-9), (
|
||||
f"screw_step_deg×screw_max_cycles = {step * cycles}° 不是 360° 的整数倍"
|
||||
)
|
||||
|
||||
@@ -0,0 +1,182 @@
|
||||
"""S09 拧出后**先竖直提出孔口**：结构上有序，行为上提对高度。
|
||||
|
||||
为什么单独钉住（2026-09-24 仿真实测，两次栽在同一个地方）
|
||||
--------------------------------------------------------
|
||||
仿真里**没有螺纹几何**：S09 的"拧出"只让规在孔里转，规**仍然插在孔里**
|
||||
（真机也只是沿螺纹退回几个毫米）。于是紧跟着的横向动作 —— S10 换孔 / S11 回 home
|
||||
—— 会把规从孔里**拽**出来：实测抓取锁被拽断、规掉在台面上。
|
||||
|
||||
最坑的是**它不在这里报错**：两分钟后下游只表现为"下一孔 S07 下探 35mm 判不到
|
||||
接触"（工具上已经没规了），日志指不到真因（规则 R5.1 的静默失效）。
|
||||
|
||||
所以这里有两条断言，缺一不可：
|
||||
* **结构**：``_step_unscrew`` 在"成功返回"之前必须调用提出动作 —— 顺序错了
|
||||
（比如把提出挪到 S10 里）就等于没做，因为横向动作已经发生了；
|
||||
* **行为**：提出目标是**跟踪高**（``_track_z()`` = 悬停高 + 50mm），不是写死的
|
||||
毫米数；已经在跟踪高上时不动（否则每次换孔白等 1 秒多）。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import ast
|
||||
import pathlib
|
||||
from typing import Any, List
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from arm_application.gauge_assembly_node import GaugeAssembly
|
||||
|
||||
NODE_SRC = (
|
||||
pathlib.Path(__file__).resolve().parents[1]
|
||||
/ "arm_application" / "gauge_assembly_node.py"
|
||||
)
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# 结构：提出动作必须在 S09 的"成功出口"之前
|
||||
# =============================================================================
|
||||
def _node_ast() -> ast.Module:
|
||||
return ast.parse(NODE_SRC.read_text(encoding="utf-8"))
|
||||
|
||||
|
||||
def _find_method(name: str) -> ast.FunctionDef:
|
||||
for node in ast.walk(_node_ast()):
|
||||
if isinstance(node, ast.FunctionDef) and node.name == name:
|
||||
return node
|
||||
raise AssertionError(f"节点里找不到方法 {name}")
|
||||
|
||||
|
||||
def _call_lines(func: ast.AST, attr: str) -> List[int]:
|
||||
out: List[int] = []
|
||||
for node in ast.walk(func):
|
||||
if isinstance(node, ast.Call):
|
||||
f = node.func
|
||||
if isinstance(f, ast.Attribute) and f.attr == attr:
|
||||
out.append(node.lineno)
|
||||
return out
|
||||
|
||||
|
||||
def test_unscrew_lifts_out_of_the_hole_before_succeeding():
|
||||
"""拧出成功后必须先提出孔口，再返回 ``UNSCREW_DONE``。"""
|
||||
fn = _find_method("_step_unscrew")
|
||||
lifts = _call_lines(fn, "_lift_clear_of_hole")
|
||||
assert lifts, (
|
||||
"S09 结束时没有竖直提出孔口 —— S10/S11 的横向动作会把规从孔里拽出来"
|
||||
"（实测：锁被拽断、规掉在台面上，下游表现为『S07 判不到接触』）"
|
||||
)
|
||||
exits = [
|
||||
n.lineno
|
||||
for n in ast.walk(fn)
|
||||
if isinstance(n, ast.Return)
|
||||
and isinstance(n.value, ast.Attribute)
|
||||
and n.value.attr == "UNSCREW_DONE"
|
||||
]
|
||||
assert exits, "S09 没有成功出口？"
|
||||
assert min(lifts) < max(exits), (
|
||||
"提出孔口必须在**成功返回之前**：顺序反了等于没做（横向动作已经发生）"
|
||||
)
|
||||
|
||||
|
||||
def test_lift_helper_exists_and_uses_the_track_height_idiom():
|
||||
"""提出高度用 ``_track_z()``（悬停高 + 50mm），不写死毫米数。"""
|
||||
fn = _find_method("_lift_clear_of_hole")
|
||||
assert _call_lines(fn, "_track_z"), (
|
||||
"提出高度必须用 _track_z()：这是流程里『工具已离开工件』的既有定义，"
|
||||
"写死毫米数会多出一套没人维护的口径"
|
||||
)
|
||||
assert _call_lines(fn, "_goto"), "提出是一次真实运动（_goto），不是改个变量"
|
||||
src = ast.get_source_segment(NODE_SRC.read_text(encoding="utf-8"), fn) or ""
|
||||
assert "hold_xy" in src, "提出必须是**纯 Z**：横向分量会把规别在孔壁上"
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# 行为：提对高度 / 已在高度上不动 / 拿不到状态不炸
|
||||
# =============================================================================
|
||||
class _StubLogger:
|
||||
def __init__(self, sink: List[str]) -> None:
|
||||
self._sink = sink
|
||||
|
||||
def info(self, msg: str) -> None:
|
||||
self._sink.append(msg)
|
||||
|
||||
def warn(self, msg: str) -> None:
|
||||
self._sink.append(msg)
|
||||
|
||||
|
||||
class _StubAssembly:
|
||||
"""``_lift_clear_of_hole`` 只用这几样东西；测试里不建任何 ROS 实体。"""
|
||||
|
||||
move_speed = 0.05
|
||||
vert_tol_rad = 0.02
|
||||
|
||||
def __init__(self, z: Any, track_z: float = 0.707, raise_on_goto: bool = False) -> None:
|
||||
self.logs: List[str] = []
|
||||
self.moves: List[Any] = []
|
||||
self._z = z
|
||||
self._tz = float(track_z)
|
||||
self._raise = bool(raise_on_goto)
|
||||
|
||||
def _wait_state(self, _timeout: float) -> Any:
|
||||
if self._z is None:
|
||||
return None, None
|
||||
Tee = np.eye(4)
|
||||
Tee[0, 3] = 0.5130
|
||||
Tee[1, 3] = -0.0020
|
||||
Tee[2, 3] = float(self._z)
|
||||
return np.zeros(7), Tee
|
||||
|
||||
def _track_z(self) -> float:
|
||||
return self._tz
|
||||
|
||||
def get_logger(self) -> Any:
|
||||
return _StubLogger(self.logs)
|
||||
|
||||
def _goto(self, q: Any, Tee: Any, target: Any, **kw: Any) -> Any:
|
||||
if self._raise:
|
||||
raise RuntimeError("轨迹被拒")
|
||||
self.moves.append((np.array(target, dtype=float), kw))
|
||||
return True, q, Tee
|
||||
|
||||
|
||||
def _stub(z: Any, track_z: float = 0.707, raise_on_goto: bool = False) -> _StubAssembly:
|
||||
return _StubAssembly(z, track_z, raise_on_goto)
|
||||
|
||||
|
||||
def test_deep_in_the_hole_lifts_to_track_height():
|
||||
"""规还在孔里（实测规尖比工件面低 30mm）→ 提到跟踪高，纯 Z。"""
|
||||
st = _stub(0.628)
|
||||
GaugeAssembly._lift_clear_of_hole(st, "k1") # type: ignore[arg-type]
|
||||
|
||||
assert len(st.moves) == 1, "应当恰好走一次提出"
|
||||
target, kw = st.moves[0]
|
||||
assert target[0] == pytest.approx(0.5130) and target[1] == pytest.approx(-0.0020), (
|
||||
"提出必须是**纯 Z**：XY 一动就是把规别在孔壁上"
|
||||
)
|
||||
assert target[2] == pytest.approx(0.707), "目标高度就是跟踪高（悬停 + 50mm）"
|
||||
assert kw.get("hold_xy") is True
|
||||
assert kw.get("tag") == "S09_exit_k1"
|
||||
assert any("竖直提出" in m for m in st.logs), "日志要能回看这次提出（不然失败时查不到）"
|
||||
|
||||
|
||||
def test_already_high_enough_does_not_move():
|
||||
"""已经比跟踪高还高 → 一步都不走（否则每次换孔白等一秒多）。"""
|
||||
st = _stub(0.80)
|
||||
GaugeAssembly._lift_clear_of_hole(st, "k1") # type: ignore[arg-type]
|
||||
assert st.moves == []
|
||||
assert st.logs == []
|
||||
|
||||
|
||||
def test_no_joint_state_is_not_fatal():
|
||||
"""拿不到状态只告警：这一刻孔里已经没有规要保护，硬失败会把成功的装配变成失败。"""
|
||||
st = _stub(None)
|
||||
GaugeAssembly._lift_clear_of_hole(st, "k1") # type: ignore[arg-type]
|
||||
assert st.moves == []
|
||||
assert st.logs == [], "拿不到状态连日志都不用刷（静默跳过即可）"
|
||||
|
||||
|
||||
def test_goto_exception_is_swallowed_with_a_warning():
|
||||
"""提出失败（轨迹被拒）只告警，流程继续 —— 后面 S10/S11 仍会走到安全位。"""
|
||||
bad = _stub(0.628, raise_on_goto=True)
|
||||
GaugeAssembly._lift_clear_of_hole(bad, "k1") # type: ignore[arg-type]
|
||||
assert any("提出孔口失败" in m for m in bad.logs)
|
||||
@@ -25,21 +25,34 @@ from arm_application.gauge_fsm import (
|
||||
State,
|
||||
DESCEND_BUDGET_FLOOR_M,
|
||||
GAUGE_SHOULDER_ABOVE_TIP_M,
|
||||
WRENCH_MAX_AGE_S,
|
||||
descend_budget_m,
|
||||
descend_iter_budget,
|
||||
descend_needed_m,
|
||||
descend_progress,
|
||||
descend_tol_m,
|
||||
relief_tol_m,
|
||||
tool_clearance_m,
|
||||
wrench_live,
|
||||
wrench_touch,
|
||||
is_joint_limit_error,
|
||||
pixel_stable,
|
||||
points_stable,
|
||||
hover_z_guard,
|
||||
hover_z_for_tool_clearance,
|
||||
ratchet_cycles,
|
||||
resolve_next,
|
||||
rotation_verdict,
|
||||
screw_stop,
|
||||
tangential_jump,
|
||||
drag_verdict,
|
||||
ratchet_delta_rad,
|
||||
ratchet_travel_ok,
|
||||
SNAP_DRAG_TOL_M,
|
||||
)
|
||||
|
||||
from arm_application.gauge_config import DEFAULTS
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# 主线：k1 → k2 → 复位
|
||||
@@ -456,6 +469,34 @@ def test_descend_tol_rejects_nonpositive_step():
|
||||
descend_tol_m(-0.0005)
|
||||
|
||||
|
||||
def test_relief_tolerance_is_strictly_smaller_than_the_relief_move():
|
||||
"""卸载回退的容差必须**严格小于**回退量（与下压步长同一条纪律）。
|
||||
|
||||
2026-09-24 实测踩到：回退 0.50mm 而容差被写死 0.0005 m（同量级），
|
||||
目标 z 的浮点回减让 ``dist`` 比 0.0005 略小 ⇒ ``_goto`` 入口就判"已到位"
|
||||
⇒ 卸载成为空操作，而残余力判据比的是两个相同读数 ⇒ 假报"已放干净"。
|
||||
之后 S08 带着 ~70N 轴向力去转（夹持摩擦上限 ≈73N）→ 掉件(E1)。
|
||||
"""
|
||||
for dz_mm in (0.05, 0.2, 0.5, 1.0, 3.0):
|
||||
dz = dz_mm / 1000.0
|
||||
tol = relief_tol_m(dz)
|
||||
assert 0.0 < tol < dz, f"回退 {dz_mm}mm 的容差 {tol * 1000:.4f}mm 不小于回退量"
|
||||
|
||||
|
||||
def test_relief_tolerance_has_a_floor_but_never_swallows_the_move():
|
||||
"""回退量极小时容差退不下去，但**仍不许**大于等于回退量。"""
|
||||
dz = 1e-5
|
||||
tol = relief_tol_m(dz)
|
||||
assert 0.0 < tol <= dz * 0.5
|
||||
|
||||
|
||||
def test_relief_tolerance_rejects_nonpositive_move():
|
||||
with pytest.raises(ValueError):
|
||||
relief_tol_m(0.0)
|
||||
with pytest.raises(ValueError):
|
||||
relief_tol_m(-0.0005)
|
||||
|
||||
|
||||
def test_descend_iter_budget_covers_the_whole_depth():
|
||||
"""步数上限必须够走完整个下探深度（否则会在"还没到底"时被上限截断）。"""
|
||||
step_m, depth_m = 0.0005, 0.025
|
||||
@@ -519,8 +560,6 @@ def test_shipped_budget_covers_shipped_gauge_and_hover():
|
||||
以后谁把 contact_max_depth_mm 调回去、或把 tool_tip_below_flange_m 改大
|
||||
到不匹配，这里就会红。
|
||||
"""
|
||||
from arm_application.gauge_config import DEFAULTS
|
||||
|
||||
budget_m = DEFAULTS["contact_max_depth_mm"] / 1000.0
|
||||
tip_below = DEFAULTS["tool_tip_below_flange_m"]
|
||||
# 出厂配置里 workpiece_surface_z_m = 0（运行期用 stage2 实测孔位兜底），
|
||||
@@ -532,6 +571,57 @@ def test_shipped_budget_covers_shipped_gauge_and_hover():
|
||||
)
|
||||
|
||||
|
||||
def test_wrench_touch_is_sign_agnostic():
|
||||
"""腕部 FT 判据取**绝对增量**，不赌正负号。
|
||||
|
||||
正负号方向取决于传感器安装（真机法兰系、仿真 link7 系，压下去是 +z 还是 -z
|
||||
都可能）。赌错了就是"整段流程静默失效"：下发正常、日志正常、就是停不住。
|
||||
取绝对值只会更早停一点点，这个方向的错是安全的 —— 与既有
|
||||
stage2_locate_node._contact_detected 的口径（abs(fz - baseline)）也一致。
|
||||
"""
|
||||
assert wrench_touch(20.0, 0.0, 8.0)
|
||||
assert wrench_touch(-20.0, 0.0, 8.0)
|
||||
# 基线本身不是 0（带载）时同样只看增量
|
||||
assert wrench_touch(54.0, 46.0, 8.0)
|
||||
assert wrench_touch(38.0, 46.0, 8.0)
|
||||
|
||||
|
||||
def test_wrench_touch_below_threshold_does_not_fire():
|
||||
assert not wrench_touch(7.9, 0.0, 8.0)
|
||||
assert not wrench_touch(46.0, 46.0, 8.0)
|
||||
# 边界：等于阈值算触发（与 tangential_jump 的 >= 口径一致）
|
||||
assert wrench_touch(8.0, 0.0, 8.0)
|
||||
|
||||
|
||||
def test_wrench_touch_threshold_is_reachable_before_grip_slips():
|
||||
"""阈值的量级必须**远低于**夹持摩擦能传的上限，否则等于设了个永远达不到的数。
|
||||
|
||||
这是"用 300 当阈值"那个 bug 的一般化回归：夹持段 µ=0.25、实测握力 ≈46N
|
||||
⇒ 两指合计能传约 µ·N ≈ 23N。接触刚度 5e4 N/m，所以 8N 阈值 ≈ 0.16mm
|
||||
过压就判到，而打滑要到 ≈0.46mm —— 留了约 3 倍余量。
|
||||
"""
|
||||
mu, grip_n = 0.25, 46.0
|
||||
slip_limit_n = mu * grip_n * 2 # 两指
|
||||
assert DEFAULTS["contact_axial_delta_n"] < slip_limit_n / 2, (
|
||||
f"阈值 {DEFAULTS['contact_axial_delta_n']}N 太接近夹持摩擦上限 "
|
||||
f"{slip_limit_n:.1f}N —— 打滑前可能来不及判到"
|
||||
)
|
||||
|
||||
|
||||
def test_wrench_live_uses_freshness_not_magnitude():
|
||||
"""判活看"够不够新"，**不看数值大小**。
|
||||
|
||||
静止时轴向力本来就接近 0。拿数值判活会把"安静的传感器"误判成掉线 ——
|
||||
这正是 2026-09-24 在触觉上踩过的同一个坑（判据从 |fnormal|>1.0 改成
|
||||
"够新 + connected"）。所以这里专门断言 age 有值但数值为 0 → 活的。
|
||||
"""
|
||||
assert wrench_live(0.0)
|
||||
assert wrench_live(WRENCH_MAX_AGE_S)
|
||||
assert not wrench_live(WRENCH_MAX_AGE_S + 1e-6)
|
||||
# None = 一帧都没收到过（话题名错/传感器没起来）→ 不是活的
|
||||
assert not wrench_live(None)
|
||||
|
||||
|
||||
def test_descend_iter_budget_zero_depth_means_no_iterations():
|
||||
assert descend_iter_budget(0.0, 0.0005) == 0
|
||||
with pytest.raises(ValueError):
|
||||
@@ -546,3 +636,279 @@ def test_descend_progress_only_counts_real_downward_motion():
|
||||
assert not descend_progress(0.6316, 0.63159, step_m) # 只走了 0.01mm：被死区吃掉
|
||||
assert not descend_progress(0.6316, 0.6316, step_m) # 完全没动（空转）
|
||||
assert not descend_progress(0.6316, 0.6317, step_m) # 往上跑 ≠ 推进
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# 绕轴旋转的验收判据（S01/S08/S09 共用）
|
||||
# =============================================================================
|
||||
def test_rotation_verdict_accepts_a_full_turn():
|
||||
assert rotation_verdict(-180.0, -179.7, 0.0, 5.0) == "ok"
|
||||
assert rotation_verdict(+180.0, +175.1, 0.0, 5.0) == "ok" # 5° 之内算到位
|
||||
|
||||
|
||||
def test_rotation_verdict_calls_a_partial_turn_a_joint_limit():
|
||||
"""2026-09-24 实测的那一条：**只转了 16.4°**（被关节7 的上限截住）。
|
||||
|
||||
期望 +180、实测 +16.4：方向对、只是没走完 ⇒ 判"贴限位"（调用方张开爪
|
||||
让出余量再转），**不是**"偏差过大"直接 ABORT —— 后者把一整轮白扔，
|
||||
而且日志看起来像"这台臂坏了"。
|
||||
"""
|
||||
assert rotation_verdict(+180.0, +16.4, 0.1, 5.0) == "limit"
|
||||
# 完全没动也是同一类（只是余量更少）
|
||||
assert rotation_verdict(+180.0, +0.4, 0.0, 5.0) == "limit"
|
||||
|
||||
|
||||
def test_rotation_verdict_still_stops_on_wrong_direction_or_overshoot():
|
||||
"""判"贴限位"必须**卡住方向**：反向转/过冲都是真问题，不能拿去让余量重试。"""
|
||||
assert rotation_verdict(+180.0, -170.0, 0.0, 5.0) == "short" # 转反了
|
||||
assert rotation_verdict(+180.0, +260.0, 0.0, 5.0) == "short" # 过冲
|
||||
# 小幅指令（≤10°）几乎没动时**不算**"完全没动"那条特例，于是落进
|
||||
# "方向对但没走完" ⇒ 同样判 limit。这是有意的：让余量那一步是免费的
|
||||
# （张开爪时规在打滑），宁可白让一次，也别把"其实贴住了"判成"臂坏了"。
|
||||
assert rotation_verdict(+8.0, +0.2, 0.0, 5.0) == "limit"
|
||||
|
||||
|
||||
def test_rotation_verdict_axis_misalignment_beats_everything():
|
||||
"""转轴不共线（>5°）时，任何角度数字都没有意义 —— 必须先报它。"""
|
||||
assert rotation_verdict(+180.0, +179.0, 6.0, 5.0) == "axis"
|
||||
assert rotation_verdict(+180.0, +16.4, 6.0, 5.0) == "axis"
|
||||
|
||||
|
||||
def test_ratchet_step_of_180_never_fits_the_rizon_joint7_range():
|
||||
"""棘轮一步 180° 在 Rizon4 的关节7 行程（±175°）里**永远**放不下。
|
||||
|
||||
这是 S09 中止的物理根源，也解释了为什么转腕前要先让余量：
|
||||
贴着上限时反向那一步只剩十几度（实测 16.4°）。
|
||||
"""
|
||||
from arm_application.gauge_kin import prewind
|
||||
|
||||
lim = (-3.0543, 3.0543) # Rizon4 关节7：±175°
|
||||
# 贴在上限（棘轮回转每轮都停在这儿）→ 想再往 + 转：放不下，必须让余量
|
||||
plan = prewind(q_now_rad=math.radians(171.0), want_deg=180.0, direction=+1,
|
||||
limits=lim, margin_deg=8.0)
|
||||
assert plan.clamped
|
||||
# 同一个位置往 - 转：放得下（这正是"让余量"那一步）
|
||||
plan = prewind(q_now_rad=math.radians(171.0), want_deg=180.0, direction=-1,
|
||||
limits=lim, margin_deg=8.0)
|
||||
assert not plan.clamped
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# 悬停高度自检（悬停按相机定、工具比相机长 ⇒ 会捅进工件）
|
||||
# =============================================================================
|
||||
def test_hover_guard_fires_for_the_exact_case_we_hit():
|
||||
"""2026-09-24 实测：仿真悬停 0.632、工具长 0.245、工件面 0.412。
|
||||
|
||||
尖端 0.632−0.245=0.387，比工件面低 25mm ⇒ 悬停这一步就把工具捅进去了，
|
||||
紧接着 S05 的横向对准把工件拖走 92mm。这里必须报"要改"并给出几何值。
|
||||
"""
|
||||
fixed, why = hover_z_guard(0.632, 0.412, 0.245, 0.002)
|
||||
assert why, "这种悬停必须被判为不安全"
|
||||
assert fixed == pytest.approx(0.412 + 0.245 + 0.002)
|
||||
assert "钻进工件" in why
|
||||
assert "hole_hover_z_m = 0.659" in why # 要能直接照着改配置
|
||||
|
||||
|
||||
def test_hover_guard_is_silent_when_the_tool_stays_clear():
|
||||
"""几何对的配置一个字都不许说（否则每次启动都刷一堆无用告警）。"""
|
||||
fixed, why = hover_z_guard(0.659, 0.412, 0.245, 0.002)
|
||||
assert why == ""
|
||||
assert fixed == pytest.approx(0.659)
|
||||
# 更高也照过（悬停高一点只是拍得远，不算错）
|
||||
assert hover_z_guard(0.700, 0.412, 0.245, 0.002)[1] == ""
|
||||
|
||||
|
||||
def test_hover_guard_uses_want_clearance_when_configuring():
|
||||
"""修正值必须落在"刚好 want_clearance"的位置，而不是随手抬一点。"""
|
||||
fixed, _ = hover_z_guard(0.50, 0.412, 0.245, 0.005)
|
||||
assert fixed == pytest.approx(0.412 + 0.245 + 0.005)
|
||||
assert hover_z_for_tool_clearance(0.412, 0.245, 0.005) == pytest.approx(fixed)
|
||||
|
||||
|
||||
def test_hover_guard_tolerates_millimetre_level_shortfall():
|
||||
"""夹在目标间隙附近（差不到 1mm）不算错：工件面 z 本身就有几毫米不确定度。"""
|
||||
assert hover_z_guard(0.6580, 0.412, 0.245, 0.002)[1] == "" # 差 1.0mm 之内
|
||||
assert hover_z_guard(0.6560, 0.412, 0.245, 0.002)[1] != "" # 差 3mm：判错
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# S05 精对位时"工件被工具推走"的看门狗
|
||||
# =============================================================================
|
||||
def test_drag_verdict_says_nothing_on_the_first_frame():
|
||||
"""第一拍没有上一拍可比 —— 不能因为"没有历史"就报警。"""
|
||||
assert drag_verdict((0.49, 0.0), None)[0] is False
|
||||
|
||||
|
||||
def test_drag_verdict_ignores_measurement_jitter():
|
||||
"""正常对准时两孔中点不动（动的是目标孔，不是中点）—— 亚毫米抖动不算。"""
|
||||
ok, d = drag_verdict((0.4905, 0.0002), (0.4900, 0.0000), (0.4899, 0.0000))
|
||||
assert ok is False
|
||||
assert d == pytest.approx(math.hypot(0.0005, 0.0002), abs=1e-9)
|
||||
|
||||
|
||||
def test_drag_verdict_fires_on_one_big_jump():
|
||||
"""单拍就挪了 8mm：刚体不会自己挪回去，不必等第二拍确认。"""
|
||||
ok, d = drag_verdict((0.4820, 0.0), (0.4900, 0.0))
|
||||
assert ok is True
|
||||
assert d == pytest.approx(0.008)
|
||||
|
||||
|
||||
def test_drag_verdict_needs_two_frames_when_it_is_only_3_plus_mm():
|
||||
"""只大过 3mm 一点点：要求上一拍也朝同方向挪过，避免一次误检就中止流程。"""
|
||||
mid = (0.4860, 0.0)
|
||||
prev = (0.4900, 0.0)
|
||||
# 上一拍没动（d2 = 0）⇒ 只当噪声
|
||||
assert drag_verdict(mid, prev, (0.4901, 0.0))[0] is False
|
||||
# 上一拍也挪了 4mm、方向一致 ⇒ 判被推走
|
||||
assert drag_verdict(mid, prev, (0.4940, 0.0))[0] is True
|
||||
# 上一拍也挪了 4mm，但方向**相反**（工件抖动/误检）⇒ 不判
|
||||
assert drag_verdict(mid, prev, (0.4820, 0.0))[0] is False
|
||||
|
||||
|
||||
def test_drag_verdict_matches_what_we_actually_saw_in_gazebo():
|
||||
"""2026-09-24 实测那一轮的真实数字：工件每拍被推 11.8mm（残差恒 11mm）。
|
||||
|
||||
这条是回归测试：当时的现场没有任何判据报警，日志只说"检查手眼/相机安装"，
|
||||
于是一路追到迭代用满。现在同样的数列必须在**第一拍**就判出来。
|
||||
"""
|
||||
ks = [0.4910, 0.4910, 0.4792, 0.4674, 0.4566, 0.4458, 0.4350, 0.4239, 0.4121]
|
||||
mids = [(k - 0.029, 0.0) for k in ks] # 中点 = k1 左移两孔间距的一半
|
||||
flags = [
|
||||
drag_verdict(mids[i], mids[i - 1] if i >= 1 else None,
|
||||
mids[i - 2] if i >= 2 else None)[0]
|
||||
for i in range(len(mids))
|
||||
]
|
||||
assert flags == [False, False, True, True, True, True, True, True, True]
|
||||
assert SNAP_DRAG_TOL_M < 0.0118
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# 棘轮：目标角（绝对值）→ 增量。2026-09-24 S09 第一步 ABORT 的根因
|
||||
# =============================================================================
|
||||
def test_ratchet_delta_wraps_a_target_that_is_past_the_limit():
|
||||
"""实测那一笔：关节在 +158.9°、目标 +338.9° ⇒ 增量是 +180°（不是 +338.9°）。
|
||||
|
||||
差值的折算是整个棘轮的地基：把它当成 338.9° 会让"行程够不够"算错、
|
||||
让余量那一步算到限位外面，最后报一句与真因无关的"偏差过大"。
|
||||
"""
|
||||
d = ratchet_delta_rad(math.radians(338.9), math.radians(158.9))
|
||||
assert math.degrees(d) == pytest.approx(180.0, abs=1e-6)
|
||||
|
||||
|
||||
def test_ratchet_rewind_target_stays_inside_the_limit():
|
||||
"""让余量那一步必须落在限位内 —— 这正是当时失败的地方（−180° < −175°）。"""
|
||||
lo, up = math.radians(-175.0), math.radians(175.0)
|
||||
q_now = math.radians(158.9)
|
||||
delta = ratchet_delta_rad(math.radians(338.9), q_now)
|
||||
rewind = q_now - delta # 让出整步
|
||||
assert lo < rewind < up
|
||||
assert math.degrees(rewind) == pytest.approx(-21.1, abs=1e-6)
|
||||
# 让完余量再前进整步 ⇒ 关节回到原处、工具净转 +180°
|
||||
retry = rewind + delta
|
||||
assert math.degrees(retry) == pytest.approx(158.9, abs=1e-6)
|
||||
|
||||
|
||||
def test_ratchet_delta_is_the_same_number_for_both_wrapped_aims():
|
||||
"""+338.9° 与 −21.1° 是同一个关节位置（差一整圈）—— 增量必须一致。"""
|
||||
q_now = math.radians(158.9)
|
||||
a = ratchet_delta_rad(math.radians(338.9), q_now)
|
||||
b = ratchet_delta_rad(math.radians(-21.1), q_now)
|
||||
assert a == pytest.approx(b, abs=1e-9)
|
||||
|
||||
|
||||
def test_ratchet_delta_covers_a_plain_back_and_forth_cycle():
|
||||
"""正常的棘轮一步（不贴限位）：从 +150° 拧 −180°、再回转 +180°。"""
|
||||
q = math.radians(150.0)
|
||||
d = ratchet_delta_rad(math.radians(-30.0), q)
|
||||
assert math.degrees(d) == pytest.approx(-180.0, abs=1e-6)
|
||||
d2 = ratchet_delta_rad(math.radians(150.0), q + d)
|
||||
assert math.degrees(d2) == pytest.approx(180.0, abs=1e-6)
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# 棘轮的**转向**必须由工艺意图决定，不能交给"谁更短"
|
||||
# 2026-09-24 实测：S08 第 3 轮 ABORT（步长 180° 的刀口）
|
||||
# =============================================================================
|
||||
def test_ratchet_180_degree_step_flips_direction_on_a_tenth_of_a_degree_read():
|
||||
"""**实测那一笔**：调用方读到 q=+162.0 ⇒ 目标 −18.0；棘轮晚 0.1° 读到
|
||||
q=+162.1 ⇒ "最短路径"给出 **+179.9°**（方向反了）。
|
||||
|
||||
180° 正好落在 ±180° 的刀口上：0.1° 的读数差就能决定"往前"还是"往后"。
|
||||
结果是棘轮朝螺纹反方向走 → 贴限位 → 让余量 → 重试 → **净位移 0** ⇒ ABORT，
|
||||
而现场什么都没坏。所以转向必须由调用方按工艺意图给定。
|
||||
"""
|
||||
d_naive = ratchet_delta_rad(math.radians(-18.0), math.radians(162.1))
|
||||
assert math.degrees(d_naive) == pytest.approx(+179.9, abs=0.05) # 旧口径：翻向
|
||||
d_screw = ratchet_delta_rad(math.radians(-18.0), math.radians(162.1), -1)
|
||||
assert math.degrees(d_screw) == pytest.approx(-180.1, abs=0.05), "拧入必须往 − 转"
|
||||
d_back = ratchet_delta_rad(math.radians(342.0), math.radians(162.1), +1)
|
||||
assert math.degrees(d_back) == pytest.approx(+179.9, abs=0.05), "回转必须往 + 转"
|
||||
|
||||
|
||||
def test_directed_delta_always_lands_on_the_target_and_keeps_the_sign():
|
||||
"""给了方向：增量符号一定等于指定方向，且转过去**就是**目标角（差整圈）。"""
|
||||
q = math.radians(162.1)
|
||||
for want_sign, target_deg in ((+1, 342.0), (+1, -18.0), (-1, -18.0), (-1, 342.0)):
|
||||
d = ratchet_delta_rad(math.radians(target_deg), q, want_sign)
|
||||
assert d != 0.0
|
||||
assert math.copysign(1.0, d) == want_sign, (want_sign, target_deg, math.degrees(d))
|
||||
# 转过去与目标角**同一个位置**（差整数圈）
|
||||
turns = (q + d - math.radians(target_deg)) / (2.0 * math.pi)
|
||||
assert abs(turns - round(turns)) < 1e-9
|
||||
# 且是那一侧**最小**的一步（再多转一整圈就不是"最小"了）
|
||||
assert abs(d) <= 2.0 * math.pi + 1e-9
|
||||
|
||||
|
||||
def test_directed_delta_does_not_waste_a_full_turn_when_already_there():
|
||||
"""已经在目标角上时不许"为了凑方向"白转一整圈 —— 那是 1 秒多的空转。"""
|
||||
q = math.radians(30.0)
|
||||
assert ratchet_delta_rad(q, q, +1) == 0.0
|
||||
assert ratchet_delta_rad(q, q, -1) == 0.0
|
||||
# 差一点点也算"就在那儿"（1e-12 rad 量级）而不是 359.999…°
|
||||
assert abs(ratchet_delta_rad(q, q - 1e-12, +1)) < 1e-9
|
||||
|
||||
|
||||
def test_plain_shortest_path_is_still_available_for_non_ratchet_callers():
|
||||
"""``direction=0`` 保留"最短路径"语义（给目标本来就明确在某一侧的调用方）。"""
|
||||
d = ratchet_delta_rad(math.radians(338.9), math.radians(158.9))
|
||||
assert math.degrees(d) == pytest.approx(180.0, abs=1e-6)
|
||||
assert math.degrees(ratchet_delta_rad(math.radians(10.0), math.radians(350.0))) == pytest.approx(20.0)
|
||||
|
||||
|
||||
# =============================================================================
|
||||
# 棘轮步长 vs 关节行程：S08 开头就要判，不能等第 3 轮贴限位
|
||||
# =============================================================================
|
||||
def test_ratchet_travel_rejects_180_on_the_rizon_joint7():
|
||||
"""180° 的步长在 ±175° 的关节 7 上**永远**放不下（实测第 3 轮 ABORT）。
|
||||
|
||||
一个来回要 2×180° = 360°，比行程 350° 还大 —— 无论怎么让余量都腾不出
|
||||
空间，所以这不是"调调就好"，是**配置错**，必须在 S08 开头就拦下来。
|
||||
"""
|
||||
lim = (-3.0543, 3.0543) # Rizon4/10 关节7：±175°
|
||||
v = ratchet_travel_ok(180.0, lim, 8.0)
|
||||
assert not v.ok
|
||||
assert v.need_deg == pytest.approx(376.0)
|
||||
assert v.travel_deg == pytest.approx(350.0, abs=0.1)
|
||||
assert v.max_step_deg == pytest.approx(167.0, abs=0.1)
|
||||
assert "screw_step_deg" in v.why, "报错要直接指出该改哪个键"
|
||||
assert "screw_max_cycles" in v.why, "还要提醒累计必须是整数圈"
|
||||
|
||||
|
||||
def test_ratchet_travel_accepts_the_shipped_step_and_margin():
|
||||
"""出厂值（120°）必须过 —— 否则 S08 一进门就 ABORT，仿真永远跑不完。"""
|
||||
from arm_application.gauge_config import DEFAULTS
|
||||
|
||||
lim = (-3.0543, 3.0543)
|
||||
v = ratchet_travel_ok(DEFAULTS["screw_step_deg"], lim, 8.0)
|
||||
assert v.ok, v.why
|
||||
# 建议值必须落在上限内，否则"照着改"会再撞一次
|
||||
assert v.max_step_deg >= DEFAULTS["screw_step_deg"]
|
||||
|
||||
|
||||
def test_ratchet_travel_passes_when_limits_are_unknown_but_says_so():
|
||||
"""拿不到限位不能猜着拦：放行，理由交给驱动器报 JOINT_LIMIT 兜底。"""
|
||||
v = ratchet_travel_ok(180.0, None, 8.0)
|
||||
assert v.ok
|
||||
assert v.travel_deg == float("inf")
|
||||
assert v.max_step_deg == float("inf")
|
||||
assert v.why == ""
|
||||
|
||||
@@ -0,0 +1,151 @@
|
||||
"""``GripperClient.dropped()``：E1 掉件判据只认**开口塌陷**，不认状态位抖动。
|
||||
|
||||
为什么值得单独钉住（2026-09-24 实测踩到）：
|
||||
仿真里 ``GripperStatus.is_gripped`` 是**推断值**（"指令到了合拢侧 + 实际开口
|
||||
比指令大出容差"）。S08 拧规时会在"合到某个开口 / 全合"之间发指令，只要某一帧
|
||||
的指令落到与实际开口同一档，``is_gripped`` 就翻成 False —— 于是
|
||||
**S08 第 2/12 轮报「通止规掉了」**（爪里其实好好的），整条流程转去等人工
|
||||
"放好通止规"，而仿真里没有这个人。
|
||||
|
||||
判据改成"夹住时的开口 − 当前开口 ≥ 5mm"以后，这种抖动不再报掉件；而真的掉了
|
||||
（两指合到底、开口塌几十毫米）仍然会报。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from typing import Any, List
|
||||
|
||||
from arm_msgs.msg import GripperStatus
|
||||
|
||||
|
||||
class _StubNode:
|
||||
"""``GripperClient`` 只用到这三个方法；测试里不建任何 ROS 实体。"""
|
||||
|
||||
def __init__(self) -> None:
|
||||
self.warnings: List[str] = []
|
||||
|
||||
def create_client(self, srv_type: Any, name: str) -> None:
|
||||
return None
|
||||
|
||||
def create_subscription(self, msg_type: Any, topic: str, cb: Any, qos: Any) -> None:
|
||||
self._cb = cb
|
||||
return None
|
||||
|
||||
def get_logger(self) -> Any:
|
||||
return _StubLogger(self.warnings)
|
||||
|
||||
|
||||
class _StubLogger:
|
||||
def __init__(self, sink: List[str]) -> None:
|
||||
self._sink = sink
|
||||
|
||||
def info(self, msg: str) -> None:
|
||||
self._sink.append(msg)
|
||||
|
||||
def warn(self, msg: str) -> None:
|
||||
self._sink.append(msg)
|
||||
|
||||
def error(self, msg: str) -> None:
|
||||
self._sink.append(msg)
|
||||
|
||||
|
||||
def _status(is_gripped: bool, position: float) -> GripperStatus:
|
||||
msg = GripperStatus()
|
||||
msg.is_gripped = bool(is_gripped)
|
||||
msg.position = float(position)
|
||||
return msg
|
||||
|
||||
|
||||
def _client():
|
||||
from arm_application.gauge_assembly_node import GripperClient
|
||||
|
||||
node = _StubNode()
|
||||
return GripperClient(node, "contract", "/gripper/status"), node
|
||||
|
||||
|
||||
def test_no_drop_before_ever_gripped():
|
||||
"""装配前爪里本来就空：状态位说什么都不算掉件（否则一开场就 E1）。"""
|
||||
client, _ = _client()
|
||||
client._on_contract(_status(False, 0.0))
|
||||
assert client.dropped() is False
|
||||
|
||||
|
||||
def test_a_wide_open_flap_does_not_poison_the_baseline():
|
||||
"""**本次踩到的那个假 E1 的真因**：全开时 is_gripped 也闪 True（实测 99.29mm）。
|
||||
|
||||
以前"第一次判到夹住"就记基准 ⇒ 基准被记成 99.29mm ⇒ 塌陷量算成 77mm ⇒
|
||||
报"通止规掉了"，而规好好地在爪里、真实开口一直是 21.9mm。
|
||||
现在基准只由明确调用 adopt_current_grip 记录：状态位怎么闪都不动它。
|
||||
"""
|
||||
client, _ = _client()
|
||||
client._on_contract(_status(True, 99.29)) # 全开时的假 GRIPPING
|
||||
assert client._grip_aperture is None, "状态位不许写基准"
|
||||
assert client.dropped() is False
|
||||
client._on_contract(_status(True, 21.9)) # 真正夹住规之后
|
||||
client.adopt_current_grip("测试：确认夹住")
|
||||
assert client._grip_aperture == 21.9
|
||||
client._on_contract(_status(False, 21.8))
|
||||
assert client.dropped() is False, "开口没塌就不是掉件"
|
||||
|
||||
|
||||
def test_is_gripped_flap_is_not_a_drop():
|
||||
"""**本次踩到的那个假 E1**：指令切换让 is_gripped 翻 False，开口没塌。"""
|
||||
client, _ = _client()
|
||||
client._on_contract(_status(True, 24.9)) # 夹住 φ30 的规，开口停在 24.9mm
|
||||
client.adopt_current_grip("测试：确认夹住")
|
||||
assert client.dropped() is False
|
||||
client._on_contract(_status(False, 24.8)) # 指令落到同一档 → 推断值翻 False
|
||||
assert client.dropped() is False, "开口没塌就不是掉件"
|
||||
|
||||
|
||||
def test_real_drop_is_detected_by_aperture_collapse():
|
||||
"""真的掉了：两指一路合到底，开口塌到几毫米。"""
|
||||
client, _ = _client()
|
||||
client._on_contract(_status(True, 24.9))
|
||||
client.adopt_current_grip("测试：确认夹住")
|
||||
client._on_contract(_status(False, 0.2))
|
||||
assert client.dropped() is True
|
||||
|
||||
|
||||
def test_aperture_jitter_within_the_threshold_is_not_a_drop():
|
||||
"""实测抖动是亚毫米量级；阈值 5mm 不该被它触发。"""
|
||||
client, _ = _client()
|
||||
client._on_contract(_status(True, 24.9))
|
||||
client.adopt_current_grip("测试：确认夹住")
|
||||
client._on_contract(_status(False, 24.9 - 4.9))
|
||||
assert client.dropped() is False
|
||||
|
||||
|
||||
def test_without_aperture_reading_falls_back_to_the_old_criterion():
|
||||
"""拿不到 position（老固件/别的夹爪）时退回旧判据：会抖，但不至于"永远判不了掉件"。"""
|
||||
client, _ = _client()
|
||||
client._on_contract(_status(True, float("nan")))
|
||||
assert client.adopt_current_grip("测试：确认夹住") is None
|
||||
assert client._grip_aperture is None
|
||||
client._on_contract(_status(False, float("nan")))
|
||||
assert client.dropped() is True, "没有开口读数时不能变成「永远不报掉件」"
|
||||
|
||||
|
||||
def test_settle_waits_for_the_motion_to_finish():
|
||||
"""``settle`` 不能读"动作还没做完"的旧值。
|
||||
|
||||
实测：``close`` 服务返回时手指还在动，立刻读开口读到的是**全开 99.93mm** ——
|
||||
于是"第 1 轮夹紧后：开口 99.93mm（夹住时 99.29mm）"这种荒唐日志。
|
||||
``settle`` 要一直等到连续两帧一样才认。
|
||||
"""
|
||||
client, _ = _client()
|
||||
client._on_contract(_status(True, 99.93))
|
||||
assert client.settle(settle_s=0.2) == 99.93
|
||||
# 动作进行中：值一直在变 ⇒ 不到稳定就不返回（这里只验证它会返回**最终**值）
|
||||
client._on_contract(_status(False, 30.0))
|
||||
assert client.settle(settle_s=0.2) == 30.0
|
||||
|
||||
|
||||
def test_adopt_current_grip_records_what_is_in_the_jaws_now():
|
||||
"""基准 = "确认夹住那一刻"的开口；之后状态位再翻也不改参照。"""
|
||||
client, _ = _client()
|
||||
client._on_contract(_status(True, 21.9))
|
||||
assert client.adopt_current_grip("测试") == 21.9
|
||||
client._on_contract(_status(False, 21.4))
|
||||
assert client.grip_aperture() == 21.9
|
||||
assert client.collapse_mm() == 0.5
|
||||
@@ -0,0 +1,190 @@
|
||||
"""回归：节点类之间的**回调方法重名**必须被拦住。
|
||||
|
||||
为什么值得一条专门的测试（2026-09-24 踩到的真实故障，代价是两轮排查方向全错）：
|
||||
|
||||
Stage2Locate.__init__ 里建了
|
||||
create_subscription(WrenchStamped, ft_wrench_topic, self._on_ft, ...)
|
||||
Stage3Search（子类）里定义了
|
||||
def _on_ft(self, msg): self._ft = (time.monotonic(), msg)
|
||||
|
||||
Python 的方法解析顺序让**子类覆盖父类**，而两个订阅都已经在 __init__ 里
|
||||
建好了。于是那条 WrenchStamped 订阅的回调实际是子类那个 —— 它把消息塞进
|
||||
stage3 的 RobotStates 缓存，而父类的 `_ft_fz` 永远不被赋值。
|
||||
|
||||
现象："S07 腕部 FT 没有数据（话题 /sim/ft_sensor_wrench）"，
|
||||
而在 shell 里 `ros2 topic echo` 那条话题**数据完全正常**、
|
||||
QoS 也完全兼容（实测 BEST_EFFORT 与 RELIABLE 订阅者都收得到）。
|
||||
于是排查方向被引到"话题名写错 / QoS 不匹配 / 桥接没起来"上，全部走偏。
|
||||
|
||||
真机上更阴：两个话题都存在，`self._ft` 里 RobotStates 与 WrenchStamped
|
||||
交替覆盖 → `_ft_mag()` 时有时无 → 腕部力看门狗时灵时不灵（偶发，最难查）。
|
||||
|
||||
规则：**整条继承链上，`_on_*` 形式的方法名必须唯一 —— 除非子类显式
|
||||
``super().<同名>(...)`` 委托回父类**。后者是"扩展"而不是"遮蔽"，是允许的
|
||||
（``GaugeAssembly._on_color`` 就属于这种：先 ``super()._on_color(msg)`` 存帧，
|
||||
再补记"收到时刻"）。判定标准就是有没有那一次委托调用 ——
|
||||
没有委托的同名覆盖，一定会让父类的那份状态**永远不更新**，而且不报任何错。
|
||||
|
||||
命名空间不够用就改名字（`_on_wrench` / `_on_robot_states` 比 `_on_ft` 好，
|
||||
后者里那个 "ft" 到底指腕部六维力还是手指触觉，本来就含糊）——
|
||||
不要靠"反正签名不同"来赌，Python 不检查签名。
|
||||
|
||||
为什么用 AST 读源码而不是 import 后反射：
|
||||
这几个节点模块 import 了 ROS 生成的消息包（``arm_msgs`` 等），
|
||||
在没有 source 过工作空间的 pytest 环境里 import 不进来。
|
||||
而这条规则是**纯源码层面**的，不需要真的构造节点。
|
||||
这也是本仓库既有的做法（见 ``test_gauge_step_events.py``）。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import ast
|
||||
import pathlib
|
||||
from typing import Dict, List
|
||||
|
||||
_APP = pathlib.Path(__file__).resolve().parents[1] / "arm_application"
|
||||
|
||||
#: 继承链上的源文件。
|
||||
_CHAIN_FILES = (
|
||||
"stage2_locate_node.py",
|
||||
"stage3_search_node.py",
|
||||
"pipeline_node.py",
|
||||
"gauge_assembly_node.py",
|
||||
)
|
||||
|
||||
#: 从哪个类开始往上走（装配流程真正实例化的那个）。
|
||||
_LEAF = "GaugeAssembly"
|
||||
|
||||
|
||||
class _Cls:
|
||||
"""一个类的：基类名、本类定义的 `_on_*` 方法（名字 → AST 节点）。"""
|
||||
|
||||
def __init__(self, name: str, bases: List[str], handlers: Dict[str, ast.AST]) -> None:
|
||||
self.name = name
|
||||
self.bases = bases
|
||||
self.handlers = handlers
|
||||
|
||||
|
||||
def _classes_in(path: pathlib.Path) -> Dict[str, _Cls]:
|
||||
tree = ast.parse(path.read_text(encoding="utf-8"))
|
||||
out: Dict[str, _Cls] = {}
|
||||
for node in tree.body:
|
||||
if not isinstance(node, ast.ClassDef):
|
||||
continue
|
||||
bases = [b.id for b in node.bases if isinstance(b, ast.Name)]
|
||||
handlers = {
|
||||
item.name: item
|
||||
for item in node.body
|
||||
if isinstance(item, (ast.FunctionDef, ast.AsyncFunctionDef))
|
||||
and item.name.startswith("_on_")
|
||||
}
|
||||
out[node.name] = _Cls(node.name, bases, handlers)
|
||||
return out
|
||||
|
||||
|
||||
def _table() -> Dict[str, _Cls]:
|
||||
table: Dict[str, _Cls] = {}
|
||||
for name in _CHAIN_FILES:
|
||||
table.update(_classes_in(_APP / name))
|
||||
return table
|
||||
|
||||
|
||||
def _chain() -> List[str]:
|
||||
"""从 ``_LEAF`` 沿基类往上，返回类名列表（子类在前）。"""
|
||||
table = _table()
|
||||
order: List[str] = []
|
||||
cur = _LEAF
|
||||
while cur and cur in table:
|
||||
order.append(cur)
|
||||
bases = table[cur].bases
|
||||
cur = bases[0] if bases else ""
|
||||
assert len(order) >= 3, f"继承链没解析出来（只找到 {order}）—— 改过类名？"
|
||||
return order
|
||||
|
||||
|
||||
def _delegates_to_super(func: ast.AST, name: str) -> bool:
|
||||
"""``func`` 的方法体里有没有 ``super().<name>(...)`` 这一次委托调用。"""
|
||||
for node in ast.walk(func):
|
||||
if not isinstance(node, ast.Call):
|
||||
continue
|
||||
callee = node.func
|
||||
if not (isinstance(callee, ast.Attribute) and callee.attr == name):
|
||||
continue
|
||||
owner = callee.value
|
||||
if (
|
||||
isinstance(owner, ast.Call)
|
||||
and isinstance(owner.func, ast.Name)
|
||||
and owner.func.id == "super"
|
||||
):
|
||||
return True
|
||||
return False
|
||||
|
||||
|
||||
def test_leaf_class_is_reachable_from_every_chain_file():
|
||||
"""先确认这条测试自己在看对文件 —— 文件改名/移动时别静默变成空跑。"""
|
||||
chain = _chain()
|
||||
assert _LEAF in chain
|
||||
for expected in ("Stage2Locate", "Stage3Search", "AssemblyPipeline"):
|
||||
assert expected in chain, f"继承链里没有 {expected}（链 = {chain}）"
|
||||
|
||||
|
||||
def test_no_silent_callback_shadowing_across_inheritance_chain():
|
||||
"""同名 `_on_*` 只允许两种情形：唯一，或子类显式 ``super()`` 委托。
|
||||
|
||||
挂了怎么修（二选一）：
|
||||
a) 在子类那个方法体里加 ``super().<同名>(msg)`` —— 表示"我是扩展"；
|
||||
b) 改掉其中一个的名字（带来源，如 ``_on_wrench`` / ``_on_robot_states``）。
|
||||
别什么都不做：父类那份状态会永远不更新，而**不会有任何报错**。
|
||||
"""
|
||||
table = _table()
|
||||
owner: Dict[str, str] = {}
|
||||
problems: List[str] = []
|
||||
|
||||
# _chain() 是"子类在前"，所以后出现的才是父类。
|
||||
for cls_name in _chain():
|
||||
cls = table.get(cls_name)
|
||||
if cls is None:
|
||||
continue
|
||||
for handler in sorted(cls.handlers):
|
||||
owner.setdefault(handler, cls_name)
|
||||
|
||||
# 再逐个查：凡是有多个类定义了的，子类必须委托。
|
||||
for handler, owner_cls in owner.items():
|
||||
definers = [c for c in _chain() if c in table and handler in table[c].handlers]
|
||||
if len(definers) < 2:
|
||||
continue
|
||||
for cls_name in definers:
|
||||
if cls_name == definers[-1]:
|
||||
continue # 链最靠上的那个（基类）不需要委托
|
||||
cls = table[cls_name]
|
||||
if _delegates_to_super(cls.handlers[handler], handler):
|
||||
continue
|
||||
problems.append(
|
||||
f"{handler}: 定义在 {', '.join(definers)} —— {cls_name} 覆盖了"
|
||||
f"{owner_cls} 的同名回调，但方法体里没有 super().{handler}(...) 委托。\n"
|
||||
f" 实际生效的只有 {cls_name} 那个，父类那份状态永远不会更新。"
|
||||
)
|
||||
|
||||
assert not problems, (
|
||||
"回调被静默遮蔽。症状是「某路数据永远收不到」而话题本身完全正常，"
|
||||
"极难从现象倒推到这里：\n " + "\n ".join(problems)
|
||||
)
|
||||
|
||||
|
||||
def test_wrench_and_robot_states_callbacks_are_not_the_same_method():
|
||||
"""专门钉住这一对名字 —— 它们是最容易被合并成 `_on_ft` 的两个。
|
||||
|
||||
腕部 FT（WrenchStamped，100Hz，法兰系）与整机状态（RobotStates，含外部
|
||||
力/力矩）是两个**不同的**话题、不同的消息类型，缓存也必须是两个字段：
|
||||
``_ft_fz``（轴向力标量）与 ``_ft``（整帧 RobotStates）。
|
||||
"""
|
||||
table = _table()
|
||||
s2 = set(table["Stage2Locate"].handlers)
|
||||
s3 = set(table["Stage3Search"].handlers)
|
||||
|
||||
assert "_on_wrench" in s2, (
|
||||
"Stage2Locate 里腕部 FT 的回调应叫 _on_wrench（不能再叫 _on_ft："
|
||||
"会被 Stage3Search 的同名方法遮蔽）"
|
||||
)
|
||||
assert "_on_robot_states" in s3, "Stage3Search 里整机状态的回调应叫 _on_robot_states"
|
||||
assert not (s2 & s3), f"两个类还有重名的回调: {sorted(s2 & s3)}"
|
||||
@@ -566,6 +566,7 @@ def prepare_urdf(
|
||||
rizon_type: str = "Rizon4",
|
||||
arm_prefix: str = "sim",
|
||||
robot_sn: str = "",
|
||||
wrist_ft_topic: str = "",
|
||||
) -> Tuple[str, NormalizeReport]:
|
||||
"""展开 + 归一化，返回 (URDF 文本, 归一化报告)。
|
||||
|
||||
@@ -576,6 +577,10 @@ def prepare_urdf(
|
||||
（SDF 里只存了插件名和参数文件路径，接口声明不在 SDF 里，实测确认）。
|
||||
所以若 RSP 发的是**未归一化**的那份，控制器注册的关节名就会和 Gazebo 里的
|
||||
对不上，症状是 "Joint 'sim_joint1' not found in the robot model"。
|
||||
|
||||
:param wrist_ft_topic: 腕部 FT 的 gz 话题名。**从 wrist_ft.yaml 传进来**，
|
||||
而不是让 xacro 里再写一份 —— 话题名有三处要对上（配置、URDF、
|
||||
应用层按 sn 推出的名字），两处写死迟早漂移。空 = 用 xacro 的默认值。
|
||||
"""
|
||||
root = repo_root()
|
||||
xacro_path = Path(xacro_path) if xacro_path else default_xacro(root)
|
||||
@@ -590,16 +595,18 @@ def prepare_urdf(
|
||||
f"缺了会导致 Gazebo 报 'Failed to load system plugin'。"
|
||||
)
|
||||
|
||||
urdf = expand_xacro(
|
||||
xacro_path,
|
||||
{
|
||||
"controllers_file": str(Path(controllers_file).resolve()),
|
||||
"load_gripper": "true" if load_gripper else "false",
|
||||
"with_sensors": "true" if with_sensors else "false",
|
||||
"rizon_type": rizon_type,
|
||||
"arm_prefix": arm_prefix,
|
||||
"robot_sn": robot_sn,
|
||||
},
|
||||
)
|
||||
xacro_args = {
|
||||
"controllers_file": str(Path(controllers_file).resolve()),
|
||||
"load_gripper": "true" if load_gripper else "false",
|
||||
"with_sensors": "true" if with_sensors else "false",
|
||||
"rizon_type": rizon_type,
|
||||
"arm_prefix": arm_prefix,
|
||||
"robot_sn": robot_sn,
|
||||
}
|
||||
# 只在给了值时才覆盖，否则保持 xacro 里那个有注释、能单独跑 xacro 的默认值。
|
||||
if wrist_ft_topic:
|
||||
xacro_args["wrist_ft_topic"] = wrist_ft_topic
|
||||
|
||||
urdf = expand_xacro(xacro_path, xacro_args)
|
||||
normalized, report = normalize_urdf(urdf)
|
||||
return normalized, report
|
||||
|
||||
@@ -0,0 +1,329 @@
|
||||
"""仿真"抓取锁"（grasp lock）：让夹爪夹住的物体在仿真里真的不会滑掉。
|
||||
|
||||
=============================================================================
|
||||
它在链路上的位置
|
||||
=============================================================================
|
||||
真机：夹爪 46N 握力 + 指垫摩擦 → 工件被"抱住"，机械臂怎么动都不掉。
|
||||
仿真：夹持力 = 位置误差 × 接触刚度（见 gauge/model.sdf 的 kp 说明）。
|
||||
规站着不动时够用；一旦 S01 绕工具轴快转 150°、或 pre_lift 走 60~100mm
|
||||
的连续轨迹，规就在指间打滑（实测工具长度 0.243m → 0.285m、歪 62°），
|
||||
S07 随后拿到的几何全错 —— 现象是"判不到接触"或"规掉了(E1)"。
|
||||
|
||||
本模块做的唯一一件事：**在夹爪确认夹住时，用 DetachableJoint 插件把物体
|
||||
与手指用一个固定关节锁起来；夹爪张开时解锁。**
|
||||
|
||||
这不是"作弊绕过物理"：刚性接触求解器给不出静摩擦迟滞，也不模拟指垫橡胶
|
||||
的贴合，而真实夹持恰恰靠这两件事。锁补的就是这一块。物体与指垫之间的
|
||||
**接触对仍然存在**，所以触觉、腕部 FT 的读数照旧是真实的接触力。
|
||||
|
||||
=============================================================================
|
||||
为什么用 `ign topic` 命令行而不是 ros_gz_bridge
|
||||
=============================================================================
|
||||
触发消息是 `ignition.msgs.Empty`。bridging 一个"空消息"要走
|
||||
ros_gz_bridge 的 Empty 映射，而这条链路上**没有任何东西需要它出现在 ROS
|
||||
侧** —— 唯一的目的就是把 gz 的那个插件叫一下。多接一个 bridge 就多一个
|
||||
"配错了不报错"的点（bridge 类型名对不上时它只是安静地什么也不桥）。
|
||||
|
||||
代价是每次触发起一个短命进程（几十毫秒）。触发频率是"每次夹/放一次"，
|
||||
不是每个控制周期 —— 这个代价可以忽略。
|
||||
|
||||
=============================================================================
|
||||
静默失效的四个点（改配置时全都要对上）
|
||||
=============================================================================
|
||||
① <child_model> = 机器人 spawn 的 -name（launch 里 rizon_sim）
|
||||
② <child_link> = Gazebo 里**真实存在**的连杆名（URDF 里与固定关节相连的
|
||||
连杆会被 sdformat 合并，手指只剩 sim_left_finger_mount）
|
||||
③ attach/detach 话题名与 gauge/model.sdf 里的插件一致
|
||||
④ 插件确实写进了 gauge 的 SDF
|
||||
|
||||
①②③④ 全都不会自己报错：锁不触发时，现象只是"规莫名掉件"。
|
||||
所以 :func:`check_grasp_lock_sdf` 把它变成启动时的一条告警/报错。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import subprocess
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
from typing import List, Optional
|
||||
|
||||
import yaml
|
||||
|
||||
#: 相对仓库根的默认配置路径。
|
||||
DEFAULT_GRASP_LOCK_REL = "devices/sim/gazebo/config/grasp_lock.yaml"
|
||||
|
||||
#: 被 lock 的物体模型 SDF 的默认位置（用来做静态核对）。
|
||||
DEFAULT_OBJECT_SDF_TMPL = "devices/sim/gazebo/models/{object}/model.sdf"
|
||||
|
||||
|
||||
class GraspLockError(RuntimeError):
|
||||
"""抓取锁配置缺失或写错。消息里必须给出修法（现场就靠这句话）。"""
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class GraspLockSpec:
|
||||
"""抓取锁的一份完整描述。"""
|
||||
|
||||
enabled: bool
|
||||
object: str
|
||||
object_link: str
|
||||
robot_model: str
|
||||
robot_link: str
|
||||
attach_topic: str
|
||||
detach_topic: str
|
||||
state_topic: str
|
||||
publish_timeout_ms: int
|
||||
|
||||
def describe(self) -> str:
|
||||
"""给启动日志用的一句话。"""
|
||||
if not self.enabled:
|
||||
return "抓取锁：关闭（仿真靠摩擦夹持，机械臂快速运动时可能滑件）"
|
||||
return (
|
||||
f"抓取锁：开 → {self.object}.{self.object_link} 锁到 "
|
||||
f"{self.robot_model}.{self.robot_link}"
|
||||
f"（attach={self.attach_topic} detach={self.detach_topic}）"
|
||||
)
|
||||
|
||||
|
||||
def load_grasp_lock(
|
||||
path: Optional[Path] = None, root: Optional[Path] = None
|
||||
) -> GraspLockSpec:
|
||||
"""读抓取锁配置。找不到就返回一份"关闭"的默认值（仿真仍能跑）。
|
||||
|
||||
**为什么文件缺失不报错**：抓取锁是仿真里的**补救措施**，不是必需品 ——
|
||||
关掉它只是回到"靠摩擦夹持"，多数轻动作仍然能跑。让仿真因为一个可选补救
|
||||
措施起不来，是拿主流程给可选项陪葬。
|
||||
但**文件存在而字段写错**时必须报错：那说明有人动过它、而且写错了。
|
||||
"""
|
||||
if path is None:
|
||||
base = root if root is not None else Path.cwd()
|
||||
path = Path(base) / DEFAULT_GRASP_LOCK_REL
|
||||
path = Path(path)
|
||||
|
||||
if not path.is_file():
|
||||
return GraspLockSpec(
|
||||
enabled=False,
|
||||
object="gauge",
|
||||
object_link="body",
|
||||
robot_model="",
|
||||
robot_link="",
|
||||
attach_topic="/sim/grasp_lock/attach",
|
||||
detach_topic="/sim/grasp_lock/detach",
|
||||
state_topic="/sim/grasp_lock/state",
|
||||
publish_timeout_ms=3000,
|
||||
)
|
||||
|
||||
try:
|
||||
doc = yaml.safe_load(path.read_text(encoding="utf-8")) or {}
|
||||
except yaml.YAMLError as exc:
|
||||
raise GraspLockError(
|
||||
f"抓取锁配置 YAML 解析失败: {path}\n 原因: {exc}"
|
||||
) from exc
|
||||
|
||||
if not isinstance(doc, dict):
|
||||
raise GraspLockError(f"{path} 的顶层应该是映射（key: value 形式）。")
|
||||
|
||||
enabled = bool(doc.get("enabled", True))
|
||||
|
||||
def _text(key: str, default: str = "") -> str:
|
||||
return str(doc.get(key, default) or "").strip()
|
||||
|
||||
spec = GraspLockSpec(
|
||||
enabled=enabled,
|
||||
object=_text("object", "gauge") or "gauge",
|
||||
object_link=_text("object_link", "body") or "body",
|
||||
robot_model=_text("robot_model"),
|
||||
robot_link=_text("robot_link"),
|
||||
attach_topic=_text("attach_topic", "/sim/grasp_lock/attach"),
|
||||
detach_topic=_text("detach_topic", "/sim/grasp_lock/detach"),
|
||||
state_topic=_text("state_topic", "/sim/grasp_lock/state"),
|
||||
publish_timeout_ms=int(doc.get("publish_timeout_ms", 3000)),
|
||||
)
|
||||
|
||||
if not enabled:
|
||||
return spec
|
||||
|
||||
missing = [
|
||||
k
|
||||
for k in ("robot_model", "robot_link", "attach_topic", "detach_topic")
|
||||
if not getattr(spec, k)
|
||||
]
|
||||
if missing:
|
||||
raise GraspLockError(
|
||||
f"{path} 开着（enabled: true）但缺字段: {', '.join(missing)}。\n"
|
||||
f" robot_model 必须等于 launch 里 spawn 机器人的 -name（当前 rizon_sim）；\n"
|
||||
f" robot_link 必须是 Gazebo 里的**真名**（与固定关节相连的连杆会被\n"
|
||||
f" sdformat 合并，手指只剩 sim_left_finger_mount）。真名查法：\n"
|
||||
f" ros2 topic echo /sim/world_pose_info ← 看全部 child_frame_id\n"
|
||||
f" 这两个名字写错的后果是**静默失效**：锁不触发、规照样滑掉，\n"
|
||||
f" 而日志里只看得到「规掉了」。"
|
||||
)
|
||||
if spec.attach_topic == spec.detach_topic:
|
||||
raise GraspLockError(
|
||||
f"{path} 的 attach_topic 与 detach_topic 相同（{spec.attach_topic}）——\n"
|
||||
f" 那样夹住和放开会走同一条消息，锁的状态完全随机。两个必须不同。"
|
||||
)
|
||||
# 锚点不能是"会跟着张爪动的"那根手指 —— 这是 2026-09-24 用一条假 E1 换来的。
|
||||
if "finger" in spec.robot_link.lower():
|
||||
raise GraspLockError(
|
||||
f"{path} 的 robot_link 是 {spec.robot_link} —— **手指**。\n"
|
||||
f" 被锁的工件会被刚性固定在手指上，而手指会随指令张/合：\n"
|
||||
f" · S08 棘轮每轮都要\"松爪 → 回转 → 夹紧\"，张爪时手指外移几十毫米，\n"
|
||||
f" 而工件（规）还插在孔里 —— 两处约束打架，物理解算会把工件\n"
|
||||
f" **从孔里拽出来**（实测：规尖跳 +17.5mm、偏摆 64°），下一步掉地。\n"
|
||||
f" 日志里只看得到「通止规掉了(E1)」，看不出真因。\n"
|
||||
f" 修法：填**不跟着张爪动**的连杆。仿真是法兰 `sim_link7`\n"
|
||||
f" （夹爪本体 sim_grav_base_link 在 sdformat 里被合并进它了）。\n"
|
||||
f" 真名查法：ros2 topic echo /sim/world_pose_info ← 列全部 child_frame_id"
|
||||
)
|
||||
if spec.publish_timeout_ms <= 0:
|
||||
raise GraspLockError(
|
||||
f"{path} 的 publish_timeout_ms 必须为正，收到 {spec.publish_timeout_ms}"
|
||||
)
|
||||
return spec
|
||||
|
||||
|
||||
def bridge_target_links(spec: GraspLockSpec) -> List[str]:
|
||||
"""把"锁的两端"整理成一条便于核对/记录的清单（给日志与自检用）。"""
|
||||
return [f"{spec.object}.{spec.object_link}", f"{spec.robot_model}.{spec.robot_link}"]
|
||||
|
||||
|
||||
def check_grasp_lock_sdf(
|
||||
sdf_text: str, spec: GraspLockSpec, sdf_label: str = ""
|
||||
) -> List[str]:
|
||||
"""静态核对：物体 SDF 里的 DetachableJoint 插件是否与本配置一致。
|
||||
|
||||
为什么值得一查（与腕部 FT 的 check 同一个理由）：
|
||||
插件参数写错时 Gazebo **什么都不说** —— 插件照样加载（只要 child_model
|
||||
找得到），assess 话题照样发，只是关节连不上。现象只有一个：
|
||||
"规莫名其妙掉了"，而那要等到 S07/S08 才显形、且看不到真因。
|
||||
这里在**启动时**把它变成一句话。
|
||||
|
||||
返回告警列表（空 = 没问题）；enabled=false 时永远返回空。
|
||||
"""
|
||||
if not spec.enabled:
|
||||
return []
|
||||
where = f"（{sdf_label}）" if sdf_label else ""
|
||||
warnings: List[str] = []
|
||||
if not sdf_text:
|
||||
return [f"读不到物体 SDF{where}，无法核对抓取锁插件"]
|
||||
|
||||
if "DetachableJoint" not in sdf_text:
|
||||
warnings.append(
|
||||
f"物体 SDF{where} 里没有 DetachableJoint 插件 —— 抓取锁不会生效。\n"
|
||||
f" 配置说 enabled: true，但模型里没有插件时**不会报错**，\n"
|
||||
f" 只是夹住的物体依旧会滑（现象＝「规莫名掉了」，看不到真因）。\n"
|
||||
f" 修法：把插件块补进 {spec.object}/model.sdf，参数照 grasp_lock.yaml。"
|
||||
)
|
||||
return warnings
|
||||
|
||||
if spec.robot_model and f"<child_model>{spec.robot_model}</child_model>" not in sdf_text:
|
||||
warnings.append(
|
||||
f"插件里的 <child_model> 不是 {spec.robot_model} —— "
|
||||
f"它必须等于 launch 里 spawn 机器人的 -name。\n"
|
||||
f" 对不上时 DetachableJoint 在 Configure 阶段直接失败，"
|
||||
f"插件被卸载、{spec.state_topic} 也不会出现（这是唯一的线索）。"
|
||||
)
|
||||
|
||||
if spec.robot_link and f"<child_link>{spec.robot_link}</child_link>" not in sdf_text:
|
||||
warnings.append(
|
||||
f"插件里的 <child_link> 不是 {spec.robot_link}。\n"
|
||||
f" 注意这里要填 Gazebo 的**真名**：URDF 里与固定关节相连的连杆会被\n"
|
||||
f" sdformat 合并（lump），例如手指只剩 sim_left_finger_mount。\n"
|
||||
f" 名字写错时插件照样加载、attach 照样「成功」，但关节连到不存在的\n"
|
||||
f" 连杆上 → 锁不生效。真名查法：ros2 topic echo /sim/world_pose_info"
|
||||
)
|
||||
|
||||
for topic, key in ((spec.attach_topic, "attach_topic"), (spec.detach_topic, "detach_topic")):
|
||||
if topic and f"<{key}>{topic}</{key}>" not in sdf_text:
|
||||
warnings.append(
|
||||
f"插件里的 <{key}> 不是 {topic}。\n"
|
||||
f" 两侧话题名不一致时，锁的触发消息发到一个没人听的地址上 ——\n"
|
||||
f" 不报错，只是永远不锁。"
|
||||
)
|
||||
|
||||
return warnings
|
||||
|
||||
|
||||
def _ign_topic_cmd(exe: str, topic: str, timeout_ms: int) -> List[str]:
|
||||
"""拼一条 `ign topic` 发布命令（空消息）。
|
||||
|
||||
⚠️ ``-d``（duration 秒）不是可选的装饰 —— 少了它**大概率丢消息**：
|
||||
CLI 建立 publisher 与网络里发现订阅者需要一点时间，一条 fire-and-forget
|
||||
的消息在发现完成前就发完了。曾经写成 ``-w <秒>``（以为它等订阅者），
|
||||
而这个版本的 ``ign topic`` **没有 -w** —— 参数被当成多余的位置参数：
|
||||
"The following arguments were not expected: 3"
|
||||
于是"上锁/解锁"永远失败，而现象和"锁没生效"完全一样（规照样掉）。
|
||||
"""
|
||||
return [
|
||||
exe,
|
||||
"topic",
|
||||
"-t", topic,
|
||||
"-m", "ignition.msgs.Empty",
|
||||
"-p", "",
|
||||
"-d", f"{max(0.5, timeout_ms / 1000.0):.2f}",
|
||||
]
|
||||
|
||||
|
||||
def _find_ign_exe(explicit: Optional[str] = None) -> Optional[str]:
|
||||
"""找到 ign / gz 命令行。找不到返回 None（调用方决定是告警还是忽略）。"""
|
||||
import shutil
|
||||
|
||||
if explicit:
|
||||
return explicit
|
||||
for name in ("ign", "gz"):
|
||||
found = shutil.which(name)
|
||||
if found:
|
||||
return found
|
||||
return None
|
||||
|
||||
|
||||
def publish_lock_event(
|
||||
spec: GraspLockSpec,
|
||||
attach: bool,
|
||||
exe: Optional[str] = None,
|
||||
logger=None,
|
||||
) -> bool:
|
||||
"""发一条"上锁/解锁"消息。返回是否成功（失败只记日志，不抛）。
|
||||
|
||||
**为什么不抛异常**：这不是主流程的一步 —— 它是仿真里的补救措施。
|
||||
发不出去时正确行为是"继续跑，但明确告警"，而不是把整条装配流程打断：
|
||||
现场宁可看到"锁没生效、规可能滑"的提示，也不要看到"因为锁发不出去，
|
||||
装配根本没做"。
|
||||
"""
|
||||
if not spec.enabled:
|
||||
return False
|
||||
topic = spec.attach_topic if attach else spec.detach_topic
|
||||
what = "上锁" if attach else "解锁"
|
||||
exe_path = _find_ign_exe(exe)
|
||||
if exe_path is None:
|
||||
if logger is not None:
|
||||
logger.warn(
|
||||
f"抓取锁{what}失败：找不到 ign/gz 命令行（PATH 里没有）。\n"
|
||||
f" 后果：夹住的物体在机械臂连续运动时可能滑掉（现象＝「规掉了」）。"
|
||||
)
|
||||
return False
|
||||
|
||||
cmd = _ign_topic_cmd(exe_path, topic, spec.publish_timeout_ms)
|
||||
try:
|
||||
proc = subprocess.run(
|
||||
cmd,
|
||||
stdout=subprocess.PIPE,
|
||||
stderr=subprocess.STDOUT,
|
||||
timeout=max(2.0, spec.publish_timeout_ms / 1000.0 + 2.0),
|
||||
)
|
||||
except (OSError, subprocess.SubprocessError) as exc:
|
||||
if logger is not None:
|
||||
logger.warn(f"抓取锁{what}失败（{topic}）：{exc}")
|
||||
return False
|
||||
|
||||
if proc.returncode != 0:
|
||||
out = (proc.stdout or b"").decode("utf-8", "replace").strip()
|
||||
if logger is not None:
|
||||
logger.warn(
|
||||
f"抓取锁{what}失败（{topic}，退出码 {proc.returncode}）：{out[:200]}"
|
||||
)
|
||||
return False
|
||||
if logger is not None:
|
||||
logger.info(f"抓取锁{what}：{topic}")
|
||||
return True
|
||||
@@ -63,6 +63,14 @@ RViz/TF 与 Gazebo 就一致（详见 urdf_normalize.py 里那段说明）。
|
||||
- 但**反馈绝不能取它**，否则 status 永远报"开口 0 mm"而夹爪明明开着。
|
||||
适配器改用"与主关节直接相连、|multiplier| 最大的那个从动关节"反推开口
|
||||
（见 GripperLayout._pick_feedback / aperture_from_feedback）。
|
||||
|
||||
5) **仿真里的夹持靠不住，需要一把"抓取锁"**。仿真的夹持力不是设出来的，
|
||||
而是位置误差 × 接触刚度：规站着不动够用，机械臂一走连续轨迹（S01 快转
|
||||
150°、pre_lift 60~100mm）就在指间打滑 —— 实测工具长度 0.243m → 0.285m、
|
||||
规歪 62°，S07 随后拿到的几何全错。真机不会（46N 握力 + µ≈0.8 + 0.27kg）。
|
||||
所以本节点在**确认夹住之后**用 DetachableJoint 把物体固定到手指上，
|
||||
**指令要求张开时**解锁（详见 sim_grasp_lock.py；为什么不拿 is_gripped
|
||||
当解锁条件，见 :meth:`_sync_grasp_lock`）。
|
||||
"""
|
||||
from __future__ import annotations
|
||||
|
||||
@@ -77,6 +85,7 @@ from arm_msgs.msg import GripperStatus
|
||||
from arm_msgs.srv import GripperExecute
|
||||
from sensor_msgs.msg import JointState
|
||||
|
||||
from .sim_grasp_lock import GraspLockSpec, publish_lock_event
|
||||
from .sim_gripper import (
|
||||
DEFAULT_CONTROLLER,
|
||||
DEFAULT_CONTROLLERS_REL,
|
||||
@@ -142,6 +151,13 @@ class SimGripperAdapter(Node):
|
||||
self.declare_parameter("tolerance_mm", DEFAULT_GRASP_TOLERANCE_MM) # 夹住判据容差（毫米开口）
|
||||
self.declare_parameter("default_speed_pct", 50.0)
|
||||
self.declare_parameter("default_force_pct", 20.0)
|
||||
# 仿真抓取锁（见 sim_grasp_lock.py）。默认**关**：参数化之后，launch 才是
|
||||
# 唯一决定它开不开的地方 —— 单跑本节点做调试时不该莫名其妙去动 Gazebo。
|
||||
self.declare_parameter("grasp_lock_enabled", False)
|
||||
self.declare_parameter("grasp_lock_object", "gauge")
|
||||
self.declare_parameter("grasp_lock_attach_topic", "/sim/grasp_lock/attach")
|
||||
self.declare_parameter("grasp_lock_detach_topic", "/sim/grasp_lock/detach")
|
||||
self.declare_parameter("grasp_lock_timeout_ms", 3000)
|
||||
|
||||
controller = str(self.get_parameter("controller").value or DEFAULT_CONTROLLER)
|
||||
raw_ctrl = str(self.get_parameter("controllers_file").value or "")
|
||||
@@ -214,6 +230,34 @@ class SimGripperAdapter(Node):
|
||||
|
||||
# 先把夹爪摆到默认全开，免得机器人一出场就是捏着的状态。
|
||||
self._publish_target(self._target)
|
||||
|
||||
# 抓取锁：规格从参数来（launch 从 grasp_lock.yaml 派生），状态用三态：
|
||||
# None = 不知道 / False = 已解锁 / True = 已上锁
|
||||
# 之所以要 None 而不是 bool + 计数器：发布失败时要能**重试**，
|
||||
# 而"发布失败"恰恰表现为"发出去没人听"（话题名错、进程起不来），
|
||||
# 只有把"未确认"单独表示出来才可能重试。
|
||||
self._lock = GraspLockSpec(
|
||||
enabled=bool(self.get_parameter("grasp_lock_enabled").value),
|
||||
object=str(self.get_parameter("grasp_lock_object").value or "gauge"),
|
||||
object_link="",
|
||||
robot_model="",
|
||||
robot_link="",
|
||||
attach_topic=str(self.get_parameter("grasp_lock_attach_topic").value or ""),
|
||||
detach_topic=str(self.get_parameter("grasp_lock_detach_topic").value or ""),
|
||||
state_topic="",
|
||||
publish_timeout_ms=int(self.get_parameter("grasp_lock_timeout_ms").value),
|
||||
)
|
||||
self._locked: Optional[bool] = None
|
||||
if self._lock.enabled:
|
||||
self.get_logger().info(
|
||||
f"抓取锁已启用：夹住后把 {self._lock.object} 用固定关节锁到手指上，"
|
||||
f"张开时解锁（attach={self._lock.attach_topic}）。\n"
|
||||
" 它补的是 Gazebo 缺的那块物理（刚性接触没有静摩擦迟滞、也不模拟"
|
||||
"指垫贴合）——\n"
|
||||
" 真机 46N 握力 + µ≈0.8 本来就夹得住，仿真的位置控制夹持则会在"
|
||||
"连续运动时打滑。"
|
||||
)
|
||||
|
||||
self.create_timer(0.1, self._tick_status)
|
||||
|
||||
# --- 参数与描述 -------------------------------------------------------
|
||||
@@ -311,7 +355,43 @@ class SimGripperAdapter(Node):
|
||||
return msg
|
||||
|
||||
def _tick_status(self) -> None:
|
||||
self._pub_status.publish(self._status_msg())
|
||||
msg = self._status_msg()
|
||||
self._pub_status.publish(msg)
|
||||
self._sync_grasp_lock(bool(msg.is_gripped))
|
||||
|
||||
# --- 抓取锁（仿真专用）-------------------------------------------------
|
||||
def _sync_grasp_lock(self, gripped: bool) -> None:
|
||||
"""夹持状态 → 抓取锁。只在"确认夹住"时上锁，绝不自动解锁。
|
||||
|
||||
**为什么不在这里因为"判到没夹住"而解锁**：
|
||||
本适配器的 ``is_gripped`` 是**推断值**（指令到合拢位 + 实际开口被顶住），
|
||||
抓取锁要解决的恰恰是"规在指间打了滑"的情形 —— 打滑会让触觉接触
|
||||
闪断、``is_gripped`` 跟着抖。若拿它当解锁条件，就会得到
|
||||
"一抖就解锁 → 规真掉下去 → 更读不到夹持"的自毁循环。
|
||||
解锁只认**指令**（open / 开口变大的 move），那才是松手的真实原因。
|
||||
"""
|
||||
spec = self._lock
|
||||
if not spec.enabled or not gripped or self._locked is True:
|
||||
return
|
||||
if publish_lock_event(spec, True, logger=self.get_logger()):
|
||||
self._locked = True
|
||||
self.get_logger().info(
|
||||
f"抓取锁：已把 {spec.object} 锁到手指上（此后机械臂怎么动都不会滑）。"
|
||||
)
|
||||
else:
|
||||
# 失败不抛异常（锁只是补救措施），但要留痕：现象是"规掉了"而看不到真因。
|
||||
self._locked = None
|
||||
|
||||
def _release_grasp_lock(self, why: str) -> None:
|
||||
"""松开抓取锁。只在**指令明确要张开**时调用。"""
|
||||
spec = self._lock
|
||||
if not spec.enabled or self._locked is False:
|
||||
return
|
||||
if publish_lock_event(spec, False, logger=self.get_logger()):
|
||||
self._locked = False
|
||||
self.get_logger().info(f"抓取锁：已解锁（{why}）")
|
||||
else:
|
||||
self._locked = None
|
||||
|
||||
# --- 服务 -------------------------------------------------------------
|
||||
def _make_handler(self, verb: str):
|
||||
@@ -346,6 +426,7 @@ class SimGripperAdapter(Node):
|
||||
cmd = request.command
|
||||
try:
|
||||
if verb == "open":
|
||||
self._release_grasp_lock("收到 open 指令")
|
||||
self._move_to_aperture(self.layout.stroke_mm)
|
||||
note = f"全开（{self.layout.stroke_mm:.1f} mm）"
|
||||
elif verb == "close":
|
||||
@@ -354,6 +435,16 @@ class SimGripperAdapter(Node):
|
||||
elif verb == "move":
|
||||
want = float(cmd.position)
|
||||
clamped = self.layout.is_clamped(want)
|
||||
# 「开口变大」才叫松手。判据用**实测开口**而不是上一次指令：
|
||||
# 指令可以是"先夹到 30 再挪到 32"这种，比指令会误判成开爪。
|
||||
state = self._fresh_state()
|
||||
now_mm = (
|
||||
self.layout.aperture_from_feedback(state)
|
||||
if state is not None
|
||||
else None
|
||||
)
|
||||
if now_mm is not None and want > now_mm + self._tolerance_mm:
|
||||
self._release_grasp_lock(f"开口 {now_mm:.1f} → {want:.1f} mm")
|
||||
self._move_to_aperture(want)
|
||||
note = f"开口 → {want:.2f} mm"
|
||||
if clamped:
|
||||
|
||||
@@ -0,0 +1,159 @@
|
||||
#!/usr/bin/env python3
|
||||
"""仿真里"物体**现在到底在哪**"的唯一来源：``/world/<world>/pose/info``。
|
||||
|
||||
为什么要有这个模块（2026-09-24 的教训，别再让真值按名义位姿算）
|
||||
----------------------------------------------------------------
|
||||
真值路径（``perception.source=truth``）最初是**按 scene.yaml 的名义位姿**算目标点
|
||||
与孔位的 —— 也就是它假设"物体没动过"。实测这个假设站不住：
|
||||
|
||||
1. 物体是"在名义位姿上方 spawn、再落下来"的，**落地就偏 2~3mm**（实测 2.6mm）；
|
||||
2. 工件骑在台面那 36mm 通槽上，静置中还会缓慢蠕动（实测十几分钟里又挪了 1.9mm）。
|
||||
|
||||
而 2mm 的位姿误差在 S05 的像素上约 5px，在 S06 对孔上就是**规尖（φ6）进不了
|
||||
φ7 的孔** —— 现象是"S07 压到工件面、但孔没进去"，接着 S08 一转就掉件。
|
||||
现场看起来像"柔顺参数不对/夹持力不够"，其实几何从一开始就指错了两毫米。
|
||||
|
||||
所以：**真值就该用实测位姿**，名义位姿只用来做"物体是不是被搬走了/掉下去了"的
|
||||
告警。这件事被两个地方需要（真值感知的目标点、二阶段定位的孔位），
|
||||
所以实现只放这一份（R5.8）。
|
||||
|
||||
⚠️ 实测位姿是 Gazebo 的**真值**（不是测量），所以"按实测位姿算"比"按配置算"
|
||||
更接近真机上的视觉：真机也是量到哪就是哪。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
from dataclasses import replace
|
||||
from typing import Any, Dict, Iterable, Mapping, Optional, Tuple
|
||||
|
||||
from arm_sim_gazebo import sim_scene
|
||||
|
||||
#: 位姿（名 → ``(x, y, z, yaw)``，单位米/弧度）。缺的物体不在字典里。
|
||||
LivePoses = Dict[str, Tuple[float, float, float, float]]
|
||||
|
||||
|
||||
def yaw_from_quat_xyzw(quat: Iterable[float]) -> float:
|
||||
"""四元数 ``(x, y, z, w)`` → 绕 Z 的 yaw [rad]。
|
||||
|
||||
四元数→矩阵**不在这里实现**：复用全仓库唯一那份 ``handeye_lib.quat_xyzw_to_R``
|
||||
（它有回环测试钉着）。重复实现的下场见 ``sim_truth_perception`` 的模块 docstring。
|
||||
"""
|
||||
from arm_application.handeye_lib import quat_xyzw_to_R
|
||||
|
||||
R = quat_xyzw_to_R(list(quat))
|
||||
return math.atan2(float(R[1, 0]), float(R[0, 0]))
|
||||
|
||||
|
||||
def poses_from_tf(msg: Any) -> LivePoses:
|
||||
"""``/world/<world>/pose/info``（``tf2_msgs/TFMessage``）→ 名 → ``(x,y,z,yaw)``。
|
||||
|
||||
每个模型的 ``child_frame_id`` 就是模型名（ros_gz_bridge 的约定）。
|
||||
|
||||
**纯函数**（只要求对象有 ``transforms``，不依赖 ROS 类型）：这条解析错的表现是
|
||||
"实测位姿永远是空的"，于是真值退回名义位姿 —— 又是那个两毫米的错，
|
||||
没有任何一处日志会提。所以它必须能脱离节点被钉住。
|
||||
"""
|
||||
out: LivePoses = {}
|
||||
for tr in getattr(msg, "transforms", None) or []:
|
||||
name = str(getattr(tr, "child_frame_id", "") or "").strip().lstrip("/")
|
||||
if not name:
|
||||
continue
|
||||
t = tr.transform.translation
|
||||
q = tr.transform.rotation
|
||||
out[name] = (
|
||||
float(t.x),
|
||||
float(t.y),
|
||||
float(t.z),
|
||||
yaw_from_quat_xyzw((float(q.x), float(q.y), float(q.z), float(q.w))),
|
||||
)
|
||||
return out
|
||||
|
||||
|
||||
def spec_with_live_poses(
|
||||
spec: sim_scene.SceneSpec,
|
||||
poses: Mapping[str, Tuple[float, float, float, float]],
|
||||
names: Optional[Iterable[str]] = None,
|
||||
) -> sim_scene.SceneSpec:
|
||||
"""把场景**副本**里物体的位姿换成实测值；查不到实测位姿的物体保持原样。
|
||||
|
||||
为什么只换位姿、不换别的：``SceneSpec`` 里的几何（模型形状、孔环位置、
|
||||
台面）都是配置里的事实，只有"物体摆在哪"是会变的。
|
||||
|
||||
``names`` 给定时只换这几个（真值只依赖工件，没必要动别的）。
|
||||
返回**副本**（``dataclasses.replace``）：调用方别把"实测位姿"写回配置，
|
||||
否则名义位姿就永远消失了，而它正是判断"有没有被搬走"的基准。
|
||||
"""
|
||||
want = None if names is None else {str(n) for n in names}
|
||||
out = []
|
||||
changed = 0
|
||||
for obj in spec.objects:
|
||||
p = poses.get(obj.name)
|
||||
if p is None or (want is not None and obj.name not in want):
|
||||
out.append(obj)
|
||||
continue
|
||||
out.append(replace(obj, x=float(p[0]), y=float(p[1]), z=float(p[2]), yaw=float(p[3])))
|
||||
changed += 1
|
||||
if changed == 0:
|
||||
return spec
|
||||
return replace(spec, objects=out)
|
||||
|
||||
|
||||
class LiveScenePose:
|
||||
"""订阅世界模型位姿，随时给出"物体现在在哪"。仿真专有。
|
||||
|
||||
用法（两个节点都一样，所以放在这里而不是各写一份）::
|
||||
|
||||
live = LiveScenePose(self, topic) # topic 来自 scene.yaml 的 pose_info
|
||||
spec_live = live.apply(spec, [obj_name]) # 换上位姿的副本
|
||||
if live.ready: ... # 收到过帧没有
|
||||
|
||||
没配话题 / 没收到帧时 ``apply`` 原样返回 —— 调用方仍能用名义位姿干活，
|
||||
但必须**说清楚**（``LiveScenePose.why_not``），否则"退回名义位姿"这件事
|
||||
又会变成没有人知道的静默行为。
|
||||
"""
|
||||
|
||||
def __init__(self, node: Any, topic: str, qos_depth: int = 10) -> None:
|
||||
self._node = node
|
||||
self._topic = str(topic or "").strip()
|
||||
self._poses: LivePoses = {}
|
||||
self._seen = 0
|
||||
self._sub = None
|
||||
if not self._topic:
|
||||
return
|
||||
from tf2_msgs.msg import TFMessage
|
||||
|
||||
self._sub = node.create_subscription(TFMessage, self._topic, self._on_msg, qos_depth)
|
||||
|
||||
def _on_msg(self, msg: Any) -> None:
|
||||
got = poses_from_tf(msg)
|
||||
if got:
|
||||
self._seen += 1
|
||||
self._poses.update(got)
|
||||
|
||||
@property
|
||||
def topic(self) -> str:
|
||||
return self._topic
|
||||
|
||||
@property
|
||||
def ready(self) -> bool:
|
||||
"""收到过至少一帧（一帧都没有说明话题名/桥接有问题，不是"物体没动"）。"""
|
||||
return self._seen > 0
|
||||
|
||||
def pose(self, name: str) -> Optional[Tuple[float, float, float, float]]:
|
||||
return self._poses.get(name)
|
||||
|
||||
def apply(
|
||||
self,
|
||||
spec: sim_scene.SceneSpec,
|
||||
names: Optional[Iterable[str]] = None,
|
||||
) -> sim_scene.SceneSpec:
|
||||
return spec_with_live_poses(spec, self._poses, names)
|
||||
|
||||
def why_not(self, name: str) -> str:
|
||||
"""给"为什么还在用名义位姿"一个能直接贴进日志的句子。"""
|
||||
if not self._topic:
|
||||
return "scene.yaml 的 pose_info 未配（没有世界模型位姿话题）"
|
||||
if not self.ready:
|
||||
return f"话题 {self._topic} 一帧都没收到（桥接没起来？话题名/世界名对不上？）"
|
||||
return f"实测位姿里没有物体 {name!r}（模型名与 scene.yaml 的 name 不一致？）"
|
||||
@@ -0,0 +1,111 @@
|
||||
#!/usr/bin/env python3
|
||||
"""把 Gazebo 里的模型**搬到指定位姿**（`/world/<world>/set_pose`）。
|
||||
|
||||
谁在用
|
||||
------
|
||||
* ``scripts/sim_load_gauge.py``：加载通止规前把场景物体复位；
|
||||
* ``arm_sim_gazebo/sim_truth_perception.py``：启动时把真值依赖的工件复位
|
||||
（见那里的 `_maybe_startup_reset`）。
|
||||
|
||||
**实现只有这一份**：这个请求字符串以前抄过一遍，而它有一个很坑的细节
|
||||
（下面 `--reqtype` 的注释），第二份抄错的人不会知道。
|
||||
|
||||
为什么走命令行 `ign service` 而不是 ROS 服务桥
|
||||
--------------------------------------------
|
||||
仿真的场景加载本来就是用命令行 `ros_gz_sim create` 做的（见 `sim_scene.py`），
|
||||
口径一致、少一个需要维护的服务桥；而且 `ros_gz_bridge` 对**服务**的桥接需要
|
||||
额外配置，桥不上时的表现是"服务一直在、调用永远超时"。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
import re
|
||||
import subprocess
|
||||
from typing import Optional, Sequence, Tuple
|
||||
|
||||
#: Gazebo 侧的服务等待超时 [ms]。
|
||||
SERVICE_TIMEOUT_MS = 3000
|
||||
#: 命令行整体超时 [s]（要大于上面那个，否则会被外面先掐掉）。
|
||||
CMD_TIMEOUT_S = 10.0
|
||||
|
||||
|
||||
def world_from_gz_topic(topic: str) -> Optional[str]:
|
||||
"""从 ``/world/<world>/pose/info`` 里取出 world 名；取不到返回 ``None``。
|
||||
|
||||
world 名必须与 ``worlds/assembly.world`` 的文件名一致，而 scene.yaml 里
|
||||
已经写着那条话题了 —— 所以**不要**再单独配一个 world 名参数：
|
||||
两份写法迟早不一致，而症状是"服务调用超时"（看起来像 Gazebo 没起来）。
|
||||
"""
|
||||
m = re.fullmatch(r"/world/([^/]+)/pose(?:/info)?/?", (topic or "").strip())
|
||||
return m.group(1) if m else None
|
||||
|
||||
|
||||
def yaw_to_quat(yaw_rad: float) -> Tuple[float, float, float, float]:
|
||||
"""绕 Z 的 yaw [rad] → 四元数 ``(x, y, z, w)``。
|
||||
|
||||
只支持绕 Z：场景里的物体位姿本来就是"平面位姿 + yaw"（``SceneObject``
|
||||
也只有 yaw）—— 这样写反而不会被"顺手支持一下全姿态"拖进来一堆没测过的路径。
|
||||
"""
|
||||
return (0.0, 0.0, math.sin(float(yaw_rad) / 2.0), math.cos(float(yaw_rad) / 2.0))
|
||||
|
||||
|
||||
def _run(cmd: Sequence[str], timeout: float = CMD_TIMEOUT_S) -> Tuple[int, str]:
|
||||
"""跑一条外部命令，返回 ``(退出码, 合并输出)``。**不抛异常**（调用方自己判）。"""
|
||||
try:
|
||||
p = subprocess.run(list(cmd), capture_output=True, text=True, timeout=timeout, check=False)
|
||||
return p.returncode, (p.stdout or "") + (p.stderr or "")
|
||||
except FileNotFoundError:
|
||||
return 127, f"找不到可执行文件：{cmd[0]}"
|
||||
except subprocess.TimeoutExpired:
|
||||
return 124, f"命令超时（{timeout}s）：{' '.join(cmd)}"
|
||||
|
||||
|
||||
def set_model_pose(
|
||||
world: str,
|
||||
name: str,
|
||||
pos: Sequence[float],
|
||||
quat: Sequence[float],
|
||||
timeout_s: float = CMD_TIMEOUT_S,
|
||||
) -> Tuple[bool, str]:
|
||||
"""把模型 ``name`` 搬到世界位姿 ``pos`` / ``quat``（四元数 xyzw）。
|
||||
|
||||
返回 ``(成功?, 输出)``。**失败只返回 False，不抛异常**：调用方拿到的场景
|
||||
可能只是"少了一个可选物体"，因为一个失败的复位让整个流程起不来不值得。
|
||||
|
||||
注意 ``--reqtype`` 必须写 ``ignition.msgs.Pose``：本环境实测写
|
||||
``gz.msgs.Pose`` 会报 "Unable to create request"，一个字母的差别、
|
||||
报错信息完全没提原因。
|
||||
"""
|
||||
req = (
|
||||
f'name: "{name}", '
|
||||
f"position: {{x: {float(pos[0]):.6f}, y: {float(pos[1]):.6f}, z: {float(pos[2]):.6f}}}, "
|
||||
"orientation: {"
|
||||
f"x: {float(quat[0]):.8f}, y: {float(quat[1]):.8f}, "
|
||||
f"z: {float(quat[2]):.8f}, w: {float(quat[3]):.8f}"
|
||||
"}"
|
||||
)
|
||||
code, out = _run(
|
||||
[
|
||||
"ign", "service",
|
||||
"-s", f"/world/{world}/set_pose",
|
||||
"--reqtype", "ignition.msgs.Pose",
|
||||
"--reptype", "ignition.msgs.Boolean",
|
||||
"--timeout", str(int(SERVICE_TIMEOUT_MS)),
|
||||
"--req", req,
|
||||
],
|
||||
timeout=timeout_s,
|
||||
)
|
||||
ok = code == 0 and "data: true" in out
|
||||
return ok, out.strip()
|
||||
|
||||
|
||||
def set_model_pose_yaw(
|
||||
world: str,
|
||||
name: str,
|
||||
pos: Sequence[float],
|
||||
yaw_rad: float,
|
||||
timeout_s: float = CMD_TIMEOUT_S,
|
||||
) -> Tuple[bool, str]:
|
||||
"""``set_model_pose`` 的平面版：只有 yaw。"""
|
||||
return set_model_pose(world, name, pos, yaw_to_quat(yaw_rad), timeout_s)
|
||||
@@ -94,6 +94,36 @@ class Operating:
|
||||
part_object: str = "workpiece"
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class PoseInfo:
|
||||
"""Gazebo 世界模型位姿回读的桥接参数 + "物体被撞走"的判据。
|
||||
|
||||
为什么要有它：``perception.source=truth`` 是**纯几何**的 —— 它按本文件的
|
||||
名义位姿算孔位，**不去问 Gazebo 物体现在到底在哪**。物体一旦被碰走，
|
||||
"真值"就开始撒谎，而每一处现象都指向别的原因（详见 scene.yaml 里
|
||||
``pose_info`` 那段注释）。这个结构就是让仿真能把"名义 vs 实际"对上。
|
||||
"""
|
||||
|
||||
gz_topic: str
|
||||
ros_topic: str
|
||||
ros_type: str = "tf2_msgs/msg/TFMessage"
|
||||
gz_type: str = "gz.msgs.Pose_V"
|
||||
#: 部件相对名义位姿的平移超过它 [m] 就算"被搬走了" → 告警（**但仍然发点**，
|
||||
#: 因为真值几何已经跟着实测位姿走，见 sim_live_pose）。
|
||||
drift_tol_m: float = 0.002
|
||||
#: 同上，姿态 [deg]。
|
||||
drift_tol_deg: float = 2.0
|
||||
#: 平移超过它才**停发**真值点 [m]。这个量级说明物体掉了/被搬走了，
|
||||
#: 场景前提没了（装配没有意义）。2mm 那个量级天天都有（spawn 落地 +
|
||||
#: 工件在通槽上蠕动），拿它当停发线会把监视器变成"每轮都误报的假故障"。
|
||||
drift_stop_m: float = 0.02
|
||||
#: 启动时把被盯的物体**实际搬回**名义位姿（仿真专有，默认关）。
|
||||
#: 为什么需要、为什么只在启动窗口内做，见 ``sim_truth_perception`` 的
|
||||
#: ``_maybe_startup_reset``（一句话：spawn 落地会偏 2~3mm，那不是"被撞走"，
|
||||
#: 但看门狗分不出来；而运行中的漂移必须报错、不能悄悄搬回去）。
|
||||
reset_on_start: bool = False
|
||||
|
||||
|
||||
@dataclass
|
||||
class SceneSpec:
|
||||
"""scene.yaml 的解析结果。"""
|
||||
@@ -105,6 +135,8 @@ class SceneSpec:
|
||||
objects: List[SceneObject] = field(default_factory=list)
|
||||
# 特征名 → (下限, 上限) 像素门槛。没写 = 不设门槛（只报告不判定）。
|
||||
gates_px: Dict[str, Tuple[float, float]] = field(default_factory=dict)
|
||||
# 世界模型位姿回读。**没写 = 不做漂移监视**（老场景文件仍然能读）。
|
||||
pose_info: Optional[PoseInfo] = None
|
||||
source: Optional[Path] = None
|
||||
|
||||
|
||||
@@ -229,10 +261,69 @@ def load_scene(path: Optional[Path] = None, root: Optional[Path] = None) -> Scen
|
||||
operating=operating,
|
||||
objects=objects,
|
||||
gates_px=_parse_gates(p, doc.get("gates_px")),
|
||||
pose_info=_parse_pose_info(p, doc.get("pose_info")),
|
||||
source=p,
|
||||
)
|
||||
|
||||
|
||||
def _parse_pose_info(path: Path, raw: object) -> Optional[PoseInfo]:
|
||||
"""解析 pose_info。**没写就返回 None**（= 不做漂移监视）。
|
||||
|
||||
与 gates_px 不同，这一段是**可选**的：老场景文件没有它也必须能读。
|
||||
但只要写了，就必须写全 —— 少了话题名会得到一个"订阅了不存在的话题、
|
||||
永远没有数据"的监视器，那比没有监视更糟（看起来"监视器没报警"）。
|
||||
"""
|
||||
if raw is None:
|
||||
return None
|
||||
if not isinstance(raw, dict):
|
||||
raise SceneError(
|
||||
f"{path} 的 pose_info 不是映射。期望形如：\n"
|
||||
f" pose_info:\n"
|
||||
f" gz_topic: /world/assembly/pose/info\n"
|
||||
f" ros_topic: /sim/world_pose_info"
|
||||
)
|
||||
missing = [k for k in ("gz_topic", "ros_topic") if not str(raw.get(k, "")).strip()]
|
||||
if missing:
|
||||
raise SceneError(
|
||||
f"{path} 的 pose_info 缺字段 {missing}。\n"
|
||||
f" 这两个话题名必须都写：gz_topic 是 Gazebo 侧的 "
|
||||
f"/world/<world>/pose/info，ros_topic 是桥过来之后的名字。"
|
||||
)
|
||||
try:
|
||||
tol_m = float(raw.get("drift_tol_m", 0.002))
|
||||
tol_deg = float(raw.get("drift_tol_deg", 2.0))
|
||||
stop_m = float(raw.get("drift_stop_m", 0.02))
|
||||
except (TypeError, ValueError) as exc:
|
||||
raise SceneError(f"{path} 的 pose_info 容差不是数字: {exc}") from exc
|
||||
if stop_m <= tol_m:
|
||||
# 停发线必须**明显大于**告警线，否则"告警"永远和"停发"一起出现，
|
||||
# 而现实里 2~3mm 的偏差天天有（spawn 落地 + 工件在通槽上蠕动）——
|
||||
# 那会让每一轮开局都被假故障拦死，现场学会忽略告警，监视器就废了。
|
||||
raise SceneError(
|
||||
f"{path} 的 pose_info.drift_stop_m={stop_m} 必须大于 drift_tol_m={tol_m}。\n"
|
||||
f" drift_tol_m 是**告警**线（几何已按实测算，不影响发点），"
|
||||
f"drift_stop_m 是**停发**线（物体掉了/被搬走了）。"
|
||||
)
|
||||
reset = raw.get("reset_on_start", False)
|
||||
if not isinstance(reset, bool):
|
||||
# 不做 `bool(reset)` 强转：YAML 里 `reset_on_start: "false"` 是**真值**，
|
||||
# 而这类错误的表现是"开关关了却没关"，排查时没人会去怀疑引号。
|
||||
raise SceneError(
|
||||
f"{path} 的 pose_info.reset_on_start 不是布尔量（收到 {reset!r}）。\n"
|
||||
f" 写 true / false；不要加引号（\"false\" 在 YAML 里是真值）。"
|
||||
)
|
||||
return PoseInfo(
|
||||
gz_topic=str(raw["gz_topic"]).strip(),
|
||||
ros_topic=str(raw["ros_topic"]).strip(),
|
||||
ros_type=str(raw.get("ros_type") or "tf2_msgs/msg/TFMessage").strip(),
|
||||
gz_type=str(raw.get("gz_type") or "gz.msgs.Pose_V").strip(),
|
||||
drift_tol_m=tol_m,
|
||||
drift_tol_deg=tol_deg,
|
||||
drift_stop_m=stop_m,
|
||||
reset_on_start=reset,
|
||||
)
|
||||
|
||||
|
||||
def _parse_gates(path: Path, raw: object) -> Dict[str, Tuple[float, float]]:
|
||||
"""解析 gates_px。写成 {名字: [下限, 上限]}。
|
||||
|
||||
|
||||
@@ -35,6 +35,24 @@ torch/ultralytics，而且这台机器**离线**装不上，于是 perception_no
|
||||
* 目标在相机背后 / 相机查不到 TF 时**不发假点**（和 perception_node 的
|
||||
"没有可用深度就不发点"同一条规矩）。宁可让伺服看门狗超时，也不给它一个
|
||||
幻影目标 —— 后者会让机械臂朝错误方向走，而且看起来像"标定坏了"。
|
||||
* **物体被撞走了就不再发点**。真值感知的整个前提是"物体就在 scene.yaml 写的位置"，
|
||||
前提破了，算出来的就是**假测量**。所以本节点自己订阅 ``/world/<world>/pose/info``
|
||||
（scene.yaml 的 ``pose_info`` 段）盯着部件的实际位姿，偏离名义位姿超过容差就停下
|
||||
并**指名报错**。
|
||||
为什么必须由这一侧盯（2026-09-24 的教训）：``perception.source=truth`` 是纯几何的，
|
||||
它从不问 Gazebo 物体现在在哪。实测工件掉进台面通槽、落到地面之后，"真值"开始撒谎，
|
||||
而现象全都指向别处（二阶段报"两孔间距 169mm 不像 k1/k2"、S07 一路下压不接触），
|
||||
看起来像检测/标定/阈值坏了 —— 真正的原因只有一个，却没有任何一处日志说出来。
|
||||
**谁假设的，谁负责查。**
|
||||
* **开局那一次例外**（scene.yaml 的 ``pose_info.reset_on_start``）：物体是"在名义位姿
|
||||
上方 spawn、再落下来"的，落稳后实测偏 2.6mm（越过 2mm 容差）—— 那不是"被撞走"，
|
||||
但看门狗分不出来，于是每轮开局都可能被这条假故障拦死。所以在**开局窗口内**
|
||||
（``STARTUP_RESET_WINDOW_S``）把它 set_pose 回名义位姿，只做一次；
|
||||
过了窗口再漂就是运行中被撞，必须报错停下、**绝不**搬（见
|
||||
``startup_reset_decision`` 与 ``_maybe_startup_reset``）。
|
||||
* 不自己算四元数→旋转矩阵（复用 ``handeye_lib.quat_xyzw_to_R``）。
|
||||
这段换算以前在 4 个 launch 文件里各抄了一遍、错了 3 处，症状是
|
||||
"总能到附近、永远差一点"。数学实现只能有一份。
|
||||
* 不自己算四元数→旋转矩阵（复用 ``handeye_lib.quat_xyzw_to_R``）。
|
||||
这段换算以前在 4 个 launch 文件里各抄了一遍、错了 3 处，症状是
|
||||
"总能到附近、永远差一点"。数学实现只能有一份。
|
||||
@@ -45,7 +63,7 @@ from __future__ import annotations
|
||||
import math
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
from typing import Optional, Sequence, Tuple
|
||||
from typing import Dict, Optional, Sequence, Tuple
|
||||
|
||||
import numpy as np
|
||||
import rclpy
|
||||
@@ -54,8 +72,9 @@ from rclpy.node import Node
|
||||
from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy
|
||||
from rclpy.time import Time
|
||||
from tf2_ros import Buffer, TransformListener
|
||||
from tf2_msgs.msg import TFMessage
|
||||
|
||||
from arm_sim_gazebo import sim_scene
|
||||
from arm_sim_gazebo import sim_live_pose, sim_pose_service, sim_scene
|
||||
|
||||
#: 与 perception_node / servo_node 一致：点话题用 BEST_EFFORT + KEEP_LAST(5)。
|
||||
#: 匹配不上 QoS 的后果是"话题在、消息发了、订阅方一条也收不到"，
|
||||
@@ -69,6 +88,12 @@ POINT_QOS = QoSProfile(
|
||||
#: 目标点离相机光心太近（或跑到背后）就不发：z<=0 在相机系里是"背后"。
|
||||
MIN_CAM_Z_M = 0.01
|
||||
|
||||
#: 启动复位的**时间窗口** [s]：只在这段时间内把工件搬回名义位姿。
|
||||
#: 过了这段时间还漂 = 运行中被撞走/被压走，那时**绝不能**悄悄搬回去
|
||||
#: （机器人可能正握着它，搬回去等于把现场证据抹掉并让真值开始撒谎），
|
||||
#: 必须照旧报错停下。整个理由见 ``_maybe_startup_reset``。
|
||||
STARTUP_RESET_WINDOW_S = 20.0
|
||||
|
||||
|
||||
class TruthError(RuntimeError):
|
||||
"""真值目标解不出来（场景 / 模型 / 参数有问题）。
|
||||
@@ -78,6 +103,42 @@ class TruthError(RuntimeError):
|
||||
"""
|
||||
|
||||
|
||||
#: ``startup_reset_decision`` 的决定：不动 / 再等等 / 现在搬 / 放弃。
|
||||
RESET_SKIP = "skip"
|
||||
RESET_WAIT = "wait"
|
||||
RESET_NOW = "reset"
|
||||
RESET_GIVE_UP = "give_up"
|
||||
|
||||
|
||||
def startup_reset_decision(
|
||||
now_s: float,
|
||||
deadline_s: Optional[float],
|
||||
window_s: float,
|
||||
pose_known: bool,
|
||||
any_drift: bool,
|
||||
) -> Tuple[str, Optional[float]]:
|
||||
"""开局"要不要把物体搬回名义位姿"的判据 → ``(决定, 新的截止时刻)``。
|
||||
|
||||
**纯函数**：写错的表现两头都很糟 ——
|
||||
* 判成"运行中也搬"：把"谁把它撞歪了"的现场抹掉，甚至把工件从爪里拽走，
|
||||
现象会变成"孔位对不上/下压不接触"，比原故障更难查；
|
||||
* 判成"永远等"：开局那 2.6mm 落地偏差每轮都把真值点拦死，
|
||||
S04 报「一个视觉目标点都没收到」，看起来像真值节点根本没起来。
|
||||
|
||||
所以它必须能脱离节点被钉住（``test_sim_truth_perception.py``）。
|
||||
"""
|
||||
dl = float(deadline_s) if deadline_s is not None else float(now_s) + float(window_s)
|
||||
if now_s > dl:
|
||||
return RESET_GIVE_UP, dl
|
||||
if not pose_known:
|
||||
# 还没收到 pose/info 帧：此时"有没有漂移"根本无从判断，
|
||||
# 等下去（窗口到点会自己变成 give_up）。
|
||||
return RESET_WAIT, dl
|
||||
if not any_drift:
|
||||
return RESET_SKIP, dl
|
||||
return RESET_NOW, dl
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class TruthTarget:
|
||||
"""真值目标：一个**世界系**点，以及它是怎么算出来的（给人看）。"""
|
||||
@@ -190,6 +251,56 @@ def matrix_from_transform(tr) -> np.ndarray:
|
||||
return T
|
||||
|
||||
|
||||
def pose_deviation(
|
||||
obj: sim_scene.SceneObject,
|
||||
p: Sequence[float],
|
||||
quat: Sequence[float],
|
||||
) -> Tuple[float, float, float]:
|
||||
"""实际位姿相对名义位姿的偏差 → ``(平移 [m], 姿态夹角 [deg], 实测 yaw [rad])``。
|
||||
|
||||
**纯函数**：这条判据搞错了的表现是"监视器静默失声" ——
|
||||
要么永远不报（物体明明被撞走了却没人说），要么天天误报（现场学会忽略它）。
|
||||
两种情况都比没有监视更糟，所以它必须能脱离节点被钉住。
|
||||
"""
|
||||
from arm_application.handeye_lib import quat_xyzw_to_R
|
||||
|
||||
d = math.dist((float(obj.x), float(obj.y), float(obj.z)), [float(v) for v in p[:3]])
|
||||
R_now = quat_xyzw_to_R([float(v) for v in quat])
|
||||
yaw_now = math.atan2(float(R_now[1, 0]), float(R_now[0, 0]))
|
||||
# 姿态用**整段旋转差**的夹角，不只比 yaw：板被碰一下往往是"翘起来夹着转了一点"，
|
||||
# 只比 yaw 会漏掉翘起，而翘起恰恰最影响孔的成像位置。
|
||||
c, s = math.cos(float(obj.yaw)), math.sin(float(obj.yaw))
|
||||
R_ref = np.array([[c, -s, 0.0], [s, c, 0.0], [0.0, 0.0, 1.0]])
|
||||
cosang = (float(np.trace(R_ref.T @ R_now)) - 1.0) / 2.0
|
||||
ang = math.degrees(math.acos(max(-1.0, min(1.0, cosang))))
|
||||
return (d, ang, yaw_now)
|
||||
|
||||
|
||||
def pose_drift(
|
||||
obj: sim_scene.SceneObject,
|
||||
p: Sequence[float],
|
||||
quat: Sequence[float],
|
||||
tol_m: float,
|
||||
tol_deg: float,
|
||||
) -> Optional[str]:
|
||||
"""偏差超过容差时的**说明文字**；在容差内返回 ``None``（= 没漂移）。
|
||||
|
||||
给日志/告警用的人话版；数值判据在 :func:`pose_deviation`（同一个实现，
|
||||
免得两处各算一遍距离）。
|
||||
"""
|
||||
d, ang, yaw_now = pose_deviation(obj, p, quat)
|
||||
if d <= tol_m and ang <= tol_deg:
|
||||
return None
|
||||
return (
|
||||
f"名义 (x,y,z)=({obj.x:.4f},{obj.y:.4f},{obj.z:.4f}) "
|
||||
f"yaw={math.degrees(float(obj.yaw)):.1f}°\n"
|
||||
f" 实际 (x,y,z)=({p[0]:.4f},{p[1]:.4f},{p[2]:.4f}) "
|
||||
f"yaw={math.degrees(yaw_now):.1f}°\n"
|
||||
f" 偏差 |Δp|={d * 1000:.1f}mm、姿态差 {ang:.1f}°"
|
||||
f"（容差 {tol_m * 1000:.1f}mm / {tol_deg:.1f}°）"
|
||||
)
|
||||
|
||||
|
||||
class SimTruthPerception(Node):
|
||||
"""把真值目标点按固定频率发到 ``visual_servo/filtered_point``。"""
|
||||
|
||||
@@ -214,9 +325,15 @@ class SimTruthPerception(Node):
|
||||
self.declare_parameter("publish_hz", 20.0)
|
||||
"""发布频率：不必跟着相机帧率，伺服 100Hz 控制会自己插值/外推。"""
|
||||
|
||||
self.declare_parameter("pose_info_topic", "")
|
||||
"""世界模型位姿话题（``tf2_msgs/TFMessage``）。
|
||||
|
||||
空 = 用 scene.yaml 的 ``pose_info.ros_topic``；场景里也没配 = 真值**按名义
|
||||
位姿**算（几何会偏，且没有"物体被搬走"的告警）。"""
|
||||
|
||||
scene_file = str(self.get_parameter("scene_file").value or "").strip()
|
||||
self._scene_file = Path(scene_file) if scene_file else None
|
||||
offset = self.get_parameter("offset_m").value or [0.0, 0.0, 0.0]
|
||||
self._offset = offset = self.get_parameter("offset_m").value or [0.0, 0.0, 0.0]
|
||||
# 长度的校验在 target_point_world 里（纯函数，有测试钉着），这里不重复。
|
||||
self._camera_frame = str(self.get_parameter("camera_frame").value or "").strip()
|
||||
if not self._camera_frame:
|
||||
@@ -231,6 +348,9 @@ class SimTruthPerception(Node):
|
||||
# 目标点在启动时算一次就够：场景是静态的（工件被推动后位置会变，
|
||||
# 但那种情况下"真值"本身也就失效了 —— 此时应该重跑 gen_sim_handeye
|
||||
# 或重启，而不是悄悄跟着一个被撞歪的位姿走）。
|
||||
# ⚠️ 这句话在 2026-09-24 之前只是**注释**：工件真的被碰走了，没有任何人
|
||||
# 发现（详见 scene.yaml 的 pose_info 段）。现在由 _setup_drift_watch
|
||||
# 把它变成**运行时的判据**：一旦对上不，立刻停发并指名报错。
|
||||
self._spec, self._target = load_truth_target(
|
||||
self._scene_file,
|
||||
str(self.get_parameter("target_object").value or ""),
|
||||
@@ -244,6 +364,17 @@ class SimTruthPerception(Node):
|
||||
TransformListener(self._tf, self, spin_thread=True)
|
||||
self._sent = 0
|
||||
self._warned_tf = 0
|
||||
self._warned_drift = 0
|
||||
self._t_start: Optional[float] = None
|
||||
# 目标点按**实测**位姿算（见模块 docstring 与 sim_live_pose），
|
||||
# 位姿没变就不重算（算一次要读模型 SDF，20Hz 下不能每拍都读）。
|
||||
self._target_live: Optional[TruthTarget] = None
|
||||
self._target_key: Optional[Tuple[float, ...]] = None
|
||||
self._warned_nominal = 0
|
||||
# 启动复位（scene.yaml 的 pose_info.reset_on_start）：见 _maybe_startup_reset。
|
||||
self._reset_done = False
|
||||
self._reset_deadline: Optional[float] = None
|
||||
self._setup_drift_watch()
|
||||
|
||||
hz = max(1.0, min(100.0, float(self.get_parameter("publish_hz").value)))
|
||||
self.create_timer(1.0 / hz, self._tick)
|
||||
@@ -260,7 +391,254 @@ class SimTruthPerception(Node):
|
||||
)
|
||||
|
||||
# ------------------------------------------------------------------
|
||||
# 漂移监视：真值感知的"前提自检"
|
||||
# ------------------------------------------------------------------
|
||||
def _setup_drift_watch(self) -> None:
|
||||
"""订阅世界模型位姿，盯住"被真值当作几何基准"的物体现在在哪。
|
||||
|
||||
两件事一起做，因为它们是同一份数据：
|
||||
* **量测**：给真值目标/孔位提供**实测**位姿（``sim_live_pose``）——
|
||||
这才是真值该用的几何（见模块 docstring）；
|
||||
* **告警**：实测位姿相对 ``scene.yaml`` 的名义位姿偏了多少。
|
||||
超过 ``drift_tol_m`` **告警但继续发**（几何已经跟着实测走，
|
||||
没有幻影目标）；超过 ``drift_stop_m`` 才**停发**并指名报错
|
||||
（那个量级说明物体掉了/被搬走了，场景前提没了）。
|
||||
|
||||
为什么告警与停发要分开（2026-09-24 实测）：
|
||||
物体 spawn 落地就偏 2.6mm、骑在通槽上还会缓慢再挪 1.9mm ——
|
||||
这个量级**天天都会出现**，而它正好压在"2mm"这条线上。用同一条阈值
|
||||
既停发又告警，结果是"每轮开局都可能被假故障拦死"，
|
||||
而现场的正确反应是"忽略这条告警" —— 监视器一旦被学会忽略就废了。
|
||||
|
||||
只盯**工件**：它是二阶段孔位与伺服目标的几何基准。
|
||||
通止规刻意不盯 —— 它本来就要被夹走、被搬动，那正是流程的一部分。
|
||||
"""
|
||||
pi = self._spec.pose_info
|
||||
topic = str(self.get_parameter("pose_info_topic").value or "").strip()
|
||||
if not topic and pi is not None:
|
||||
topic = pi.ros_topic
|
||||
# 只盯工件：它是二阶段孔位与伺服目标的几何基准。
|
||||
want = self._spec.operating.part_object
|
||||
self._nominal_obj = next((o for o in self._spec.objects if o.name == want), None)
|
||||
self._tol_m = float(pi.drift_tol_m if pi is not None else 0.002)
|
||||
self._tol_deg = float(pi.drift_tol_deg if pi is not None else 2.0)
|
||||
self._stop_m = float(getattr(pi, "drift_stop_m", 0.02) if pi is not None else 0.02)
|
||||
# 启动复位要用 world 名（服务路径里），而 world 名从 gz_topic 推出来 ——
|
||||
# 不另配一个参数：两份写法迟早不一致，症状是"服务调用超时"。
|
||||
self._reset_world = (
|
||||
sim_pose_service.world_from_gz_topic(pi.gz_topic) if pi is not None else None
|
||||
)
|
||||
self._reset_on_start = bool(pi.reset_on_start) if pi is not None else False
|
||||
self._live = sim_live_pose.LiveScenePose(self, topic)
|
||||
|
||||
if self._nominal_obj is None:
|
||||
self.get_logger().warn(
|
||||
f"scene.yaml 的 operating.part_object={want!r} 不在 objects 里 —— "
|
||||
f"实测位姿无从取得（只能按名义位姿算，几何会偏）。"
|
||||
)
|
||||
if not topic:
|
||||
self.get_logger().info(
|
||||
"没有世界模型位姿话题（scene.yaml 的 pose_info 未配）—— "
|
||||
"真值将**按名义位姿**算目标点与孔位（物体一动就偏，实测偏 2~3mm "
|
||||
"就会让规尖进不了孔），而且没有「物体被搬走」的告警。"
|
||||
)
|
||||
return
|
||||
self.get_logger().info(
|
||||
f"已订阅 {topic}：真值目标按**实测**位姿算；与该物体在 scene.yaml 的"
|
||||
f"名义位姿相差超过 {self._tol_m * 1000:.1f}mm / {self._tol_deg:.1f}° 告警，"
|
||||
f"超过 {self._stop_m * 1000:.0f}mm 停发。"
|
||||
)
|
||||
if self._reset_on_start:
|
||||
if self._reset_world is None:
|
||||
self.get_logger().error(
|
||||
f"pose_info.reset_on_start=true，但从 gz_topic "
|
||||
f"{pi.gz_topic!r} 里认不出 world 名 —— **启动复位不会执行**。\n"
|
||||
f" 期望形如 /world/<world>/pose/info，world 名 = "
|
||||
f"worlds/assembly.world 的文件名。\n"
|
||||
f" 后果：spawn 落地的 2~3mm 偏差会被当成"
|
||||
f"「场景被动过」，告警一直刷。"
|
||||
)
|
||||
else:
|
||||
self.get_logger().info(
|
||||
f"启动复位已开：开局（{STARTUP_RESET_WINDOW_S:.0f}s 内）会把被盯的物体"
|
||||
f"搬回名义位姿（set_pose /world/{self._reset_world}/set_pose）。"
|
||||
)
|
||||
|
||||
# ------------------------------------------------------------------
|
||||
# 实测位姿 → 目标点
|
||||
# ------------------------------------------------------------------
|
||||
def _live_deviation(self) -> Optional[Tuple[float, float, str]]:
|
||||
"""实测位姿相对名义位姿的偏差 ``(d [m], ang [deg], 说明)``；没有实测返回 None。"""
|
||||
obj = getattr(self, "_nominal_obj", None)
|
||||
if obj is None:
|
||||
return None
|
||||
p = self._live.pose(obj.name)
|
||||
if p is None:
|
||||
return None
|
||||
d, ang, _ = pose_deviation(obj, p[:3], sim_pose_service.yaw_to_quat(p[3]))
|
||||
reason = (
|
||||
pose_drift(obj, p[:3], sim_pose_service.yaw_to_quat(p[3]), self._tol_m, self._tol_deg)
|
||||
or ""
|
||||
)
|
||||
return (d, ang, reason)
|
||||
|
||||
def _current_target(self) -> Tuple[TruthTarget, bool]:
|
||||
"""按实测位姿算目标点（位姿没变就复用）。返回 ``(目标, 是否按实测算的)``。
|
||||
|
||||
实测位姿拿不到时**退回名义位姿**并把这个事实报给调用方 ——
|
||||
静默退回是这条链上最贵的错（几何偏两毫米、没有任何日志提起）。
|
||||
"""
|
||||
obj = getattr(self, "_nominal_obj", None)
|
||||
p = self._live.pose(obj.name) if obj is not None else None
|
||||
if obj is None or p is None:
|
||||
return self._target, False
|
||||
key = tuple(round(float(v), 7) for v in p)
|
||||
if self._target_key == key and self._target_live is not None:
|
||||
return self._target_live, True
|
||||
spec_live = self._live.apply(self._spec, [obj.name])
|
||||
live_obj = pick_object(spec_live, obj.name)
|
||||
t = target_point_world(spec_live, live_obj, self._offset)
|
||||
self._target_key = key
|
||||
self._target_live = t
|
||||
return t, True
|
||||
|
||||
# ------------------------------------------------------------------
|
||||
# 启动复位：把"落地的 2~3mm"与"被撞走"分开
|
||||
# ------------------------------------------------------------------
|
||||
def _maybe_startup_reset(self) -> None:
|
||||
"""开局把被盯的物体**实际搬到**名义位姿（只做一次，且有时间窗）。
|
||||
|
||||
为什么需要（2026-09-24 实测，别再当玄学）：
|
||||
物体是"在名义位姿上方 spawn、再落下来"的，落稳后实测偏 **2.6mm**
|
||||
（(0.5175, 0.0008) vs 名义 (0.5200, 0.0000)），正好越过 2mm 容差 ——
|
||||
它不是"被撞走"，但看门狗**分不出来**，于是每一轮开局都可能被这条
|
||||
假故障拦死：S04 等不到目标点、报「一个视觉目标点都没收到」。
|
||||
而把它 set_pose 回名义位姿之后，它**停在那儿不动**（实测）——
|
||||
说明名义位姿本身就是稳定平衡点，差的只是 spawn 那一下的落点。
|
||||
|
||||
为什么只在窗口内做、且**只做一次**：
|
||||
运行中漂移的唯一合法解释是"被机器人撞了/压了"。那时悄悄搬回去等于
|
||||
抹掉现场证据（而现象会变成"孔位对不上/下压不接触"，更难查），
|
||||
还可能把工件从爪里拽出来。所以过了窗口就照旧报错停下。
|
||||
|
||||
判据本身在纯函数 ``startup_reset_decision`` 里（有单测钉着），
|
||||
这里只负责"按决定去调服务、并把话说明白"。
|
||||
"""
|
||||
if self._reset_done or not self._reset_on_start or self._reset_world is None:
|
||||
return
|
||||
now = self.get_clock().now().nanoseconds * 1e-9
|
||||
dev = self._live_deviation()
|
||||
decision, deadline = startup_reset_decision(
|
||||
now_s=now,
|
||||
deadline_s=self._reset_deadline,
|
||||
window_s=STARTUP_RESET_WINDOW_S,
|
||||
pose_known=self._live.ready,
|
||||
any_drift=bool(dev is not None and (dev[2] or dev[0] > self._tol_m)),
|
||||
)
|
||||
self._reset_deadline = deadline
|
||||
if decision == "wait":
|
||||
return
|
||||
if decision == "skip":
|
||||
self._reset_done = True # 本来就在位（或还没量到足够信息）：不必搬
|
||||
return
|
||||
if decision == "give_up":
|
||||
# 窗口内没成功（比如 set_pose 服务不通）→ 放弃并说清楚，
|
||||
# 之后就是告警/停发的正常行为（真值几何已经跟着实测算，不影响发点）。
|
||||
self._reset_done = True
|
||||
self.get_logger().warn(
|
||||
f"启动复位窗口（{STARTUP_RESET_WINDOW_S:.0f}s）内没能把物体搬回名义位姿 —— "
|
||||
f"放弃复位；真值几何按实测位姿算（不受影响），"
|
||||
f"但告警会一直刷「与名义位姿不一致」。"
|
||||
)
|
||||
return
|
||||
|
||||
self._reset_done = True
|
||||
obj = getattr(self, "_nominal_obj", None)
|
||||
if obj is None:
|
||||
return
|
||||
reason = (dev[2] if dev else "") or ""
|
||||
ok, out = sim_pose_service.set_model_pose_yaw(
|
||||
self._reset_world, obj.name, (obj.x, obj.y, obj.z), float(obj.yaw)
|
||||
)
|
||||
if ok:
|
||||
self.get_logger().warn(
|
||||
f"启动复位：{obj.name!r} 不在名义位姿"
|
||||
f"（{reason.splitlines()[-1].strip() if reason else ''}）"
|
||||
f"—— 已 set_pose 回 ({obj.x:.4f},{obj.y:.4f},{obj.z:.4f}) "
|
||||
f"yaw={math.degrees(float(obj.yaw)):.1f}°。\n"
|
||||
f" 这是**开局**的正常现象（spawn 落地会偏 2~3mm），不是故障；"
|
||||
f"复位后实测位姿等于名义位姿。"
|
||||
)
|
||||
else:
|
||||
self.get_logger().error(
|
||||
f"启动复位失败：{obj.name!r} 没能 set_pose 回名义位姿 —— {out[:200]}\n"
|
||||
f" 影响有限：真值几何按实测位姿算，仍能发点；但告警会一直刷。"
|
||||
)
|
||||
|
||||
def _tick(self) -> None:
|
||||
# 前提自检的**活性检查**：订阅了话题却一帧都没收到，等于没有监视 ——
|
||||
# 而那正是"以为有保护、实际裸奔"。所以这里必须把这种情况说出来。
|
||||
if self._live.topic and not self._live.ready:
|
||||
if self._t_start is None:
|
||||
self._t_start = self.get_clock().now().nanoseconds * 1e-9
|
||||
elif self.get_clock().now().nanoseconds * 1e-9 - self._t_start > 8.0:
|
||||
self.get_logger().warn(
|
||||
"盯物体位姿的话题**一帧都没收到** —— 真值只能退回**名义位姿**"
|
||||
"（几何会偏，实测偏 2~3mm 就能让规尖进不了孔）。\n"
|
||||
" 常见原因：gazebo_sim.launch.py 里的 pose_info_bridge 没起来，"
|
||||
"或 scene.yaml 的 pose_info.gz_topic 与 world 名对不上\n"
|
||||
" （必须等于 /world/<world>/pose/info，world 名 = "
|
||||
"worlds/assembly.world 的文件名）。",
|
||||
throttle_duration_sec=10.0,
|
||||
)
|
||||
# 先给"开局落地偏差"一次纠偏机会（见 _maybe_startup_reset）。
|
||||
self._maybe_startup_reset()
|
||||
|
||||
dev = self._live_deviation()
|
||||
if dev is None and self._nominal_obj is not None and self._live.ready:
|
||||
# 话题有帧，就是没有这个模型 → 名字对不上，说出来。
|
||||
self.get_logger().warn(
|
||||
f"实测位姿里没有物体 {self._nominal_obj.name!r}："
|
||||
f"{self._live.why_not(self._nominal_obj.name)}",
|
||||
throttle_duration_sec=10.0,
|
||||
)
|
||||
elif dev is not None:
|
||||
d, ang, reason = dev
|
||||
if d > self._stop_m:
|
||||
# 这个量级说明物体掉了/被搬走了：场景前提没了，**不发点**。
|
||||
self._warned_drift += 1
|
||||
if self._warned_drift in (1, 5, 30) or self._warned_drift % 300 == 0:
|
||||
self.get_logger().error(
|
||||
f"仿真物体 {self._nominal_obj.name!r} 已经不在 scene.yaml 说的地方了 —— "
|
||||
f"**停止发真值点**（偏了 {d * 1000:.0f}mm，超过 "
|
||||
f"{self._stop_m * 1000:.0f}mm 这条线）。\n"
|
||||
f" {reason}\n"
|
||||
f" ⚠ 为什么停：物体掉了/被搬走之后，装配就没有意义了 ——"
|
||||
f"再发点等于指挥机械臂去装一个不存在的工件。\n"
|
||||
f" 修法：把 {self._nominal_obj.name!r} 放回 scene.yaml 的位置"
|
||||
f"（真机现场就改 scene.yaml），或重启仿真让它按场景重新 spawn。\n"
|
||||
f" 排查提示：台面沿 X 有一道 36mm 通槽（|y| ≤ 18mm）；"
|
||||
f"工件若跨槽时被压翻就会掉下去。"
|
||||
)
|
||||
return
|
||||
if reason:
|
||||
# 2~3mm 这个量级**天天都有**（spawn 落地 + 通槽上缓慢蠕动），
|
||||
# 而几何已经跟着实测走，所以只告警、**继续发点**。
|
||||
self.get_logger().warn(
|
||||
f"物体 {self._nominal_obj.name!r} 与 scene.yaml 的名义位姿不一致 —— "
|
||||
f"真值几何已按**实测**位姿算，不受影响；只是提醒你场景被动过。\n"
|
||||
f" {reason}\n"
|
||||
f" （超过 {self._stop_m * 1000:.0f}mm 才停发）",
|
||||
throttle_duration_sec=30.0,
|
||||
)
|
||||
|
||||
target, live = self._current_target()
|
||||
if not live and self._warned_nominal < 3:
|
||||
self._warned_nominal += 1
|
||||
self.get_logger().warn(
|
||||
f"真值目标暂时按**名义位姿**算（{self._live.why_not(target.object_name)}）—— "
|
||||
f"物体一动几何就偏，实测偏 2~3mm 就会让规尖进不了孔。"
|
||||
)
|
||||
try:
|
||||
tr = self._tf.lookup_transform(self._camera_frame, self._world_frame, Time())
|
||||
except Exception as exc: # noqa: BLE001 —— tf2 的异常类型很杂，统一当"还没有"
|
||||
@@ -274,14 +652,14 @@ class SimTruthPerception(Node):
|
||||
throttle_duration_sec=5.0,
|
||||
)
|
||||
return
|
||||
p = apply_transform(matrix_from_transform(tr), self._target.world_xyz)
|
||||
p = apply_transform(matrix_from_transform(tr), target.world_xyz)
|
||||
if p[2] < MIN_CAM_Z_M:
|
||||
# 目标在相机背后或贴到镜头上：不发！发出去伺服会朝反方向走。
|
||||
self.get_logger().warn(
|
||||
f"真值目标不在相机前方（相机系 z={p[2] * 1000:.0f}mm < "
|
||||
f"{MIN_CAM_Z_M * 1000:.0f}mm）—— 不发点。"
|
||||
f"世界系点 ({self._target.world_xyz[0]:.3f}, "
|
||||
f"{self._target.world_xyz[1]:.3f}, {self._target.world_xyz[2]:.3f})"
|
||||
f"世界系点 ({target.world_xyz[0]:.3f}, "
|
||||
f"{target.world_xyz[1]:.3f}, {target.world_xyz[2]:.3f})"
|
||||
f" 不在 {self._camera_frame} 视野里；先让臂摆到能看到它的位置。",
|
||||
throttle_duration_sec=5.0,
|
||||
)
|
||||
|
||||
@@ -0,0 +1,159 @@
|
||||
"""仿真腕部力扭矩（wrist force/torque）的配置与桥接规则。
|
||||
|
||||
这一层只做两件事，别的都不做：
|
||||
|
||||
1. 从 ``devices/sim/gazebo/config/wrist_ft.yaml`` 读话题与关节名
|
||||
2. 生成 ros_gz_bridge 的参数（gz 消息 → ROS 消息）
|
||||
|
||||
**为什么不需要适配器节点**（与触觉不同）：
|
||||
触觉必须有 ``sim_tactile_adapter``，因为要做的语义工作很多 —— 轴向映射、
|
||||
基线相减、单位换算、接触超时、把 ``Contacts`` 折算成 contact 标志。
|
||||
腕部 FT 不需要任何这些：Gazebo 的 force_torque 直接给的就是**牛顿**，
|
||||
坐标系是 ``child`` 连杆系（工具竖直时 z 即轴向），而 ros_gz_bridge
|
||||
**原生支持** gz.msgs.Wrench → geometry_msgs/msg/WrenchStamped（实测确认）。
|
||||
于是仿真的输出与真机 ``/<sn>/ft_sensor_wrench`` **同名同类型同单位**，
|
||||
应用层一行分支都不用写。
|
||||
|
||||
少一层就少一个"配错了不报错"的地方，所以这里刻意不加节点。
|
||||
|
||||
与真机的对应：
|
||||
真机 /<sn>/ft_sensor_wrench geometry_msgs/WrenchStamped（法兰系，牛顿）
|
||||
仿真 /sim/ft_sensor_wrench geometry_msgs/WrenchStamped（link7 系，牛顿）
|
||||
← 名字由 site.yaml 的 robot.arms[sim].sn 决定
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
from typing import Any, Dict, List, Optional
|
||||
|
||||
import yaml
|
||||
|
||||
#: 相对仓库根的默认配置路径。
|
||||
DEFAULT_WRIST_FT_REL = "devices/sim/gazebo/config/wrist_ft.yaml"
|
||||
|
||||
|
||||
class WristFtConfigError(RuntimeError):
|
||||
"""腕部 FT 配置缺失或写错。消息里必须给出修法（现场就靠这句话）。"""
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class WristFtSpec:
|
||||
"""腕部 FT 的一份完整描述。"""
|
||||
|
||||
topic: str
|
||||
joint: str
|
||||
ros_type: str
|
||||
gz_type: str
|
||||
publish_hz: float
|
||||
|
||||
|
||||
def load_wrist_ft(
|
||||
path: Optional[Path] = None, root: Optional[Path] = None
|
||||
) -> WristFtSpec:
|
||||
"""读腕部 FT 配置。找不到或写错时抛 WristFtConfigError，消息里给出修法。"""
|
||||
if path is None:
|
||||
base = root if root is not None else Path.cwd()
|
||||
path = Path(base) / DEFAULT_WRIST_FT_REL
|
||||
path = Path(path)
|
||||
|
||||
if not path.is_file():
|
||||
raise WristFtConfigError(
|
||||
f"腕部 FT 配置不存在: {path}\n"
|
||||
f" 它声明了仿真腕部力扭矩的话题名与挂载关节（桥接从它派生）。\n"
|
||||
f" 缺了它，S07 的接触判据在仿真里永远没有数据源。\n"
|
||||
f" 确认仓库根下存在 {DEFAULT_WRIST_FT_REL}。"
|
||||
)
|
||||
|
||||
try:
|
||||
doc = yaml.safe_load(path.read_text(encoding="utf-8")) or {}
|
||||
except yaml.YAMLError as exc:
|
||||
raise WristFtConfigError(
|
||||
f"腕部 FT 配置 YAML 解析失败: {path}\n 原因: {exc}"
|
||||
) from exc
|
||||
|
||||
if not isinstance(doc, dict):
|
||||
raise WristFtConfigError(f"{path} 的顶层应该是映射（key: value 形式）。")
|
||||
|
||||
missing = [
|
||||
k for k in ("topic", "joint", "ros_type", "gz_type") if not str(doc.get(k, "")).strip()
|
||||
]
|
||||
if missing:
|
||||
raise WristFtConfigError(
|
||||
f"{path} 缺字段: {', '.join(missing)}。\n"
|
||||
f" topic 必须与 arm.sim.sensors.xacro 里 wrist_ft_topic 的默认值\n"
|
||||
f" 完全一致，也要与应用层按 sn 推出的名字一致 —— 对不上时桥接器只是\n"
|
||||
f" 订阅了一个不存在的话题，不报错，而 S07 会一直判不到接触。"
|
||||
)
|
||||
|
||||
try:
|
||||
hz = float(doc.get("publish_hz", 100.0))
|
||||
except (TypeError, ValueError) as exc:
|
||||
raise WristFtConfigError(
|
||||
f"{path} 的 publish_hz 不是数字: {doc.get('publish_hz')!r}"
|
||||
) from exc
|
||||
if hz <= 0.0:
|
||||
raise WristFtConfigError(f"{path} 的 publish_hz 必须为正，收到 {hz}")
|
||||
|
||||
return WristFtSpec(
|
||||
topic=str(doc["topic"]).strip(),
|
||||
joint=str(doc["joint"]).strip(),
|
||||
ros_type=str(doc["ros_type"]).strip(),
|
||||
gz_type=str(doc["gz_type"]).strip(),
|
||||
publish_hz=hz,
|
||||
)
|
||||
|
||||
|
||||
def bridge_args(spec: WristFtSpec) -> List[str]:
|
||||
"""生成 ros_gz_bridge parameter_bridge 的话题规则。
|
||||
|
||||
只做**传输**：gz 类型 → ROS 类型，话题名两侧保持同名。
|
||||
这条规则的终点就是应用层真正订阅的那个话题，所以话题名必须一次写对 ——
|
||||
桥接器没有"改名"能力，写错只会得到"话题上没数据"。
|
||||
"""
|
||||
return [f"{spec.topic}@{spec.ros_type}[{spec.gz_type}"]
|
||||
|
||||
|
||||
def check_wrist_ft_sensor(
|
||||
urdf_text: str, spec: WristFtSpec
|
||||
) -> List[str]:
|
||||
"""描述级静态检查：URDF 里到底有没有那个腕部 FT 传感器。
|
||||
|
||||
为什么需要（与 ``check_tactile_collisions`` 同一个理由）：
|
||||
Gazebo 对**挂错关节**的传感器不报错 —— 传感器会被静默丢弃，
|
||||
话题永远没有数据，现象是"S07 判不到接触"。光看 launch 日志完全看不出来
|
||||
（桥接器和 Gazebo 都说一切正常）。这里在启动时就把它指出来。
|
||||
|
||||
返回告警列表（空 = 没问题）；**不抛异常** —— 缺了它仿真仍能跑，
|
||||
只是 S07 的判据要退回触觉兜底，那是降级不是崩溃。
|
||||
"""
|
||||
warnings: List[str] = []
|
||||
if not urdf_text:
|
||||
return ["URDF 文本为空，无法核对腕部 FT 传感器"]
|
||||
|
||||
if f'reference="{spec.joint}"' not in urdf_text:
|
||||
near = [ln for ln in urdf_text.splitlines() if "gazebo reference=" in ln and "joint" in ln]
|
||||
warnings.append(
|
||||
f"URDF 里没有 <gazebo reference=\"{spec.joint}\"> —— "
|
||||
f"腕部 FT 传感器挂错关节了，它会被 Gazebo 静默丢弃、话题永远没数据。\n"
|
||||
f" 期望挂在 {spec.joint}（最后一个 revolute 关节：它下游挂着整个腕部以下"
|
||||
f" 的载荷，这才是工程意义上的腕部 FT）。\n"
|
||||
f" 注意不能挂 fixed 关节（如 flange_joint）——物理引擎不把它当独立关节"
|
||||
f" 求解，传感器永远没输出且不报错。\n"
|
||||
f" 当前 URDF 里的 <gazebo reference> 关节有: {', '.join(sorted(set(near))) or '（无）'}"
|
||||
)
|
||||
|
||||
if spec.topic not in urdf_text:
|
||||
warnings.append(
|
||||
f"URDF 里找不到腕部 FT 的话题 {spec.topic} —— 传感器的话题名与配置不一致。\n"
|
||||
f" 两侧必须一致：config 的 topic、arm.sim.sensors.xacro 的 wrist_ft_topic、\n"
|
||||
f" 以及应用层按 sn 推出的 /{{sn}}/ft_sensor_wrench（仿真 sn = sim）。"
|
||||
)
|
||||
|
||||
if "force_torque" not in urdf_text:
|
||||
warnings.append(
|
||||
"URDF 里没有任何 type=\"force_torque\" 传感器 —— 腕部 FT 不会被创建。"
|
||||
)
|
||||
|
||||
return warnings
|
||||
@@ -59,6 +59,20 @@ class HoleTruth:
|
||||
#: 两孔的像素半径（环的物理半径按透视缩放；Hough 路径给的是它自己的估计，
|
||||
#: 真值路径给它一个**可信的**值 —— 上下游只把它当"尺度参考"用）
|
||||
hole_radii: List[float] = field(default_factory=list)
|
||||
#: 两孔**环心的世界坐标**（米），与 ``hole_points`` 一一对应、同序。
|
||||
#:
|
||||
#: 为什么真值路径要把三维也带出来（2026-09-24 的教训）：
|
||||
#: 原来只给像素，调用方（stage2）再拿**深度图**反投影回三维。仿真里
|
||||
#: 那一步是错的，而且错法很隐蔽：孔是**贯通孔**，孔位正下方就是台面
|
||||
#: 通槽 —— 深度相机在孔位看到的是**槽底/地面**（实测 0.62m），
|
||||
#: 而工件面其实只有 0.21m。拿 0.21m 的像素尺度去乘 0.62m 的深度，
|
||||
#: 三角化出的 k1/k2 间距变成 169mm（真值 58mm），被间距门槛丢掉。
|
||||
#: 真值路径**本来就知道**孔在世界系的精确位置，没有理由再绕一圈深度图。
|
||||
hole_world: List[Tuple[float, float, float]] = field(default_factory=list)
|
||||
#: 工件**上表面中心**的世界坐标（粗居中用；同样是"真值就该直接用"）。
|
||||
part_center_world: Optional[Tuple[float, float, float]] = None
|
||||
#: 工件上表面的世界 z（= 二阶段的对孔基准面）。
|
||||
part_top_z: Optional[float] = None
|
||||
#: 工件在画面里占的框（孔检测的 ROI 用；也是"工件中心"的来源）
|
||||
bbox: Tuple[int, int, int, int] = (0, 0, 0, 0)
|
||||
#: 给人看的说明（进了日志才知道这个数是哪来的）
|
||||
@@ -154,9 +168,11 @@ def locate_holes(
|
||||
|
||||
pts: List[Tuple[float, float]] = []
|
||||
rad: List[float] = []
|
||||
holes_world: List[Tuple[float, float, float]] = []
|
||||
for ring in rings:
|
||||
wx, wy = _object_world_xy(obj, ring.x, ring.y)
|
||||
got = project_point(T_cam_world, intr, (wx, wy, float(obj.z) + ring.z))
|
||||
wz = float(obj.z) + float(ring.z)
|
||||
got = project_point(T_cam_world, intr, (wx, wy, wz))
|
||||
if got is None:
|
||||
return HoleTruth(
|
||||
ok=False,
|
||||
@@ -165,6 +181,7 @@ def locate_holes(
|
||||
u, v, z = got
|
||||
pts.append((u, v))
|
||||
rad.append(float(intr[0]) * float(ring.radius) / z)
|
||||
holes_world.append((wx, wy, wz))
|
||||
|
||||
# k1/k2 的左右（上下）规则与 Hough 路径一致：水平对按 x，竖直对按 y。
|
||||
(p, q) = pts
|
||||
@@ -173,11 +190,24 @@ def locate_holes(
|
||||
if pts[0][key] > pts[1][key]:
|
||||
pts = [pts[1], pts[0]]
|
||||
rad = [rad[1], rad[0]]
|
||||
holes_world = [holes_world[1], holes_world[0]]
|
||||
|
||||
bbox = object_bbox_px(T_cam_world, intr, sim_scene.world_boxes(spec, obj, root), image_shape)
|
||||
boxes = sim_scene.world_boxes(spec, obj, root)
|
||||
bbox = object_bbox_px(T_cam_world, intr, boxes, image_shape)
|
||||
if bbox is None:
|
||||
return HoleTruth(ok=False, why="工件整体投影不出画面框（角点都在相机背后）")
|
||||
|
||||
# 工件上表面中心与上表面 z：从各碰撞体的世界 AABB 取（AABB 是轴对齐的，
|
||||
# 只有 yaw 的物体上它正好等于"外接矩形"，对"粗居中"这个用途是合适的）。
|
||||
part_center: Optional[Tuple[float, float, float]] = None
|
||||
part_top: Optional[float] = None
|
||||
if boxes:
|
||||
xs = [float(v) for b in boxes for v in (b[0], b[1])]
|
||||
ys = [float(v) for b in boxes for v in (b[2], b[3])]
|
||||
zs = [float(v) for b in boxes for v in (b[4], b[5])]
|
||||
part_top = max(zs)
|
||||
part_center = (0.5 * (min(xs) + max(xs)), 0.5 * (min(ys) + max(ys)), part_top)
|
||||
|
||||
dist_px = math.hypot(pts[0][0] - pts[1][0], pts[0][1] - pts[1][1])
|
||||
note = (
|
||||
f"{obj.name}（模型 {obj.model}） 环心距 {dist_px:.1f}px "
|
||||
@@ -188,6 +218,9 @@ def locate_holes(
|
||||
why="",
|
||||
hole_points=[(float(pts[0][0]), float(pts[0][1])), (float(pts[1][0]), float(pts[1][1]))],
|
||||
hole_radii=[float(rad[0]), float(rad[1])],
|
||||
hole_world=holes_world,
|
||||
part_center_world=part_center,
|
||||
part_top_z=part_top,
|
||||
bbox=bbox,
|
||||
note=note,
|
||||
)
|
||||
|
||||
@@ -103,11 +103,57 @@ def _prepare(context, *args, **kwargs):
|
||||
world_path, _world_origin = resolve_world(world, root, fallback=default_world(root))
|
||||
world_path = str(world_path)
|
||||
|
||||
# 腕部 FT 的话题名先读出来，喂给 xacro —— 这个话题名有三处要对上
|
||||
# （配置、URDF 里的 <topic>、应用层按 sn 推出的 /{sn}/ft_sensor_wrench），
|
||||
# 让配置当唯一源头，xacro 不再自己写一份。
|
||||
from arm_sim_gazebo.sim_wrist_ft import (
|
||||
WristFtConfigError,
|
||||
bridge_args as wrist_ft_bridge_args,
|
||||
check_wrist_ft_sensor,
|
||||
)
|
||||
from arm_sim_gazebo.sim_wrist_ft import load_wrist_ft
|
||||
|
||||
try:
|
||||
wft = load_wrist_ft(root=root)
|
||||
except WristFtConfigError as exc:
|
||||
raise RuntimeError(f"\n[gazebo_sim] 仿真腕部 FT 配置有问题：\n{exc}\n") from exc
|
||||
|
||||
# 抓取锁：仿真里唯一能保证"夹住的东西不会在机械臂连续运动时滑掉"的机制，
|
||||
# 说明见 devices/sim/gazebo/config/grasp_lock.yaml 与 sim_grasp_lock.py。
|
||||
from arm_sim_gazebo.sim_grasp_lock import (
|
||||
DEFAULT_OBJECT_SDF_TMPL,
|
||||
GraspLockError,
|
||||
check_grasp_lock_sdf,
|
||||
)
|
||||
from arm_sim_gazebo.sim_grasp_lock import load_grasp_lock
|
||||
|
||||
try:
|
||||
glock = load_grasp_lock(root=root)
|
||||
except GraspLockError as exc:
|
||||
raise RuntimeError(
|
||||
f"\n[gazebo_sim] 仿真抓取锁配置有问题：\n{exc}\n"
|
||||
f" （只想先跑起来的话，把 grasp_lock.yaml 的 enabled 改成 false）\n"
|
||||
) from exc
|
||||
|
||||
# 静态核对：物体 SDF 里到底有没有那个插件、参数对不对得上。
|
||||
# 差一处就**静默失效**（现象只是"规莫名掉了"），所以必须在这里说清楚。
|
||||
glock_warnings: List[str] = []
|
||||
if glock.enabled:
|
||||
_obj_sdf = root / DEFAULT_OBJECT_SDF_TMPL.format(object=glock.object)
|
||||
try:
|
||||
_sdf_text = _obj_sdf.read_text(encoding="utf-8")
|
||||
except OSError:
|
||||
_sdf_text = ""
|
||||
glock_warnings = check_grasp_lock_sdf(
|
||||
_sdf_text, glock, sdf_label=str(_obj_sdf)
|
||||
)
|
||||
|
||||
urdf, report = prepare_urdf(
|
||||
xacro_path=default_xacro(root),
|
||||
controllers_file=default_controllers(root),
|
||||
load_gripper=load_gripper != "false",
|
||||
with_sensors=with_sensors != "false",
|
||||
wrist_ft_topic=wft.topic,
|
||||
)
|
||||
wname = world_name(world_path)
|
||||
missing_base = check_base_plugins(Path(world_path))
|
||||
@@ -120,7 +166,10 @@ def _prepare(context, *args, **kwargs):
|
||||
LogInfo(msg=f"[gazebo_sim] 仓库根: {root}"),
|
||||
LogInfo(msg=f"[gazebo_sim] 世界: {world_path} (world 名: {wname})"),
|
||||
LogInfo(msg=f"[gazebo_sim] URDF 归一化: {report.summary()}"),
|
||||
LogInfo(msg=f"[gazebo_sim] {glock.describe()}"),
|
||||
]
|
||||
for _w in glock_warnings:
|
||||
actions.append(LogInfo(msg=f"[gazebo_sim] ⚠ 抓取锁检查: {_w}"))
|
||||
if missing_base:
|
||||
actions.append(
|
||||
LogInfo(
|
||||
@@ -191,6 +240,43 @@ def _prepare(context, *args, **kwargs):
|
||||
)
|
||||
)
|
||||
|
||||
# --- 3b) 世界模型位姿回读：物体被撞走时唯一能指名报错的数据源 ------------
|
||||
# 为什么值得一条独立桥接：`perception.source=truth` 是**纯几何**的，
|
||||
# 它按 scene.yaml 的**名义位姿**算孔位，从不问 Gazebo 物体现在在哪。
|
||||
# 工件被碰走（实测：掉进台面通槽落到地面）之后，"真值"开始撒谎，
|
||||
# 而现象全都指向别处（S05 报"两孔间距 169mm"、S07 一路下压不接触）。
|
||||
# 这一条把 gz 的"每个模型实际位姿"搬进 ROS，让 sim_truth_perception
|
||||
# 能把它和 scene.yaml 对上、指名报"工件不在它该在的地方"。
|
||||
# 具体参数（话题名/类型/漂移容差）在 scene.yaml 的 pose_info 段。
|
||||
from arm_sim_gazebo.sim_scene import load_scene as _load_scene
|
||||
|
||||
try:
|
||||
_spec = _load_scene(None, root)
|
||||
except Exception as exc: # noqa: BLE001 —— 场景读不出来要说清是场景的问题
|
||||
raise RuntimeError(f"\n[gazebo_sim] 场景配置有问题：\n{exc}\n") from exc
|
||||
if _spec.pose_info is not None:
|
||||
_pi = _spec.pose_info
|
||||
actions.append(
|
||||
Node(
|
||||
package="ros_gz_bridge",
|
||||
executable="parameter_bridge",
|
||||
name="pose_info_bridge",
|
||||
output="screen",
|
||||
arguments=[f"{_pi.gz_topic}@{_pi.ros_type}[{_pi.gz_type}"],
|
||||
# gz 的话题名固定是 /world/<world>/pose/info（命名空间由 gz 决定，
|
||||
# 应用侧改不了），ros_topic 只是"到 ROS 这一侧叫什么"。
|
||||
# ros_gz_bridge 没有改名能力，所以这里用 remap 把名字对到
|
||||
# scene.yaml 声明的那一个 —— 两侧对不上的表现是
|
||||
# "桥接器起来了但一个消息都没有"，那是个假故障。
|
||||
remappings=[(_pi.gz_topic, _pi.ros_topic)],
|
||||
)
|
||||
)
|
||||
actions.append(
|
||||
LogInfo(
|
||||
msg=f"[gazebo_sim] 世界模型位姿回读: {_pi.gz_topic} → {_pi.ros_topic}"
|
||||
)
|
||||
)
|
||||
|
||||
# --- 4) 相机：桥接 → 适配器 → （世界相机）补 TF --------------------------
|
||||
# 只在挂了传感器时才做。--no-sensors 时跳过是必要的：适配器会去订阅
|
||||
# 永远不存在的 gz 话题，它**不会报错**，只是静默没输出 —— 而"相机没图"
|
||||
@@ -357,6 +443,26 @@ def _prepare(context, *args, **kwargs):
|
||||
)
|
||||
)
|
||||
|
||||
# 4e) 腕部 FT：只需桥接，**不需要适配器** ------------------------------
|
||||
# 与触觉不同：触觉要轴向映射/基线/单位换算/contact 折算，所以有适配器；
|
||||
# 腕部 FT 的 gz 输出本来就是牛顿、坐标系就是 child(link7) 系，而
|
||||
# ros_gz_bridge **原生支持** gz.msgs.Wrench → WrenchStamped（实测确认）。
|
||||
# 于是仿真产出的就是真机同名的 /<sn>/ft_sensor_wrench（同名同类型同单位），
|
||||
# 应用层一行分支都不用写。少一层节点就少一个"配错了不报错"的地方。
|
||||
if with_sensors != "false":
|
||||
for _w in check_wrist_ft_sensor(urdf, wft):
|
||||
actions.append(LogInfo(msg=f"[gazebo_sim] ⚠ 腕部 FT 检查: {_w}"))
|
||||
|
||||
actions.append(
|
||||
Node(
|
||||
package="ros_gz_bridge",
|
||||
executable="parameter_bridge",
|
||||
name="wrist_ft_bridge",
|
||||
output="screen",
|
||||
arguments=wrist_ft_bridge_args(wft),
|
||||
)
|
||||
)
|
||||
|
||||
# --- 5) spawn 机器人 -----------------------------------------------------
|
||||
# -world 必须给对：world 名写错时 Gazebo **不报错**，只是实体没被创建，
|
||||
# 表现成"launch 一切正常但 RViz 里什么都没有"。
|
||||
@@ -533,6 +639,14 @@ def _prepare(context, *args, **kwargs):
|
||||
"mimics": mimics,
|
||||
"robot_description": urdf,
|
||||
"namespace": "/gripper",
|
||||
# 抓取锁（仿真专用，见 sim_grasp_lock.py）：
|
||||
# 夹住后把物体固定到手指上，避免"位置控制夹持 +
|
||||
# 连续轨迹"导致的打滑掉件。
|
||||
"grasp_lock_enabled": bool(glock.enabled),
|
||||
"grasp_lock_object": str(glock.object),
|
||||
"grasp_lock_attach_topic": str(glock.attach_topic),
|
||||
"grasp_lock_detach_topic": str(glock.detach_topic),
|
||||
"grasp_lock_timeout_ms": int(glock.publish_timeout_ms),
|
||||
},
|
||||
{"use_sim_time": True},
|
||||
],
|
||||
|
||||
@@ -0,0 +1,240 @@
|
||||
"""sim_grasp_lock.py 的测试：配置读取、与物体 SDF 的静态核对、触发命令拼装。
|
||||
|
||||
为什么这个模块值得单独一套测试：它管的**全是静默失效**。
|
||||
抓取锁不生效时 Gazebo 什么都不说 —— 插件没写进模型、child_model 名字不对、
|
||||
话题名两侧不一致，全都表现为同一个现象："规莫名其妙掉了"（要等到 S07/S08、
|
||||
而且日志里看不到真因）。所以这里把"能不能发现"逐条钉住。
|
||||
|
||||
三条重点（都是真实踩到的）：
|
||||
* 插件参数与配置**必须两侧一致**，而名字有好几个（child_model/child_link/
|
||||
attach_topic/detach_topic），漏一个就是静默失效 —— 所以 check 要逐项查，
|
||||
而不是只查"插件在不在"。
|
||||
* child_link 必须是 Gazebo 的**真名**：URDF 里与固定关节相连的连杆会被
|
||||
sdformat 合并（lump），手指那样的名字在 Gazebo 里根本不存在。
|
||||
* 配置文件缺失**不该报错**（锁是可选的补救措施），但文件在、字段写错
|
||||
必须报错（那说明有人动过它并且写错了）。
|
||||
|
||||
全部不依赖 ROS、不依赖 Gazebo。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import pytest
|
||||
import yaml
|
||||
|
||||
from arm_sim_gazebo.sim_grasp_lock import (
|
||||
DEFAULT_GRASP_LOCK_REL,
|
||||
GraspLockError,
|
||||
GraspLockSpec,
|
||||
bridge_target_links,
|
||||
check_grasp_lock_sdf,
|
||||
load_grasp_lock,
|
||||
)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 配置读取
|
||||
# ---------------------------------------------------------------------------
|
||||
def _write(tmp_path, doc) -> "object":
|
||||
p = tmp_path / "grasp_lock.yaml"
|
||||
p.write_text(yaml.safe_dump(doc, allow_unicode=True), encoding="utf-8")
|
||||
return p
|
||||
|
||||
|
||||
def _ok_doc() -> dict:
|
||||
return {
|
||||
"enabled": True,
|
||||
"object": "gauge",
|
||||
"object_link": "body",
|
||||
"robot_model": "rizon_sim",
|
||||
"robot_link": "sim_left_finger_mount",
|
||||
"attach_topic": "/sim/grasp_lock/attach",
|
||||
"detach_topic": "/sim/grasp_lock/detach",
|
||||
"state_topic": "/sim/grasp_lock/state",
|
||||
"publish_timeout_ms": 3000,
|
||||
}
|
||||
|
||||
|
||||
def test_repo_config_loads_and_describes():
|
||||
"""仓库里真实的那份配置必须能读、且 describe() 不空。"""
|
||||
spec = load_grasp_lock(root=__import__("pathlib").Path(__file__).resolve().parents[3])
|
||||
assert spec.enabled in (True, False)
|
||||
assert spec.object
|
||||
assert "抓取锁" in spec.describe()
|
||||
if spec.enabled:
|
||||
assert spec.robot_model and spec.robot_link
|
||||
assert spec.attach_topic != spec.detach_topic
|
||||
|
||||
|
||||
def test_missing_file_is_disabled_not_error(tmp_path):
|
||||
"""文件不在 → 返回"关闭"，而不是抛。锁是可选的补救措施，不该拦住仿真。"""
|
||||
spec = load_grasp_lock(tmp_path / "nope.yaml")
|
||||
assert spec.enabled is False
|
||||
|
||||
|
||||
def test_enabled_requires_names(tmp_path):
|
||||
"""开着但缺 robot_model/robot_link → 必须抛，且消息里说清后果。"""
|
||||
doc = _ok_doc()
|
||||
doc.pop("robot_link")
|
||||
with pytest.raises(GraspLockError) as ei:
|
||||
load_grasp_lock(_write(tmp_path, doc))
|
||||
msg = str(ei.value)
|
||||
assert "robot_link" in msg
|
||||
# 必须解释"写错会静默失效"，否则现场只会去改阈值
|
||||
assert "静默失效" in msg
|
||||
|
||||
|
||||
def test_attach_and_detach_must_differ(tmp_path):
|
||||
"""两个话题相同 = 上锁解锁走同一条消息 → 状态随机。必须拦住。"""
|
||||
doc = _ok_doc()
|
||||
doc["detach_topic"] = doc["attach_topic"]
|
||||
with pytest.raises(GraspLockError) as ei:
|
||||
load_grasp_lock(_write(tmp_path, doc))
|
||||
assert "attach_topic" in str(ei.value)
|
||||
|
||||
|
||||
def test_finger_anchor_is_rejected_and_explains_the_yank(tmp_path):
|
||||
"""锚点不许是**手指**：张爪时会把被锁的工件从孔里拽出来（2026-09-24 实测）。
|
||||
|
||||
症状只有一句"通止规掉了(E1)"，看不到真因 —— 所以必须在**读配置时**就拦住，
|
||||
并且把"为什么"和"改成什么"写进消息里。
|
||||
"""
|
||||
doc = _ok_doc()
|
||||
doc["robot_link"] = "sim_left_finger_mount"
|
||||
with pytest.raises(GraspLockError) as ei:
|
||||
load_grasp_lock(_write(tmp_path, doc))
|
||||
msg = str(ei.value)
|
||||
assert "sim_left_finger_mount" in msg
|
||||
assert "sim_link7" in msg, "必须给出正确的锚点，否则现场还得自己猜"
|
||||
assert "拽" in msg or "拉出" in msg, "必须说清物理后果（工件被拽出孔）"
|
||||
|
||||
|
||||
def test_bad_yaml_reports_path(tmp_path):
|
||||
"""YAML 语法错 → 消息里带路径（现场一眼知道改哪个文件）。"""
|
||||
p = tmp_path / "grasp_lock.yaml"
|
||||
p.write_text("enabled: [true\n", encoding="utf-8")
|
||||
with pytest.raises(GraspLockError) as ei:
|
||||
load_grasp_lock(p)
|
||||
assert "grasp_lock.yaml" in str(ei.value)
|
||||
|
||||
|
||||
def test_disabled_skips_required_fields(tmp_path):
|
||||
"""enabled: false 时不该因为缺字段而报错 —— 关了就是关了。"""
|
||||
doc = _ok_doc()
|
||||
doc["enabled"] = False
|
||||
doc.pop("robot_model")
|
||||
doc.pop("robot_link")
|
||||
spec = load_grasp_lock(_write(tmp_path, doc))
|
||||
assert spec.enabled is False
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 与物体 SDF 的核对
|
||||
# ---------------------------------------------------------------------------
|
||||
def _sdf(**over) -> str:
|
||||
"""一份"长得像"插件块的 SDF 文本（只关心被查的那几个字段）。"""
|
||||
vals = {
|
||||
"child_model": "rizon_sim",
|
||||
"child_link": "sim_left_finger_mount",
|
||||
"attach_topic": "/sim/grasp_lock/attach",
|
||||
"detach_topic": "/sim/grasp_lock/detach",
|
||||
}
|
||||
vals.update(over)
|
||||
return (
|
||||
"<sdf><model name='gauge'><link name='body'/></model>\n"
|
||||
"<plugin filename='ignition-gazebo-detachable-joint-system' "
|
||||
"name='ignition::gazebo::systems::DetachableJoint'>\n"
|
||||
f" <child_model>{vals['child_model']}</child_model>\n"
|
||||
f" <child_link>{vals['child_link']}</child_link>\n"
|
||||
f" <attach_topic>{vals['attach_topic']}</attach_topic>\n"
|
||||
f" <detach_topic>{vals['detach_topic']}</detach_topic>\n"
|
||||
"</plugin></sdf>"
|
||||
)
|
||||
|
||||
|
||||
def _spec(**over) -> GraspLockSpec:
|
||||
base = dict(
|
||||
enabled=True,
|
||||
object="gauge",
|
||||
object_link="body",
|
||||
robot_model="rizon_sim",
|
||||
robot_link="sim_left_finger_mount",
|
||||
attach_topic="/sim/grasp_lock/attach",
|
||||
detach_topic="/sim/grasp_lock/detach",
|
||||
state_topic="/sim/grasp_lock/state",
|
||||
publish_timeout_ms=3000,
|
||||
)
|
||||
base.update(over)
|
||||
return GraspLockSpec(**base)
|
||||
|
||||
|
||||
def test_consistent_sdf_has_no_warning():
|
||||
assert check_grasp_lock_sdf(_sdf(), _spec()) == []
|
||||
|
||||
|
||||
def test_missing_plugin_is_reported():
|
||||
"""模型里没有插件 → 必须报，而且要说清"不会报错，只是规照样掉"。"""
|
||||
warns = check_grasp_lock_sdf("<sdf><model name='gauge'/></sdf>", _spec())
|
||||
assert len(warns) == 1
|
||||
assert "DetachableJoint" in warns[0]
|
||||
assert "不会报错" in warns[0]
|
||||
|
||||
|
||||
def test_wrong_child_model_is_reported():
|
||||
"""child_model 与 spawn 的 -name 不一致 → 插件 Configure 直接失败。"""
|
||||
warns = check_grasp_lock_sdf(_sdf(child_model="rizon"), _spec())
|
||||
assert any("<child_model>" in w for w in warns)
|
||||
|
||||
|
||||
def test_wrong_child_link_is_reported_and_explains_lumping():
|
||||
"""child_link 用了 URDF 名（Gazebo 里被 lump 掉的真名不存在）→ 必须报。"""
|
||||
warns = check_grasp_lock_sdf(_sdf(child_link="sim_left_finger"), _spec())
|
||||
msg = "\n".join(warns)
|
||||
assert "<child_link>" in msg
|
||||
# 解释"要填真名、真名怎么查"，否则现场只会照抄 URDF
|
||||
assert "合并" in msg and "world_pose_info" in msg
|
||||
|
||||
|
||||
def test_topic_mismatch_is_reported():
|
||||
warns = check_grasp_lock_sdf(_sdf(attach_topic="/attachable/attach"), _spec())
|
||||
assert any("attach_topic" in w for w in warns)
|
||||
|
||||
|
||||
def test_disabled_lock_never_warns():
|
||||
"""关掉时不该报 —— 没插件是预期状态，不该刷告警让人忽略真告警。"""
|
||||
assert check_grasp_lock_sdf("<sdf/>", _spec(enabled=False)) == []
|
||||
|
||||
|
||||
def test_empty_sdf_is_reported():
|
||||
warns = check_grasp_lock_sdf("", _spec())
|
||||
assert warns and "读不到" in warns[0]
|
||||
|
||||
|
||||
def test_target_links_lists_both_ends():
|
||||
links = bridge_target_links(_spec())
|
||||
assert links == ["gauge.body", "rizon_sim.sim_left_finger_mount"]
|
||||
|
||||
|
||||
def test_default_rel_path_matches_repo_layout():
|
||||
"""默认路径必须真的是仓库里那份（改了目录而忘了改常量 = 锁永远静默关闭）。"""
|
||||
assert DEFAULT_GRASP_LOCK_REL.endswith("config/grasp_lock.yaml")
|
||||
|
||||
|
||||
def test_topic_cmd_publishes_with_duration_not_wait():
|
||||
"""`ign topic` 的参数必须带 -d（duration），**不能**带 -w。
|
||||
|
||||
2026-09-24 踩到：写成了 ``-w <秒>``（以为它"等订阅者"），而这个版本的
|
||||
ign topic 没有 -w —— 参数变成多余的位置参数：
|
||||
"The following arguments were not expected: 3"
|
||||
于是上锁/解锁永远失败，而现象和"锁根本没生效"一模一样（规照样掉），
|
||||
只看日志根本分不出来。这里把命令行钉死。
|
||||
"""
|
||||
from arm_sim_gazebo.sim_grasp_lock import _ign_topic_cmd
|
||||
|
||||
cmd = _ign_topic_cmd("ign", "/sim/grasp_lock/attach", 3000)
|
||||
assert "-d" in cmd
|
||||
assert "-w" not in cmd
|
||||
assert cmd[cmd.index("-t") + 1] == "/sim/grasp_lock/attach"
|
||||
assert cmd[cmd.index("-m") + 1] == "ignition.msgs.Empty"
|
||||
# duration 必须是正的浮点秒数
|
||||
assert float(cmd[cmd.index("-d") + 1]) > 0.0
|
||||
@@ -0,0 +1,90 @@
|
||||
"""sim_pose_service.py 的测试：把模型 set_pose 到指定位姿的那一层。
|
||||
|
||||
为什么这层要单独测：它唯一的实现细节（`--reqtype ignition.msgs.Pose`）
|
||||
**错一个字母就只是"服务调用失败"**，而报错信息完全不提原因；而调用方
|
||||
（场景加载脚本、真值感知的启动复位）都把它当"可选步骤"处理 ——
|
||||
于是它坏掉的表现是"物体没被复位"，没有任何一处日志指名它。
|
||||
|
||||
这里只测**纯函数**（world 名解析、yaw→四元数）：真正调 `ign` 需要 Gazebo，
|
||||
放在 `test_check_sim_scene.py` 那种"要环境"的地方没意义（离线机器跑不了）。
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
|
||||
import pytest
|
||||
|
||||
from arm_sim_gazebo.sim_pose_service import (
|
||||
set_model_pose,
|
||||
set_model_pose_yaw,
|
||||
world_from_gz_topic,
|
||||
yaw_to_quat,
|
||||
)
|
||||
|
||||
|
||||
@pytest.mark.parametrize(
|
||||
"topic,want",
|
||||
[
|
||||
("/world/assembly/pose/info", "assembly"),
|
||||
("/world/assembly/pose", "assembly"),
|
||||
(" /world/assembly/pose/info ", "assembly"),
|
||||
("/world/assembly/pose/info/", "assembly"),
|
||||
("/world/sim_room/pose/info", "sim_room"),
|
||||
],
|
||||
)
|
||||
def test_world_from_gz_topic(topic, want):
|
||||
assert world_from_gz_topic(topic) == want
|
||||
|
||||
|
||||
@pytest.mark.parametrize(
|
||||
"topic",
|
||||
["", None, "assembly", "/world/pose/info", "/world/assembly/other", "world/assembly/pose/info"],
|
||||
)
|
||||
def test_world_from_gz_topic_rejects(topic):
|
||||
"""认不出来必须返回 None（调用方据此**跳过**复位并说清楚），不能猜。
|
||||
|
||||
猜错的后果是"服务名拼错了"，而 `ign service` 对不存在的服务只会超时 ——
|
||||
现象就变成"复位偶发失败"，没人会连到 world 名上。
|
||||
"""
|
||||
assert world_from_gz_topic(topic) is None
|
||||
|
||||
|
||||
def test_yaw_to_quat_matches_rotation():
|
||||
for yaw in (0.0, 0.3, -1.2, math.pi):
|
||||
x, y, z, w = yaw_to_quat(yaw)
|
||||
# 绕 Z 的单位四元数 → 旋转矩阵，第一列必须是 (cos yaw, sin yaw, 0)
|
||||
c, s = math.cos(yaw), math.sin(yaw)
|
||||
R = [
|
||||
[1 - 2 * (y * y + z * z), 2 * (x * y - z * w), 2 * (x * z + y * w)],
|
||||
[2 * (x * y + z * w), 1 - 2 * (x * x + z * z), 2 * (y * z - x * w)],
|
||||
]
|
||||
assert R[0][0] == pytest.approx(c, abs=1e-12)
|
||||
assert R[1][0] == pytest.approx(s, abs=1e-12)
|
||||
assert R[0][1] == pytest.approx(-s, abs=1e-12)
|
||||
assert (x, y) == (0.0, 0.0)
|
||||
|
||||
|
||||
def test_set_model_pose_yaw_is_the_planar_wrapper(monkeypatch):
|
||||
"""平面版必须是同一个实现（不是抄一份）：这里钉住它转发的内容一样。"""
|
||||
seen = []
|
||||
|
||||
def fake(world, name, pos, quat, timeout_s=0.0): # noqa: ANN001
|
||||
seen.append((world, name, tuple(pos), tuple(quat)))
|
||||
return True, "data: true"
|
||||
|
||||
monkeypatch.setattr("arm_sim_gazebo.sim_pose_service.set_model_pose", fake)
|
||||
ok, _ = set_model_pose_yaw("assembly", "workpiece", (0.52, 0.0, 0.40), math.pi / 2)
|
||||
assert ok
|
||||
assert seen == [("assembly", "workpiece", (0.52, 0.0, 0.40), yaw_to_quat(math.pi / 2))]
|
||||
|
||||
|
||||
def test_set_model_pose_reports_failure_without_raising():
|
||||
"""失败只返回 False + 输出：调用方要能"降级继续"，不能因为复位失败就崩。
|
||||
|
||||
这里用一个不存在的 world（服务不存在）走真命令行路径 —— 只要环境里有
|
||||
`ign` 就会走到"超时/失败"，没有 `ign` 则返回 127，两种都必须是 (False, 原因)。
|
||||
"""
|
||||
ok, out = set_model_pose("no_such_world_xyz", "nope", (0, 0, 0), (0, 0, 0, 1), timeout_s=2.0)
|
||||
assert ok is False
|
||||
assert out # 必须带原因，不能返回空字符串
|
||||
@@ -318,6 +318,127 @@ def test_load_scene_records_source_path(tmp_path):
|
||||
assert load_scene(root=tmp_path).source == p
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# pose_info（世界模型位姿回读）—— 可选段，写错必须当场拦住
|
||||
# ---------------------------------------------------------------------------
|
||||
def test_load_scene_without_pose_info_is_fine(tmp_path):
|
||||
"""**老场景文件没有这一段也必须能读**：它是可选的，缺了只是不做漂移监视。"""
|
||||
_write_scene(tmp_path, [_obj()])
|
||||
assert load_scene(root=tmp_path).pose_info is None
|
||||
|
||||
|
||||
def test_load_scene_parses_pose_info(tmp_path):
|
||||
_write_scene(tmp_path, [_obj()])
|
||||
p = tmp_path / "devices/sim/gazebo/config/scene.yaml"
|
||||
doc = yaml.safe_load(p.read_text(encoding="utf-8"))
|
||||
doc["pose_info"] = {
|
||||
"gz_topic": "/world/assembly/pose/info",
|
||||
"ros_topic": "/sim/world_pose_info",
|
||||
}
|
||||
p.write_text(yaml.safe_dump(doc, allow_unicode=True), encoding="utf-8")
|
||||
pi = load_scene(root=tmp_path).pose_info
|
||||
assert pi is not None
|
||||
assert pi.gz_topic == "/world/assembly/pose/info"
|
||||
assert pi.ros_topic == "/sim/world_pose_info"
|
||||
# 类型有默认值：ros_gz_bridge 只认这一对，改了就"桥接器起来但没消息"
|
||||
assert pi.ros_type == "tf2_msgs/msg/TFMessage"
|
||||
assert pi.gz_type == "gz.msgs.Pose_V"
|
||||
assert (pi.drift_tol_m, pi.drift_tol_deg) == (0.002, 2.0)
|
||||
|
||||
|
||||
def test_load_scene_pose_info_missing_topic_raises(tmp_path):
|
||||
"""只写一半话题名 = 订阅一个不存在的话题 = **永远不会报警**的监视器。
|
||||
|
||||
那比没有监视更糟（人会以为有保护），所以必须当场报错，而不是取默认值。
|
||||
"""
|
||||
_write_scene(tmp_path, [_obj()])
|
||||
p = tmp_path / "devices/sim/gazebo/config/scene.yaml"
|
||||
doc = yaml.safe_load(p.read_text(encoding="utf-8"))
|
||||
doc["pose_info"] = {"gz_topic": "/world/assembly/pose/info"}
|
||||
p.write_text(yaml.safe_dump(doc, allow_unicode=True), encoding="utf-8")
|
||||
with pytest.raises(SceneError) as e:
|
||||
load_scene(root=tmp_path)
|
||||
assert "ros_topic" in str(e.value)
|
||||
|
||||
|
||||
def test_load_scene_pose_info_not_mapping_raises(tmp_path):
|
||||
_write_scene(tmp_path, [_obj()])
|
||||
p = tmp_path / "devices/sim/gazebo/config/scene.yaml"
|
||||
doc = yaml.safe_load(p.read_text(encoding="utf-8"))
|
||||
doc["pose_info"] = ["/world/assembly/pose/info"]
|
||||
p.write_text(yaml.safe_dump(doc, allow_unicode=True), encoding="utf-8")
|
||||
with pytest.raises(SceneError) as e:
|
||||
load_scene(root=tmp_path)
|
||||
assert "pose_info 不是映射" in str(e.value)
|
||||
|
||||
|
||||
def test_pose_info_reset_on_start_defaults_off(tmp_path):
|
||||
"""**默认不搬物体**：老场景文件不写这一项时，行为必须与以前完全一样。
|
||||
|
||||
这个开关是"开局把物体 set_pose 回名义位姿"，默认开的话等于替所有老场景
|
||||
改了语义（而且是在运行中搬物体），风险远大于收益。
|
||||
"""
|
||||
_write_scene(tmp_path, [_obj()])
|
||||
p = tmp_path / "devices/sim/gazebo/config/scene.yaml"
|
||||
doc = yaml.safe_load(p.read_text(encoding="utf-8"))
|
||||
doc["pose_info"] = {
|
||||
"gz_topic": "/world/assembly/pose/info",
|
||||
"ros_topic": "/sim/world_pose_info",
|
||||
}
|
||||
p.write_text(yaml.safe_dump(doc, allow_unicode=True), encoding="utf-8")
|
||||
assert load_scene(root=tmp_path).pose_info.reset_on_start is False
|
||||
|
||||
|
||||
def test_pose_info_reset_on_start_must_be_bool(tmp_path):
|
||||
"""`reset_on_start: "false"` 在 YAML 里是**真值** —— 必须当场拦，不能强转。
|
||||
|
||||
这类错误的表现是"开关关了却没关"：排查时没人会想到去怀疑引号。
|
||||
"""
|
||||
_write_scene(tmp_path, [_obj()])
|
||||
p = tmp_path / "devices/sim/gazebo/config/scene.yaml"
|
||||
doc = yaml.safe_load(p.read_text(encoding="utf-8"))
|
||||
doc["pose_info"] = {
|
||||
"gz_topic": "/world/assembly/pose/info",
|
||||
"ros_topic": "/sim/world_pose_info",
|
||||
"reset_on_start": "false",
|
||||
}
|
||||
p.write_text(yaml.safe_dump(doc, allow_unicode=True), encoding="utf-8")
|
||||
with pytest.raises(SceneError) as e:
|
||||
load_scene(root=tmp_path)
|
||||
assert "reset_on_start" in str(e.value)
|
||||
|
||||
|
||||
def test_shipped_scene_has_reset_on_start(tmp_path):
|
||||
"""仓库自带场景**开着**启动复位：不开的话开局 2.6mm 落地偏差会拦住真值点。
|
||||
|
||||
实测数据与完整理由写在 scene.yaml 那段注释和
|
||||
``sim_truth_perception._maybe_startup_reset`` 里。
|
||||
"""
|
||||
from arm_sim_gazebo.sim_scene import repo_root
|
||||
|
||||
spec = load_scene(None, repo_root())
|
||||
assert spec.pose_info is not None
|
||||
assert spec.pose_info.reset_on_start is True
|
||||
|
||||
|
||||
def test_shipped_scene_world_name_matches_the_world_file():
|
||||
"""scene.yaml 里 pose_info.gz_topic 的 world 名必须与真实 world 文件名一致。
|
||||
|
||||
不一致的后果是**静默的**：桥接器订阅一个不存在的话题，永远没有消息，
|
||||
漂移监视形同不存在 —— 而它正是为了防"静默失效"才加的。
|
||||
这条测试是那两个名字之间唯一的联系。
|
||||
"""
|
||||
from arm_sim_gazebo.sim_scene import repo_root
|
||||
|
||||
root = repo_root()
|
||||
spec = load_scene(None, root)
|
||||
assert spec.pose_info is not None, "仓库自带的 scene.yaml 应当配了 pose_info"
|
||||
worlds = sorted((root / "devices/sim/gazebo/worlds").glob("*.world"))
|
||||
assert worlds, "worlds 目录里一个 .world 都没有？"
|
||||
world = worlds[0].stem
|
||||
assert spec.pose_info.gz_topic == f"/world/{world}/pose/info"
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Bounds
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
@@ -30,11 +30,18 @@ import pytest
|
||||
from arm_sim_gazebo import sim_scene
|
||||
from arm_sim_gazebo.sim_truth_perception import (
|
||||
MIN_CAM_Z_M,
|
||||
RESET_GIVE_UP,
|
||||
RESET_NOW,
|
||||
RESET_SKIP,
|
||||
RESET_WAIT,
|
||||
STARTUP_RESET_WINDOW_S,
|
||||
TruthError,
|
||||
apply_transform,
|
||||
load_truth_target,
|
||||
matrix_from_transform,
|
||||
pick_object,
|
||||
pose_drift,
|
||||
startup_reset_decision,
|
||||
target_point_world,
|
||||
)
|
||||
|
||||
@@ -253,3 +260,105 @@ def test_offset_length_is_checked_not_guessed(tmp_path):
|
||||
def test_camera_backside_guard_is_positive():
|
||||
"""背后判据用 z<=0，且阈值是个正数（写成 0 会把贴镜头的点也发出去）。"""
|
||||
assert MIN_CAM_Z_M > 0.0
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 漂移判据（真值感知的"前提自检"）
|
||||
# ---------------------------------------------------------------------------
|
||||
def _obj_at(x=0.52, y=0.0, z=0.40, yaw=0.0):
|
||||
return sim_scene.SceneObject(name="workpiece", model="workpiece", x=x, y=y, z=z, yaw=yaw)
|
||||
|
||||
|
||||
def _quat_z(yaw_deg: float):
|
||||
h = math.radians(yaw_deg) / 2.0
|
||||
return (0.0, 0.0, math.sin(h), math.cos(h))
|
||||
|
||||
|
||||
def test_pose_drift_quiet_when_the_part_is_where_the_scene_says():
|
||||
"""在位 → 必须**完全安静**。误报会让现场学会忽略这个监视，比没有更糟。"""
|
||||
assert pose_drift(_obj_at(), (0.52, 0.0, 0.40), _quat_z(0.0), 0.002, 2.0) is None
|
||||
# 容差边界内也算安静（1mm / 1°）
|
||||
assert pose_drift(_obj_at(), (0.521, 0.0, 0.40), _quat_z(1.0), 0.002, 2.0) is None
|
||||
|
||||
|
||||
def test_pose_drift_reports_the_case_we_actually_hit():
|
||||
"""2026-09-24 的真实故障：工件掉进台面通槽、落到地面（z≈0）还转了半圈。
|
||||
|
||||
这一拍必须报，而且要把**名义/实际/偏差**都写出来 —— 现场看到这句话就该
|
||||
立刻去检查工件，而不是去调检测阈值。
|
||||
"""
|
||||
msg = pose_drift(_obj_at(), (0.626, -0.034, 0.0), _quat_z(-54.0), 0.002, 2.0)
|
||||
assert msg is not None
|
||||
assert "名义" in msg and "实际" in msg
|
||||
# 掉下去约 0.4m（连同 XY 偏移合计 415mm），这个量级必须一眼看到
|
||||
assert "415.2mm" in msg
|
||||
assert "-54.0°" in msg
|
||||
|
||||
|
||||
def test_pose_drift_sees_tilt_not_just_yaw():
|
||||
"""姿态差用**整段旋转差**：板被碰一下常常是"翘起来夹着转一点"。
|
||||
|
||||
只比 yaw 会漏掉纯翘起 —— 而翘起同样把孔在图上挪走。
|
||||
这里绕 x 轴翘 5°、yaw 不变：必须报。
|
||||
"""
|
||||
h = math.radians(5.0) / 2.0
|
||||
quat_tilt = (math.sin(h), 0.0, 0.0, math.cos(h))
|
||||
assert pose_drift(_obj_at(), (0.52, 0.0, 0.40), quat_tilt, 0.002, 2.0) is not None
|
||||
# 而只翘 1°（在 2° 容差内）应当安静
|
||||
h1 = math.radians(1.0) / 2.0
|
||||
assert pose_drift(
|
||||
_obj_at(), (0.52, 0.0, 0.40), (math.sin(h1), 0.0, 0.0, math.cos(h1)), 0.002, 2.0
|
||||
) is None
|
||||
|
||||
|
||||
def test_pose_drift_measures_a_part_with_nonzero_yaw():
|
||||
"""名义 yaw 不是 0 时（现场摆歪了）也要比得准：转回同一角度 = 安静。"""
|
||||
obj = _obj_at(yaw=math.radians(30.0))
|
||||
assert pose_drift(obj, (0.52, 0.0, 0.40), _quat_z(30.0), 0.002, 2.0) is None
|
||||
assert pose_drift(obj, (0.52, 0.0, 0.40), _quat_z(0.0), 0.002, 2.0) is not None
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# 启动复位判据（把"spawn 落地的 2~3mm"和"被撞走"分开）
|
||||
# ---------------------------------------------------------------------------
|
||||
def test_startup_reset_only_fires_inside_the_window():
|
||||
"""**运行中绝不搬物体**：过了窗口一律 give_up，交给漂移看门狗报错。
|
||||
|
||||
这是这一整套里最要紧的一条：在运行中悄悄把物体搬回去，等于把
|
||||
"谁把它撞歪了"的证据抹掉，还可能把工件从爪里拽走。
|
||||
"""
|
||||
d, _ = startup_reset_decision(
|
||||
now_s=100.0, deadline_s=20.0, window_s=20.0, pose_known=True, any_drift=True
|
||||
)
|
||||
assert d == RESET_GIVE_UP
|
||||
|
||||
|
||||
def test_startup_reset_waits_until_it_knows_the_pose():
|
||||
"""还没收到 pose/info 帧就不能判断"要不要搬" —— 必须等，不能瞎搬。"""
|
||||
d, _ = startup_reset_decision(
|
||||
now_s=1.0, deadline_s=None, window_s=20.0, pose_known=False, any_drift=True
|
||||
)
|
||||
assert d == RESET_WAIT
|
||||
|
||||
|
||||
def test_startup_reset_is_silent_when_the_part_is_in_place():
|
||||
"""物体本来就在名义位姿 → 什么都不做（没有漂移就没有理由碰它）。"""
|
||||
d, _ = startup_reset_decision(
|
||||
now_s=1.0, deadline_s=None, window_s=20.0, pose_known=True, any_drift=False
|
||||
)
|
||||
assert d == RESET_SKIP
|
||||
|
||||
|
||||
def test_startup_reset_fires_for_the_settle_case_we_actually_hit():
|
||||
"""2026-09-24 的真实开局：落稳后偏 2.6mm（越过 2mm 容差）→ 搬回名义位姿。"""
|
||||
d, dl = startup_reset_decision(
|
||||
now_s=1.0, deadline_s=None, window_s=20.0, pose_known=True, any_drift=True
|
||||
)
|
||||
assert d == RESET_NOW
|
||||
assert dl == pytest.approx(21.0) # 截止时刻 = 首次判定时刻 + 窗口
|
||||
|
||||
|
||||
def test_startup_reset_window_is_bounded_and_positive():
|
||||
"""窗口必须有界：`None`/0 会让"要不要在运行中搬物体"变成不可控。"""
|
||||
assert STARTUP_RESET_WINDOW_S > 0.0
|
||||
assert STARTUP_RESET_WINDOW_S <= 120.0
|
||||
|
||||
@@ -293,3 +293,65 @@ def test_object_bbox_px_clips_to_image():
|
||||
x1, y1, x2, y2 = bbox
|
||||
assert 0 <= x1 <= x2 <= IMAGE[1] - 1
|
||||
assert 0 <= y1 <= y2 <= IMAGE[0] - 1
|
||||
|
||||
|
||||
# ------------------------------------------------- 真值三维（2026-09-24 新增）
|
||||
def test_locate_holes_also_returns_exact_world_geometry(tmp_path):
|
||||
"""真值必须**同时**给三维 —— 调用方不该再去反投影。
|
||||
|
||||
为什么钉这条：仿真里孔是贯通孔、孔位正下方就是台面通槽，深度相机在孔位
|
||||
看到的是槽底/地面（0.62m）而非工件面（0.21m）。stage2 原先就拿那些像素去
|
||||
查深度图反投影，于是 58mm 的孔距算成 169mm、被间距门槛丢掉，
|
||||
现象是"S05 每拍都丢、最后超时"，看起来像检测坏了。
|
||||
真值本来就精确知道孔在世界系哪儿 —— 这条测试保证它一直带出来。
|
||||
"""
|
||||
spec = _build_repo(tmp_path)
|
||||
res = locate_holes(_cam_looking_down((0.50, 0.0, 0.632)), INTR, IMAGE,
|
||||
spec=spec, root=tmp_path)
|
||||
assert res.ok, res.why
|
||||
assert len(res.hole_world) == 2
|
||||
# 环的局部 z=0.010，物体 z=0.40 → 世界 z=0.410
|
||||
for (x, y, z) in res.hole_world:
|
||||
assert z == pytest.approx(0.410, abs=1e-9)
|
||||
assert y == pytest.approx(0.0, abs=1e-9)
|
||||
# 世界系孔距必须**正好**是 58mm（不像像素那样有透视缩放）
|
||||
d = math.dist(res.hole_world[0], res.hole_world[1])
|
||||
assert d == pytest.approx(2 * RING_X, rel=1e-9)
|
||||
# 上表面中心与上表面 z：板厚 12mm，底面 z=0.40
|
||||
assert res.part_top_z == pytest.approx(0.412, abs=1e-9)
|
||||
assert res.part_center_world is not None
|
||||
assert res.part_center_world[2] == pytest.approx(0.412, abs=1e-9)
|
||||
assert res.part_center_world[:2] == pytest.approx((0.50, 0.0), abs=1e-9)
|
||||
|
||||
|
||||
def test_world_geometry_follows_yaw(tmp_path):
|
||||
"""物体带 yaw 时，真值三维必须跟着转 —— 否则"真值"是错的。
|
||||
|
||||
只比像素会漏掉这类错：像素上转 90° 的两孔与没转的两孔**看起来都合理**，
|
||||
但世界坐标差了一整条孔距。
|
||||
"""
|
||||
_build_repo(tmp_path)
|
||||
cfg = tmp_path / "devices/sim/gazebo/config/scene.yaml"
|
||||
doc = yaml.safe_load(cfg.read_text(encoding="utf-8"))
|
||||
doc["objects"][0]["pose"]["yaw"] = math.pi / 2
|
||||
cfg.write_text(yaml.safe_dump(doc, allow_unicode=True), encoding="utf-8")
|
||||
spec = sim_scene.load_scene(root=tmp_path)
|
||||
res = locate_holes(_cam_looking_down((0.50, 0.0, 0.632)), INTR, IMAGE,
|
||||
spec=spec, root=tmp_path)
|
||||
assert res.ok, res.why
|
||||
# 两个孔都转到 y 方向：|Δy| = 58mm，|Δx| ≈ 0
|
||||
dx = abs(res.hole_world[0][0] - res.hole_world[1][0])
|
||||
dy = abs(res.hole_world[0][1] - res.hole_world[1][1])
|
||||
assert dx == pytest.approx(0.0, abs=1e-9)
|
||||
assert dy == pytest.approx(2 * RING_X, rel=1e-9)
|
||||
|
||||
|
||||
def test_locate_holes_without_two_rings_has_no_world_geometry(tmp_path):
|
||||
"""失败时不能留下半份三维 —— 那样调用方会拿着空列表去当坐标用。"""
|
||||
spec = _build_repo(tmp_path, rings=((0.0, 0.0),))
|
||||
res = locate_holes(_cam_looking_down((0.50, 0.0, 0.632)), INTR, IMAGE,
|
||||
spec=spec, root=tmp_path)
|
||||
assert not res.ok
|
||||
assert res.hole_world == []
|
||||
assert res.part_center_world is None
|
||||
assert res.part_top_z is None
|
||||
|
||||
Reference in New Issue
Block a user