把当前控制栈收成一份可克隆的源码：腕部看不见时用顶视一次定到基座，二阶段对孔和手外标定跟着进同一份树。
现场 site.yaml、权重和编译产物仍不入库；模板只留占位符。 Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
arm_dev
+283
-37
@@ -26,14 +26,14 @@ robot:
|
||||
# >>> 切换机械臂：改成 robots 里某一台的完整 SN <<<
|
||||
sn: "Rizon4-XXXXXX"
|
||||
# Studio Tools 名（一字不差）。空=不切换。查：bash ~/ros2/scripts/rdk_switch_tool.sh --list
|
||||
tool_name: ""
|
||||
tool_name: "tool1"
|
||||
# 每台登记 SN + IP，以及该臂自己的相机/夹爪/手眼/点位（覆盖下面的全局段）。
|
||||
robots:
|
||||
- sn: "Rizon4-XXXXXX"
|
||||
arm: left
|
||||
ip: "192.168.2.XXX"
|
||||
pc_ip: "192.168.2.XXX"
|
||||
iface: "enp4s0"
|
||||
iface: "enxXXXXXXXXXXXX"
|
||||
label: "左臂89"
|
||||
# 本臂装了 Orisys 视触觉（只有左臂有）。脚本靠这一项决定要不要拉触觉服务，
|
||||
# 而不是靠"SN 等于某个具体值"这种写死的判断（换台臂就会失效）。
|
||||
@@ -67,7 +67,11 @@ robot:
|
||||
# 锚点 `&servo_left`：下面的"仿真台"条目直接引用它 —— 仿真建的就是左臂，
|
||||
# 悬停/锁高必须与 89 **一模一样**，抄两份迟早漂移。
|
||||
servo: &servo_left
|
||||
ee_z_m: 0.35 # 跟踪锁高：法兰相对基座 Z [m]
|
||||
# 2026-09-29 底座加高后由 0.35 → 0.36：工件上表面实测抬高 10mm
|
||||
# （0.012 → 0.022），锁高同步 +0.011 ⇒ 相机到工件的视距**与加高前一致**
|
||||
# （≈0.213m，跟踪那一档已实拍验证过检出稳定），不是随手抬的。
|
||||
# 规矩：ee_z_m 要高于 servo.hole_hover_z_m（二阶段得往**下**走，不能往上返）。
|
||||
ee_z_m: 0.36 # 跟踪锁高：法兰相对基座 Z [m]
|
||||
min_ee_z_m: 0.20
|
||||
safe_z_m: 0.45
|
||||
z_min_m: 0.22
|
||||
@@ -77,13 +81,93 @@ robot:
|
||||
# >>> 二阶段孔定位悬停锁高：只改这里，91 用下面那段 <<<
|
||||
# 法兰 Z [m]；要兼顾：够低拍清 k1/k2 螺纹孔，够高通止规尖不碰工件。
|
||||
# 调试：bash ~/ros2/scripts/run_stage2_locate.sh --arm 89 --hover-only
|
||||
hole_hover_z_m: 0.23 # ← 二阶段悬停高度调这一行
|
||||
# 腕部 FT 话题**不在这里配**：默认按 sn 推导 = /<sn>/ft_sensor_wrench，
|
||||
# 由真机 flexiv_robot_states_broadcaster 无条件发布，仿真侧也是同名同类型
|
||||
# （见 devices/sim/gazebo/config/wrist_ft.yaml）。
|
||||
# 只有换 FT 来源时才需要显式写 servo.ft_wrench_topic，例如改用
|
||||
# /<sn>/external_wrench_in_tcp（已做重力/负载补偿，基线≈0，
|
||||
# 对"轻碰"更灵敏，但对负载变化更敏感）。
|
||||
# = 工件上表面 0.022（本臂 gauge_assembly.workpiece_surface_z_m，加底座后重测）
|
||||
# + 工具长 0.245 + 间隙 0.002
|
||||
# 2026-09-29 实测：按 0.23 悬停时工具尖端落在工件面**下** 5~10mm，
|
||||
# 悬停这一步就把通止规插进了工件（见本臂 gauge_assembly 段的完整说明）。
|
||||
#
|
||||
# 2026-09-29 二次修正 —— 上面那条"工具几何"只保证**不碰工件**，它保证不了
|
||||
# **相机看得见孔**，而二阶段的第一步就是拍 k1/k2。旧值 0.259 的后果实测：
|
||||
# 相机光心在法兰下方 0.136m（手眼 T_cam_to_gripper + 29.7° 倾角）
|
||||
# 法兰 0.259 → 相机离工件面 0.123m → 检出 0/16，conf 峰值 0.012（噪声）
|
||||
# 法兰 0.309 → 相机离工件面 0.173m → 检出 16/16，conf 0.87，
|
||||
# k0/k1/k2 置信度全 0.999~1.000，k1-k2 间距 156px 稳定
|
||||
# gauge_fsm.hover_z_guard 的设计注释也写着"腕部相机要离工件面 ~0.17m
|
||||
# 才看得清孔"，所以 0.309 才是这条同时满足"相机看得见"和"规尖不碰"的值
|
||||
# （规尖此时在面上 64mm，比旧值更安全）。
|
||||
# 最后 2mm 的贴近由 stage2 的**深度闭环轻降**（final_gap_mm）完成 ——
|
||||
# 它按实测间隙往下走，与绝对悬停高无关，所以抬高悬停不会让尖撞工件。
|
||||
#
|
||||
# 2026-09-29 三次修正（工件加了底座，工作平面重新感知）：
|
||||
# 拿 gongjian0929 在**实时图**上出 k0/k1/k2，再取各点邻域深度投到基座系：
|
||||
# k1 = +0.0220 / +0.0221 / +0.0223 m（多帧重复性 0.4mm 内）
|
||||
# k2 = +0.0223 / +0.0224 m
|
||||
# k0 = +0.0165 / +0.0186 m（偏低约 6mm 且抖，局部读数噪声大）
|
||||
# 三点"中心 vs 外环"深度差 ≤1mm ⇒ 三点都落在**上表面**上，不是孔底 ⇒
|
||||
# 上表面取 k1/k2 的一致值 **0.022**（原 0.012 是加底座前的旧值）。
|
||||
# 悬停随之 0.309 → 0.320（同样 +0.011），让"相机到工件面"的**视距保持
|
||||
# ≈0.17m 不变** —— 0.309 那一档是实拍验证过"16/16 检出、conf 0.87、
|
||||
# k1-k2 间距 156px 稳定"的工作点，改高度就是在改视距，不能随手抬。
|
||||
# 几何兜底仍然满足：0.320 − 工具长 0.245 = 尖端 0.075，工件面 0.022
|
||||
# ⇒ 尖端在面上方 53mm（要求 ≥ final_gap 2mm）。
|
||||
hole_hover_z_m: 0.320 # ← 二阶段悬停高度调这一行
|
||||
# FT 数据源：默认按 sn 推导 = /<sn>/ft_sensor_wrench，由真机
|
||||
# flexiv_robot_states_broadcaster 发布。但 **2026-10-08 实测该话题恒为
|
||||
# 全零（0,0,0）** —— RDK 那侧的原始 FT 传感没在出数（`ft_sensor_raw` 为 0），
|
||||
# 于是应用里"abs(fz−基线)>2.5N 判接触"的兜底**永远判不到接触**（静默失效）。
|
||||
# 同一时刻 /<sn>/external_wrench_in_tcp 是有数的（≈负载 54N，且随受力变化），
|
||||
# 所以真机改用它。判接触只看"相对轻降起始基线的增量"，与 54N 这个偏置无关，
|
||||
# 用这个源可直接恢复接触兜底，不需要改代码。
|
||||
# 仿真**别跟着用**（仿真没有这个话题）—— 见本文件 sim 段的显式覆盖。
|
||||
# 注意：这不是话题名写错，而是"哪个数据源有数"的取舍（真机 RDK 未启用原始 FT）。
|
||||
ft_wrench_topic: "/Rizon4_XXXXXX/external_wrench_in_tcp"
|
||||
# ---- 工件几何（2026-09-29 现场实测标定）----
|
||||
# 真机段的坐标都是**基座系**；本机基座与台面同高 ⇒ 台面 z ≈ 0。
|
||||
gauge_assembly:
|
||||
# 工件**上表面**的世界 z [m]。悬停高度的几何判据（gauge_fsm.hover_z_guard）
|
||||
# 与 S07 下探预算都拿它当基准面。
|
||||
#
|
||||
# ⚠️ 为什么不能留全局段的默认 0.0：0.0 = "未知"，运行期退回"用 stage2 实测
|
||||
# 孔位 z 兜底" —— 但兜底发生在**悬停之后**，悬停本身还是按 0.0 算的。
|
||||
# 后果（2026-09-29 实测）：
|
||||
# 悬停 0.247 − 工具长 0.245 = 工具尖端 z≈0.002，而工件面在 0.0074
|
||||
# ⇒ 尖端在工件面**下** 5~10mm。悬停这一步就把通止规插进工件，接着
|
||||
# S05 的横向对准等于拿规尖**拨工件**：两孔中点两拍之间被推走 19.1mm，
|
||||
# 残差 27.3 → 42.7mm 反向发散（日志看着像手眼标定坏了），最后被
|
||||
# drag_verdict 看门狗拦下（exit=6）。
|
||||
#
|
||||
# 实测依据（旧，加底座前）：stage2 解出的两孔基座系 z = 0.00743 / 0.00715 m
|
||||
# （/tmp/stage2_result.json 的 k1_base_m / k2_base_m）。孔是通孔，
|
||||
# 深度相机在孔口附近读数会偏深 ⇒ 上表面取工件板厚 **0.012**。
|
||||
#
|
||||
# 实测依据（2026-09-29 加底座后**重新感知**，现行值 = 0.022）：
|
||||
# 不再靠"板厚"推，而是直接量上表面 —— 让 gongjian0929 在实时图上出
|
||||
# k0/k1/k2，对每个点取 (±6px) 邻域深度的中位数，用同一帧 TF
|
||||
# (base←flange · flange←cam) 投到基座系：
|
||||
# k1 = +0.0220 / +0.0221 / +0.0223，k2 = +0.0223 / +0.0224（重复性 0.4mm）
|
||||
# k0 = +0.0165 / +0.0186（偏低 6mm 且抖 ⇒ 该点邻域读数不可靠，不参与定面）
|
||||
# 判"是不是孔底"：各点中心 (±6px) 与外环 (12~20px) 深度差 ≤1mm ⇒ 齐平
|
||||
# ⇒ 三点都在上表面。ROI 高度直方图同为单峰平台，p75≈0.0228、p95≈0.0247
|
||||
# ⇒ 上表面 0.022（比加底座前的 0.012 高 10mm）✓ 与"工件加了底座变高"一致。
|
||||
# 法兰 Z=0.356 时该点光学视距实测 0.204m（模型仍检出 conf 0.862）⇒
|
||||
# 模型对 0.17~0.20m 有一档余量，但两侧都不要顶到边界。
|
||||
# 复核（会同时打印实测工件面 + 悬停几何自检）：
|
||||
# bash ~/ros2/scripts/run_stage2_locate.sh --arm 89 --hover-only
|
||||
# python3 scripts/lib/real_geometry_check.py --sn Rizon4-XXXXXX
|
||||
workpiece_surface_z_m: 0.022
|
||||
# 工具长（规尖在法兰下方的距离）—— **2026-10-08 真机感知标定**：0.223m。
|
||||
# 方法：hover(法兰 0.320) 下腕部相机抓一帧，取规杆最深像素 + 深度，用
|
||||
# 手眼 T_base = T_base_flange·T_flange_cam 投到基座 ⇒ 规尖 z=0.0971，
|
||||
# 故 0.320 − 0.0971 = 0.2229m。同帧反投影链的独立校验：k1/k2 基座 z
|
||||
# 精确复现上次实测 0.0219/0.0204（误差 <0.1mm），链路可信。
|
||||
# 工具长 = tool1 夹爪 TCP 0.147 + 规露出 0.076。
|
||||
# ⚠️ 旧值 0.245 是照抄仿真的（TCP 0.200 + 规 0.043），比真机长 22mm：
|
||||
# 它让几何误以为悬停时尖端只在面上方 53mm（实际 75mm），轻降预算因此
|
||||
# 少 22mm，**永远到不了孔口**（现象就是"机械臂不下臂"）。
|
||||
# 落在本文件下方注释记录的合理区间 0.204~0.233 内。
|
||||
# 复核：python3 scripts/sense_tool_length.py --sn Rizon4-XXXXXX
|
||||
# python3 scripts/lib/real_geometry_check.py --sn Rizon4-XXXXXX
|
||||
tool_tip_below_flange_m: 0.223
|
||||
# >>> 仿真那台臂（Gazebo）也登记成 robots[] 里的一台 <<<
|
||||
# 为什么必须登记：应用是按"当前臂"的名字去查 frame / joint / 相机话题的
|
||||
#（`sim_env.sh` 注释里说的"仿真与真机共用同一套 site.yaml"）。仿真的 TF 与关节
|
||||
@@ -153,6 +237,10 @@ robot:
|
||||
# 夹好规后读到的 TF 值写进全局段的 tool_tip_below_flange_m。
|
||||
servo:
|
||||
<<: *servo_left
|
||||
# 仿真 FT：由 sim_wrist_ft 发布，与真机 /<sn>/ft_sensor_wrench 同名同类型同单位。
|
||||
# 必须显式覆盖 —— 上面 *servo_left 带下来的 ft_wrench_topic 是**真机**的
|
||||
# external_wrench_in_tcp，仿真里没有这个话题（不覆盖 ⇒ 仿真 FT 收不到）。
|
||||
ft_wrench_topic: "/sim/ft_sensor_wrench"
|
||||
ee_z_m: 0.72 # 一阶段跟踪锁高（法兰，世界系）
|
||||
# 不必随悬停一起抬：跟踪时工具尖端离面 ~60mm，
|
||||
# 只要**高于悬停**就不碰工件（0.72 > 0.657）。
|
||||
@@ -188,7 +276,7 @@ robot:
|
||||
arm: right
|
||||
ip: "192.168.2.XXX"
|
||||
pc_ip: "192.168.2.XXX"
|
||||
iface: "enp4s0"
|
||||
iface: "enxXXXXXXXXXXXX"
|
||||
label: "右臂91"
|
||||
camera:
|
||||
name: d405_right # 话题 /d405_right/... 禁止和 89 的 /d405_left 混用
|
||||
@@ -221,6 +309,9 @@ robot:
|
||||
hover_offset_ee_xy_m: [0.0, 0.0] # RGI 与法兰同轴；爪心偏了再改这两项 [m]
|
||||
# 二阶段孔定位悬停锁高（91 装了传感器后同样可用）
|
||||
hole_hover_z_m: 0.22
|
||||
# FT 数据源同 89：真机 ft_sensor_wrench 恒零，改用有数的 external_wrench_in_tcp
|
||||
# （见 89 段 ft_wrench_topic 的完整说明）。
|
||||
ft_wrench_topic: "/Rizon4_XXXXXX/external_wrench_in_tcp"
|
||||
# --control：先只降到 ee_z_m（XY 不动），到位后才 hover，用手眼把爪心压到工件正上方
|
||||
|
||||
paths:
|
||||
@@ -228,19 +319,24 @@ paths:
|
||||
# 1) 把 .onnx / .pt 放到 /home/USER/ros2/models/yolo/
|
||||
# 2) 这一行改成那个绝对路径（写 .onnx 即可；TensorRT 会找同名 .engine）
|
||||
# 3) backend=tensorrt 时必须先导出引擎（只需做一次）：
|
||||
# bash ~/ros2/scripts/export_yolo_trt.sh /home/USER/ros2/models/yolo/detect_best_0.onnx 640
|
||||
# bash ~/ros2/scripts/weights.sh convert <源文件> --name <名字> --group yolo
|
||||
# 4) yolo.imgsz 必须等于 ONNX 输入边长（当前 640）
|
||||
# 5) 改权重/imgsz 后重启：bash ~/ros2/scripts/run_visual_servo_rt.sh [--control]
|
||||
# 6) 换完确认标签没丢：bash ~/ros2/scripts/weights.sh doctor --check-engines
|
||||
# 当前：detect_best_0.onnx（2026-09-04 从 ~/下载 移入，640 输入）
|
||||
yolo_weights: "/home/USER/ros2/models/yolo/detect_best_0.onnx"
|
||||
# 当前：gongjian0929（2026-09-29 从 ~/下载 转入）。
|
||||
# ⚠️ 这是个 **pose 模型**（YOLO26s-pose）：一个工件框 + 3 个关键点
|
||||
# k0=跟踪孔 / k1,k2=对孔。它同时替代了原来的跟踪模型和入孔模型，
|
||||
# 所以下面 hole_weights 也指向同一份。
|
||||
# ultralytics 解不了 pose 的 TensorRT engine，运行时走
|
||||
# arm_application/pose_detector.py 的自解码（见该模块头）。
|
||||
# 仍然用 .engine（实测 ~2.6 ms/帧；onnx 只有 CPU provider，约 27 ms）。
|
||||
yolo_weights: "/home/USER/ros2/models/yolo/gongjian0929.onnx"
|
||||
# 换模型：把 models/yolo/ 下别的 .onnx / .pt 绝对路径填进来即可。
|
||||
# 未在用的模型已移出仓库（体积大且干扰 `weights.sh list`），要恢复见 models/README.md。
|
||||
# 入孔 YOLO（backend=yolo 时把孔当框）。two_stage 用下面的工件权重。
|
||||
# 原先指向 hole_insert/best.pt —— 那个文件不存在（悬空引用）。这里改成实际存在的
|
||||
# detect_best_1.onnx，这样 backend: yolo 至少是能跑的。查悬空引用：
|
||||
# bash ~/ros2/scripts/weights.sh doctor
|
||||
hole_weights: "/home/USER/ros2/models/hole_insert/detect_best_1.onnx"
|
||||
# 入孔模型（backend=yolo 时把孔当框）。two_stage 用下面的工件权重。
|
||||
# pose 模型两处共用同一份 —— 它既能框工件，又直接给 k1/k2 关键点。
|
||||
# 查悬空引用：bash ~/ros2/scripts/weights.sh doctor
|
||||
hole_weights: "/home/USER/ros2/models/yolo/gongjian0929.onnx"
|
||||
# 下面是缺省（未匹配到 robots[] 时）。真机以 robots[].paths 为准：89 / 91 各一份。
|
||||
handeye_file: "/home/USER/ros2/handeye/results/89_eye_in_hand.yaml"
|
||||
handeye_waypoints: "/home/USER/ros2/handeye/poses/89_eye_in_hand.yaml"
|
||||
@@ -251,11 +347,23 @@ paths:
|
||||
|
||||
handeye:
|
||||
mode: "eye_in_hand"
|
||||
# >>> 标定板尺寸（内角点数 + 方格边长 m）；改这里即可 <<<
|
||||
# 例：A5 板 12×17 方格 → 内角点 11×16，方格 11 mm
|
||||
board_cols: 8 # 内角点列数 (方格列数 - 1)
|
||||
board_rows: 11 # 内角点行数 (方格行数 - 1)
|
||||
square_size_m: 0.015 # 方格边长 [m]
|
||||
# >>> 标定板尺寸：**按模式各一份**（内角点数 + 方格边长 m）<<<
|
||||
#
|
||||
# 为什么必须按模式分开写（而不是两个模式共用一组键）：
|
||||
# 眼在手上与眼在手外通常用**两块不同的物理板**，方格边长不一样。共用一组键
|
||||
# 的后果不是报错，而是"跑另一种模式时静默用错方格边长"—— 标定照样跑完、
|
||||
# 结果只是系统性偏，现场几乎联想不到板尺寸（历史反例见 handeye/README.md 第 3 节）。
|
||||
# 内角点 = 方格数 − 1；列/行顺序不用纠结，检测端两种顺序都会试
|
||||
#（见 handeye_lib.detect_chessboard）。
|
||||
boards:
|
||||
eye_in_hand: # A5 板：9×12 方格
|
||||
cols: 8 # 内角点列数
|
||||
rows: 11 # 内角点行数
|
||||
square_size_m: 0.015 # 方格边长 [m]
|
||||
eye_to_hand: # A4 板：12×9 方格
|
||||
cols: 11
|
||||
rows: 8
|
||||
square_size_m: 0.020
|
||||
# 运动更顺滑：慢速 + 到位等静止 + 零速 pin 后再拍照
|
||||
velocity_scale: 0.05
|
||||
move_time_sec: 8.0
|
||||
@@ -358,6 +466,18 @@ yolo:
|
||||
backend: "tensorrt" # 强制 .engine；没有则启动失败。导出：bash ~/ros2/scripts/export_yolo_trt.sh
|
||||
mp_infer: true # YOLO 独立进程，不和 ROS 抢 GIL
|
||||
min_conf: 0.12 # 新锁门槛；已锁定的框可略低于此
|
||||
# 重新捕获门槛：**跳到新位置**（离上一帧框中心 >max_track_px）必须达到的置信度。
|
||||
# 为什么必须比 min_conf 高得多 —— 2026-10-08 真机实测（一阶段 servo_node 日志）：
|
||||
# 0659.551 死区保持 |e_xy|=3.0mm ← 已经收敛到 3mm
|
||||
# 0659.598 k0 置信度 0.00 < 0.30，本帧不用它
|
||||
# 0659.6xx 重新捕获 conf=0.15 dpx=503 ← 503px 外、conf 0.15 的假框
|
||||
# feat_base=(0.231,-0.484) z=-0.021 ← 跟踪点瞬间横移 427mm
|
||||
# 0670.985 === 一阶段已锁定 === ← "锁定"在离工件 500mm 的位置
|
||||
# 那串假框实测 conf 0.13~0.47，而真 k0 是 0.87。旧写法把这里接成 min_conf=0.12
|
||||
# ⇒ 0.15 的假框被原样采信、perception 的 kf.reset() 直接跳过去，伺服再拦也只是
|
||||
# 拦一帧（假框帧间稳定，3 帧后 FeatureJumpGate 也会认）。
|
||||
# 取 0.55：高于假框上沿 0.47，低于真 k0 0.87。宁可原地不动，也不追假点。
|
||||
reacquire_conf: 0.55
|
||||
vision_hz: 30.0
|
||||
infer_max_width: 640 # 进 GPU 进程前缩到 640，减小进程间拷图，避免队列堵
|
||||
box_ema_alpha: 1.00 # 绿框用预测补偿后的框，不再 EMA
|
||||
@@ -370,6 +490,17 @@ yolo:
|
||||
z_ref_m: 0.22
|
||||
min_depth_m: 0.04
|
||||
max_depth_m: 0.55
|
||||
# 特征点的**相机量程下限**：比这更近的深度读数一定来自机械臂自身（爪上夹的
|
||||
# 通止规 / 相机座），不是工件。**判据是实测的两条带，不是几何推导**：
|
||||
# 机械臂自身（随臂一起动）：0.078 ~ 0.087 m ← 2026-10-08 被误锁的假目标
|
||||
# 工件面： 0.19 ~ 0.29 m ← 锁高下正常跟踪时的 p_cam.z
|
||||
# 取 0.14：距近带 53mm、距远带 50mm，两边都不顶边界。
|
||||
# 几何上为什么近带被占满：规尖在法兰下 0.223m 而相机只在法兰下 0.124m（handeye
|
||||
# cam_in_flange z=0.1236）⇒ 规杆整段都在相机近场；且相机光轴与法兰 Z 差 29.7°，
|
||||
# 所以"光轴视距"不等于"竖直距离"，只能靠实测划带。
|
||||
# 撞了会怎样：假目标在相机前 0.08m 且**随臂一起动**，伺服误差恒为 0 ——
|
||||
# "一阶段已锁定"会锁在自己的工具上（见上面 reacquire_conf 的记录）。
|
||||
feature_min_range_m: 0.14
|
||||
work_z_max_m: 0.50
|
||||
meas_blend: 0.85 # 3D 跟手；抖则略降
|
||||
z_alpha_min: 0.10
|
||||
@@ -378,7 +509,26 @@ yolo:
|
||||
camera_frame: "d405_color_optical_frame"
|
||||
object_frame: "tracked_object"
|
||||
depth_topic: "/d405/aligned_depth_to_color/image_raw"
|
||||
feature_mode: "plane" # plane=框内近表面立体点（工件本体）；box=单目射线
|
||||
feature_mode: "keypoint" # keypoint=pose 模型的 k0（跟踪孔）；plane=框内近表面立体点；box=单目射线
|
||||
# feature_mode=keypoint 时用：跟踪用哪个关键点、它的置信度低于多少就不信它。
|
||||
# k0 = 跟踪孔（见 pose_detector 的约定）。k0 不可用时**借 k0 相对检测框的归一化
|
||||
# 偏移**在框上重建 k0 —— 位置连续、工件动时还跟着走。
|
||||
# 刻意**不**回落"框中心"（那是另一个点，离 k0 有 ~89mm，真机上就是一次不该有的
|
||||
# 横向快移，见 2026-10-08 一阶段 150s 没锁上的记录）；只有在**从未**见过可信 k0
|
||||
# 的冷启动帧才回落到框中心/框内深度。
|
||||
keypoint_index: 0
|
||||
keypoint_min_conf: 0.30
|
||||
keypoint_win_px: 6 # k0 处取多大窗口的中位深度
|
||||
# ---- 跟踪点可信性的三道守卫（实现：feature_guard.py，单一数据源）----
|
||||
# 为什么需要：2026-10-08 真机 arm 89，一阶段真 k0（conf 0.87 uv=(410,246)）被一串
|
||||
# **高置信度假 k0** 顶掉，伺服 1s 内把臂从 home 带到工作盒角落 (0.083,-0.515)，
|
||||
# 之后 148s 全是"检测漏检"、150s 超时。假 k0 的 conf 是 0.53~0.73（真值 0.87）
|
||||
# ⇒ 置信度没有可分区间；但它们**贴在画面边上**、且**借深度**：
|
||||
# 766.96 uv=(76,75) 768.16 uv=(143,92) 768.78 uv=(12,30) 770.07 uv=(431,226)
|
||||
keypoint_border_px: 6 # k0 离画面边缘 < 此值即判位置不可信（孔心不在画面边上）
|
||||
track_max_step_m: 0.04 # 已发布跟踪点的每帧位移上限 [m]（实测真实运动 ≤15mm/帧）
|
||||
reacquire_far_px: 120 # 远于此像素距离的"重新捕获"要连续确认（dpx 那个门限）
|
||||
reacquire_confirm_frames: 3 # 远框要连续几帧指向同一处才采信
|
||||
plane_inlier_m: 0.008 # 拟合平面内点阈值 [m]
|
||||
plane_band_m: 0.06 # 近表面深度带宽 [m]
|
||||
|
||||
@@ -409,7 +559,16 @@ servo:
|
||||
predict_enable: true # 基座系恒速预测，补视觉延迟
|
||||
predict_sec: 0.16 # 额外前瞻 [s]
|
||||
predict_v_max: 0.80 # 工件速度上限 [m/s]，防乱飞
|
||||
predict_jump_m: 0.08 # 基座 XY 跳变大于此则重锁速度
|
||||
predict_jump_m: 0.08 # 基座 XY 跳变大于此则视为"疑似假跳"，转入确认流程
|
||||
# 大跳变要连续多少帧指向同一新位置才采信（见 feature_jump_guard.py 的注释）。
|
||||
# 30Hz 下 3 帧 ≈ 0.1s：真的重定位 0.1s 就被认；假点（乱跳）永远认不了。
|
||||
# 为什么不能是 1：2026-10-08 真机，一阶段已收敛到 3mm 后 k0 掉信、跟踪点瞬间
|
||||
# 横移 89mm，旧逻辑单帧就 reset 采信 → 伺服以 0.45m/s 去追假点 → 彻底丢失。
|
||||
jump_confirm_frames: 3
|
||||
# 特征点位置合理性：必须落在工作盒 XY 内（±此余量 [m]），否则判误检、当"没有目标"。
|
||||
# 为什么需要它：2026-10-08 真机，模型在运动模糊下把特征点丢到基座后方
|
||||
# (-0.10,-0.65)（工件其实在 (0.5,-0.12)），伺服一路追到工作盒边缘卡住。
|
||||
feat_ws_margin_m: 0.05
|
||||
kp_xy: 4.20 # 很跟手：v = kp × 误差，再经加速度整形；抖则降回 3.5
|
||||
ki_xy: 0.00
|
||||
kd_xy: 0.20
|
||||
@@ -419,10 +578,20 @@ servo:
|
||||
i_sep_pos_m: 0.008
|
||||
deadzone_m: 0.003 # 3 mm；收紧提高对准精度。若抖动调回 0.006
|
||||
deadzone_z_m: 0.012
|
||||
lock_hold_sec: 0.7 # 死区驻留多久才发 /visual_servo/locked（串联 0.6s 已够）
|
||||
lock_hold_sec: 0.7 # 旧死区驻留（混合模式不再用它进二阶段）
|
||||
lock_still_sec: 3.0
|
||||
lock_still_move_m: 0.015
|
||||
|
||||
e_xy_clip_m: 0.200 # 大误差仍按 v_max 走，靠 a_max 加速
|
||||
v_max_xy: 0.45 # 法兰 XY 上限 450 mm/s
|
||||
v_max_z: 0.04
|
||||
# 远处限速：目标离得比 slow_far_m 远时 XY 限速到 v_slow_xy（**与"目标真不真"无关**
|
||||
# 的一道防线）。为什么需要：2026-10-08 真机一阶段被一串高置信度假 k0（conf 0.53~0.73）
|
||||
# 顶掉，伺服 0.7s 内以 0.45m/s 把臂从 home (0.686,-0.113) 带到工作盒角落
|
||||
# (0.083,-0.515)，之后 148s 全是"检测漏检"、150s 超时。加了这道之后：真 k0（离
|
||||
# 145mm）只是晚 ~1.8s 到位，假目标最多把臂带走几厘米。
|
||||
slow_far_m: 0.05 # 目标离得超过此距离即限速 [m]
|
||||
v_slow_xy: 0.08 # 限速值 [m/s]（真目标到位后仍回到 v_max_xy）
|
||||
min_ee_z_m: 0.20
|
||||
workspace_xyz_min: [0.08, -0.55, 0.18] # 法兰工作盒；盒外只允许往回走，不再卡死 no traj
|
||||
workspace_xyz_max: [0.85, 0.55, 0.65]
|
||||
@@ -490,10 +659,18 @@ gripper:
|
||||
# bash ~/ros2/scripts/run_hole_detect.sh --arm 89 --preview
|
||||
hole_detect:
|
||||
backend: "two_stage" # two_stage | yolo | hough
|
||||
# backend=yolo 时用。原为 hole_insert/best.pt（文件不存在），已改为实际存在的模型。
|
||||
weights: "/home/USER/ros2/models/hole_insert/detect_best_1.onnx"
|
||||
# 二阶段工件检测权重（YOLO 先框出工件，再 Hough 找 k1/k2 螺纹孔）
|
||||
workpiece_weights: "/home/USER/ros2/models/hole_insert/detect_best_1.onnx"
|
||||
# backend=yolo 时用。
|
||||
weights: "/home/USER/ros2/models/yolo/gongjian0929.onnx"
|
||||
# 二阶段工件检测权重。pose 模型既框工件、又直接给 k1/k2 关键点，
|
||||
# 所以和上面的跟踪权重是同一份。
|
||||
workpiece_weights: "/home/USER/ros2/models/yolo/gongjian0929.onnx"
|
||||
# pose 关键点优先：检测器给了 k1/k2 就直接用，不用再找圆。
|
||||
# 关键点不可信（低于 keypoint_min_conf / 两点几乎重合）时自动回落到
|
||||
# 亮环 + Hough 那套老路径。
|
||||
keypoint_enable: true
|
||||
keypoint_min_conf: 0.30
|
||||
keypoint_min_dist_px: 15.0
|
||||
keypoint_hole_radius_px: 12.0 # 关键点没有半径；反投影时按这个半径取孔周深度环
|
||||
config: "/home/USER/ros2/src/arm_application/config/hole_detect.yaml"
|
||||
conf: 0.25
|
||||
iou: 0.7
|
||||
@@ -532,16 +709,66 @@ hole_detect:
|
||||
# bash ~/ros2/scripts/run_stage2_locate.sh --arm 89 # 完整流程
|
||||
# bash ~/ros2/scripts/run_stage2_locate.sh --arm 89 --hover-only # 只悬停调高度
|
||||
stage2:
|
||||
target_hole: "k1" # 对准目标孔：k1=图像左孔 / k2=右孔（见 k1_label）
|
||||
k1_label: "left" # k1 是画面里的左孔；现场相反就改 right
|
||||
align_tol_m: 0.0007 # 对准到位判据 [m]
|
||||
align_max_iters: 8 # 拍照→修正→复拍 迭代上限
|
||||
# 对准目标孔。pose 模型下 k1/k2 是**模型学出来的固定语义**（不是图像左右）：
|
||||
# hole_points[0] = 模型的 k1，hole_points[1] = 模型的 k2
|
||||
# 下游是 `k1_b = pts[0] if k1_label=="left" else pts[1]`，所以：
|
||||
# k1_label: "left" → target_hole=k1 打的是**模型的 k1**（当前设置，推荐）
|
||||
# k1_label: "right" → target_hole=k1 打的是模型的 k2（等价于老 Hough 的"图像左孔"）
|
||||
# 实测这台臂的视角下模型给的是 k1 在画面右、k2 在画面左；对孔顺序要反过来就改这里。
|
||||
target_hole: "k1"
|
||||
k1_label: "left"
|
||||
# ---- 对准判据：工具**用视觉量到的规尖**，不是法兰轴线（2026-10-08 真机定稿）----
|
||||
# 实测（同一帧、同一条手眼链，n=8）：
|
||||
# · 规杆轴线相对法兰轴线 = (+10.7, −0.1)mm（σ≈0.5mm）；最深像素法 (+8.8,−1.8)mm
|
||||
# · 而 site 原来把 hover_offset_ee_xy_m 写成 [0,0]（"平行爪同轴"），于是精对准
|
||||
# 把**法兰**对到孔上，规尖实际离孔心 ~10mm ⇒ 轻降时 φ6 规尖骑在 φ7 孔沿上，
|
||||
# 腕力从 −1N 直接蹦到 −31N→−63N 卡死（现象是"轻降在离面 4mm 停住"）。
|
||||
# · 孔检测本身的重复性极好：k1 基座坐标 σ=(0.17,0.04)mm —— 精度不是瓶颈。
|
||||
# 所以：在同一帧里同时解「规尖基座 XY」和「孔基座 XY」，两者一起走手眼链，
|
||||
# 链路误差与装夹变动一起抵消。规尖量取规则见 _tip_offset_from_depth（四条判据）。
|
||||
align_tol_m: 0.00025 # 对准到位判据 [m]。检测重复性 0.04mm 撑得住 0.25mm。
|
||||
# ⚠️ 不要回到 0.7mm：那会让"规偏 10mm"这种系统偏差
|
||||
# 在容差里被放过去（原来残差 0.69mm 看着"通过"，其实
|
||||
# 是拿法兰对的孔）。收到 0.25mm 才逼它必须真的对上规尖。
|
||||
align_max_iters: 12 # 拍照→修正→复拍 迭代上限（收紧容差后需要更多拍）
|
||||
align_tip_vision: true # true=精对准用「视觉规尖」；false=退回旧的「法兰+偏置」
|
||||
tip_near_m: 0.15 # 规尖量取：近端阈值，规比这更近 [m]
|
||||
tip_min_m: 0.04 # 有效深度下限 [m]
|
||||
tip_max_m: 0.60 # 有效深度上限 [m]
|
||||
tip_search_band_frac: 0.25 # 中央竖带半宽 = 该比例 × 图宽（规夹在爪里，不会靠边）
|
||||
tip_min_px: 150 # 近端连通域最小像素数（少了就是没看到规）
|
||||
tip_calib_n: 4 # 定偏移需要的一致拍数（够了就冻结）
|
||||
tip_offset_tol_m: 0.003 # 样本相对中位偏移的最大允许偏差 [m]（剔偶发外点 + 冻结判据）
|
||||
tip_len_min_m: 0.15 # 反投影出的「法兰z − 规尖z」必须落在此区间 ——
|
||||
tip_len_max_m: 0.32 # 这条同时挡住"深度读到台面/工件"（主干守卫）
|
||||
coarse_max_moves: 2 # 工件粗居中（bbox 中心）移动上限
|
||||
# —— 粗居中的两条误检守卫（单源：代码读这两个键，见 _coarse_target_reject）——
|
||||
# 为什么需要：2026-10-08 真机 arm 89，粗居中拿被画面左边缘切掉的半框
|
||||
# bbox=(0,142,97,222) 反投影出盒外的 (0.280,-0.747)（偏差 211.5mm），旧逻辑
|
||||
# **不判误检、只把目标截断到盒边**，臂照样横移 56mm 离开工件 ⇒ 精定位再也
|
||||
# 看不到 k1/k2 ⇒ exit=4。判据改成"盒外/偏移过大 ⇒ 判误检、原地不动"。
|
||||
coarse_max_offset_m: 0.12 # 目标离夹爪轴线超过此值就不像粗居中量 [m]
|
||||
coarse_border_px: 2 # bbox 贴边（被画面切）即弃用 [px]；见 _bbox_on_border
|
||||
# —— 柔顺运动（夹爪持规，防急加速/急停/过冲把规震脱）：速度放慢 + 平滑刹车 ——
|
||||
align_speed: 0.014 # 对准段笛卡尔速度 [m/s]（原0.03，放缓防冲）
|
||||
move_speed: 0.022 # 升降/粗居中段速度 [m/s]（原0.05，放缓防震规）
|
||||
final_gap_mm: 2.0 # 对准后轻降目标：规尖悬在孔面上方这么高 [mm]（不接触；安全下限1.2mm自动停）
|
||||
final_descent_speed: 0.005 # 最后轻降速度 [m/s]（很慢，柔顺）
|
||||
# ✅ 2026-10-08 定稿：下降量**只由几何决定**（法兰 FK − 工具长 − 工件面 − final_gap），
|
||||
# 深度读数只做交叉核对、不再参与任何决策。两段真机事故方向相反、都指向这条：
|
||||
# · 2026-09-29（多降→撞工件）：环形深度中值报 gap0=141.0mm，几何真值只有 53mm
|
||||
# （probe 0.245 版），多降 ~86mm 把通止规撞进孔，工件被 +X 推走 5mm。
|
||||
# ⇒ 读数往**大**错一次就撞。
|
||||
# · 2026-10-08（少降→到不了 2mm）：同一个读数悬停时报 144.0mm（几何真值 75mm），
|
||||
# 只降 24mm 后又翻成 −7.0mm，触发 SAFE_FLOOR 把轻降停在离面 ~50mm 处，
|
||||
# 最终日志却打印"悬在孔面上方 ~2.0mm"（其实停在 ~18mm）。
|
||||
# ⇒ 读数往**小**错一次就永远到不了目标。
|
||||
# 同一套读数两个方向各错一次、幅度 150mm 量级、随位姿乱跳 —— 它对"能降多少"
|
||||
# 和"该不该停"都没有发言权。现在它只在偏差 >10mm 时喊一句"深度测距与几何不符"。
|
||||
# 现行几何：悬停 0.320 − 工具长 0.223 − 工件面 0.022 = 75mm 间隙 ⇒ 降 73mm 到 2mm。
|
||||
# 三道**物理/几何**保险：descent_budget_m（配置上限）+ 腕部 FT 接触检测（停+退 1mm）
|
||||
# + 任一步 _goto 走不到位就停。
|
||||
# 缺几何（工具长/工件面没配）时**拒绝下降**并报错（保持悬停），绝不拿测距去赌。
|
||||
final_descent_enable: true # 对准后是否轻降到 final_gap_mm（false=停在悬停高）
|
||||
verticalize_tol_deg: 0.8 # 垂直朝下判据 [deg]
|
||||
settle_sec: 0.45 # 到位后静止多久再拍照
|
||||
@@ -615,6 +842,22 @@ hole_detect:
|
||||
# =============================================================================
|
||||
perception:
|
||||
source: "yolo"
|
||||
# 一阶段：wrist=D405 主跟踪，画面内无 conf≥wrist_handoff_conf 则 D435 一次性定 K0。
|
||||
# overhead=整段只用顶视（实验）。非法值启动即报错。
|
||||
track_camera: "wrist"
|
||||
wrist_handoff_conf: 0.5
|
||||
overhead:
|
||||
min_depth_m: 0.25
|
||||
max_depth_m: 1.80
|
||||
feature_min_range_m: 0.30
|
||||
work_z_max_m: 1.50
|
||||
z_ref_m: 0.70
|
||||
feature_mode: "plane"
|
||||
hold_point_on_miss: true
|
||||
oneshot_conf: 0.5
|
||||
cooldown_s: 1.5
|
||||
frame_max_age_s: 0.4
|
||||
oneshot_hold_s: 8.0
|
||||
truth:
|
||||
# 真值目标物体。留空 = 场景文件里的 operating.part_object（推荐留空：
|
||||
# "哪个是工件"是场景的知识，换场景不该改这里）。
|
||||
@@ -845,8 +1088,11 @@ go_home:
|
||||
speed: 0.25
|
||||
|
||||
network:
|
||||
# User1=enp4s0 本机 <PC_IP> ↔ <左臂SN>=.101 / <右臂SN>=.100
|
||||
prefer_iface: "enp4s0"
|
||||
# 2026-09-29 改线：两台臂 → 交换机 → USB-C 千兆网卡（ASIX AX88179）
|
||||
# enxXXXXXXXXXXXX = 该 USB 网卡（名字由网卡 MAC 派生，换网卡会变）
|
||||
# 本机 <PC_IP> ↔ <左臂SN>(左89)=.101 / <右臂SN>(右91)=.100
|
||||
# 说明：enp4s0 仍是办公网（192.168.1.x）；两台臂已不在 enp4s0 上。
|
||||
prefer_iface: "enxXXXXXXXXXXXX"
|
||||
|
||||
# 仿真平台（当前 Gazebo 尚未接线，详见 devices/sim/gazebo/README.md）
|
||||
# 设计约束：仿真与真机共用同一套参数（servo.* / hole_detect.stage2.*），
|
||||
|
||||
Reference in New Issue
Block a user