工具几何进配置:S07 悬停/下探不再靠拍,预算由几何推导

真机与仿真**同参数同结果**的一个几何错误:流程里的"悬停高度"是按**相机**
定的(要看清孔得离工件面 ~0.17m),不是按**工具**定的 —— 而工具装在法兰
下面、比相机长得多。仿真里实测:

    悬停法兰 0.632 − 工具长 0.243 = 尖端 0.389
    工件上表面                             0.412
    → **悬停这一刻,通止规已经插进工件 23mm**

后果极具误导性:S07 一路压到顶都等不到"台肩落面"那一下,最后报
"下探 N mm 都没触发接触判据" —— 看起来像**力阈值标错了**,实际是几何错。
上一轮就是照这个假象去调阈值的,白费。

新增三个纯函数 + 两个配置项:
  - gauge_fsm.tool_clearance_m(法兰 z, 工具长, 面 z)  → 尖端离面多高(负=已插入)
  - gauge_fsm.descend_needed_m(...)                   → 台肩落面还需下压多少
        判"到底了"的是**台肩落面**,不是尖端碰面。台肩在规尖上方 30mm,
        所以按尖端算会**少算 30mm** —— 那正是"预算 25mm 永远到不了落面"的 bug。
  - gauge_fsm.descend_budget_m(needed)                → 预算 = 1.5×需求,含绝对下限
  - site.yaml: tool_tip_below_flange_m(0.243 的来历写清了:
        TCP 0.200 + 规尖到指腹接触面 0.043,而 0.043 不是"夹持段中心到尖端"
        的 0.0555 —— 指腹接触面比 TCP 原点高 12.5mm)
  - site.yaml: workpiece_surface_z_m(0 = 用 stage2 实测孔位兜底)

S07 启动时现在会打印(本轮实测输出):
    S07 工具几何:悬停法兰 z=0.6317、工具尖端在法兰下方 243.0mm
      → 尖端 z=0.3887,工件上表面 z=0.4117
      悬停时尖端离面 -23.0mm(**已经插进工件了**);台肩落面还需下压 +7.0mm
配的预算不够走完落面那一段时,就地按几何改大并告警(不是静默用错的数)。

回归测试 5 项,其中 test_shipped_budget_covers_shipped_gauge_and_hover
专防"预算又调回不够"这个 bug 复发。

Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
arm_dev
2026-09-24 14:52:55 +08:00
parent 36a59c81bf
commit 1ac5472788
5 changed files with 249 additions and 1 deletions
+27
View File
@@ -647,6 +647,33 @@ gauge_assembly:
# 下压 30mm 台肩才落面 —— 25mm 的上限根本到不了,
# 只会报"下探 25mm 都没触发接触判据"然后 ABORT。
# 35 = 30(台肩落面)+ 5(对位残差/工件厚度公差余量)。
# 注意:这个值**不必**手工算 —— S07 启动时会按下面两个
# 工具几何量算出"台肩落面还需要多少",不够就地改大并告警。
# ---- 工具几何(S07 的"下去多少才到底"完全由这两个值决定)----
# 为什么必须显式配:流程里的"悬停高度"是按**相机**定的(要看清孔得离面 ~0.17m),
# 不是按**工具**定的;而工具装在法兰下面、可能比相机长得多。两者一冲突就会出现:
# 悬停法兰 0.632 − 工具长 0.243 = 尖端 0.389,工件上表面 0.412
# → **悬停这一刻工具已经插进工件 23mm**
# 于是 S07 一路压到顶都等不到"台肩落面"那一下,报"没触发接触判据",
# 看起来像力阈值标错了 —— 实际是几何错。真机同参数同结果。
# S07 启动时会把这个差值算出来印在日志里(负值会明确标注"已经插进工件了")。
tool_tip_below_flange_m: 0.243 # 工具尖端(规尖)在法兰下方的距离 [m]
# 怎么量/怎么算(这个 0.243 的来历):
# TCP(两指中点)在法兰下方 0.200
# + 规尖在 TCP 下方 0.043
# 为什么是 0.043 而不是规"夹持段中心到尖端"的 0.0555:
# 规尖要对齐的是**指腹接触面**,而指腹接触面不在
# TCP 原点上 —— URDF 里 inner_finger_pad_joint
# z=0.0235、finger_tcp_joint z=0.0360(同父链节
# 同方向)→ 指腹中心比 TCP 高 12.5mm。
# 所以 0.0555 − 0.0125 = 0.043。
# ⚠️ 换夹爪/换规必须重量(量法:夹住规之后用
# TF 直接读 flange→规尖 的 z 差,别靠推)。
workpiece_surface_z_m: 0.0 # 工件**上表面**的世界 z [m](对孔基准面)
# 0 = 未知 → 运行期用 stage2 实测的孔位 z 兜底
#(孔就在上表面上)。仿真里是 0.412。
# 只在"stage2 还没跑就想知道几何对不对"时才需要手填。
contact_tangential_delta: 300.0 # 接触判据:两路切向任一相对基线增量超过它 [任意单位]
contact_safe_delta: 2000.0 # 法向安全上限 [任意单位]:超了立刻抬起并中止(E2)
# 必须 > contact_tangential_delta,否则自检就报矛盾