From 1ac5472788e3860ccdcb21e87b4b13f7d0ed3447 Mon Sep 17 00:00:00 2001 From: arm_dev Date: Thu, 24 Sep 2026 14:52:55 +0800 Subject: [PATCH] =?UTF-8?q?=E5=B7=A5=E5=85=B7=E5=87=A0=E4=BD=95=E8=BF=9B?= =?UTF-8?q?=E9=85=8D=E7=BD=AE=EF=BC=9AS07=20=E6=82=AC=E5=81=9C/=E4=B8=8B?= =?UTF-8?q?=E6=8E=A2=E4=B8=8D=E5=86=8D=E9=9D=A0=E6=8B=8D=EF=BC=8C=E9=A2=84?= =?UTF-8?q?=E7=AE=97=E7=94=B1=E5=87=A0=E4=BD=95=E6=8E=A8=E5=AF=BC?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 真机与仿真**同参数同结果**的一个几何错误：流程里的"悬停高度"是按**相机** 定的（要看清孔得离工件面 ~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 --- config/site.example.yaml | 27 +++++++ .../arm_application/gauge_assembly_node.py | 54 +++++++++++++ .../arm_application/gauge_config.py | 21 +++++- .../arm_application/gauge_fsm.py | 75 +++++++++++++++++++ src/arm_application/test/test_gauge_fsm.py | 73 ++++++++++++++++++ 5 files changed, 249 insertions(+), 1 deletion(-) diff --git a/config/site.example.yaml b/config/site.example.yaml index 3e228c0..fe3320e 100644 --- a/config/site.example.yaml +++ b/config/site.example.yaml @@ -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，否则自检就报矛盾 diff --git a/src/arm_application/arm_application/gauge_assembly_node.py b/src/arm_application/arm_application/gauge_assembly_node.py index 4907395..46075d7 100644 --- a/src/arm_application/arm_application/gauge_assembly_node.py +++ b/src/arm_application/arm_application/gauge_assembly_node.py @@ -97,9 +97,12 @@ from arm_application.gauge_fsm import ( Event, FsmError, GaugeContext, + GAUGE_SHOULDER_ABOVE_TIP_M, GaugeFsm, State, + descend_budget_m, descend_iter_budget, + descend_needed_m, descend_progress, descend_tol_m, is_joint_limit_error, @@ -108,6 +111,7 @@ from arm_application.gauge_fsm import ( ratchet_cycles, screw_stop, tangential_jump, + tool_clearance_m, ) from arm_application.gauge_gripper import ( CONTRACT_STATUS_TOPIC, @@ -1455,6 +1459,56 @@ class GaugeAssembly(AssemblyPipeline): z0 = float(Tee[2, 3]) step_m = self.cfg.contact_step_mm / 1000.0 max_depth_m = self.cfg.contact_max_depth_mm / 1000.0 + + # ---- 工具几何自检：预算必须是**由几何推出来的**，不能拍 ------------------ + # 判"到底了"的那一下是**台肩落到工件上表面**（不是尖端碰面）。工件上表面 + # 就是 stage2 实测的孔位 z（孔在面上）；台肩在规尖上方 30mm（通端长 30）。 + # + # 这里做两件事： + # ① 把"悬停时工具尖端离面还有多高"算出来印出来。**负值说明悬停这一步 + # 就已经把工具捅进工件了**（悬停高度是按相机定的、工具比相机长就 + # 会这样，真机同参数同结果），这是个几何错误，藏得很深：现象是 + # S07 一路压到顶都判不到接触，看起来像力阈值标错了。 + # ② 如果配的预算**不够**走完"台肩落面"那一段，就地按几何**改大**并 + # 大声报出来。2026-09-24 仿真里踩过：预算 25mm 而台肩在尖端上方 + # 30mm → 永远到不了落面那一下，报出来的原因是假的（"没触发接触 + # 判据"），真实原因只是预算不够。 + surface_z = float(self.cfg.workpiece_surface_z_m) + if surface_z <= 0.0: + # 没配就取 stage2 实测的孔位 z —— 孔就在工件上表面上。 + k1 = (self._snap_final or {}).get("k1_base_m") or [] + surface_z = float(k1[2]) if len(k1) >= 3 else 0.0 + if surface_z > 0.0: + clear_m = tool_clearance_m( + z0, # 悬停时的法兰高度 = 当前 Tee 的 z（S06 刚把法兰锁到悬停高） + self.cfg.tool_tip_below_flange_m, + surface_z, + ) + needed_m = descend_needed_m( + z0, self.cfg.tool_tip_below_flange_m, surface_z, + GAUGE_SHOULDER_ABOVE_TIP_M, + ) + self.get_logger().info( + f"S07 工具几何：悬停法兰 z={z0:.4f}、工具尖端在法兰下方 " + f"{self.cfg.tool_tip_below_flange_m * 1000:.1f}mm → 尖端 z=" + f"{z0 - self.cfg.tool_tip_below_flange_m:.4f}，工件上表面 z={surface_z:.4f}\n" + f" 悬停时尖端离面 {clear_m * 1000:+.1f}mm" + f"{'（**已经插进工件了**）' if clear_m < 0 else ''}；" + f"台肩落面还需下压 {needed_m * 1000:+.1f}mm" + ) + if needed_m > max_depth_m: + new_depth_m = descend_budget_m(needed_m) + self.get_logger().warn( + f"S07 下探预算不足：配的是 {max_depth_m * 1000:.1f}mm，" + f"但「台肩落面」这一段就要 {needed_m * 1000:.1f}mm" + f"（台肩在尖端上方 {GAUGE_SHOULDER_ABOVE_TIP_M * 1000:.0f}mm）。\n" + f" 按几何自动改大为 {new_depth_m * 1000:.1f}mm。" + f"要固定这个值，改 site.yaml 的 " + f"gauge_assembly.contact_max_depth_mm（建议 ≥ " + f"{new_depth_m * 1000:.0f}）" + ) + max_depth_m = new_depth_m + # 这一步的到位容差**必须小于步长**（见 gauge_fsm.descend_tol_m 的说明： # 容差≥步长时 _goto 判"已在容差内"直接返回，一步都不走 → 静默自旋）。 tol_step_m = descend_tol_m(step_m) diff --git a/src/arm_application/arm_application/gauge_config.py b/src/arm_application/arm_application/gauge_config.py index 97b97eb..b8cfac6 100644 --- a/src/arm_application/arm_application/gauge_config.py +++ b/src/arm_application/arm_application/gauge_config.py @@ -96,6 +96,8 @@ _TYPES: Dict[str, type] = { "contact_speed_m_s": float, "contact_step_mm": float, "contact_max_depth_mm": float, + "tool_tip_below_flange_m": float, + "workpiece_surface_z_m": float, "contact_tangential_delta": float, "contact_safe_delta": float, "screw_step_deg": float, @@ -136,6 +138,9 @@ _UNITS: Dict[str, str] = { "contact_speed_m_s": "m/s", "contact_step_mm": "mm", "contact_max_depth_mm": "mm", + # 这两个是**长度**不是"毫米量"：用 m 口径，与 hover_z 同一把尺 + "tool_tip_below_flange_m": "m", + "workpiece_surface_z_m": "m", "contact_tangential_delta": "au", "contact_safe_delta": "au", "screw_step_deg": "deg", @@ -200,7 +205,9 @@ DEFAULTS: Dict[str, Any] = { # S07 "contact_speed_m_s": 0.02, "contact_step_mm": 0.5, - "contact_max_depth_mm": 25.0, + "contact_max_depth_mm": 35.0, + "tool_tip_below_flange_m": 0.243, + "workpiece_surface_z_m": 0.0, # 0 = 未知，运行期用 stage2 实测孔位兜底 "contact_tangential_delta": 300.0, "contact_safe_delta": 2000.0, # S08 @@ -249,6 +256,8 @@ class GaugeConfig: contact_speed_m_s: float contact_step_mm: float contact_max_depth_mm: float + tool_tip_below_flange_m: float + workpiece_surface_z_m: float contact_tangential_delta: float contact_safe_delta: float screw_step_deg: float @@ -303,6 +312,10 @@ class GaugeConfig: f" S06 对位容差 {self.align_tol_mm:.1f}mm（悬停高取自 servo.hole_hover_z_m）", f" S07 切向突变阈值 +{self.contact_tangential_delta:.0f}（两通道任一，现场标定），" f"安全上限 +{self.contact_safe_delta:.0f}，最多下探 {self.contact_max_depth_mm:.0f}mm", + 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 算「悬停时工具到底入没入工件」，" + "入了会**明确报错**——见 gauge_fsm.tool_clearance_m）", f" S07 步进 {self.contact_step_mm:.2f}mm @ {self.contact_speed_m_s * 1000:.0f}mm/s，" f"到位容差 {descend_tol_m(self.contact_step_mm / 1000.0) * 1000:.3f}mm" "（=步长的 40%，**推导出来的、不是可调项**：容差 ≥ 步长会让每一步都被判成" @@ -436,6 +449,12 @@ def _sanity(cfg: GaugeConfig) -> None: "前者是「立刻抬臂」的安全上限，后者只是「停止下压」。" "两者相等的话，接触一开始就会触发安全上限" ) + if cfg.tool_tip_below_flange_m <= 0.0: + raise GaugeConfigError( + f"tool_tip_below_flange_m 必须为正（工具尖端在法兰**下方**多少米），" + f"收到 {cfg.tool_tip_below_flange_m}。" + "填 0 会让「悬停时工具会不会已经插进工件」这个判断失效" + ) if cfg.snap_tol_px <= 0.0: raise GaugeConfigError(f"gauge_assembly.snap_tol_px 必须为正，收到 {cfg.snap_tol_px}") if cfg.snap_tol_mm <= 0.0: diff --git a/src/arm_application/arm_application/gauge_fsm.py b/src/arm_application/arm_application/gauge_fsm.py index 2125c1b..2218e82 100644 --- a/src/arm_application/arm_application/gauge_fsm.py +++ b/src/arm_application/arm_application/gauge_fsm.py @@ -408,6 +408,81 @@ def descend_progress(z_prev: float, z_now: float, step_m: float) -> bool: return (float(z_prev) - float(z_now)) >= step_m * DESCEND_PROGRESS_FRAC +#: 通止规**台肩**（COLLAR）在规尖上方的距离 [m]。 +#: 取自 devices/sim/gazebo/models/gauge/model.sdf：通端 φ6×30mm，台肩在 30~33mm。 +#: 判"到底了"的那一下就是台肩落到工件上表面 —— 所以它决定了 S07 至少要下压 +#: 多少（见 descend_needed_m）。改规的模型时必须一起改这里，否则预算会算错。 +GAUGE_SHOULDER_ABOVE_TIP_M: float = 0.030 + + +def tool_clearance_m( + flange_z_m: float, + tool_tip_below_flange_m: float, + surface_z_m: float, +) -> float: + """工具尖端离工件上表面还有多高 [m]。**负 = 已经插进去了。** + + 为什么需要这个：流程里"悬停高度"是按**相机**定的，不是按**工具**定的。 + 腕部相机要看清楚孔，就得离工件面 ~0.17m；而工具（装在法兰下面）可能比 + 相机长得多。两者一冲突，就会出现下面这种极难查的局面： + + 悬停高度 0.632（法兰）− 工具长 0.243 = 0.389 + 工件上表面 0.412 + → 工具尖端已经插进工件 23mm + + 此时"悬停"这一步本身已经把工具捅进孔里了，S07 再往下压就一路压到顶、 + 等不到"台肩落面"那一下，最后报"下探 N mm 都没触发接触判据"然后 ABORT。 + 现象看起来像力阈值标错了，实际是**几何**错了。 + + 真机同参数同结果（不是仿真特有的）：只要悬停高度是按相机定的、而工具 + 比相机长，就会这样。所以这个值要在**启动时**算出来并报出来。 + """ + tip_z = float(flange_z_m) - float(tool_tip_below_flange_m) + return tip_z - float(surface_z_m) + + +def descend_needed_m( + flange_z_m: float, + tool_tip_below_flange_m: float, + surface_z_m: float, + tool_shoulder_above_tip_m: float, +) -> float: + """S07 要判"台肩落面"，从当前法兰高度起还需要下压多少 [m]。 + + 判"到底了"的那一下不是工具尖端碰面，而是**台肩落到工件上表面**； + 台肩在工具尖端上方 ``tool_shoulder_above_tip_m``（规是通端长 30mm）。 + 所以需要的下压量 = 台肩到面的距离，而不是尖端到面的距离。 + + 返回值可能是负的（台肩已经在面以下）—— 那说明几何配错了，调用方应当 + 报错而不是"往下走一个负距离"。 + """ + shoulder_z = ( + float(flange_z_m) + - float(tool_tip_below_flange_m) + + float(tool_shoulder_above_tip_m) + ) + return shoulder_z - float(surface_z_m) + + +#: S07 下探预算相对"实际需要下压量"的余量系数。 +#: 预算是**兜底**用的（不是用来判成败的）：真正让循环停下的是接触判据。 +#: 给 1.5 倍余量是为了容纳对位残差、板厚公差、台肩倒角还没完全落面等。 +DESCEND_BUDGET_FRAC: float = 1.5 +#: 下探预算的绝对下限 [m]：几何算出来很小（比如台肩已快贴面）时也要留够， +#: 否则一旦对位有点偏，"预算用完"会先于"判到接触"触发，报出来的原因是假的。 +DESCEND_BUDGET_FLOOR_M: float = 0.008 + + +def descend_budget_m(needed_m: float) -> float: + """由"实际需要下压多少"推出 S07 的下探预算 [m]。 + + 不要手工填这个数：填小了会在台肩落面**之前**就用完预算（报"下探 N mm + 都没触发接触判据"，而真实原因只是预算不够）；填大了会在判据失灵时压得 + 更深（更危险）。由几何推出来，两个毛病都没有。 + """ + return max(float(needed_m) * DESCEND_BUDGET_FRAC, DESCEND_BUDGET_FLOOR_M) + + def screw_stop(normal_delta: float, shear_max: float, normal_th: float, shear_th: float) -> bool: """S08：旋转装配的停止判据 —— **切向力>阈值 且 法向力突增**。 diff --git a/src/arm_application/test/test_gauge_fsm.py b/src/arm_application/test/test_gauge_fsm.py index 6b7364a..0e2f9da 100644 --- a/src/arm_application/test/test_gauge_fsm.py +++ b/src/arm_application/test/test_gauge_fsm.py @@ -23,9 +23,14 @@ from arm_application.gauge_fsm import ( GaugeContext, GaugeFsm, State, + DESCEND_BUDGET_FLOOR_M, + GAUGE_SHOULDER_ABOVE_TIP_M, + descend_budget_m, descend_iter_budget, + descend_needed_m, descend_progress, descend_tol_m, + tool_clearance_m, is_joint_limit_error, pixel_stable, points_stable, @@ -459,6 +464,74 @@ def test_descend_iter_budget_covers_the_whole_depth(): assert descend_iter_budget(depth_m, step_m) < 200 +def test_tool_clearance_is_negative_when_tool_already_inside(): + """悬停高度按相机定、工具比相机长 → 悬停那一刻工具已经插进工件。 + + 这是 2026-09-24 仿真里实测到的局面（也是真机同参数同结果）： + 悬停法兰 0.632 − 工具长 0.243 = 尖端 0.389，工件上表面 0.412 + → 尖端在面下 23mm + 现象是 S07 一路压到顶都判不到接触，看起来像力阈值标错了 —— 其实是几何错。 + """ + gap = tool_clearance_m(0.632, 0.243, 0.412) + assert gap == pytest.approx(-0.023, abs=1e-9) + + +def test_tool_clearance_positive_when_tool_is_short(): + assert tool_clearance_m(0.632, 0.100, 0.412) == pytest.approx(0.120, abs=1e-9) + + +def test_descend_needed_measures_shoulder_not_tip(): + """判"到底了"的是**台肩落面**，所以需要的下压量按台肩算，不按尖端算。 + + 台肩在尖端上方 30mm：尖端要往下 30mm，台肩才落到面上。 + 按尖端算会少算 30mm —— 那正是"预算 25mm 永远到不了落面"那个 bug。 + """ + # 尖端正好在面上（法兰 = 面 0.412 + 工具长 0.243 = 0.655） + # → 还要再下压 30mm 台肩才落面 + assert descend_needed_m(0.655, 0.243, 0.412, 0.030) == pytest.approx(0.030) + + # 悬停在 0.632（= 仿真里那个按相机定的值）→ 台肩还差 7mm + assert descend_needed_m(0.632, 0.243, 0.412, 0.030) == pytest.approx(0.007) + + # 按尖端算会少算台肩那 30mm —— 差的就是出问题的那个量 + tip_only = tool_clearance_m(0.655, 0.243, 0.412) + assert tip_only == pytest.approx(0.0) + assert descend_needed_m(0.655, 0.243, 0.412, 0.030) - tip_only == pytest.approx(0.030) + + +def test_descend_budget_has_margin_and_floor(): + """预算要有余量（容纳对位残差/板厚公差），且再小也有下限。 + + 下限的理由：几何算出来"只差 2mm"时，如果预算就填 2mm，一点对位残差就会 + 让"预算用完"先于"判到接触"触发 —— 报出来的原因是假的。 + """ + # 需求很小时吃下限：0.002×1.5 = 0.003 < 0.008 + assert descend_budget_m(0.002) == pytest.approx(DESCEND_BUDGET_FLOOR_M) + # 需求正常时按系数放大：0.030×1.5 = 0.045 + assert descend_budget_m(0.030) == pytest.approx(0.045) + + +def test_shipped_budget_covers_shipped_gauge_and_hover(): + """出厂配置必须自洽：预算要够走完"台肩落面"。 + + 这是那个 bug 的回归测试 —— 当时预算 25mm 而台肩在尖端上方 30mm， + 永远到不了落面那一下，报"下探 25mm 都没触发接触判据"（假原因）。 + 以后谁把 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 实测孔位兜底）， + # 所以这里取一个"最坏"的悬停-面高差：仿真里实测法兰 0.632、面 0.412。 + needed = descend_needed_m(0.632, tip_below, 0.412, GAUGE_SHOULDER_ABOVE_TIP_M) + assert budget_m >= needed, ( + f"下探预算 {budget_m*1000:.1f}mm 不够走完台肩落面所需的 " + f"{needed*1000:.1f}mm —— 会在到落面之前就用完预算" + ) + + def test_descend_iter_budget_zero_depth_means_no_iterations(): assert descend_iter_budget(0.0, 0.0005) == 0 with pytest.raises(ValueError):