From 37b85aa09bf4aeaf642626682a8d10d7e75e2e73 Mon Sep 17 00:00:00 2001 From: arm_dev Date: Tue, 13 Oct 2026 19:27:19 +0800 Subject: [PATCH] =?UTF-8?q?=E6=82=AC=E5=81=9C=E4=B8=80=E6=AC=A1=E8=B5=B0?= =?UTF-8?q?=E5=AE=8C=E5=90=8E=E4=B8=8B=E9=99=8D=E4=BC=9A=E6=BC=82=E5=87=A0?= =?UTF-8?q?=E6=AF=AB=E7=B1=B3=EF=BC=8C=E8=BF=99=E4=B8=80=E4=BF=9D=E5=AD=98?= =?UTF-8?q?=E7=82=B9=E6=8A=8A=E8=A7=82=E5=AF=9F=E9=AB=98=E8=A1=A5=E4=BA=A4?= =?UTF-8?q?=E3=80=81=E6=8E=A5=E8=A7=A6=E9=97=A8=E6=8E=A7=E5=92=8C=E5=8E=82?= =?UTF-8?q?=E5=AE=B6=20MoveL=20=E6=8E=A5=E5=88=B0=E8=83=BD=E6=8F=92?= =?UTF-8?q?=E8=BF=9B=E5=8E=BB=E7=9A=84=E7=8A=B6=E6=80=81=EF=BC=8C=E5=90=8E?= =?UTF-8?q?=E9=9D=A2=E5=A5=BD=E5=AF=B9=E7=85=A7=E9=94=81=20XY=20=E7=BA=AF?= =?UTF-8?q?=20Z=E3=80=82?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Cursor --- config/site.example.yaml | 2 + config/site.yaml | 3 + docs/HANDBOOK.md | 7 +- docs/HOLE_DETECT.md | 11 +- scripts/rdk_goto_xyz.py | 29 +- scripts/rdk_release_movel.py | 4 +- .../arm_application/overhead_oneshot.py | 78 ++- .../arm_application/path_profile.py | 22 +- .../arm_application/servo_node.py | 13 +- .../arm_application/stage2_locate_node.py | 602 +++++++++++++++++- .../test/test_overhead_oneshot.py | 157 ++++- .../test/test_stage2_descent_clamp.py | 64 +- .../test/test_stage2_tip_align.py | 209 +++++- 13 files changed, 1122 insertions(+), 79 deletions(-) diff --git a/config/site.example.yaml b/config/site.example.yaml index f195d64..848298b 100644 --- a/config/site.example.yaml +++ b/config/site.example.yaml @@ -770,6 +770,8 @@ hole_detect: align_speed: 0.014 # 对准段笛卡尔速度 [m/s]（原0.03，放缓防冲） move_speed: 0.022 # 升降/粗居中段速度 [m/s]（原0.05，放缓防震规） hole_visual_only: true + # 本分支实验：true=悬停拍一次并走完到孔，下降后再补 XY；main 保存点应保持 false。 + hole_keypoint_once: true final_gap_mm: 2.0 # 对准后贴面轻降目标：规尖悬在孔面上方这么高 [mm]（不接触） observe_gap_mm: 15.0 # --no-descent 时的观测下降：只降到离面这么高 [mm]， # 方便看规尖偏没偏。必须 ≥ final_gap_mm。 diff --git a/config/site.yaml b/config/site.yaml index 0695fb9..792d5c3 100644 --- a/config/site.yaml +++ b/config/site.yaml @@ -758,6 +758,9 @@ hole_detect: move_speed: 0.022 # 升降/粗居中段速度 [m/s]（原0.05，放缓防震规） # 对小孔关键点：只用像素射线交到 workpiece_surface_z_m，不读深度图。 hole_visual_only: true + # 本分支实验（exp/hole-keypoint-once）：true=悬停拍一次，框心→k1 一次走完 + # （40mm 上限），下降后再补交平面漂的 XY。main 保存点是 false（多走多拍）。 + hole_keypoint_once: true final_gap_mm: 2.0 # 对准后贴面轻降目标：规尖悬在孔面上方这么高 [mm]（不接触） observe_gap_mm: 15.0 # --no-descent 时的观测下降：只降到离面这么高 [mm]， # 方便看规尖偏没偏。必须 ≥ final_gap_mm。 diff --git a/docs/HANDBOOK.md b/docs/HANDBOOK.md index 0b458a1..bcc81a5 100644 --- a/docs/HANDBOOK.md +++ b/docs/HANDBOOK.md @@ -72,7 +72,7 @@ bash ~/ros2/scripts/run_all.sh --arm 89 --init-z 0.35 --hover-z 0.34 一阶段默认 **D405 实时推理**，D435 待命。YOLO-pose 这一路不关。连续 `D405_MISS_BEFORE_D435_S`（0.5 s）没有 conf≥`perception.wrist_handoff_conf`（0.5） 的目标，才用顶视 D435 **拍一张**，把点用眼在手外 T 变到基座（不再乘法兰）。 -这一段运动是厂家 `MoveL`：脚本先停真机控制栈（感知不停），XY 对准该点， +这一段运动是厂家 `MoveL`（SDK `vel` 0.3 m/s、`acc` 1.5 m/s²）：脚本先停真机控制栈（感知不停），XY 对准该点， 工具尖高度保持当前，再冷启动 `run_real.sh --no-stop`。冷启动回来之后才开始计 `servo.d435_handback_sec`（3 s）。赶路和杀栈期间不下发 D405。 停稳之后改回 D405，跟踪点是工件框的矩形中心，不追 k0。 @@ -80,7 +80,10 @@ bash ~/ros2/scripts/run_all.sh --arm 89 --init-z 0.35 --hover-z 0.34 会让工具尖进入工件、或超出工作盒的值直接拒绝，不会改成另一个高度再往下走。 不写则一阶段锁高仍按 site，对孔悬停仍是 `hole_hover_z_m`（89 为 0.320）。 框心对准连续 `servo.wrist_align_sec`（0.5 s）→ `/visual_servo/locked`。 -二阶段粗居中工件框，再多拍 k1/k2 闭环对准，轻降后在观测高度补一次 XY。 +本分支（`exp/hole-keypoint-once`）二阶段是**单次精拍**：悬停拍一次 k1/k2， +把框心到目标孔一次走完（步长上限 40mm，不再截成 12mm），先降到观察高约 15mm 再交孔，斜线一步把 XY 补偿（≤3mm）和剩余 Z 收到贴面。轻降接触门控是 `final_gap+4mm`。 +`hole_detect.stage2.hole_keypoint_once: true`。看 `/tmp/stage2_result.json` 的 `snaps`。 +`main` 保存点仍是多走多拍。 孔坐标把关键点像素交到 `workpiece_surface_z_m`，不读深度图。下臂停在 `tool10120938` 的 TCP 长度（0.2245 m）加上 `final_gap_mm` 之上，不按旧 tool1 的 0.223 m。 只测 D435 这一段位移时加 `--d435-only`：不交 D405、不进二阶段，并且要求先停 `run_real.sh`。 diff --git a/docs/HOLE_DETECT.md b/docs/HOLE_DETECT.md index 36b66c3..284268a 100644 --- a/docs/HOLE_DETECT.md +++ b/docs/HOLE_DETECT.md @@ -99,8 +99,15 @@ bash ~/ros2/scripts/run_all.sh --arm 89 --depth 6 --pitch 1.0 ## 二阶段一次性拍照定位（stage2，与一阶段跟踪分开跑） 先悬停垂直朝下 → 拍一张照 → `detect_best_1.onnx` 框工件 → Hough 解 k1/k2 孔心 → -深度（孔外工作面中值）+ 内参 + 手眼换算基座坐标 → 移动**规尖**到目标孔正上方 → -复拍验证（≤`align_max_iters` 次，收敛判据 `align_tol_m`）→ 锁定保持。 +像素射线交到 `workpiece_surface_z_m`（不读深度图）+ 内参 + 手眼换算基座坐标 → +移动**规尖**到目标孔正上方。 + +本分支 `hole_keypoint_once: true`：悬停拍一次，把框心到 k1 的半孔距（约 30mm） +一次走完（不再用多走多拍的 12mm 封顶），走后核验只记账。轻降先到观察高约 15mm +（单步 20mm，约 3 步），再交孔测补偿，斜线一步把 XY（≤3mm）和剩余 Z 收到 `final_gap_mm`。 +规尖沿用悬停冻结值，不在近距重测。两孔间距掉出 50–70mm 或要补超过 8mm 则不动、 +也不贴面。轻降力接触门控是 `final_gap_mm + 4mm`（贴面 2mm 时 = 6mm）。看 +`/tmp/stage2_result.json` 的 `snaps[]`。`main` 保存点是多走多拍闭环。 > **对的是"规尖"，不是"法兰轴线"**（2026-10-08 定稿）。通止规夹在爪里，相对法兰轴线 > 可以整体偏出若干毫米；旧写法把**法兰**对到孔上，规尖就偏着压在孔沿上，轻降一压 diff --git a/scripts/rdk_goto_xyz.py b/scripts/rdk_goto_xyz.py index 031c579..9128bd8 100644 --- a/scripts/rdk_goto_xyz.py +++ b/scripts/rdk_goto_xyz.py @@ -133,6 +133,7 @@ def main() -> int: parser = argparse.ArgumentParser(description="RDK MoveL：工具尖走到世界系的一个点") parser.add_argument("--xyz", nargs=3, type=float, metavar=("X", "Y", "Z"), help="目标点，米，机器人世界系") parser.add_argument("--vel", type=float, default=0.05, help="TCP 线速度 m/s，默认 0.05") + parser.add_argument("--acc", type=float, default=None, help="TCP 加速度 m/s²，交给 SDK acc；不给则用厂家默认") parser.add_argument("--max-step-m", type=float, default=0.40, help="离当前点超过这么多米就拒绝，除非 --go") parser.add_argument("--go", action="store_true", help="允许超过 --max-step-m") parser.add_argument( @@ -168,7 +169,11 @@ def main() -> int: target_xyz[2] = float(pose[2]) dist = math.dist(pose[:3], target_xyz) print(f"目标 xyz=({target_xyz[0]:.4f}, {target_xyz[1]:.4f}, {target_xyz[2]:.4f}) m") - print(f"直线距离 {dist * 1000:.0f} mm 速度 {args.vel:.3f} m/s 姿态保持当前") + acc_txt = "厂家默认" if args.acc is None else f"{float(args.acc):.3f} m/s²" + print( + f"直线距离 {dist * 1000:.0f} mm 速度 {args.vel:.3f} m/s" + f" 加速度 {acc_txt} 姿态保持当前" + ) if dist > args.max_step_m and not args.go: print(f"超过 {args.max_step_m * 1000:.0f} mm，拒绝。确认无误后加 --go。") return 2 @@ -177,17 +182,17 @@ def main() -> int: print(f"MoveL 欧拉角 deg xyz=({euler_deg[0]:.2f}, {euler_deg[1]:.2f}, {euler_deg[2]:.2f})") robot.SwitchMode(flexivrdk.Mode.NRT_PRIMITIVE_EXECUTION) - robot.ExecutePrimitive( - "MoveL", - { - "target": flexivrdk.Coord( - target_xyz, - euler_deg, - ["WORLD", "WORLD_ORIGIN"], - ), - "vel": float(args.vel), - }, - ) + prim = { + "target": flexivrdk.Coord( + target_xyz, + euler_deg, + ["WORLD", "WORLD_ORIGIN"], + ), + "vel": float(args.vel), + } + if args.acc is not None: + prim["acc"] = float(args.acc) + robot.ExecutePrimitive("MoveL", prim) print("MoveL 已下发，等 reachedTarget …") t0 = time.monotonic() while True: diff --git a/scripts/rdk_release_movel.py b/scripts/rdk_release_movel.py index d4b1d05..fb8ea56 100644 --- a/scripts/rdk_release_movel.py +++ b/scripts/rdk_release_movel.py @@ -122,7 +122,8 @@ def main() -> int: ap = argparse.ArgumentParser() ap.add_argument("--xyz", nargs=3, type=float, required=True, metavar=("X", "Y", "Z")) ap.add_argument("--arm", default=os.environ.get("SITE_ROBOT_ARM", "89")) - ap.add_argument("--vel", type=float, default=0.05) + ap.add_argument("--vel", type=float, default=0.3, help="TCP 线速度 m/s，交给 SDK vel") + ap.add_argument("--acc", type=float, default=1.5, help="TCP 加速度 m/s²，交给 SDK acc") args = ap.parse_args() held = _control_pids() @@ -144,6 +145,7 @@ def main() -> int: "--xyz", f"{x}", f"{y}", f"{z}", "--keep-z", "--go", "--vel", f"{float(args.vel)}", + "--acc", f"{float(args.acc)}", ] print("厂家 MoveL：XY 对准 D435 这一点，工具尖高度保持当前") got = subprocess.run(movel, check=False) diff --git a/src/arm_application/arm_application/overhead_oneshot.py b/src/arm_application/arm_application/overhead_oneshot.py index bc1f539..d838b0f 100644 --- a/src/arm_application/arm_application/overhead_oneshot.py +++ b/src/arm_application/arm_application/overhead_oneshot.py @@ -28,6 +28,8 @@ from arm_application.path_profile import ( # 伺服和测试从本模块再导 D435_NEAR_CRUISE_MPS, D435_NEAR_HOP_MAX_M, D435_NEAR_LEFTOVER_M, + D435_VENDOR_MOVEL_ACC_MPS2, + D435_VENDOR_MOVEL_VEL_MPS, D435_VERT_TILT_RAD, D435_SETTLE_S, FROZEN_J_HOP_MAX_M, @@ -301,7 +303,7 @@ def oneshot_concat_hold_ori(q0, J, segments, *, R=None): def oneshot_full_cruise_mps(leftover_m: float, far_cruise_mps: float) -> float: - """整段巡航：工件上方 30 mm/s；远处封顶 ``D435_FULL_CRUISE_MPS``，不许再弹到 80。""" + """整段巡航：工件上方 45 mm/s；远处封顶 ``D435_FULL_CRUISE_MPS``，不许再弹到远处 hop 上限。""" far = max(1e-4, float(far_cruise_mps)) if float(leftover_m) <= D435_NEAR_LEFTOVER_M: return min(D435_NEAR_CRUISE_MPS, far) @@ -509,6 +511,80 @@ def pixel_on_base_plane( return np.asarray(point, dtype=float).reshape(3) +def cam_xy_bias_axes(T_cam_to_base: np.ndarray): + """基座水平面上两个单位向量：``(视线前方, 画面右)``。 + + 视线 = 相机 +Z 投到基座 XY（前倾 30° 时的交平面敏感轴）； + 画面右 = 相机 +X 投到基座 XY（立体基线 / 图像 u 方向）。 + 水平投影退化时返回 None，不许拿 (0,0) 当轴。 + """ + T = np.asarray(T_cam_to_base, dtype=float).reshape(4, 4) + look = T[:3, :3] @ np.array([0.0, 0.0, 1.0], dtype=float) + right = T[:3, :3] @ np.array([1.0, 0.0, 0.0], dtype=float) + look_xy = np.asarray(look[:2], dtype=float) + right_xy = np.asarray(right[:2], dtype=float) + n_l = float(np.linalg.norm(look_xy)) + n_r = float(np.linalg.norm(right_xy)) + if n_l < 1e-6 or n_r < 1e-6: + return None + return look_xy / n_l, right_xy / n_r + + +# 单次精拍这一步必须能走完「框心 → k1」。两孔间距约 60mm，半距 30mm。 +# 多走多拍的 12mm 封顶在这里会把一半孔距留下（2026-10-13 真机：指令 29mm 只走 12mm）。 +ONESHOT_ALIGN_CAP_M = 0.040 +# 下降后补 XY：一跳上限。2026-10-13 两跳合计 4.3mm 把近距检测带崩。 +ONCE_TRIM_MAX_STEP_M = 0.003 +ONCE_TRIM_REFUSE_M = 0.008 +HOLE_PAIR_TRIM_MIN_M = 0.050 +HOLE_PAIR_TRIM_MAX_M = 0.070 +# 悬停 → 观察高（约 15mm）单步上限。左臂间隙约 58mm，20mm × 3 步走完。 +# 观察高 → 贴面不再走这段，改斜线一步（XY 补偿 + 剩余 Z）。 +ONCE_APPROACH_STEP_M = 0.020 + + +def oneshot_align_step_m(err_m: float) -> float: + """单次精拍允许的步长 [m]。不超过半孔距余量；超过 cap 才截断。""" + e = abs(float(err_m)) + cap = float(ONESHOT_ALIGN_CAP_M) + if e <= cap: + return e + return cap + + +def hole_pair_span_m(p0, p1) -> float: + """两孔基座 XY 间距 [m]。""" + a = np.asarray(p0, dtype=float).reshape(-1) + b = np.asarray(p1, dtype=float).reshape(-1) + return float(np.hypot(float(a[0]) - float(b[0]), float(a[1]) - float(b[1]))) + + +def hole_pair_span_ok(span_m: float) -> bool: + """下降后补孔用的间距窗。真机 k1/k2 约 60mm；45mm 是近距检测漂了。""" + s = float(span_m) + return HOLE_PAIR_TRIM_MIN_M <= s <= HOLE_PAIR_TRIM_MAX_M + + +def flange_z_for_gap_m(tip_len_m: float, surface_z_m: float, gap_m: float) -> float: + """规尖离面 ``gap_m`` 时的法兰 Z [m]。与轻降几何同一条式子。""" + return float(tip_len_m) + float(surface_z_m) + float(gap_m) + + +def decompose_base_xy(delta_xy, T_cam_to_base: np.ndarray): + """把基座 XY 残差拆成沿视线 / 沿画面右 [m]。投影退化返回 None。""" + axes = cam_xy_bias_axes(T_cam_to_base) + if axes is None: + return None + look_xy, right_xy = axes + d = np.asarray(delta_xy, dtype=float).reshape(2) + return { + "along_look_m": float(np.dot(d, look_xy)), + "along_right_m": float(np.dot(d, right_xy)), + "look_xy": [float(look_xy[0]), float(look_xy[1])], + "right_xy": [float(right_xy[0]), float(right_xy[1])], + } + + def px_to_cam( u: float, v: float, z: float, fx: float, fy: float, cx: float, cy: float ) -> np.ndarray: diff --git a/src/arm_application/arm_application/path_profile.py b/src/arm_application/arm_application/path_profile.py index 1189568..ab80453 100644 --- a/src/arm_application/arm_application/path_profile.py +++ b/src/arm_application/arm_application/path_profile.py @@ -11,10 +11,15 @@ from typing import Optional, Tuple import numpy as np -# 整段同一巡航：site ``v_slow_xy`` × 本系数。1.0 → 80 mm/s（二阶段对孔同档上限）。 -D435_MOVE_SPEED_SCALE = 1.0 +# 整段同一巡航：site ``v_slow_xy`` × 本系数。1.5 → 120 mm/s（远处 hop 上限）。 +D435_MOVE_SPEED_SCALE = 1.5 +# D435 厂家 MoveL：SDK ``vel`` [m/s]、``acc`` [m/s²]。 +# 厂家示例自由空间 0.6 / 近接触 0.2；本档 0.3 和 1.5，短行程也能爬到巡航。 +# 只改 vel、不给 acc 时，短 hop 仍被默认加速度卡住，肉眼看不出。 +D435_VENDOR_MOVEL_VEL_MPS = 0.3 +D435_VENDOR_MOVEL_ACC_MPS2 = 1.5 # 缺路程时的兜底加速度。真机 hop 用 ``trap_acc_for_hop``，和二阶段同一条刹车。 -D435_MOVE_ACC_MPS2 = 0.125 +D435_MOVE_ACC_MPS2 = 0.1875 # action 走完后再停这么久，才重测 TF。二阶段 ``_goto`` 的 settle_s。 D435_SETTLE_S = 0.25 # 冻结雅可比一次只信这么远。二阶段 ``_plan_and_send`` 默认 ``max_seg`` 同源。 @@ -23,7 +28,7 @@ FROZEN_J_HOP_MAX_M = 0.08 # 剩差进入这一带 = 工件上方。真机最后一跳 37–75 mm @ 80 mm/s 会晃并偏转。 D435_NEAR_LEFTOVER_M = 0.08 D435_NEAR_HOP_MAX_M = 0.015 -D435_NEAR_CRUISE_MPS = 0.030 +D435_NEAR_CRUISE_MPS = 0.045 # 法兰倾角超过这个才单独收垂直。和 XY 捆在同一跳会把腕部漏成 Y 过冲。 D435_VERT_TILT_RAD = 0.015 D435_MID_LEFTOVER_M = 0.15 @@ -33,10 +38,11 @@ D435_ARC_SAGITTA_FRAC = 0.12 D435_ARC_SAGITTA_MAX_M = 0.05 # 整段一次走完。起步必须够长：40 mm 就爬到 80 mm/s 是真机那下弹射 # （头尾速度本来就是 0，不是初速度；平移也不下发角速度）。 -# 峰值加速度 a_max = π v² / (4 d_acc)；100 mm @ 60 mm/s → 约 0.028 m/s²。 -D435_FULL_ACC_M = 0.100 -D435_FULL_DEC_M = 0.080 -D435_FULL_CRUISE_MPS = 0.060 +# 峰值加速度 a_max = π v² / (4 d_acc)；150 mm @ 90 mm/s → 约 0.042 m/s²。 +# 巡航相对原 60 mm/s 加 50%；加减速路程同比加长，起步时间差不多。 +D435_FULL_ACC_M = 0.150 +D435_FULL_DEC_M = 0.120 +D435_FULL_CRUISE_MPS = 0.090 def hybrid_cruise_vxy( diff --git a/src/arm_application/arm_application/servo_node.py b/src/arm_application/arm_application/servo_node.py index 2fa2186..afcab20 100644 --- a/src/arm_application/arm_application/servo_node.py +++ b/src/arm_application/arm_application/servo_node.py @@ -52,6 +52,8 @@ from arm_application.handeye_lib import cam_point_to_base, cam_to_base_T from arm_application.overhead_oneshot import ( D435_NEAR_CRUISE_MPS, D435_SETTLE_S, + D435_VENDOR_MOVEL_ACC_MPS2, + D435_VENDOR_MOVEL_VEL_MPS, StillLock, at_lock_height, d405_commands_arm, @@ -1545,8 +1547,9 @@ class ServoNode(TolerantNode): def _d435_vendor_movel(self) -> bool: """D435 这一点：停真机栈 → 厂家 MoveL → 冷启动控制栈。 - 流畅度先不优化。杀栈再拉起大约多几秒，但 RDK 独占，D405 的点 - 在整段 busy 期间都不会下发。停稳计时从冷启动回来之后才开始。 + 线速度 / 加速度走 ``D435_VENDOR_MOVEL_VEL_MPS``、``D435_VENDOR_MOVEL_ACC_MPS2``， + 交给 SDK 的 ``vel`` / ``acc``。杀栈再拉起大约多几秒，RDK 独占， + D405 的点在整段 busy 期间都不会下发。停稳计时从冷启动回来之后才开始。 """ with self._lock: goal = None if self._p_feat_base is None else np.asarray( @@ -1570,10 +1573,14 @@ class ServoNode(TolerantNode): "--arm", arm, "--vel", - "0.05", + f"{float(D435_VENDOR_MOVEL_VEL_MPS):.3f}", + "--acc", + f"{float(D435_VENDOR_MOVEL_ACC_MPS2):.3f}", ] self.get_logger().info( f"D435 厂家 MoveL XY=({goal[0]:.3f},{goal[1]:.3f}) " + f"vel={D435_VENDOR_MOVEL_VEL_MPS*1000:.0f}mm/s " + f"acc={D435_VENDOR_MOVEL_ACC_MPS2:.1f}m/s² " "—— 先停控制栈，MoveL 后再冷启动；期间不下发 D405" ) t0 = time.monotonic() diff --git a/src/arm_application/arm_application/stage2_locate_node.py b/src/arm_application/arm_application/stage2_locate_node.py index 8506e4f..01531c4 100644 --- a/src/arm_application/arm_application/stage2_locate_node.py +++ b/src/arm_application/arm_application/stage2_locate_node.py @@ -8,10 +8,10 @@ 2. hover — 闭环升到/降到 robots[].servo.hole_hover_z_m（悬停锁高） 3. coarse — 拍照，工件（pose 模型给框；k1/k2 用关键点）粗移到 bbox 中心上方 4. fine — 拍照，two_stage（YOLO+HoughCircles）解 k1/k2 孔心像素 - 5. align — 深度（孔外工作面中值，防孔底拉偏）+ 内参 + 手眼 + TF - 算出孔在基座系 XY，把夹爪/通止规轴线移到目标孔正上方， - 复拍验证，迭代到 align_tol_m - 6. hold — 锁定关节，发布 /stage2/result(JSON) + /stage2/overlay + 5. align — 像素射线交工件面 + 内参 + 手眼，把规尖移到目标孔正上方。 + ``hole_keypoint_once`` 时只走一次，核验拍只记账； + 否则复拍迭代到 align_tol_m + 6. hold — 锁定关节，发布 /stage2/result(JSON) + /tmp/stage2_result.json 高度调试（不动 XY、不拍照定位）： bash ~/ros2/scripts/run_stage2_locate.sh --arm 89 --hover-only @@ -51,7 +51,17 @@ from arm_application.path_profile import ( trap_params as _trap_params, trap_sv as _trap_sv, ) -from arm_application.overhead_oneshot import pixel_on_base_plane +from arm_application.overhead_oneshot import ( + ONCE_APPROACH_STEP_M, + ONCE_TRIM_MAX_STEP_M, + ONCE_TRIM_REFUSE_M, + decompose_base_xy, + flange_z_for_gap_m, + hole_pair_span_m, + hole_pair_span_ok, + oneshot_align_step_m, + pixel_on_base_plane, +) from arm_application.handeye_realsense import resolve_realsense_topics from arm_application.gauge_boot import perception_source_of from arm_application.gauge_fsm import drag_verdict, resolve_hover_z, wrench_all_zero @@ -60,18 +70,24 @@ from arm_application.hole_detector import config_from_site, weights_from_site from arm_application.site_io import deep_merge, load_site_dict from arm_application.ros_node import TolerantNode -def contact_armed_gap_m(observe_gap_m: float) -> float: +def contact_armed_gap_m(observe_gap_m: float, final_gap_m: float = 0.002) -> float: """轻降力接触门槛对应的几何间隙。离面更远时腕部力会漂几牛，不能当碰到。 - 2026-10-13：悬停 0.42 往下到法兰 0.384 时 ΔFz=+2.87N 超过 2.5N， - 几何间隙仍约 138mm。立刻停再交给三阶段抬到 hover+50mm，插孔没做。 + 2026-10-13 远场：法兰 0.384、间隙 138mm、ΔFz=+2.87N。 + 2026-10-13 近场仍假：间隙 9.5mm、ΔFz=+2.77N，尖几何上还在面上约 1cm， + 碰不到板。观察间隙 15mm 会把这一档武装起来，所以门控只收到 + ``final_gap_m + 4mm``（贴面 2mm 时 = 6mm）。``observe_gap_m`` 仍从调用方 + 传入，避免两处签名分叉，但不放宽门控。 """ - return max(float(observe_gap_m), 0.020) + 0.005 + _ = observe_gap_m + return float(final_gap_m) + 0.004 -def contact_armed_by_gap(gap_m: float, observe_gap_m: float) -> bool: +def contact_armed_by_gap( + gap_m: float, observe_gap_m: float, final_gap_m: float = 0.002 +) -> bool: """当前几何间隙够近，才许把腕部力突变当成碰到工件。""" - return float(gap_m) <= contact_armed_gap_m(observe_gap_m) + return float(gap_m) <= contact_armed_gap_m(observe_gap_m, final_gap_m) # 一次 hop 之后实测关节变化小于这个值，就认为「臂根本没跟随指令」。 @@ -250,6 +266,11 @@ class Stage2Locate(TolerantNode): self.surface_z_m = _surface_z # 对小孔只用关键点像素和已知工件面，不读深度图。缺了就按旧的深度路径。 self.hole_visual_only = _as_bool(stage2.get("hole_visual_only"), False) + # 本分支实验：悬停拍一次 k1/k2，走一次，复拍只记账不改 XY。 + # 键必须接线——写了 true 却仍多走多拍，等于没做实验。 + self.hole_keypoint_once = _as_bool(stage2.get("hole_keypoint_once"), False) + self._ws_clipped = False + self._hole_snaps: List[Dict[str, Any]] = [] if self.hover_z_user_set and (_tool_len <= 0.0 or _surface_z <= 0.0): self._hover_refused = ( "写了 --hover-z，但工具长度或工件面未知，无法确认尖端在面之上，拒绝执行" @@ -393,9 +414,15 @@ class Stage2Locate(TolerantNode): f" weights={self._weights}\n" f" image={self.image_topic} depth={self.depth_topic}\n" f" target={self.target_hole} (k1={self.k1_label}) tol={self.align_tol*1000:.1f}mm " - f"hover_only={self.hover_only} hole_visual_only={self.hole_visual_only}\n" + f"hover_only={self.hover_only} hole_visual_only={self.hole_visual_only} " + f"hole_keypoint_once={self.hole_keypoint_once}\n" f" 工具长={self.tool_tip_below_flange_m:.4f}m（法兰到尖，下臂不得低于面+间隙）" ) + if self.hole_keypoint_once: + self.get_logger().warn( + "对孔方案=单次精拍（实验分支）。拍一次→走一次，复拍和下降后只记账不改 XY。" + "残差以核验拍为准，不许用 hop 到位冒充。看 /tmp/stage2_result.json 的 snaps。" + ) # ==================== 状态/运动原语（闭环 hop，同 goto_ee_plane） ==================== @@ -574,7 +601,8 @@ class Stage2Locate(TolerantNode): lo = self.ws_min + 0.005 hi = self.ws_max - 0.005 clamped = np.clip(np.asarray(target, dtype=float).reshape(3), lo, hi) - if not np.allclose(clamped, target, atol=1e-6): + self._ws_clipped = bool(not np.allclose(clamped, target, atol=1e-6)) + if self._ws_clipped: self.get_logger().warn( f"目标超出工作盒，已截断: ({target[0]:.3f},{target[1]:.3f},{target[2]:.3f}) → " f"({clamped[0]:.3f},{clamped[1]:.3f},{clamped[2]:.3f})" @@ -1595,7 +1623,7 @@ class Stage2Locate(TolerantNode): return None return float(np.median(depth[ring])) - def _descend_above_hole(self, q, Tee): + def _descend_above_hole(self, q, Tee, *, target_gap_m=None, step_m=None): """对准后柔顺轻降，把规尖悬在孔面上方 final_gap_m（不接触）。 **下降量只由几何决定**（这条是 2026-10-08 真机修出来的）:: @@ -1622,11 +1650,21 @@ class Stage2Locate(TolerantNode): 2. 腕部 FT 接触检测：``force.z`` 相对悬空基线突变 ⇒ 立即停 + 回退 1mm； 3. 任一步 ``_goto`` 没走到位（多半是碰到东西/被挡）⇒ 停止，保持当前高度。 """ - # final_descent_enable：降到贴面 final_gap（约 2mm）。 - # 关掉时（--no-descent）仍做**观测下降**，停在 observe_gap（约 15mm）， - # 让人能看出规尖偏没偏，又不把 φ6 规压到 φ7 孔沿上。 - target_gap = self.final_gap_m if self.final_descent_enable else self.observe_gap_m - kind = "贴面轻降" if self.final_descent_enable else "观测下降" + # final_descent_enable：多走多拍降到贴面 final_gap（约 2mm）。 + # 关掉时（--no-descent）仍做**观测下降**，停在 observe_gap（约 15mm）。 + # 单次精拍由调用方传入 target_gap_m=观察高，贴面由随后斜线一步完成。 + if target_gap_m is None: + target_gap = ( + self.final_gap_m if self.final_descent_enable else self.observe_gap_m + ) + else: + target_gap = float(target_gap_m) + step_cap = 0.004 if step_m is None else float(step_m) + kind = ( + "贴面轻降" + if target_gap <= float(self.final_gap_m) + 1e-6 + else "观测下降" + ) self._state("descend_above_hole") tip_len = float(self.tool_tip_below_flange_m) surf = float(self.surface_z_m) @@ -1709,7 +1747,25 @@ class Stage2Locate(TolerantNode): gap_now = float(Tee[2, 3]) - tip_len - surf if ( base_fz is not None - and contact_armed_by_gap(gap_now, self.observe_gap_m) + and self._contact_detected(base_fz) + and not contact_armed_by_gap( + gap_now, self.observe_gap_m, self.final_gap_m + ) + ): + if not getattr(self, "_ft_gap_disarmed_logged", False): + self._ft_gap_disarmed_logged = True + armed_m = contact_armed_gap_m( + self.observe_gap_m, self.final_gap_m + ) + self.get_logger().info( + f" ΔFz 超门槛但间隙 {gap_now * 1000:.1f}mm > 接触门控 " + f"{armed_m * 1000:.1f}mm，不当作碰到" + ) + if ( + base_fz is not None + and contact_armed_by_gap( + gap_now, self.observe_gap_m, self.final_gap_m + ) and self._contact_detected(base_fz) ): self.get_logger().warn("腕部力突变（规尖触到工件），立即停止并回退 1mm（安全优先）") @@ -1726,7 +1782,7 @@ class Stage2Locate(TolerantNode): remaining = (float(Tee[2, 3]) - tip_len - surf) - target_gap if remaining <= 1e-4: break - step = min(remaining, 0.004) # 每步 ≤4mm，很慢很柔 + step = min(remaining, step_cap) # 贴面默认 ≤4mm；观察高 20mm，见 step_m p = Tee[:3, 3].copy() target = np.array([p[0], p[1], p[2] - step]) ok, q, Tee = self._goto( @@ -1750,6 +1806,404 @@ class Stage2Locate(TolerantNode): ) return True, q, Tee + def _stamp_bias( + self, + rec: Dict[str, Any], + *, + tgt_b: np.ndarray, + gxy: np.ndarray, + tag: str, + ) -> Dict[str, Any]: + """把基座残差拆成视线 / 画面右，写入记录。投影退化时键仍在、值为 None。""" + rec = dict(rec) + rec["tag"] = tag + rec["scheme"] = "keypoint_once" if self.hole_keypoint_once else "multi_snap" + rec["cam_frame"] = self.cam_frame + rec["image_topic"] = self.image_topic + err_vec = np.array( + [float(tgt_b[0]) - float(gxy[0]), float(tgt_b[1]) - float(gxy[1])], + dtype=float, + ) + rec["err_xy_m"] = [round(float(err_vec[0]), 5), round(float(err_vec[1]), 5)] + rec["workspace_clipped"] = bool(self._ws_clipped) + T_cam = self._lookup(self.base_frame, self.cam_frame) + rec["along_look_mm"] = None + rec["along_right_mm"] = None + rec["look_xy"] = None + rec["right_xy"] = None + rec["cam_origin_base_m"] = None + if T_cam is None: + self.get_logger().warn(f"{tag}：没有 cam TF，残差分解缺轴") + return rec + rec["cam_origin_base_m"] = [ + round(float(T_cam[0, 3]), 5), + round(float(T_cam[1, 3]), 5), + round(float(T_cam[2, 3]), 5), + ] + decomp = decompose_base_xy(err_vec, T_cam) + if decomp is None: + self.get_logger().warn(f"{tag}：光轴水平投影退化，残差分解缺轴") + return rec + rec["along_look_mm"] = round(float(decomp["along_look_m"]) * 1000.0, 2) + rec["along_right_mm"] = round(float(decomp["along_right_m"]) * 1000.0, 2) + rec["look_xy"] = [round(float(v), 5) for v in decomp["look_xy"]] + rec["right_xy"] = [round(float(v), 5) for v in decomp["right_xy"]] + self.get_logger().info( + f"[{tag}] 残差分解 视线={rec['along_look_mm']:+.2f}mm " + f"画面右={rec['along_right_mm']:+.2f}mm " + f"ΔXY=({err_vec[0]*1000:+.2f},{err_vec[1]*1000:+.2f})mm\n" + f" 画面右为正：孔在规的画面右侧（目视就是规偏左）。" + f"视线为正：孔在光轴水平投影前方（工件面写低 / 交深了会往这偏）。" + ) + return rec + + def _observe_holes(self, q, Tee, final: Dict[str, Any], *, tag: str): + """核验拍：只记账，不改 XY。拍不到就保持原 final，并标明没复测。""" + try: + _bgr, _depth_m, _intr, res = self._snap_holes(skip_center_gate=True) + except Exception as exc: # noqa: BLE001 + self.get_logger().error(f"{tag}失败，残差仍是开环指令值：{exc}") + final = dict(final) + final["residual_remeasured"] = False + return q, Tee, final + if not res.get("status"): + self.get_logger().error( + f"{tag}丢孔（{res.get('msg')}），残差仍是开环指令值，不是 hop 到位" + ) + final = dict(final) + final["residual_remeasured"] = False + return q, Tee, final + hb = res.get("hole_base") + z_wp = float(res.get("z_wp") or 0.0) + if hb is None or len(hb) != 2 or z_wp <= 0.0: + self.get_logger().error(f"{tag}没有孔的基座坐标，残差未复测") + final = dict(final) + final["residual_remeasured"] = False + return q, Tee, final + left_b = np.asarray(hb[0], dtype=float) + right_b = np.asarray(hb[1], dtype=float) + k1_b, k2_b = (left_b, right_b) if self.k1_label == "left" else (right_b, left_b) + tgt_b = k1_b if self.target_hole == "k1" else k2_b + Tee_now = self._lookup(self.base_frame, self.ee_frame) + if Tee_now is not None: + Tee = Tee_now + pts = res.get("hole_points") or [] + if self._tip_off is not None: + gxy = np.array( + [float(Tee[0, 3]) + float(self._tip_off[0]), + float(Tee[1, 3]) + float(self._tip_off[1])] + ) + tip_src = "视觉规尖(开环本步)" + else: + gxy = self._gauge_xy(Tee) + tip_src = "法兰+偏置" + err = float(np.hypot(float(tgt_b[0]) - gxy[0], float(tgt_b[1]) - gxy[1])) + rec = { + "ok": err <= self.align_tol, + "arm": self.prefix.rstrip("_"), + "target_hole": self.target_hole, + "k1_base_m": [round(float(v), 5) for v in k1_b], + "k2_base_m": [round(float(v), 5) for v in k2_b], + "k1_px": ( + [round(float(pts[0][0]), 1), round(float(pts[0][1]), 1)] + if len(pts) >= 2 else None + ), + "k2_px": ( + [round(float(pts[1][0]), 1), round(float(pts[1][1]), 1)] + if len(pts) >= 2 else None + ), + "workpiece_surface_z_m": round(z_wp, 4), + "gauge_xy_m": [round(float(gxy[0]), 5), round(float(gxy[1]), 5)], + "gauge_source": tip_src, + "tip_offset_m": ( + [round(float(self._tip_off[0]), 5), round(float(self._tip_off[1]), 5)] + if self._tip_off is not None else None + ), + "residual_mm": round(err * 1000.0, 2), + "hover_z_m": round(float(Tee[2, 3]), 4), + "residual_remeasured": True, + "align_trusted": self._tip_off is not None, + } + rec = self._stamp_bias(rec, tgt_b=tgt_b, gxy=gxy, tag=tag) + snaps = list(final.get("snaps") or []) + snaps.append({k: rec[k] for k in ( + "tag", "residual_mm", "err_xy_m", "along_look_mm", "along_right_mm", + "k1_base_m", "k2_base_m", "k1_px", "k2_px", "gauge_xy_m", + "gauge_source", "tip_offset_m", "hover_z_m", "workspace_clipped", + ) if k in rec}) + merged = dict(final) + merged.update(rec) + merged["snaps"] = snaps + merged["ok"] = bool(rec["ok"] and rec["align_trusted"]) + draw = res.get("draw_img") + if draw is None: + draw = _bgr + if draw is not None: + self._publish_overlay(draw) + self._publish_result(merged) + self.get_logger().info( + f"[{tag}] k1=({k1_b[0]:.4f},{k1_b[1]:.4f}) " + f"工具=({gxy[0]:.4f},{gxy[1]:.4f})[{tip_src}] " + f"复测残差={err*1000:.2f}mm（此拍不改 XY）" + ) + return q, Tee, merged + + def _oneshot_align_move(self, q, Tee, tgt_b, gxy, err): + """单次精拍只走这一步。返回 (q, Tee, 是否走了)。""" + if err <= self.align_tol: + self.get_logger().info( + f"单次精拍开环残差 {err*1000:.2f}mm 已在容差内，这一步不横移" + ) + return q, Tee, False + raw = np.array([float(tgt_b[0]) - float(gxy[0]), float(tgt_b[1]) - float(gxy[1])]) + n_raw = float(np.linalg.norm(raw)) + max_step = oneshot_align_step_m(n_raw) + step = raw + if n_raw > max_step + 1e-9: + step = raw * (max_step / n_raw) + self.get_logger().warn( + f" 单次精拍步长 {n_raw*1000:.1f}mm 超过 " + f"{max_step*1000:.0f}mm（不像框心到 k1 的半孔距），截断。" + "截断后下降补 XY 也补不完，会留下厘米级残差" + ) + else: + self.get_logger().info( + f" 单次精拍这一步走完 {n_raw*1000:.1f}mm（框心到目标孔，不再用 12mm 封顶）" + ) + move = np.array([ + float(Tee[0, 3]) + float(step[0]), + float(Tee[1, 3]) + float(step[1]), + self.hover_z, + ]) + hop_tol = 0.00022 if err < 0.003 else max(0.0003, min(0.0005, 0.25 * err)) + hop_v = self.align_speed if err > 0.005 else min(self.align_speed, 0.016) + self._state("align_move_once") + ok, q, Tee = self._goto( + q, Tee, move, verticalize=True, + tol_m=hop_tol, tol_tilt=self.vert_tol_rad * 2.0, + tag="align_once", v=hop_v, + max_hops=8, settle_s=max(0.35, self.settle_sec), + min_dur=0.35, strict_xy=True, + ) + self._pin(q) + if not ok: + self.get_logger().warn( + "单次精拍这一步没走到位；残差以随后核验拍为准，不把 hop 容差当对上" + ) + return q, Tee, True + + def _trim_xy_keep_hover_tip(self, q, Tee, final: Dict[str, Any], z_target_m=None): + """观察高重交孔，规尖沿用悬停冻住的偏移。只许一跳、XY ≤3mm。 + + 2026-10-13 真机两遍：走后核验 0.4~0.7mm，降后交平面漂 4~5mm；观测高度 + 深度把夹爪当尖，两拍差 8~36mm，重测守卫把该补的 4mm 也拒掉了。悬停规尖 + 只有 0.1mm 量级，沿用它，只补孔 XY。没有冻住的偏移就不许用法兰凑。 + + 同日第二次：两跳合计 +4.3mm X 把近距检测带崩（k1 像素 446→403，间距 + 60→45mm，残差 2.6→14.8mm），流水线还把走之前的 2.61mm 当终值。所以 + 只许一跳、步长 ≤3mm；超过 8mm 根本不动。 + + ``z_target_m`` 有值时这一跳同时收到贴面法兰高（观察高测补偿，斜线一步 + 贴面）。贴面复拍只记账：近了画面更容易漂，不按近距那一拍撤回观察高的解。 + """ + if self._tip_off is None: + self.get_logger().error( + "下降后补 XY 需要悬停冻住的规尖偏移；没有就不许用法兰凑，保持原地" + ) + return q, Tee, final + off = np.asarray(self._tip_off, dtype=float).reshape(2) + final_in = final + z_hold = float(Tee[2, 3]) + + def _measure(src: Dict[str, Any], tag: str): + nonlocal Tee + try: + _bgr, _depth_m, _intr, res = self._snap_holes(skip_center_gate=True) + except Exception as exc: # noqa: BLE001 + self.get_logger().warn(f"{tag}复拍失败，保持原地：{exc}") + return None + if not res.get("status"): + self.get_logger().warn(f"{tag}丢孔（{res.get('msg')}），不再改 XY") + return None + hb = res.get("hole_base") + z_wp = float(res.get("z_wp") or 0.0) + if hb is None or len(hb) != 2 or z_wp <= 0.0: + self.get_logger().warn(f"{tag}没有孔的基座坐标，不再改 XY") + return None + left_b = np.asarray(hb[0], dtype=float) + right_b = np.asarray(hb[1], dtype=float) + k1_b, k2_b = ( + (left_b, right_b) if self.k1_label == "left" else (right_b, left_b) + ) + tgt = k1_b if self.target_hole == "k1" else k2_b + Tee_now = self._lookup(self.base_frame, self.ee_frame) + if Tee_now is not None: + Tee = Tee_now + gxy = np.array( + [float(Tee[0, 3]) + float(off[0]), float(Tee[1, 3]) + float(off[1])] + ) + raw = np.array([float(tgt[0]) - gxy[0], float(tgt[1]) - gxy[1]]) + err = float(np.linalg.norm(raw)) + pair = hole_pair_span_m(k1_b, k2_b) + rec = dict(src) + rec["residual_mm"] = round(err * 1000.0, 2) + rec["tip_offset_m"] = [round(float(off[0]), 5), round(float(off[1]), 5)] + rec["gauge_xy_m"] = [round(float(gxy[0]), 5), round(float(gxy[1]), 5)] + rec["gauge_source"] = "视觉规尖(悬停冻结，下降后只重交孔)" + rec["k1_base_m"] = [round(float(v), 5) for v in k1_b] + rec["k2_base_m"] = [round(float(v), 5) for v in k2_b] + rec["hover_z_m"] = round(float(Tee[2, 3]), 4) + rec["pair_span_mm"] = round(pair * 1000.0, 2) + rec["residual_remeasured"] = True + rec["align_trusted"] = True + rec = self._stamp_bias(rec, tgt_b=tgt, gxy=gxy, tag=tag) + snaps = list(src.get("snaps") or []) + snaps.append({ + "tag": rec.get("tag"), + "residual_mm": rec.get("residual_mm"), + "err_xy_m": rec.get("err_xy_m"), + "along_look_mm": rec.get("along_look_mm"), + "along_right_mm": rec.get("along_right_mm"), + "k1_base_m": rec.get("k1_base_m"), + "gauge_xy_m": rec.get("gauge_xy_m"), + "gauge_source": rec.get("gauge_source"), + "tip_offset_m": rec.get("tip_offset_m"), + "hover_z_m": rec.get("hover_z_m"), + "pair_span_mm": rec.get("pair_span_mm"), + }) + rec["snaps"] = snaps + rec["ok"] = err <= self.align_tol + rec["trim_reverted"] = False + self._publish_result(rec) + self.get_logger().info( + f"[{tag}] 工具=({gxy[0]:.4f},{gxy[1]:.4f})" + f"[悬停冻结] 残差={err * 1000:.2f}mm 间距={pair * 1000:.1f}mm —— 不重测规尖" + ) + return rec, err, raw, pair + + def _revert(xy_flange) -> None: + nonlocal q, Tee + back = np.array([float(xy_flange[0]), float(xy_flange[1]), z_hold]) + okb, q, Tee = self._goto( + q, Tee, back, verticalize=True, + tol_m=0.0004, tol_tilt=self.vert_tol_rad * 2.0, + tag="trim_xy_revert", v=min(float(self.align_speed), 0.010), + max_hops=4, settle_s=0.30, strict_xy=True, + ) + self._pin(q) + if not okb: + self.get_logger().error("下降后补孔撤回没走到，位置以当前 FK 为准") + + if not rclpy.ok(): + return q, Tee, final_in + measured = _measure(final_in, "下降后补孔 0") + if measured is None: + return q, Tee, final_in + rec, err, raw, pair = measured + z_goal = z_hold + blend_z = False + if z_target_m is not None: + z_goal = min(z_hold, float(z_target_m)) + blend_z = (z_hold - z_goal) > 0.0005 + if not hole_pair_span_ok(pair): + self.get_logger().error( + f"下降后两孔间距 {pair * 1000:.1f}mm 不在 50–70mm，检测漂了，不动" + ) + return q, Tee, final_in + if err > ONCE_TRIM_REFUSE_M: + self.get_logger().error( + f"下降后要对 {err * 1000:.1f}mm，超过 {ONCE_TRIM_REFUSE_M * 1000:.0f}mm " + "—— 不像交平面漂移，停在原地" + ) + return q, Tee, final_in + need_xy = err > self.align_tol + if (not need_xy) and (not blend_z): + self.get_logger().info( + f"下降后 XY 已在容差内（{err * 1000:.2f}mm），高度保持 {z_hold:.4f}" + ) + return q, Tee, rec + step = np.zeros(2, dtype=float) if not need_xy else np.asarray(raw, dtype=float) + if need_xy and err > ONCE_TRIM_MAX_STEP_M: + step = raw * (ONCE_TRIM_MAX_STEP_M / err) + xy_before = np.array([float(Tee[0, 3]), float(Tee[1, 3])]) + move = np.array([ + float(Tee[0, 3]) + float(step[0]), + float(Tee[1, 3]) + float(step[1]), + z_goal, + ]) + hop_tag = "trim_blend_to_final" if blend_z else "trim_xy_keep_tip" + hop_v = ( + min(float(getattr(self, "final_descent_speed", 0.005) or 0.005), 0.008) + if blend_z + else min(float(self.align_speed), 0.010) + ) + if blend_z: + self.get_logger().info( + f"观察高斜线贴面 Δxy=({step[0] * 1000:+.1f},{step[1] * 1000:+.1f})mm " + f"Δz={(z_goal - z_hold) * 1000:+.1f}mm → 法兰 Z {z_goal:.4f}" + ) + ok, q, Tee = self._goto( + q, Tee, move, verticalize=True, + tol_m=0.0004, tol_tilt=self.vert_tol_rad * 2.0, + tag=hop_tag, v=hop_v, + max_hops=4, settle_s=0.30, strict_xy=True, + ) + self._pin(q) + if not ok: + self.get_logger().warn("下降后 XY 修正没走到，停；残差以走前重测为准") + return q, Tee, rec + if abs(float(Tee[2, 3]) - z_goal) > 0.002: + self.get_logger().error( + f"下降后修正把高度从 {z_hold:.4f} 带到了 {float(Tee[2, 3]):.4f}" + f"（目标 {z_goal:.4f}），撤回" + ) + _revert(xy_before) + rec["trim_reverted"] = True + rec["ok"] = err <= self.align_tol + return q, Tee, rec + after = _measure(rec, "下降后补孔核验") + if blend_z: + if after is None or (not hole_pair_span_ok(after[3])) or after[1] > ONCE_TRIM_REFUSE_M: + self.get_logger().warn( + "贴面复拍不可信，斜线位姿保留（补偿按观察高那一拍）" + ) + rec["close_snap_untrusted"] = True + rec["hover_z_m"] = round(float(Tee[2, 3]), 4) + rec["ok"] = err <= self.align_tol + self._publish_result(rec) + return q, Tee, rec + rec2, err2, _raw2, _pair2 = after + rec2["ok"] = err2 <= self.align_tol + rec2["trim_reverted"] = False + rec2["close_snap_untrusted"] = False + self._publish_result(rec2) + return q, Tee, rec2 + if after is None: + self.get_logger().error("下降后补孔走完复拍失败，撤回这一跳") + _revert(xy_before) + rec["trim_reverted"] = True + rec["ok"] = err <= self.align_tol + return q, Tee, rec + rec2, err2, _raw2, pair2 = after + worse_pair = (not hole_pair_span_ok(pair2)) or (pair2 < pair - 0.002) + worse_res = err2 > err + 1e-6 + if worse_pair or worse_res: + self.get_logger().error( + f"下降后补了 {float(np.linalg.norm(step)) * 1000:.1f}mm 后变差" + f"（残差 {err * 1000:.2f}→{err2 * 1000:.2f}mm，" + f"间距 {pair * 1000:.1f}→{pair2 * 1000:.1f}mm），撤回这一跳" + ) + _revert(xy_before) + rec["trim_reverted"] = True + rec["ok"] = err <= self.align_tol + rec["snaps"] = rec2.get("snaps") or rec.get("snaps") + self._publish_result(rec) + return q, Tee, rec + rec2["ok"] = err2 <= self.align_tol + rec2["trim_reverted"] = False + self._publish_result(rec2) + return q, Tee, rec2 + def _trim_xy_after_descent(self, q, Tee, final: Dict[str, Any]): """下降之后、保持当前高度，用这一高度的规尖和孔再对一次 XY。 @@ -1759,9 +2213,24 @@ class Stage2Locate(TolerantNode): 只动了 0.2mm。画面里针在孔的上方一截，主要是 15mm 间隙的透视，横着的 那 2mm 才是要补的。 - 只用本高度新量的规尖，不沿用悬停时冻住的偏移。总修正超过 8mm 就不动 - —— 那一档是夹爪/杆身，不是针。量不到规尖也不动，悬停对准的结果留着。 + 多走多拍：只用本高度新量的规尖，不沿用悬停时冻住的偏移。总修正超过 8mm + 就不动 —— 那一档是夹爪/杆身，不是针。量不到规尖也不动，悬停对准的结果留着。 + 单次精拍：观测高度深度会把夹爪当尖（2026-10-13 两拍差 8~36mm），改走 + ``_trim_xy_keep_hover_tip``。 """ + if getattr(self, "hole_keypoint_once", False): + # 打桩测试把 stub 当 self 传入未绑定方法，必须点到类上的实现。 + z_target = None + if bool(getattr(self, "final_descent_enable", False)): + tip = float(getattr(self, "tool_tip_below_flange_m", 0.0) or 0.0) + surf = float(getattr(self, "surface_z_m", 0.0) or 0.0) + if tip > 0.0 and surf > 0.0: + z_target = flange_z_for_gap_m( + tip, surf, getattr(self, "final_gap_m", 0.002) + ) + return Stage2Locate._trim_xy_keep_hover_tip( + self, q, Tee, final, z_target_m=z_target + ) if not self.tip_vision: return q, Tee, final final_in = final @@ -2306,11 +2775,26 @@ class Stage2Locate(TolerantNode): if off is not None and tlen is not None: self._tip_samp.append((off, tlen)) self._refresh_tip_offset() + if self.hole_keypoint_once and self._tip_off is None: + self._tip_off = np.asarray(off, dtype=float).reshape(2) + self._tip_len_m = float(tlen) + self.get_logger().warn( + f"单次精拍凑不满 tip_calib_n={self.tip_calib_n}，本帧规尖偏移 " + f"({off[0]*1000:+.1f},{off[1]*1000:+.1f})mm 仅用于这一步，未冻结" + ) tip_src = ( f"视觉规尖(校准中 n={len(self._tip_samp)} {why})" - if self._tip_off is None + if (self._tip_off is None or ( + self.hole_keypoint_once and len(self._tip_samp) < self.tip_calib_n + )) else "视觉规尖(已冻结)" ) + if self.hole_keypoint_once and self._tip_off is not None: + tip_src = ( + "视觉规尖(已冻结)" + if len(self._tip_samp) >= self.tip_calib_n + else "视觉规尖(单次精拍·本帧)" + ) else: self.get_logger().warn(f" 规尖视觉量取失败：{why}") elif self._tip_off is not None: @@ -2349,7 +2833,28 @@ class Stage2Locate(TolerantNode): "hover_z_m": round(float(Tee[2, 3]), 4), "iter": it, "snap_frames": int(res.get("snap_frames") or 1), + "residual_remeasured": False, } + final = self._stamp_bias(final, tgt_b=tgt_b, gxy=gxy, tag=f"fine {it}") + snap0 = { + "tag": final.get("tag"), + "residual_mm": final.get("residual_mm"), + "err_xy_m": final.get("err_xy_m"), + "along_look_mm": final.get("along_look_mm"), + "along_right_mm": final.get("along_right_mm"), + "k1_base_m": final.get("k1_base_m"), + "k2_base_m": final.get("k2_base_m"), + "k1_px": final.get("k1_px"), + "k2_px": final.get("k2_px"), + "gauge_xy_m": final.get("gauge_xy_m"), + "gauge_source": final.get("gauge_source"), + "tip_offset_m": final.get("tip_offset_m"), + "hover_z_m": final.get("hover_z_m"), + "workspace_clipped": final.get("workspace_clipped"), + } + snaps = list(final.get("snaps") or []) + snaps.append(snap0) + final["snaps"] = snaps self.get_logger().info( f"[fine {it}] k1=({k1_b[0]:.4f},{k1_b[1]:.4f}) k2=({k2_b[0]:.4f},{k2_b[1]:.4f}) " f"zL={z_left*1000:.0f} zR={z_right*1000:.0f}mm " @@ -2415,6 +2920,34 @@ class Stage2Locate(TolerantNode): self._publish_overlay(draw) self._publish_result(final) + if self.hole_keypoint_once: + commanded_mm = round(err * 1000.0, 2) + if self._tip_off is None and self.tip_vision: + self.get_logger().error( + "单次精拍没量到规尖，这一步对的是法兰。align_trusted=false，" + "仍走一次并下降，方便目视；不把法兰对上说成成功" + ) + final["ok"] = False + final["align_trusted"] = False + final["fail_reason"] = "未量到规尖偏移，工具位置退化为法兰轴线" + q, Tee, _moved = self._oneshot_align_move(q, Tee, tgt_b, gxy, err) + q, Tee, final = self._observe_holes( + q, Tee, final, tag="走后核验" + ) + final["commanded_residual_mm"] = commanded_mm + if not final.get("residual_remeasured"): + self.get_logger().error( + f"走后核验没复测到。开环指令残差 {commanded_mm:.2f}mm，" + "不是 hop 到位，不把这次当对准成功" + ) + final["ok"] = False + self._state("aligned") + self.get_logger().info( + f"单次精拍悬停走完：指令 {commanded_mm:.2f}mm → " + f"复测 {final.get('residual_mm')}mm；下降后还要补交平面漂的那几毫米" + ) + break + if err <= self.align_tol: if self.tip_vision and self._tip_off is None: # 容差达标了，但这一拍的"工具位置"其实是**法兰**（退路了）—— @@ -2503,10 +3036,27 @@ class Stage2Locate(TolerantNode): # ---- 4.5) 柔顺轻降到孔面上方 1-2mm（不接触）---- # 下降量由**几何**决定（法兰 FK − 工具长 − 工件面 − final_gap），深度读数只 # 做交叉核对，见 _descend_above_hole 的说明。 - okd, q, Tee = self._descend_above_hole(q, Tee) + if self.hole_keypoint_once: + # 先降到观察高（约 15mm）再交孔；贴面由补孔斜线一步完成，不再 4mm 切到 2mm。 + okd, q, Tee = self._descend_above_hole( + q, Tee, + target_gap_m=self.observe_gap_m, + step_m=ONCE_APPROACH_STEP_M, + ) + else: + okd, q, Tee = self._descend_above_hole(q, Tee) if not okd: self.get_logger().warn("轻降未完全到位，保持当前高度（安全优先）") - else: + if self.hole_keypoint_once: + # 悬停那一拍的基座 XY 不能带到观测高：前倾 30°，降 50mm 交点会漂约 4mm。 + # 核验记账后补 XY；规尖沿用悬停冻结值，不在观测高度重测。 + q, Tee, final = self._observe_holes(q, Tee, final, tag="降后核验") + q, Tee, final = self._trim_xy_after_descent(q, Tee, final) + tip_len = float(self.tool_tip_below_flange_m) + surf = float(self.surface_z_m) + if tip_len > 0.0 and surf > 0.0: + self.descent_final_gap_m = float(Tee[2, 3]) - tip_len - surf + elif okd: # 悬停残差在下降 60mm 之后会变大约 2mm（相机 30°，同一孔两个高度 # 反投影对不齐）。在观测高度重测规尖，只补 XY。 q, Tee, final = self._trim_xy_after_descent(q, Tee, final) diff --git a/src/arm_application/test/test_overhead_oneshot.py b/src/arm_application/test/test_overhead_oneshot.py index 791a0fa..730b965 100644 --- a/src/arm_application/test/test_overhead_oneshot.py +++ b/src/arm_application/test/test_overhead_oneshot.py @@ -31,6 +31,8 @@ from arm_application.overhead_oneshot import ( map_uv_to_depth, D405_MISS_BEFORE_D435_S, D435_MOVE_SPEED_SCALE, + D435_VENDOR_MOVEL_ACC_MPS2, + D435_VENDOR_MOVEL_VEL_MPS, D435_NEAR_CRUISE_MPS, D435_NEAR_HOP_MAX_M, D435_NEAR_LEFTOVER_M, @@ -52,6 +54,15 @@ from arm_application.overhead_oneshot import ( oneshot_shot_timed_out, oneshot_next_hop_m, oneshot_should_submit, + cam_xy_bias_axes, + decompose_base_xy, + hole_pair_span_m, + hole_pair_span_ok, + oneshot_align_step_m, + flange_z_for_gap_m, + ONCE_APPROACH_STEP_M, + ONCE_TRIM_MAX_STEP_M, + ONCE_TRIM_REFUSE_M, pixel_on_base_plane, select_oneshot_det, trap_acc_for_hop, @@ -151,7 +162,7 @@ class TestHandoff: assert "d435_commanding" in perc def test_after_d435_wrist_tracks_box_center_then_hole_without_depth(self): - """交回 D405 追框心；二阶段多拍 k1/k2 闭环，孔坐标只交到工件面。""" + """交回 D405 追框心；本分支单次精拍 k1/k2，孔坐标只交到工件面。""" perc = ( __import__("pathlib").Path(__file__).resolve().parents[1] / "arm_application" / "perception_node.py" @@ -168,7 +179,36 @@ class TestHandoff: assert "align_max_iters" in stage2 assert "像素交工件面" in stage2 assert "det_depth = None if self.hole_visual_only else med_depth" in stage2 - assert "hole_keypoint_once" not in stage2 + assert 'self.hole_keypoint_once = _as_bool(stage2.get("hole_keypoint_once")' in stage2 + obs = stage2[stage2.find("def _observe_holes"): stage2.find("def _oneshot_align_move")] + assert "_goto(" not in obs, "核验拍不许改 XY" + assert "residual_remeasured" in obs + desc = stage2[stage2.find("okd, q, Tee = self._descend_above_hole"):] + desc = desc[:1800] + assert "if self.hole_keypoint_once:" in desc + assert 'tag="降后核验"' in desc + assert "_trim_xy_after_descent" in desc + assert "target_gap_m=self.observe_gap_m" in desc + assert "ONCE_APPROACH_STEP_M" in desc + keep = stage2[ + stage2.find("def _trim_xy_keep_hover_tip"): stage2.find("def _trim_xy_after_descent") + ] + assert "_tip_offset_from_depth" not in keep, "单次精拍下降后不许重测规尖" + assert "悬停冻结" in keep + assert "ONCE_TRIM_MAX_STEP_M" in keep + assert "hole_pair_span_ok" in keep + assert "range(3)" not in keep, "下降后补孔不许再连跳三次" + assert "撤回" in keep + assert "trim_blend_to_final" in keep + assert "斜线贴面" in keep + once_fn = stage2[stage2.find("def _oneshot_align_move"): stage2.find("def _trim_xy_keep_hover_tip")] + assert "oneshot_align_step_m(" in once_fn + assert "0.012 if n_raw > 0.025" not in once_fn + assert "from arm_application.overhead_oneshot import decompose_base_xy" in stage2 or ( + "oneshot_align_step_m" in stage2 and "decompose_base_xy" in stage2 + ) + assert "def decompose_base_xy(" not in stage2 + assert "def oneshot_align_step_m(" not in stage2 servo = ( __import__("pathlib").Path(__file__).resolve().parents[1] / "arm_application" / "servo_node.py" @@ -196,6 +236,21 @@ class TestHandoff: assert abs(euler[1]) == pytest.approx(0.0, abs=1e-6) assert abs(euler[2]) == pytest.approx(0.0, abs=1e-6) assert "rdk_release_movel.py" in servo + movel = servo[servo.find("def _d435_vendor_movel"): servo.find("def _d435_oneshot_holds_locked")] + assert "D435_VENDOR_MOVEL_VEL_MPS" in movel + assert "D435_VENDOR_MOVEL_ACC_MPS2" in movel + assert '"--acc"' in movel + assert '"0.05"' not in movel + assert D435_VENDOR_MOVEL_VEL_MPS == pytest.approx(0.3) + assert D435_VENDOR_MOVEL_ACC_MPS2 == pytest.approx(1.5) + rel = ( + __import__("pathlib").Path(__file__).resolve().parents[3] + / "scripts" / "rdk_release_movel.py" + ).read_text(encoding="utf-8") + assert f"default={D435_VENDOR_MOVEL_VEL_MPS}" in rel + assert f"default={D435_VENDOR_MOVEL_ACC_MPS2}" in rel + assert '"--acc"' in rel + assert 'prim["acc"]' in goto assert "d435_handback_sec" in servo assert "still_sec=max(0.0, self.d435_handback_sec)" in servo assert "wrist_align_sec" in servo @@ -276,8 +331,10 @@ class TestHandoff: def test_whole_path_uses_one_cruise_and_an_arc(self): """远处仍 80 mm/s。近处必须另降一档。路径是圆弧，不是对着终点做 P。""" - assert D435_MOVE_SPEED_SCALE == 1.0 - assert hybrid_cruise_vxy(0.08) == pytest.approx(0.08) + assert D435_MOVE_SPEED_SCALE == pytest.approx(1.5) + assert D435_VENDOR_MOVEL_VEL_MPS == pytest.approx(0.3) + assert D435_VENDOR_MOVEL_ACC_MPS2 == pytest.approx(1.5) + assert hybrid_cruise_vxy(0.08) == pytest.approx(0.12) hop_far, v_far = oneshot_hop_limits(0.20, 0.08) hop_mid, v_mid = oneshot_hop_limits(0.12, 0.08) hop_near, v_near = oneshot_hop_limits(0.074, 0.08) @@ -405,7 +462,8 @@ class TestHandoff: hop_max, cruise = oneshot_hop_limits(leftover, 0.08) assert leftover <= D435_NEAR_LEFTOVER_M assert hop_max == pytest.approx(0.015) - assert cruise == pytest.approx(0.030) + assert cruise == pytest.approx(D435_NEAR_CRUISE_MPS) + assert cruise == pytest.approx(0.045) assert float(np.linalg.norm(dp)) == pytest.approx(0.015, abs=1e-9) assert abs(float(dp[2])) < 1e-9 assert oneshot_needs_verticalize(0.03, 0.010, 0.015) is True @@ -443,8 +501,8 @@ class TestHandoff: assert near[0][1] == "xy" assert float(np.linalg.norm(near[0][0])) == pytest.approx(0.037, abs=1e-9) assert abs(float(near[0][0][2])) < 1e-9 - assert oneshot_full_cruise_mps(0.20, 0.08) == pytest.approx(D435_FULL_CRUISE_MPS) - assert oneshot_full_cruise_mps(0.20, 0.08) < 0.08 + assert oneshot_full_cruise_mps(0.20, 0.12) == pytest.approx(D435_FULL_CRUISE_MPS) + assert oneshot_full_cruise_mps(0.20, 0.12) < 0.12 assert oneshot_full_cruise_mps(0.05, 0.08) == pytest.approx(D435_NEAR_CRUISE_MPS) chopped = oneshot_cartesian_dp( [0.65, -0.11, 0.320], [0.58, -0.50], 0.320, gate_m=gate @@ -452,16 +510,18 @@ class TestHandoff: assert float(np.linalg.norm(chopped)) == pytest.approx(FROZEN_J_HOP_MAX_M) def test_smooth_path_is_rest_to_rest_and_slows_at_the_end(self): - """300 mm @ 60 mm/s：头尾速度 0，起步 ≥3 s，峰值加速度必须低于旧的弹射档。""" - assert D435_FULL_ACC_M == pytest.approx(0.100) - assert D435_FULL_DEC_M == pytest.approx(0.080) - assert D435_FULL_CRUISE_MPS == pytest.approx(0.060) + """300 mm @ 60 mm/s 的加减速形状：头尾速度 0，起步 ≥3 s，低于旧弹射档。""" + assert D435_FULL_ACC_M == pytest.approx(0.150) + assert D435_FULL_DEC_M == pytest.approx(0.120) + assert D435_FULL_CRUISE_MPS == pytest.approx(0.090) a_now = cosine_peak_acc(D435_FULL_CRUISE_MPS, D435_FULL_ACC_M) a_old = cosine_peak_acc(0.08, 0.040) assert a_old > 0.12 - assert a_now < 0.035 - assert a_now < 0.30 * a_old - T, v_peak, t_acc, t_cruise, d_acc, d_dec = smooth_path_params(0.30, 0.06) + assert a_now < 0.055 + assert a_now < 0.45 * a_old + T, v_peak, t_acc, t_cruise, d_acc, d_dec = smooth_path_params( + 0.30, 0.06, d_acc=0.100, d_dec=0.080 + ) assert v_peak == pytest.approx(0.06) assert d_acc == pytest.approx(0.100) assert d_dec == pytest.approx(0.080) @@ -492,7 +552,9 @@ class TestHandoff: q0 = np.zeros(7) q1 = np.zeros(7) q1[0] = 0.3 - sched, T_move, vp = smooth_joint_schedule([q0, q1], 0.30, 0.06) + sched, T_move, vp = smooth_joint_schedule( + [q0, q1], 0.30, 0.06, d_acc=0.100, d_dec=0.080 + ) assert T_move == pytest.approx(T, abs=1e-6) assert vp == pytest.approx(0.06) assert float(np.linalg.norm(sched[0][1])) == pytest.approx(0.0, abs=1e-9) @@ -709,3 +771,68 @@ class TestStillLock: p = np.array([0.50, -0.12]) assert s.update(p, 0.0, tracking_ok=False) is False assert s.update(p, 10.0, tracking_ok=False) is False + + +class TestHoleBiasAxes: + def test_look_along_x_right_along_y(self): + """相机朝 +X、画面右为 +Y：残差必须拆到这两轴，不许混。""" + T = np.eye(4) + # 列 = 相机轴在基座：X=+Y，Y=−Z，Z=+X + T[:3, :3] = np.array([ + [0.0, 0.0, 1.0], + [1.0, 0.0, 0.0], + [0.0, -1.0, 0.0], + ], dtype=float) + axes = cam_xy_bias_axes(T) + assert axes is not None + look, right = axes + np.testing.assert_allclose(look, [1.0, 0.0], atol=1e-9) + np.testing.assert_allclose(right, [0.0, 1.0], atol=1e-9) + d = decompose_base_xy([0.010, 0.006], T) + assert d is not None + assert d["along_look_m"] == pytest.approx(0.010, abs=1e-9) + assert d["along_right_m"] == pytest.approx(0.006, abs=1e-9) + + def test_straight_down_has_no_look_axis(self): + """光轴竖直朝下时水平投影退化，必须返回 None，不许用 (0,0) 硬拆。""" + T = np.eye(4) + T[1, 1] = -1.0 + T[2, 2] = -1.0 + assert cam_xy_bias_axes(T) is None + assert decompose_base_xy([0.01, 0.0], T) is None + + def test_site_wires_keypoint_once_on_this_branch(self): + """本分支 site 必须打开单次精拍，否则实验跑的还是多走多拍。""" + from pathlib import Path + + import yaml + + site = yaml.safe_load( + (Path(__file__).resolve().parents[3] / "config" / "site.yaml").read_text( + encoding="utf-8" + ) + ) + st2 = (site.get("hole_detect") or {}).get("stage2") or {} + assert st2.get("hole_keypoint_once") is True + assert st2.get("hole_visual_only") is True + + def test_oneshot_step_covers_half_hole_pair(self): + """框心到 k1 约 29mm：必须整步走完，不许再截成 12mm。""" + assert oneshot_align_step_m(0.02894) == pytest.approx(0.02894) + assert oneshot_align_step_m(0.012) == pytest.approx(0.012) + assert oneshot_align_step_m(0.050) == pytest.approx(0.040) + assert oneshot_align_step_m(0.050) > 0.012 + 1e-9 + + def test_once_trim_pair_window_and_step_cap(self): + """下降后补孔：60mm 间距可走；45mm 是检测漂了。一跳上限 3mm。""" + assert ONCE_TRIM_MAX_STEP_M == pytest.approx(0.003) + assert ONCE_TRIM_REFUSE_M == pytest.approx(0.008) + assert hole_pair_span_m([0.50, -0.10], [0.50, -0.16]) == pytest.approx(0.060) + assert hole_pair_span_ok(0.060) is True + assert hole_pair_span_ok(0.050) is True + assert hole_pair_span_ok(0.070) is True + assert hole_pair_span_ok(0.045) is False + assert hole_pair_span_ok(0.080) is False + assert ONCE_APPROACH_STEP_M == pytest.approx(0.020) + assert flange_z_for_gap_m(0.223, 0.022, 0.002) == pytest.approx(0.247) + assert flange_z_for_gap_m(0.223, 0.022, 0.015) == pytest.approx(0.260) diff --git a/src/arm_application/test/test_stage2_descent_clamp.py b/src/arm_application/test/test_stage2_descent_clamp.py index 905925c..11faa10 100644 --- a/src/arm_application/test/test_stage2_descent_clamp.py +++ b/src/arm_application/test/test_stage2_descent_clamp.py @@ -37,6 +37,7 @@ from typing import Any, List, Optional import numpy as np +from arm_application.overhead_oneshot import ONCE_APPROACH_STEP_M from arm_application.stage2_locate_node import ( Stage2Locate, contact_armed_by_gap, @@ -265,23 +266,71 @@ def test_zero_tool_length_refuses_to_descend(): def test_ft_contact_stops_and_backs_off(): """腕部力突变必须立即停并回退 —— 这是与测距无关的**物理**兜底。 - 只有几何间隙已经贴近工件才认接触。两步（8mm）时还离面 45mm，力门槛不算。 + 力从第 8 步（间隙约 21mm）就超门槛，但门控是 ``final_gap+4mm``（6mm）。 + 21mm / 10mm 这一档必须继续降；进到 6mm 才许停，且不许把 2mm 目标走完。 """ st = _StubStage2(contact_after_steps=8) ok, Tee, st = _run(st) assert ok is True, "接触后停住即算安全收工" assert any("腕部力突变" in m for m in st.logs), "碰到工件却没喊" - # 进到接触门控之后才停，不许把整段预算走完 - assert sum(st.steps_mm) < _BUDGET * 1000 - 5.0, "力突变后还在继续降" - # 回退 1mm：最终位置应高于"最后一步的下发目标" + assert any("不当作碰到" in m for m in st.logs), "21mm 间隙时力超了也要明说不当成碰到" + assert sum(st.steps_mm) > 32.0, "力从 21mm 间隙就热，必须穿过 10mm 假接触区" + assert sum(st.steps_mm) < _BUDGET * 1000 - 1.0, "力突变后还在继续降到 2mm" assert Tee[2, 3] > _HOVER_Z - _BUDGET, "回退没生效" def test_contact_not_armed_while_still_high_above_the_part(): - """2026-10-13：法兰 0.384、间隙 138mm、ΔFz=+2.87N 不算碰到。""" + """远场 138mm 和近场 10mm / ΔFz≈3N 都不算碰到；6mm 才武装。""" assert contact_armed_by_gap(0.1375, 0.015) is False - assert contact_armed_by_gap(0.020, 0.015) is True + assert contact_armed_by_gap(0.020, 0.015) is False + assert contact_armed_by_gap(0.010, 0.015) is False + assert contact_armed_by_gap(0.0095, 0.015) is False + assert contact_armed_by_gap(0.006, 0.015) is True + assert contact_armed_by_gap(0.005, 0.015) is True + + +def test_oneshot_approach_stops_at_observe_gap_with_larger_steps(): + """单次精拍第一段只降到 15mm，步长 20mm（约 3 步），不许 4mm 切到 2mm。""" + import rclpy + + if not rclpy.ok(): + rclpy.init() + st = _StubStage2() + Tee0 = np.eye(4) + Tee0[2, 3] = _HOVER_Z + ok, _q, Tee = Stage2Locate._descend_above_hole( # type: ignore[arg-type] + st, np.zeros(7), Tee0, target_gap_m=0.015, step_m=ONCE_APPROACH_STEP_M + ) + assert ok is True + gap = float(Tee[2, 3]) - _TOOL_LEN - _SURFACE_Z + assert abs(gap - 0.015) < 0.001, f"应停在观察高 15mm，实际 {gap * 1000:.1f}mm" + assert max(st.steps_mm) <= ONCE_APPROACH_STEP_M * 1000 + 1e-6 + assert len(st.steps_mm) <= 3 + assert abs(sum(st.steps_mm) - (_GAP0 - 0.015) * 1000) < 1.5 + + +def test_oneshot_approach_does_not_treat_force_as_contact(): + """观察高 15mm 仍在接触门控之外，力再热也不许当碰到。""" + + class _AlwaysHot(_StubStage2): + def _contact_detected(self, _base_fz: float) -> bool: + return True + + import rclpy + + if not rclpy.ok(): + rclpy.init() + st = _AlwaysHot() + Tee0 = np.eye(4) + Tee0[2, 3] = _HOVER_Z + ok, _q, Tee = Stage2Locate._descend_above_hole( # type: ignore[arg-type] + st, np.zeros(7), Tee0, target_gap_m=0.015, step_m=ONCE_APPROACH_STEP_M + ) + assert ok is True + gap = float(Tee[2, 3]) - _TOOL_LEN - _SURFACE_Z + assert abs(gap - 0.015) < 0.001 + assert not any("腕部力突变" in m for m in st.logs) def test_ft_drift_far_from_surface_does_not_stop_on_the_first_step(): @@ -293,7 +342,8 @@ def test_ft_drift_far_from_surface_does_not_stop_on_the_first_step(): ok, _Tee, st = _run(_AlwaysHot()) assert ok is True - assert sum(st.steps_mm) >= 28.0, "离面 53mm 时力超门槛也不该第一步就停" + assert sum(st.steps_mm) >= 40.0, "离面 53mm 时力超门槛也不该第一步就停" + assert any("不当作碰到" in m for m in st.logs) assert any("腕部力突变" in m for m in st.logs) diff --git a/src/arm_application/test/test_stage2_tip_align.py b/src/arm_application/test/test_stage2_tip_align.py index 49f12eb..7451e11 100644 --- a/src/arm_application/test/test_stage2_tip_align.py +++ b/src/arm_application/test/test_stage2_tip_align.py @@ -442,6 +442,7 @@ class _TrimStub: """下降后 XY 修正用到的成员。孔在基座里不动，规尖偏移是常数。""" tip_vision = True + hole_keypoint_once = False k1_label = "left" target_hole = "k1" align_tol = 0.00025 @@ -449,15 +450,27 @@ class _TrimStub: vert_tol_rad = 0.02 base_frame = "world" ee_frame = "flange" + cam_frame = "cam" + image_topic = "/cam/color" _tip_off = np.array([0.01, 0.0]) _tip_samp: list = [] _tip_len_m = 0.22 + _ws_clipped = False + final_descent_enable = False + tool_tip_below_flange_m = 0.223 + surface_z_m = 0.022 + final_gap_m = 0.002 + final_descent_speed = 0.005 def __init__(self, err_xy=(0.0026, -0.0004), tip=None) -> None: self.hole = np.array([0.50 + err_xy[0], -0.10 + err_xy[1], 0.022]) self.tip = None if tip is False else np.array([0.0015, -0.0012]) self.moves: list = [] + self.tip_calls = 0 self._logger = _StubLogger() + self.pair_dy = -0.060 + self.hole_after_move = None + self.pair_dy_after_move = None def get_logger(self): return self._logger @@ -469,17 +482,42 @@ class _TrimStub: return None def _snap_holes(self, *, skip_center_gate: bool): + hole = self.hole + pair_dy = self.pair_dy + if self.moves: + if self.hole_after_move is not None: + hole = self.hole_after_move + if self.pair_dy_after_move is not None: + pair_dy = self.pair_dy_after_move res = { "status": True, - "hole_base": [self.hole.tolist(), (self.hole + np.array([0.0, -0.060, 0.0])).tolist()], + "hole_base": [hole.tolist(), (hole + np.array([0.0, pair_dy, 0.0])).tolist()], "z_wp": 0.127, } return None, None, {}, res def _tip_offset_from_depth(self, *_a, **_kw): + self.tip_calls += 1 if self.tip is None: return None, None, "没看到规" - return self.tip.copy(), 0.222, "px=(447,226)" + off = self.tip.copy() + if self.hole_keypoint_once: + off = off + np.array([0.018 * (self.tip_calls % 3), 0.0]) + return off, 0.222, "px=(447,226)" + + def _stamp_bias(self, rec, *, tgt_b, gxy, tag): + rec = dict(rec) + rec["tag"] = tag + rec["err_xy_m"] = [ + round(float(tgt_b[0]) - float(gxy[0]), 5), + round(float(tgt_b[1]) - float(gxy[1]), 5), + ] + rec["along_look_mm"] = None + rec["along_right_mm"] = None + return rec + + def _publish_result(self, _payload) -> None: + return None def _goto(self, _q, Tee, target, **_kw): Tee = np.array(Tee, float) @@ -535,3 +573,170 @@ def test_trim_does_not_move_when_the_tip_is_missing(): assert not st.moves assert abs(float(Tee[0, 3]) - 0.50) < 1e-9 assert final["gauge_source"] == "视觉规尖(已冻结)" + + +def test_once_trim_keeps_hover_tip_when_depth_jumps(): + """单次精拍：沿用悬停规尖，只跳一次、步长 ≤3mm，剩下的不许再追。 + + 2026-10-13：两跳合计 4.3mm 把近距检测带崩。5mm 交平面漂只许吃掉 3mm。 + """ + frozen = np.array([0.0001, -0.0003]) + st = _TrimStub(err_xy=(0.0044, -0.0024)) + st.hole_keypoint_once = True + st._tip_off = frozen.copy() + st.align_tol = 0.00025 + _q, Tee, final = _trim(st) + assert st.tip_calls == 0, "单次精拍下降后不许再走深度重测规尖" + assert abs(float(Tee[2, 3]) - 0.260) < 1e-9, "下降后修正不许改高度" + assert len(st.moves) == 1, f"只许一跳，实际 {len(st.moves)} 跳" + hop = float(np.hypot(float(st.moves[0][0]) - 0.50, float(st.moves[0][1]) + 0.10)) + assert hop <= 0.003 + 1e-9, f"一跳 {hop*1000:.2f}mm 超过 3mm" + gxy = np.array([float(Tee[0, 3]) + frozen[0], float(Tee[1, 3]) + frozen[1]]) + err = float(np.hypot(gxy[0] - st.hole[0], gxy[1] - st.hole[1])) + assert 0.0015 <= err <= 0.0025, f"3mm 一跳后应剩约 2mm，实际 {err*1000:.2f}mm" + assert final["residual_mm"] == pytest.approx(err * 1000.0, abs=0.02) + assert "悬停冻结" in final["gauge_source"] + assert final["residual_remeasured"] is True + assert final.get("trim_reverted") is False + + +def test_once_trim_refuses_without_frozen_tip(): + """没有悬停冻住的规尖，不许用法兰或乱跳的深度尖去补。""" + st = _TrimStub(err_xy=(0.0044, 0.0)) + st.hole_keypoint_once = True + st._tip_off = None + _q, Tee, final = _trim(st) + assert not st.moves + assert st.tip_calls == 0 + assert abs(float(Tee[0, 3]) - 0.50) < 1e-9 + assert any("冻住的规尖" in m for m in st._logger.msgs) + + +def test_once_trim_refuses_a_centimeter_scale_hole_shift(): + """孔 XY 要补到厘米，不是交平面那 4mm，必须停。""" + st = _TrimStub(err_xy=(0.014, 0.0)) + st.hole_keypoint_once = True + st._tip_off = np.array([0.0001, -0.0003]) + _q, Tee, final = _trim(st) + assert not st.moves + assert final["residual_mm"] == 0.22 + assert any("不像交平面" in m for m in st._logger.msgs) + + +def test_once_trim_reverts_when_pair_breaks(): + """一跳后两孔间距从 60mm 掉到 45mm：检测漂了，必须撤回。""" + frozen = np.array([0.0001, -0.0003]) + st = _TrimStub(err_xy=(0.0026, 0.0)) + st.hole_keypoint_once = True + st._tip_off = frozen.copy() + st.pair_dy_after_move = -0.045 + _q, Tee, final = _trim(st) + assert len(st.moves) == 2, "补完变差必须再走一跳撤回" + assert abs(float(Tee[0, 3]) - 0.50) < 1e-9 + assert abs(float(Tee[1, 3]) + 0.10) < 1e-9 + assert final.get("trim_reverted") is True + assert any("撤回" in m for m in st._logger.msgs) + gxy = np.array([float(Tee[0, 3]) + frozen[0], float(Tee[1, 3]) + frozen[1]]) + err = float(np.hypot(gxy[0] - st.hole[0], gxy[1] - st.hole[1])) + assert abs(err - 0.0025) < 2e-4, "撤回后残差应回到走前那一档" + assert final["residual_mm"] == pytest.approx(err * 1000.0, abs=0.05) + + +def test_once_trim_reverts_when_residual_worsens(): + """一跳后残差变大：停并撤回，终值不许还是走前那份假装没动。""" + frozen = np.array([0.0001, -0.0003]) + st = _TrimStub(err_xy=(0.0026, 0.0)) + st.hole_keypoint_once = True + st._tip_off = frozen.copy() + st.hole_after_move = np.array([0.50 + 0.015, -0.10, 0.022]) + _q, Tee, final = _trim(st) + assert len(st.moves) == 2 + assert abs(float(Tee[0, 3]) - 0.50) < 1e-9 + assert final.get("trim_reverted") is True + assert any("撤回" in m for m in st._logger.msgs) + assert final["residual_remeasured"] is True + assert float(final["residual_mm"]) < 5.0, "撤回后不许把 15mm 漂检测当终值" + + +def _enable_blend(st: _TrimStub) -> None: + """观察高 15mm（法兰 0.260）斜线收到贴面 2mm（法兰 0.247）。""" + st.final_descent_enable = True + st.tool_tip_below_flange_m = 0.223 + st.surface_z_m = 0.022 + st.final_gap_m = 0.002 + st.final_descent_speed = 0.005 + + +def test_once_trim_blend_drops_z_and_xy(): + """观察高测到的残差和剩余 Z 合成一跳，不许再切 4mm 贴面。""" + frozen = np.array([0.0001, -0.0003]) + st = _TrimStub(err_xy=(0.0044, -0.0024)) + st.hole_keypoint_once = True + st._tip_off = frozen.copy() + st.align_tol = 0.00025 + _enable_blend(st) + _q, Tee, final = _trim(st) + assert len(st.moves) == 1, f"斜线只许一跳，实际 {len(st.moves)}" + assert abs(float(Tee[2, 3]) - 0.247) < 1e-9, "必须收到贴面法兰高" + hop_xy = float(np.hypot(float(st.moves[0][0]) - 0.50, float(st.moves[0][1]) + 0.10)) + assert hop_xy <= 0.003 + 1e-9 + gxy = np.array([float(Tee[0, 3]) + frozen[0], float(Tee[1, 3]) + frozen[1]]) + err = float(np.hypot(gxy[0] - st.hole[0], gxy[1] - st.hole[1])) + assert 0.0015 <= err <= 0.0025 + assert final.get("trim_reverted") is False + + +def test_once_trim_blend_z_only_when_xy_already_in_tol(): + """观察高 XY 已在容差，斜线仍要收 Z，不许停在 15mm。""" + frozen = np.array([0.0001, -0.0003]) + st = _TrimStub(err_xy=(0.0001, -0.0003)) + st.hole_keypoint_once = True + st._tip_off = frozen.copy() + _enable_blend(st) + _q, Tee, final = _trim(st) + assert len(st.moves) == 1 + assert abs(float(Tee[0, 3]) - 0.50) < 1e-9 + assert abs(float(Tee[2, 3]) - 0.247) < 1e-9 + assert final.get("trim_reverted") is False + + +def test_once_trim_blend_refuses_pair_does_not_drop_z(): + """观察高间距已经坏了，不许斜线贴面。""" + st = _TrimStub(err_xy=(0.004, 0.0)) + st.hole_keypoint_once = True + st._tip_off = np.array([0.0001, -0.0003]) + st.pair_dy = -0.045 + _enable_blend(st) + _q, Tee, final = _trim(st) + assert not st.moves + assert abs(float(Tee[2, 3]) - 0.260) < 1e-9 + assert final["residual_mm"] == 0.22 + + +def test_once_trim_blend_keeps_pose_when_close_snap_breaks(): + """贴面复拍间距漂了：近距不可信，观察高算的斜线位姿必须留下。""" + frozen = np.array([0.0001, -0.0003]) + st = _TrimStub(err_xy=(0.0026, 0.0)) + st.hole_keypoint_once = True + st._tip_off = frozen.copy() + st.pair_dy_after_move = -0.045 + _enable_blend(st) + _q, Tee, final = _trim(st) + assert len(st.moves) == 1, "贴面复拍漂了不许撤回观察高" + assert abs(float(Tee[2, 3]) - 0.247) < 1e-9 + assert abs(float(Tee[0, 3]) - 0.50) > 1e-4, "XY 补偿被撤回了" + assert final.get("trim_reverted") is not True + assert final.get("close_snap_untrusted") is True + + +def test_once_trim_refuses_when_pair_already_broken(): + """第一拍间距已经 45mm，检测没立住，不许再动。""" + st = _TrimStub(err_xy=(0.004, 0.0)) + st.hole_keypoint_once = True + st._tip_off = np.array([0.0001, -0.0003]) + st.pair_dy = -0.045 + _q, Tee, final = _trim(st) + assert not st.moves + assert abs(float(Tee[0, 3]) - 0.50) < 1e-9 + assert final["residual_mm"] == 0.22 + assert any("检测漂了" in m for m in st._logger.msgs)