真机落地前的仿真补齐：夹持/腕力/真值位姿，让仿真能验证 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)}"
|
||||
Reference in New Issue
Block a user