arm_dev
561d7da21f
首轮咬合的顺时针进孔又走了接口上限，螺纹还没咬住就转完了。这一段改回读 extreme_speed_deg_s（现场 10°/s）。首轮逆时针出孔，以及空回每一轮的正反转， 仍走 0.90 rad/s 上限。 空回原先只做一轮。首轮咬合结束后，张开、微抬、逆时针、按本轮实测上抬降回、 合爪、顺时针这一整段固定重复 UNWIND_ROUNDS=4。抬和降留在轮内，下降量用这一轮 记住的上抬，不用上一轮的数。日志打空回第 i/4 轮；某一轮没走完就停，后面不再做。 10:40 合爪命令已经发到话题，节点被 Serial.flush/tcdrain 堵住 120s 才执行， 流水线 8s 时手指还停在 pos=400，第 2–4 轮没开始。发送改成等发送缓冲排空， 0.25s 排不空就失败返回，不再调用 tcdrain。同一次开合状态没跟上再发一次。 Co-authored-by: Cursor <cursoragent@cursor.com>
1155 lines
77 KiB
YAML
1155 lines
77 KiB
YAML
# =============================================================================
|
||
# site.yaml — 本机唯一配置入口（不进 git；从 site.example.yaml 复制）
|
||
# 路径：~/ros2/config/site.yaml
|
||
#
|
||
# 89 与 91 的手眼 / 腕部相机 / 夹爪 / 点位互相独立，禁止混用。
|
||
# 切换机械臂：改 robot.sn，或所有终端加同一 --arm 89|91。
|
||
# =============================================================================
|
||
|
||
robot:
|
||
# >>> 切换机械臂：改成 robots 里某一台的完整 SN <<<
|
||
sn: "Rizon4-063989"
|
||
# Studio Tools 名（一字不差）。空=不切换。查：bash ~/ros2/scripts/rdk_switch_tool.sh --list
|
||
# 左臂 89 的激活工具写在 robots[] 里（tool10120938）；这里只给未覆盖的臂当缺省。
|
||
tool_name: "tool1"
|
||
# 每台登记 SN + IP，以及该臂自己的相机/夹爪/手眼/点位（覆盖下面的全局段）。
|
||
robots:
|
||
- sn: "Rizon4-063989"
|
||
arm: left
|
||
ip: "192.168.2.101"
|
||
pc_ip: "192.168.2.10"
|
||
iface: "eth0"
|
||
label: "左臂89"
|
||
# 本臂装了 Orisys 视触觉（只有左臂有）。脚本靠这一项决定要不要拉触觉服务，
|
||
# 而不是靠"SN 等于某个具体值"这种写死的判断（换台臂就会失效）。
|
||
tactile: true
|
||
# 2026-10-16 下午 RDK 重读 Studio tool10120938（用户再次更新 TCP）。
|
||
tool_name: "tool10120938"
|
||
camera:
|
||
name: d405_left # 话题 /d405_left/... 与 91 分开
|
||
serial: "352122274408" # 左腕 D405（一直在用的那台）
|
||
auto_exposure: true
|
||
ae_limit_us: 4000
|
||
emitter_enabled: false
|
||
gripper:
|
||
model: "PGEA-100-20-W"
|
||
sn: "26610799"
|
||
port: "/dev/ttyUSB0"
|
||
stroke_mm: 20.0
|
||
# 大寰上电后必须初始化一次（写 0xA5），而初始化的动作是**手指先回零**
|
||
# = 张开到满行程。如果这时爪子已经夹着通止规/工件，规会被放开、掉下去。
|
||
# 2026-09-28 现场实测过：auto_init 走一遍，开口从 19.18mm 变成 20.00mm。
|
||
# 所以这里的默认是 **false**：由人决定什么时候初始化。
|
||
# · 上电后、还没装夹 → 可以临时开一次：SITE_GRIP_AUTO_INIT=true bash scripts/run_gripper.sh --arm 89
|
||
# · 已经装夹（装配过程中）→ 保持 false；不初始化也能读状态、也能开合
|
||
# （status 里的 init=need_init 只说明"绝对位置标定没做"，不影响夹持）
|
||
# 想让开机必初始化（无人值守的上电流程）就显式改 true，并确认爪里是空的。
|
||
auto_init: false
|
||
named_poses_file: "/home/cc/ros2/src/arm_control/config/named_poses_89.yaml"
|
||
paths:
|
||
handeye_file: "/home/cc/ros2/handeye/results/89_eye_in_hand.yaml"
|
||
handeye_waypoints: "/home/cc/ros2/handeye/poses/89_eye_in_hand.yaml"
|
||
handeye_workdir: "/home/cc/ros2/handeye/tmp/89"
|
||
handeye:
|
||
# Studio tcp_location（法兰系，米 + qwxyz）。与 rdk Tool.params 必须一致。
|
||
# 眼在手上：AX=XB 的手是这份 TCP，再折成 cam→法兰写 T_4x4。
|
||
# 眼在手外：AX=XB 的手同样是这份 TCP（板夹在工具上）；T_4x4 仍是 cam→基座，不折。
|
||
gripper_tool: "tool10120938"
|
||
gripper_tcp_xyz: [0.000537, -0.000264, 0.226587]
|
||
gripper_tcp_qwxyz: [1.0, 0.0, 0.0, 0.0]
|
||
# 左臂跟踪高度（只在 --arm 89 时生效，改 91 的 servo 不影响这里）
|
||
# 锚点 `&servo_left`：下面的"仿真台"条目直接引用它 —— 仿真建的就是左臂，
|
||
# 悬停/锁高必须与 89 **一模一样**，抄两份迟早漂移。
|
||
servo: &servo_left
|
||
# 2026-09-29 底座加高后由 0.35 → 0.36：工件上表面实测抬高 10mm
|
||
# （0.012 → 0.022），锁高同步 +0.011 ⇒ 相机到工件的视距**与加高前一致**
|
||
# （≈0.213m，跟踪那一档已实拍验证过检出稳定），不是随手抬的。
|
||
# 跟踪锁高与 hole_hover_z_m 相同：先到这个悬高，一阶段只动 XY。
|
||
# 对孔完成后再下降。以前 ee_z 高于悬停（0.36），对孔前会先降一截，
|
||
# 倾斜相机下孔位被重投影，偏差变大。
|
||
ee_z_m: 0.320 # 跟踪锁高：法兰相对基座 Z [m]，与 hole_hover_z_m 同一值
|
||
min_ee_z_m: 0.20
|
||
safe_z_m: 0.45
|
||
z_min_m: 0.22
|
||
xy_track: hover # 法兰(夹爪中心) XY 压在工件上，不是画面中心
|
||
ee_vertical_down: true # 法兰 +Z 朝世界 −Z
|
||
hover_offset_ee_xy_m: [0.0, 0.0] # 爪心相对法兰 XY；平行爪同轴保持 0
|
||
# >>> 二阶段孔定位悬停锁高：只改这里，91 用下面那段 <<<
|
||
# 法兰 Z [m]；要兼顾：够低拍清 k1/k2 螺纹孔，够高通止规尖不碰工件。
|
||
# 调试：bash ~/ros2/scripts/run_stage2_locate.sh --arm 89 --hover-only
|
||
# = 工件上表面 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_063989/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-063989
|
||
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-063989
|
||
# python3 scripts/lib/real_geometry_check.py --sn Rizon4-063989
|
||
# 2026-10-16 下午：RDK Tool.params("tool10120938").tcp_location
|
||
# [x,y,z,qw,qx,qy,qz] = [0.000537, -0.000264, 0.226587, 1, 0, 0, 0]
|
||
# 示教器拟合误差：平均 0.58 / 最大 0.99 / 最小 0.29 mm。
|
||
# 下臂安全距离用 TCP +Z。上午 0.226274；再往前 0.2245 是 10-12；0.223 是 tool1。
|
||
tool_tip_below_flange_m: 0.226587
|
||
# >>> 仿真那台臂（Gazebo）也登记成 robots[] 里的一台 <<<
|
||
# 为什么必须登记：应用是按"当前臂"的名字去查 frame / joint / 相机话题的
|
||
#（`sim_env.sh` 注释里说的"仿真与真机共用同一套 site.yaml"）。仿真的 TF 与关节
|
||
# 名带 `sim_` 前缀（与真机区分，避免仿真数据冒充真机），所以它需要自己的一台。
|
||
# 登记之后，**所有阶段（stage2/stage3/装配…）都不改一行代码**就能跑仿真，
|
||
# 只要 `--arm sim`（`scripts/run_gauge_assembly.sh --sim` 会自动选它）。
|
||
#
|
||
# 真机上永远不会选到它：只有显式 `--arm sim` 才会，而没有任何真机脚本会这么写。
|
||
- sn: "sim"
|
||
arm: sim
|
||
label: "仿真台"
|
||
# 仿真建模的就是左臂，触觉也只有左指（由 sim_tactile_adapter 发）
|
||
tactile: true
|
||
camera:
|
||
# 话题 /sim_d405/... ：与真机的 /d405_left 分开。为什么不用同一个名字：
|
||
# 同域里两个来源会互相覆盖，"台上这台到底是谁"就说不清了。
|
||
# 名字里的 d405 与 URDF 里的模型名一致 → 光学系 sim_d405_color_optical_frame
|
||
# 正好等于 `{camera.name}_color_optical_frame`，所以应用查 TF 不用特判。
|
||
name: sim_d405
|
||
# 仿真不需要手眼标定文件：腕部相机在 URDF 里就装在法兰上，
|
||
# 而真机那份标定文件描述的是"相机相对法兰的实际安装偏差"，
|
||
# 套到仿真上会把相机搬到一个**错误**的位置（投影全错）。
|
||
# 留空 → 启动脚本不发静态 TF（见 gauge_assembly.launch.py 的提示）。
|
||
paths:
|
||
# 仿真不"标定"，而是把 URDF 里的真值**导出**成同格式的文件：
|
||
# scripts/gen_sim_handeye.py（run_gauge_assembly.sh --sim 会自动跑）。
|
||
# 为什么必须给文件而不能留空：visual_servo 的 servo_node 需要
|
||
# T_flange_cam 矩阵做像素→笛卡尔换算；留空它就直接拒绝启动
|
||
#（"手眼标定结果缺失"），S04 跟踪起不来。
|
||
handeye_file: "/home/cc/ros2/handeye/results/sim_eye_in_hand.yaml"
|
||
# 运动参数：**大部分**与 89 共用（锚点），但**高度类**必须按仿真台的几何改写。
|
||
#
|
||
# 为什么不能直接共用：真机是"机械臂装在台面同一高度"，所以"法兰相对基座"
|
||
# 就等于"法兰相对台面"（hole_hover_z_m=0.23 就是这么来的）。
|
||
# 仿真里机械臂站在**地面**、台面 0.40m 高、工件上表面 0.412m —— 一切 Z
|
||
# 都比真机大 0.412。照抄真实值会让伺服把法兰送到 0.28m，也就是**撞进台面**。
|
||
#
|
||
# 推导（与 scene.yaml 的 operating 段同源，scripts/check_sim_scene.py 会核对）：
|
||
# 相机到工件 0.17（像素门槛反推，不能改）
|
||
# + 相机比法兰低 0.05（URDF 里 d405_z=0.05）
|
||
# + 工件上表面 0.412
|
||
# ⇒ 二阶段悬停法兰高 = 0.632
|
||
# 一阶段跟踪高 = 悬停 + 0.05（通止规离工件）= 0.682
|
||
# 所以工作盒上界必须 ≥ 0.682 —— 真机那套上界 0.65 不够，S04 一跟踪就会被
|
||
# "超出工作盒"截断（现象是"越跟踪越偏"，很难想到是配置差 3cm）。
|
||
#
|
||
# ⚠️ 上面那套"按相机"的推导**只对了一半**（2026-09-24 修正）：它没算**工具**。
|
||
# 悬停高度还要满足"工具尖端不碰工件"：
|
||
# 工件上表面 0.412 + 工具长 0.243（推算的标称值：TCP 0.200 + 规尖 0.043）
|
||
# + 间隙 0.002 = **0.657**
|
||
# 原来的 0.632 会让工具尖端落在工件面**下**（按 0.245 算是 −23mm）—— 悬停
|
||
# 这一步就把通止规捅进工件里，接着 S05 的横向对准等于拿规尖拨工件（实测把
|
||
# 工件推走 79mm），而真值孔位按工件**实测**位姿算 ⇒ 孔位跟着工具走、残差
|
||
# 恒定 11mm 永不收敛（日志看起来像"手眼标定坏了"）。真机同参数同结果：只要
|
||
# 悬停按相机定、而工具比相机长，就会这样。
|
||
# 现在三道防线：stage2 启动时按 gauge_fsm.hover_z_guard 用几何兜住；S05 有
|
||
# "工件被推走"的看门狗（gauge_fsm.drag_verdict）；S07 按几何自查下探预算。
|
||
#
|
||
# ⚠️ 为什么这里能写 0.657 这种"贴着"的值：悬停高每抬高 1mm，S07 就多下
|
||
# 1mm，而 contact_max_depth_mm=35 是按"悬停只留 2mm 间隙"配的
|
||
# （30 台肩 + 5 余量）。抬到 0.67 会让下探变成 43mm，S07 就得**自动改大**
|
||
# 预算并告警 —— 能跑，但配置就失去意义了。
|
||
#
|
||
# 工具长 0.243 只是**标称**：仿真里上料脚本每次实测并落盘
|
||
# （devices/sim/gazebo/config/sim_tool_geometry.yaml），
|
||
# run_gauge_assembly.sh --sim 用它当启动参数覆盖这里的值。真机由现场把
|
||
# 夹好规后读到的 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）。
|
||
hole_hover_z_m: 0.657 # 二阶段悬停高：**由工具几何定**，不是相机视距
|
||
# = 0.412（工件上表面）+ 0.243（工具）+ 0.002（间隙）
|
||
safe_z_m: 0.76 # 安全抬升/复位高度（高于跟踪高）
|
||
z_ref_m: 0.657 # 没有测量时的兜底 Z：绝不能是 0.22（会撞工件）
|
||
z_min_m: 0.55 # 伺服硬下限：0.55 = 工件上表面 + 0.14
|
||
min_ee_z_m: 0.55
|
||
workspace_xyz_min: [0.08, -0.55, 0.55]
|
||
workspace_xyz_max: [0.85, 0.55, 0.76]
|
||
# 仿真不跑推理：感知结果由 sim_truth_perception 按已知位姿直接给出。
|
||
# 原因：ros_gz_sim 环境里没有 torch/ultralytics，且本机离线装不上 ——
|
||
# perception_node 一启动就死，表现为"一阶段永远不锁定"。
|
||
# 详见 devices/sim/gazebo/README.md「仿真里哪些模块用真值替代」。
|
||
perception:
|
||
source: "truth"
|
||
# 几何类参数按台子改写（gauge_assembly 段的下沉覆盖）。
|
||
# 为什么需要：全局 gauge_assembly 里的值是**真机**的 —— 真机上机械臂
|
||
# 基座与台面同高，所以"工件上表面 z"就是板厚 0.012；仿真里机械臂站在
|
||
# **地面**、台面 0.40m 高、工件上表面 0.412。写死全局值会让 S07 的
|
||
# "悬停时工具尖端离面多高"自检算错（差 0.4m），现象是"一路下压判不到
|
||
# 接触"，看起来像力阈值标歪了 —— 于是排查从阈值开始走偏。
|
||
gauge_assembly:
|
||
workpiece_surface_z_m: 0.412
|
||
# 工具长这里写**推算的标称值**（TCP 在法兰下方 0.200 + 规尖在指腹面下方
|
||
# 0.043 = 0.243）。真值由上料脚本实测并作为启动参数覆盖：仿真里合爪后规会
|
||
# 在指间下滑到落座位置，下滑量随装夹变化（实测见过 243 / 254mm）。
|
||
# 写成固定值会让悬停高度算错（写小 → 规尖插进工件 ⇒ S05 拿规尖拨工件、
|
||
# 残差恒不收敛），所以**这个值在仿真里只是兜底**，实际用实测值。
|
||
tool_tip_below_flange_m: 0.243
|
||
- sn: "Rizon4-063991"
|
||
arm: right
|
||
ip: "192.168.2.100"
|
||
pc_ip: "192.168.2.10"
|
||
iface: "eth0"
|
||
label: "右臂91"
|
||
camera:
|
||
name: d405_right # 话题 /d405_right/... 禁止和 89 的 /d405_left 混用
|
||
serial: "260522274442" # 右腕新 D405
|
||
auto_exposure: true
|
||
ae_limit_us: 0 # 0=不封顶，让固件自己调亮
|
||
emitter_enabled: false
|
||
laser_power: 0
|
||
gripper:
|
||
model: "RGI-100-30-O-S-M1" # 旋转爪：run_gripper.sh --arm 91
|
||
sn: ""
|
||
port: "/dev/ttyUSB1"
|
||
stroke_mm: 30.0
|
||
auto_init: false
|
||
named_poses_file: "/home/cc/ros2/src/arm_control/config/named_poses_91.yaml"
|
||
paths:
|
||
handeye_file: "/home/cc/ros2/handeye/results/91_eye_in_hand.yaml"
|
||
handeye_waypoints: "/home/cc/ros2/handeye/poses/91_eye_in_hand.yaml"
|
||
handeye_workdir: "/home/cc/ros2/handeye/tmp/91"
|
||
# >>> 右臂跟踪高度：只改这里，89 仍用上面那段 <<<
|
||
# 法兰 Z；RGI 比平行爪高，通常要比 89 略抬一点。改完重启：
|
||
# bash ~/ros2/scripts/run_visual_servo_rt.sh --arm 91 --control
|
||
servo:
|
||
ee_z_m: 0.25 # 跟踪锁高：法兰相对基座 Z [m] ← 日常调这一行
|
||
min_ee_z_m: 0.20
|
||
safe_z_m: 0.45
|
||
z_min_m: 0.22
|
||
xy_track: hover # 用法兰系手眼 T：夹爪中心(法兰原点)压在工件正上方
|
||
ee_vertical_down: true # 法兰/RGI 轴线朝下；相机有 29° 倾角，绿框不必对准十字
|
||
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_063991/external_wrench_in_tcp"
|
||
# --control：先只降到 ee_z_m（XY 不动），到位后才 hover，用手眼把爪心压到工件正上方
|
||
|
||
paths:
|
||
# >>> YOLO 权重（标准放法，不必改代码）<<<
|
||
# 1) 把 .onnx / .pt 放到 /home/cc/ros2/models/yolo/
|
||
# 2) 这一行改成那个绝对路径（写 .onnx 即可；TensorRT 会找同名 .engine）
|
||
# 3) backend=tensorrt 时必须先导出引擎（只需做一次）：
|
||
# 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
|
||
# 当前：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/cc/ros2/models/yolo/gongjian0929.onnx"
|
||
# 换模型：把 models/yolo/ 下别的 .onnx / .pt 绝对路径填进来即可。
|
||
# 未在用的模型已移出仓库（体积大且干扰 `weights.sh list`），要恢复见 models/README.md。
|
||
# 入孔模型（backend=yolo 时把孔当框）。two_stage 用下面的工件权重。
|
||
# pose 模型两处共用同一份 —— 它既能框工件，又直接给 k1/k2 关键点。
|
||
# 查悬空引用：bash ~/ros2/scripts/weights.sh doctor
|
||
hole_weights: "/home/cc/ros2/models/yolo/gongjian0929.onnx"
|
||
# 下面是缺省（未匹配到 robots[] 时）。真机以 robots[].paths 为准：89 / 91 各一份。
|
||
handeye_file: "/home/cc/ros2/handeye/results/89_eye_in_hand.yaml"
|
||
handeye_waypoints: "/home/cc/ros2/handeye/poses/89_eye_in_hand.yaml"
|
||
handeye_calibrate: "/home/cc/ros2/handeye/calibrate.yaml"
|
||
handeye_workdir: "/home/cc/ros2/handeye/tmp/89"
|
||
# 空=默认 DDS(SHM)；勿强制 UDP-only，否则相机预览收不到 /workpiece/detection
|
||
fastdds_profile: ""
|
||
|
||
handeye:
|
||
mode: "eye_in_hand"
|
||
# >>> 标定板尺寸：**按模式各一份**（内角点数 + 方格边长 m）<<<
|
||
#
|
||
# 为什么必须按模式分开写（而不是两个模式共用一组键）：
|
||
# 眼在手上与眼在手外通常用**两块不同的物理板**，方格边长不一样。共用一组键
|
||
# 的后果不是报错，而是"跑另一种模式时静默用错方格边长"—— 标定照样跑完、
|
||
# 结果只是系统性偏，现场几乎联想不到板尺寸（历史反例见 handeye/README.md 第 3 节）。
|
||
# 内角点 = 方格数 − 1；列/行顺序不用纠结，检测端两种顺序都会试
|
||
#（见 handeye_lib.detect_chessboard）。
|
||
boards:
|
||
eye_in_hand: # A5 板：12×9 方格 → 内角点 11×8
|
||
cols: 11 # 内角点列数
|
||
rows: 8 # 内角点行数
|
||
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
|
||
settle_sec: 1.5
|
||
settle_qd_max: 0.015 # |qd|_max 低于此值才算静
|
||
settle_stable_sec: 0.6
|
||
settle_timeout_sec: 8.0
|
||
pin_holds: 2
|
||
pin_hold_sec: 0.45
|
||
skip_unstable_poses: true # 某点看不到棋盘则跳过，不整段中止
|
||
# 点位：paths.handeye_waypoints
|
||
|
||
camera:
|
||
name: "d405"
|
||
serial: "352122274408" # 全局缺省=左腕；真机以 robots[].camera.serial 为准
|
||
# USB3 推荐 848x480@30；USB2 自动降到 640x480@15，深度/右目仍开
|
||
width: 848
|
||
height: 480 # 改宽高/帧率必须重启相机：bash ~/ros2/scripts/run_camera_d405.sh --preview
|
||
fps: 30
|
||
enable_depth: true
|
||
enable_infra: true # 同一 SDK 进程开左右红外；失败则自动退回彩图+深度
|
||
preview: true
|
||
preview_stereo: true # 预览同时显示 彩图 | 深度 | 左目 | 右目（不另开相机、不抢 USB）
|
||
overlay_hold_sec: 0.10 # 预览只留很短；长残留会看起来像绿框不跟
|
||
use_site_resolution: true
|
||
auto_exposure: true
|
||
ae_limit_us: 4000 # D405 腕部近距；91 在 robots[] 里更严
|
||
emitter_enabled: false # 腕部关红外投射，否则彩图发白
|
||
laser_power: 0
|
||
|
||
# 现场三台 RealSense（按序列号钉死，禁止按「第一台 D405」乱开）
|
||
cameras:
|
||
d405_left:
|
||
model: d405
|
||
name: d405_left
|
||
serial: "352122274408"
|
||
role: left_wrist
|
||
arm: left
|
||
label: 左腕D405（89 平行夹爪）
|
||
width: 848 # 相机分辨率以本段为准（全局 camera: 段为旧路径，P2 收敛）
|
||
height: 480
|
||
emitter_enabled: false # 腕部必须关投射器，红外点阵会污染跟踪图像
|
||
d405_right:
|
||
model: d405
|
||
name: d405_right
|
||
serial: "260522274442"
|
||
role: right_wrist
|
||
arm: right
|
||
label: 右腕D405（91 旋转夹爪）
|
||
width: 848
|
||
height: 480
|
||
emitter_enabled: false
|
||
# >>> 仿真那两台（arm: sim）<<<
|
||
# 为什么仿真也要在这里登记：应用是"按当前臂"去认相机的 —— 腕部相机取
|
||
# robots[].camera.name，手外相机取这里 role=overhead* 且 arm 与当前臂一致的
|
||
# 那一条（见 gauge_assembly_node._d435_ns）。名字必须与
|
||
# devices/sim/gazebo/config/cameras.yaml 的 ros_ns 一模一样。
|
||
# 名字不带 d405_left/d435 那样的真机名：同域里两个来源会互相覆盖，
|
||
# 而 eye-to-hand 标定文件是按相机名命名的 —— 重名会让"跑一次仿真把真机标定盖掉"。
|
||
sim_d405:
|
||
model: d405
|
||
name: sim_d405
|
||
role: sim_wrist
|
||
arm: sim
|
||
label: 仿真腕部D405（= 左臂位置）
|
||
width: 848
|
||
height: 480
|
||
emitter_enabled: false
|
||
sim_d435:
|
||
model: d435
|
||
name: sim_d435
|
||
role: overhead_eye_to_hand
|
||
arm: sim
|
||
label: 仿真手外D435（世界固定）
|
||
width: 640
|
||
height: 480
|
||
fps: 30
|
||
emitter_enabled: true
|
||
|
||
d435:
|
||
model: d435
|
||
serial: "262522073161"
|
||
role: overhead_eye_to_hand
|
||
label: 双臂中间手外D435
|
||
name: d435
|
||
width: 1280
|
||
height: 720
|
||
fps: 30
|
||
auto_exposure: true
|
||
ae_limit_us: 8000
|
||
emitter_enabled: true # 顶视深度可用投射器；只看彩图可改 false
|
||
|
||
|
||
yolo:
|
||
conf: 0.10 # 检测器门槛：略低于 min_conf，让运动模糊框也能进跟踪
|
||
iou: 0.45
|
||
imgsz: 640 # 必须等于所用 ONNX 的输入边长（当前 detect_best_0 = 640）
|
||
device: "0"
|
||
half_precision: true
|
||
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
|
||
miss_clear_frames: 10 # ~0.33 s @30 Hz；期间绿框按像素速度滑行
|
||
overlay_hold_on_miss: true # 滑行失败时仍续发上一框
|
||
hold_point_on_miss: false # 眼在手上：丢检续发旧点会让臂追错位置
|
||
miss_hold_point_frames: 1
|
||
jump_px: 400.0
|
||
jump_z_m: 0.10
|
||
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
|
||
z_alpha_max: 0.40
|
||
z_alpha_gain: 1.0
|
||
camera_frame: "d405_color_optical_frame"
|
||
object_frame: "tracked_object"
|
||
depth_topic: "/d405/aligned_depth_to_color/image_raw"
|
||
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]
|
||
|
||
servo:
|
||
control_hz: 100.0
|
||
# --- 基座坐标系下的末端约束（法兰，单位 m）---
|
||
ee_x_m: 0.25
|
||
ee_y_m: 0.00
|
||
ee_z_m: 0.35 # 全局缺省锁高；真机以 robots[].servo.ee_z_m 为准（89/91 分开调）
|
||
ee_hold_xy: false # false=跟踪工件 XY；true=锁死 ee_x/ee_y
|
||
xy_track: "hover" # hover=法兰悬停在目标正上方（任意锁高） lookat=画面中心对准目标
|
||
use_static_handeye_tf: false # 只用 handeye_result.yaml，避免 TF 树抢父子帧
|
||
ee_vertical_down: true
|
||
# --- 启动回初始位（已停用：--control 不再调用 goto_ee_plane）---
|
||
safe_z_m: 0.45 # >>> 防撞安全高度：先纯 Z 抬到这里再回 home，避免扫到料框/壁
|
||
home_pose_name: "home" # 非夕初始点位（/set_named_pose：home | ready | inspect | pos_left）
|
||
home_pose_speed: 0.30 # 回初始位的速度比例 (0,1]，越小越稳
|
||
ee_hold_kp: 2.0
|
||
ee_w_max: 0.60
|
||
z_ref_m: 0.22
|
||
z_min_m: 0.22
|
||
approach_xy_gate_m: 0.040 # --control 先降到锁高，|ee_z-ee_z_m| 小于此值才开始 XY hover
|
||
v_approach_z: 0.025
|
||
watchdog_timeout_sec: 1.2
|
||
lost_timeout_sec: 1.2
|
||
stale_hold_sec: 0.35 # 丢检空窗内用基座 KF 预测继续跟（自运动安全），超过才钉住
|
||
max_extrapolate_sec: 0.35 # 预测时间上限（图像延迟 + 前瞻）
|
||
predict_enable: true # 基座系恒速预测，补视觉延迟
|
||
predict_sec: 0.16 # 额外前瞻 [s]
|
||
predict_v_max: 0.80 # 工件速度上限 [m/s]，防乱飞
|
||
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
|
||
kp_z: 0.80
|
||
ki_z: 0.00
|
||
kd_z: 0.35
|
||
i_sep_pos_m: 0.008
|
||
deadzone_m: 0.003 # 3 mm；收紧提高对准精度。若抖动调回 0.006
|
||
deadzone_z_m: 0.012
|
||
lock_hold_sec: 0.7 # 旧死区驻留（混合模式不再用它进二阶段）
|
||
# 混合模式必填。交回 D405 的等待不再用它，见 d435_handback_sec。
|
||
lock_still_sec: 3.0
|
||
# 厂家 MoveL 结束后，法兰连续这么久没再动，才打开 D405 框心。
|
||
d435_handback_sec: 2.0
|
||
# 框心对准并且法兰停稳这么久，才精拍一次小孔关键点、进对孔。实时点到此为止。
|
||
wrist_align_sec: 0.5
|
||
lock_still_move_m: 0.015 # 15 mm；超过就重计时，避免"晃一下刚好满 3 秒"
|
||
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]
|
||
a_max: 3.00 # 加速度上限（真正作用在 hover 指令上）
|
||
vel_lpf_alpha: 0.80 # 速度低通：越大越跟手（1=不滤波）
|
||
dq_window_size: 1 # 1=不做滑窗平均，去掉 ~20ms 滞后
|
||
traj_stream_hz: 50.0 # 轨迹点流率：JTC open_loop+样条，吃得起
|
||
exec_horizon_sec: 0.08 # 每个轨迹点的执行窗口
|
||
max_qdot: 2.00 # 关节速度上限 [rad/s]
|
||
min_cmd_delta_rad: 0.001
|
||
point_topic: "visual_servo/filtered_point"
|
||
traj_topic: "rizon_arm_controller/joint_trajectory"
|
||
|
||
# 大寰夹爪：官方 Modbus RTU，USB–RS485 插 IPC，不走 Studio/RDK。
|
||
# 左 89 PGEA → /dev/ttyUSB0；右 91 RGI → /dev/ttyUSB1。
|
||
# bash ~/ros2/scripts/run_gripper.sh --arm 89 --cli open|close|pos
|
||
# bash ~/ros2/scripts/run_gripper.sh --arm 91 --cli open|close|rotate 90
|
||
# bash ~/ros2/scripts/run_dual_gripper.sh
|
||
# ROS：/gripper/open|close（PGEA 单爪）或 /dual_gripper_driver/{left,right}/execute
|
||
gripper:
|
||
backend: "usb" # usb=工控机官方Modbus rdk=Studio设备（不用）
|
||
vendor: "dh"
|
||
model: "PGEA-100-20-W"
|
||
sn: "26610799" # 铭牌，不进协议
|
||
device_name: "GripperDahuanModbus"
|
||
port: "/dev/ttyUSB0" # 本机 MACROSILICON/pl2303
|
||
baud: 115200 # 官方默认 115200 8N1
|
||
slave_id: 1 # 官方默认从站 1
|
||
rs485_rts: "" # 空=不拉RTS；scan 若 OK 在 tx/tx_inv 再填
|
||
stroke_mm: 20.0
|
||
# --- 开合 / 力 / 速（大寰官方寄存器，超出会被驱动钳位）---
|
||
force_pct: 20 # 夹持力 区间 20–100 %（低于 20 会被钳到 20；PGEA-100 约 30–100 N）
|
||
speed_pct: 50 # 开合速度 区间 1–100 %
|
||
open_position: 1000 # 张开目标 区间 0–1000 ‰（1000=全开）
|
||
close_position: 0 # 闭合目标 区间 0–1000 ‰（0=全合；留缝改 50–200）
|
||
auto_init: true
|
||
state_hz: 20.0
|
||
# 双爪共用参数（run_dual_gripper.sh / load_from_site）
|
||
parallel:
|
||
stroke_min: 0.0
|
||
stroke_max: 20.0
|
||
default_speed: 50
|
||
default_force: 20
|
||
force_min: 20
|
||
force_max: 100
|
||
auto_initialize: true
|
||
rotary:
|
||
auto_initialize: false
|
||
angle_min: -32767.0
|
||
angle_max: 32767.0
|
||
max_step_deg: 30000.0
|
||
finger_open_per_mille: 1000
|
||
finger_close_per_mille: 0
|
||
default_speed: 50
|
||
default_torque: 50
|
||
default_finger_speed: 50
|
||
default_finger_force: 50
|
||
torque_min: 20
|
||
torque_max: 100
|
||
# RDK 回退字段（backend: rdk 才用）
|
||
force_n: 50.0
|
||
vel_mps: 0.05
|
||
|
||
# 入孔检测。算法参数写在这里（覆盖包内 hole_detect.yaml 默认值）。
|
||
# bash ~/ros2/scripts/run_hole_detect.sh --arm 89 --preview
|
||
hole_detect:
|
||
backend: "two_stage" # two_stage | yolo | hough
|
||
# backend=yolo 时用。
|
||
weights: "/home/cc/ros2/models/yolo/gongjian0929.onnx"
|
||
# 二阶段工件检测权重。pose 模型既框工件、又直接给 k1/k2 关键点，
|
||
# 所以和上面的跟踪权重是同一份。
|
||
workpiece_weights: "/home/cc/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/cc/ros2/src/arm_application/config/hole_detect.yaml"
|
||
conf: 0.25
|
||
iou: 0.7
|
||
imgsz: 640
|
||
center_gate_enable: true
|
||
center_max_offset_ratio_x: 0.15
|
||
center_max_offset_ratio_y: 0.18
|
||
center_gate_draw: true
|
||
refine_hole_centers: true
|
||
bbox_padding: 80
|
||
gaussian_ksize: 5
|
||
gaussian_sigma: 1.5
|
||
hough_dp: 1.2
|
||
hough_min_dist: 50
|
||
hough_param1: 100
|
||
hough_param2: 26
|
||
hough_min_radius: 6 # 悬停较高时螺纹孔半径只有几像素
|
||
hough_max_radius: 30
|
||
min_radius: 6
|
||
max_radius: 30
|
||
hole_distance_min: 90 # 悬停 0.23m 两螺纹孔约 170px；过小会配到中心孔
|
||
hole_distance_max: 700
|
||
expected_holes: 2
|
||
min_hole_sep_px: 20
|
||
max_hole_sep_px: 2000
|
||
stabilize_hold_frames: 15
|
||
stabilize_ema_alpha: 0.35
|
||
stabilize_max_jump: 60
|
||
default_depth_m: 0.10
|
||
camera_intrinsics:
|
||
fx: 425.0
|
||
fy: 425.0
|
||
cx: 640.0
|
||
cy: 360.0
|
||
# ===== 二阶段一次性拍照定位（stage2_locate_node，不做实时推理）=====
|
||
# bash ~/ros2/scripts/run_stage2_locate.sh --arm 89 # 完整流程
|
||
# bash ~/ros2/scripts/run_stage2_locate.sh --arm 89 --hover-only # 只悬停调高度
|
||
stage2:
|
||
# 对准目标孔。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]；
|
||
# 精对准同一帧有 z_wp 时改用「工件面 − 15mm」
|
||
# （3mm 挡不住噪声，会把工件面当成规尖）
|
||
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.08 # 目标离夹爪轴线超过此值就不像粗居中量 [m]；0.12 放行了 2026-10-16 的 110mm 回撤
|
||
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，放缓防震规）
|
||
# 对小孔关键点：只用像素射线交到 workpiece_surface_z_m，不读深度图。
|
||
hole_visual_only: true
|
||
# 本分支实验（exp/hole-keypoint-once）：true=悬停拍一次，框心→k1 一次走完
|
||
# （40mm 上限）。main 保存点是 false（多走多拍）。
|
||
hole_keypoint_once: true
|
||
# true=悬停走完后一条厂家 MoveL 贴面（不补拍）。对照 37b85aa 改回 false。
|
||
hole_once_lock_xy_descend: true
|
||
# 贴面相对悬停：沿 -look 走 k×剩余下降 [m/m]。0=笛卡尔锁 XY 只降 Z。
|
||
# 0.018 在框心改平面交点后两条链路都 +1.3mm X 过补，插不进。
|
||
once_ff_look_per_m: 0.0
|
||
final_gap_mm: 2.0 # 不进孔时的贴面目标：规尖离面 [mm]。笛卡尔锁 XY
|
||
# 时法兰 Z = 工具长 + 工件面 + 间隙。CLI --final-gap 覆盖。
|
||
hole_entry_mm: 1.0 # 规尖停在工件上表面以下 [mm]。2026-10-19 12:20 左臂：
|
||
# 4mm 指令在面下 1.6mm 被挡住（厂家 302064 到不了位），
|
||
# 流水线接着空等 90s。1.0mm 留在挡住点之上，仍进孔口。
|
||
# 0 = 停在 final_gap（面上）。上限 8mm（板厚 12mm）。
|
||
observe_gap_mm: 15.0 # --no-descent 时的观测下降：只降到离面这么高 [mm]，
|
||
# 方便看规尖偏没偏。必须 ≥ final_gap_mm。
|
||
# 左臂89：悬停间隙约 71.4mm（0.320−0.226587−0.022）⇒ 观测下降约 56.4mm。
|
||
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.226587 − 工件面 0.022 = 71.4mm 间隙 ⇒ 降 69.4mm 到 2mm。
|
||
# 三道**物理/几何**保险：descent_budget_m（配置上限）+ 腕部 FT 接触检测（停+退 1mm）
|
||
# + 任一步 _goto 走不到位就停。
|
||
# 缺几何（工具长/工件面没配）时**拒绝下降**并报错（保持悬停），绝不拿测距去赌。
|
||
final_descent_enable: true # true=对准后降到 final_gap_mm；false / --no-descent=只降到 observe_gap_mm
|
||
verticalize_tol_deg: 0.8 # 垂直朝下判据 [deg]
|
||
settle_sec: 0.45 # 到位后静止多久再拍照
|
||
snap_frames: 5 # 每次精拍取中值的帧数
|
||
min_depth_m: 0.06 # 有效深度范围（防孔底/桌面拉偏 XY）
|
||
max_depth_m: 0.60
|
||
linger_sec: 8.0 # 完成后继续发布结果/叠图的时长
|
||
|
||
# ===== 三阶段柔顺搜孔（stage3_search_node，Orisys 触觉 + 力闭环螺旋）=====
|
||
# 前置：二阶段已完成，臂悬停在目标孔上方。先 run_tactile.sh --cli 看力读数量级再调阈值。
|
||
# bash ~/ros2/scripts/run_stage3_search.sh --arm 89 # 搜孔+落座
|
||
# bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --depth 0 # 只搜孔定心不插入
|
||
# bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --depth 6 --pitch 1.0 # 螺纹规旋入
|
||
stage3:
|
||
# —— 力阈值全是「相对静止均值的增量」（Orisys 读数是形变任意单位且基线会漂移，
|
||
# 节点启动时自动测 1.2s 静止均值 μ0/方差 σ0，看日志「触觉基线/派生阈值」两行）——
|
||
touch_delta: 250.0 # 触面判定：fnormal > μ0 + max(4σ0, 本值)
|
||
press_delta: 200.0 # 轻触目标按压量（触面阈值之上再加这么多）
|
||
max_press_delta: 900.0 # 螺旋中允许的最大按压（超过→力闭环抬起）
|
||
safe_press_delta: 2000.0 # 安全上限：fnormal > μ0+本值 立即抬升中止
|
||
shear_delta: 600.0 # 切向拖拽保护：shear > 静止切向均值+本值 → 贴面时微抬 0.15mm（Z 钉在触面）
|
||
insert_press_delta: 1200.0 # 插入力上限（触面阈值之上；超过回退微抖重试）
|
||
kp_z_mm_per_unit: 0.001 # Z 力闭环增益：每单位读数偏差调整的毫米数
|
||
approach_step_mm: 0.8 # 触面前每步下降量（驱动死区已修，0.5mm 微步可跟踪；小步=触面冲击小）
|
||
enter_probe_mm: 2.0 # 触面深于此值→疑似直接入孔，四向微探验证后跳过螺旋
|
||
mouth_lift_mm: 2.0 # 直接入孔后抬到孔口倒角段再定心（深插横探会别死孔壁）
|
||
baseline_min: 8.0 # 触觉基线健康窗下限（<此值=几乎全 0，tare/断流）
|
||
baseline_max: 1500.0 # 上限（>此值=近饱和，接触余量不足）——出窗即中止并提示重启触觉
|
||
approach_ft_touch_n: 6.0 # approach 腕部 |F| 旁路触面判据（标定牛顿；自由空气≈2-3N）
|
||
approach_max_mm: 60.0 # 从当前位姿最多下降这么多（找不到面就中止；悬停被抬高后仍有余量）
|
||
spiral_pitch_mm: 1.0 # 螺旋螺距（< 通止规直径，保证重叠覆盖）
|
||
spiral_step_mm: 0.5 # 沿螺旋的弧长步长
|
||
spiral_r_max_mm: 4.0 # 最大搜索半径（覆盖二阶段残差 1.5mm + 余量）
|
||
spiral_expand_max: 1 # 找不到孔时扩大半径的次数（残差≤1.8mm 时代码不再扩大）
|
||
spiral_expand_step_mm: 2.0
|
||
drop_ratio: 0.45 # 入孔判据：fnormal 相对接触均值跌落比例
|
||
drop_frames: 2 # 跌落需持续的帧数（防毛刺）
|
||
center_scan: false # 入孔后十字微扫；默认关（0.8mm 扫会把已入孔的规刮偏）
|
||
center_scan_mm: 0.3
|
||
insert_depth_mm: 0.0 # 默认只搜孔+轻压，不拧入；拧入阶段再用 --depth
|
||
insert_retry: 3
|
||
screw_pitch_mm: 0.0 # 螺纹通止规螺距：>0 右旋 / <0 左旋 / 0=不旋（光面规）
|
||
search_speed: 0.03 # 搜孔笛卡尔速度 [m/s]
|
||
settle_sec: 0.25 # 每个微步后的静止时间
|
||
tactile_timeout_s: 3.0 # 触觉话题断流多久中止
|
||
ft_watchdog: true # 订阅 /<sn>/flexiv_robot_states 做腕部力看门狗
|
||
ft_safe_n: 35.0 # 腕部 |F| 安全上限（真牛顿，标定过的；连续3拍超限才中止）
|
||
linger_sec: 8.0 # 完成后继续发布结果的时长
|
||
|
||
# Orisys 视触觉（夹爪左右指摄像机，不是腕部 D405）。SDK 单独 pip 安装。
|
||
# bash ~/ros2/scripts/run_tactile.sh --cli
|
||
# bash ~/ros2/scripts/run_tactile.sh
|
||
# =============================================================================
|
||
# 感知来源：伺服用的"视觉测量"由谁产生
|
||
#
|
||
# yolo 本体 perception_node（YOLO/ONNX，需要 torch/ultralytics）—— 真机
|
||
# truth 仿真真值：**整条视觉链都由场景几何算出**，不跑任何推理
|
||
# · 一阶段目标点：arm_sim_gazebo 的 sim_truth_perception
|
||
# · 二阶段孔位 ：arm_sim_gazebo 的 truth_holes（投影 <visual>*_ring）
|
||
# external 由外部节点按**同一话题**（visual_servo/filtered_point）提供，
|
||
# 本框架不起感知进程（二阶段仍走检测器）
|
||
#
|
||
# ⚠️ 别和 yolo.backend 混了：那个是"**怎么**推理"（tensorrt/onnx），
|
||
# 这个是"**谁**给出测量"。
|
||
#
|
||
# 为什么需要这个开关：仿真环境（ros_gz_sim conda）没有 torch/ultralytics，
|
||
# 离线也装不上，perception_node 一起就死；但仿真里工件位姿本来就是已知的，
|
||
# 没必要也没法做推理。切到 truth 后，流程能在仿真里验"控制与接口"那一半，
|
||
# 而**感知那一半留给真机**——这条边界写在 devices/sim/gazebo/README.md 的
|
||
# 「仿真里哪些模块用真值替代」一节。
|
||
# =============================================================================
|
||
perception:
|
||
source: "yolo"
|
||
# 一阶段跟踪用哪台相机。wrist=腕部 D405（主跟踪）+ 顶视 D435 一次性兜底；
|
||
# overhead=整段只用 D435（实验分支才切）。非法值启动即报错，不会静默退回。
|
||
track_camera: "wrist"
|
||
# 画面内最高检测 conf 低于此 → 马上拍一张 D435，把工件 / k0 定到基座系。
|
||
# 与 perception / servo 共用这一处，不许两边各写一个 0.5。
|
||
wrist_handoff_conf: 0.5
|
||
# 顶视深度量程 + 一次性参数。腕部那组 0.04–0.55m 会把 D435 的点全丢掉。
|
||
# 缺键启动即红，不许借腕部量程、不许用代码默认冷却时间。
|
||
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" # 顶视与腕部视角差太大，pose 的 k0 经常看不见；优先 k0，否则框心
|
||
hold_point_on_miss: true # 仅 overhead 整段跟踪时用；混合模式一次性不续发
|
||
oneshot_conf: 0.5 # D435 这一张也必须 conf≥此值，不许拿噪声框当 K0
|
||
cooldown_s: 1.5 # 两次一次性之间的最短间隔，避免跟 D405 抢同一条 YOLO 队列
|
||
frame_max_age_s: 0.4 # 彩图/深度比这更旧就拒绝，不许拿过期图反投影
|
||
oneshot_hold_s: 8.0 # 伺服保持这一次设定的最长时间；超时丢掉，等下一张或 D405
|
||
truth:
|
||
# 真值目标物体。留空 = 场景文件里的 operating.part_object（推荐留空：
|
||
# "哪个是工件"是场景的知识，换场景不该改这里）。
|
||
object: ""
|
||
# 目标点相对物体**顶面中心**的偏移 [m]，在物体局部系里（会跟着 yaw 转）。
|
||
# 例：把目标从板中心挪到 k1 孔 → [0.0, -0.029, 0.0]
|
||
offset_m: [0.0, 0.0, 0.0]
|
||
# =============================================================================
|
||
# 通止规装配（看板 S00–S11）：bash ~/ros2/scripts/run_gauge_assembly.sh --arm 89
|
||
#
|
||
# 这一段全是**会动的量**，也是现场最常改的地方。改完不必改代码：
|
||
# 解析在 arm_application/gauge_config.py（键名、单位、类型都在一张表里校验），
|
||
# 拼错键名会直接报错并给出最相近的候选名，不会静默退回默认值。
|
||
#
|
||
# 单位体现在键名后缀上（_deg/_mm/_m/_s/_pct/_delta），三张表逐条校验；
|
||
# 带 _delta 的是**力增量**（Orisys 读数是形变任意单位且基线会漂移，
|
||
# 绝对值没有意义），必须现场标定：
|
||
# bash ~/ros2/scripts/run_tactile.sh --cli # 看读数量级
|
||
# 启动时节点会打印本段**实际生效**的值（含派生结论），先核对再上电。
|
||
# =============================================================================
|
||
gauge_assembly:
|
||
# ---- 总开关 ----
|
||
holes_total: 2 # 一次任务装几个孔（k1/k2）；--holes N 可临时覆盖
|
||
dry_run: false # true = 只验编排与自检，不发任何运动指令
|
||
# E1（掉件）重放时等人工确认 /gauge/reload_ok 的最长时间 [s]。超时=中止。
|
||
# 不是"再等等"：臂停在 home，让人来处理比继续跑安全。
|
||
reload_timeout_s: 180.0
|
||
# S00 自检每项（相机/触觉/夹爪状态）等"第一帧真数据"的最长秒数。
|
||
# 只看话题名会假阳性（发布者不在也算在），所以必须等真数据。
|
||
boot_timeout_s: 30.0
|
||
|
||
# ---- S01 回 home 后按工具系逆时针预绕到极限角 ----
|
||
# 预绕是为了给 S08 的棘轮留出回转行程：不预绕，拧两圈就贴关节限位。
|
||
extreme_angle_deg: 150.0 # 想转多少度（按工具系逆时针）
|
||
extreme_speed_deg_s: 10.0 # 关节角速度 [deg/s]。规装配预绕，以及插孔后首轮
|
||
# 顺时针进孔，都用这个数。首轮逆时针出孔、空回各轮的
|
||
# 正反转不读它，走接口上限 0.90 rad/s。
|
||
unwind_open_permille: 400 # 第三步松爪只开到这个千分比。0=合，1000=全开。
|
||
# 2026-10-19 从 200 改成 400，只张开这个量。
|
||
unwind_lift_mm: 3.0 # 第三步张开后只抬的 TCP Z [mm]（0.3cm）。
|
||
# 2026-10-19 指令 5mm、ZFine 后实走 +4.86mm，下降 -4.70mm，差 0.16mm。
|
||
# 空回到限位后，下降量用这次实测的抬高，不另写一个数。
|
||
# 超过 5mm 拒绝。
|
||
extreme_direction: -1 # +1=逆时针 / -1=顺时针（2026-10-19 真机 A7 转反了，关节角增大是顺时针）
|
||
extreme_margin_deg: 3.0 # 距关节限位留的余量 [deg]；撞限位不算失败（按 JOINT_LIMIT 继续）
|
||
# 但余量太小会让规划器频繁拒收，表现为"偶尔转不到位"
|
||
|
||
# ---- S02 夹持确认（预留，默认跳过）----
|
||
# 现在的触觉判据与 D435 手眼都还没就绪，先留接口：开成 true 时，
|
||
# 夹爪状态字必须明确报"夹住了"，否则 S02 判失败并中止。
|
||
grip_check: false
|
||
grip_hold_s: 0.5 # 合爪后等多久再读状态 [s]
|
||
|
||
# ---- S03 手外粗定位（S04 跟踪的兜底：跟踪丢了才来）----
|
||
# 前提是手外相机做过 eye-to-hand 标定：handeye/results/<相机>_eye_to_hand.yaml。
|
||
# 没有那个文件时 S03 会明确报"缺哪个文件、怎么生成"并回 S04，不做假装成功的反投影。
|
||
coarse_enabled: true
|
||
# 手外相机名：**以 cameras 段里 role=overhead* 且 arm 与本臂一致的那条为准**
|
||
#（就是 cameras 注册表，见 gauge_assembly_node._d435_ns）。这一项只在
|
||
# "注册表里没有手外相机"时当兜底；两者不一致会在 S00 打告警并以注册表为准。
|
||
# 所以真机/仿真各跑各的时候**不用**改这里 —— 曾经写死 d435 让仿真去查真机的相机。
|
||
coarse_camera: "d435"
|
||
coarse_hover_m: 0.25 # 粗定位后悬停在工件上方多高 [m]（可调）
|
||
coarse_conf: 0.35 # 工件检测置信度阈值（0~1）
|
||
coarse_timeout_s: 20.0 # 等手外相机出图的最长秒数（相机刚起来要一两秒）
|
||
|
||
# ---- S05 精拍定位：等画面稳了再拍 ----
|
||
# 判据是"所有采样点到均值的距离 ≤ 容差"，不是"相邻两帧差 ≤ 容差"：
|
||
# 跟踪抖动是零均值的，相邻帧之差会在画面明明在晃时判成稳定，拍出糊图。
|
||
snap_stable_s: 1.0 # 连续观察多久 [s]
|
||
snap_tol_px: 2.0 # 允许的抖动半径 [px]（越小越严，越难满）
|
||
snap_tol_mm: 0.4 # 同上，但特征=法兰位移时的容差 [mm]
|
||
snap_min_frames: 6 # 至少采到多少帧才敢说"稳定"
|
||
# 用哪个特征判"画面稳定了"：
|
||
# auto = 按 perception.source 自动选（真机 yolo→bbox；仿真真值/外部→motion）
|
||
# bbox = 工件检测框中心（像素）。需要检测器真的出框：仿真里合成图过不了
|
||
# 真机训练出来的模型（实测最高分 0.017），选它会让 S05 空转到超时
|
||
# motion = 法兰位移（mm）。相机装在法兰上，法兰不动画面就不动 —— 不依赖模型
|
||
snap_stable_feature: "auto"
|
||
|
||
# ---- S06 正悬停（悬停高度不在这里：见 servo.hole_hover_z_m）----
|
||
align_tol_mm: 1.0 # 对位残差容差 [mm]；超了回 S05 重拍（不是硬往下走）
|
||
# 孔口径 mm 级时别放大这个值：放大=直接蹭孔壁
|
||
|
||
# ---- S07 柔顺接触：步进下压，切向力突变即停 ----
|
||
contact_speed_m_s: 0.02 # 每步下压速度 [m/s]；这一步必须慢，快了就是一次冲击
|
||
contact_step_mm: 0.5 # 每步下压量 [mm]（越大越快，越小越不会过冲）
|
||
contact_max_depth_mm: 35.0 # 最多下探多少 [mm]；到了还没接触判失败（不会一直压）
|
||
# 为什么是 35 而不是 25：判"到底了"的是**台肩落到工件
|
||
# 上表面**那一下，而台肩在规尖上方 30mm（通端长 30，
|
||
# 见 models/gauge/model.sdf）。工件板厚 12mm，所以要
|
||
# 下压 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.245 # 工具尖端（规尖）在法兰下方的距离 [m]
|
||
# **这是实测值，不是推算值**：夹住规之后直接读
|
||
# TF 的 flange z − 规尖 z。
|
||
# 仿真里 `scripts/sim_load_gauge.py` 每次上料后
|
||
# 会自己把它算出来打印（"实测工具几何"那一段），
|
||
# 现场照着填即可 —— 真机同法（夹住规后直接读 TF）。
|
||
#
|
||
# 推算值（TCP 在法兰下方 0.200 + 规尖在指腹面下方
|
||
# 0.043 = 0.243）与实测 0.245 只差 2mm，说明推算
|
||
# 思路是对的；但**别再用推算**，因为下面这个理由：
|
||
#
|
||
# ⚠️ 这个数天生有 ±几毫米的不确定度：规是**靠摩擦**
|
||
# 夹住的，每次上料在爪里的位置都会差几毫米。
|
||
# 所以它只用来**诊断**（悬停时尖端离面多高、下探
|
||
# 预算够不够），**不能**当硬约束 —— S07 的自检
|
||
# 因此是"告警 + 自动放大预算"，不是"直接失败"。
|
||
# 要更准只能改夹持方式（定位台肩/加挡块），那是
|
||
# 机械方案，不是改这个数。
|
||
workpiece_surface_z_m: 0.0 # 工件**上表面**的世界 z [m]（对孔基准面）
|
||
# 0 = 未知 → 运行期用 stage2 实测的孔位 z 兜底
|
||
#（孔就在上表面上）。仿真里实测是 0.412。
|
||
# 只在"stage2 还没跑就想知道几何对不对"时才需要手填。
|
||
contact_tangential_delta: 300.0 # 触觉切向力突变阈值 [au，任意单位] —— **兜底判据**
|
||
# ⚠️ 这是"触觉"的单位，不是牛顿：真机 Orisys 输出的
|
||
# 是形变任意单位（厂商不给牛顿换算），仿真侧由
|
||
# force_scale 定义尺度。所以这个数**不能**与下面
|
||
# contact_axial_delta_n 比大小（两者量纲不同）。
|
||
#
|
||
# 它为什么只能是兜底：通止规靠**夹持摩擦**抱住，轴向力
|
||
# 传到指垫必须过摩擦面，上限 = µ·N ≈ 0.25×46N ≈ 23N
|
||
# （两指合计）。而台肩落面一步（0.5mm × 5e4 N/m）就
|
||
# 产生 25N —— 必然打滑，所以触觉**物理上读不到**那一下
|
||
# 的力，读数停在 0（而它是对的）。拿它当主判据，阈值调
|
||
# 多大都不对：调小被噪声骗，调大永远达不到。
|
||
# 2026-09-24 就是照"阈值调高了"改的，白费一轮。
|
||
# 详见 gauge_fsm.wrench_touch 与 docs/GAUGE_ASSEMBLY.md §6。
|
||
contact_axial_delta_n: 8.0 # 腕部 FT 轴向力突变阈值 [N] —— **首选判据**
|
||
# 腕部 FT 在夹持摩擦的**上游**（工件→规→夹爪→法兰
|
||
# →腕部关节），工具在爪里打不打滑都测得准，单位是真牛顿。
|
||
# 为什么只要 8N：这是"碰到了没有"、不是"压到位了没有"。
|
||
# 台肩一落面，接触刚度 5e4 N/m 意味着过压 0.1mm 就有
|
||
# 5N、0.2mm 有 10N —— 8N 对应约 0.16mm 过压，**在打滑
|
||
# 之前**就判到了（打滑上限 ≈23N ≈ 0.46mm），留 3 倍余量。
|
||
# 现场标定：空载慢慢压到台肩落面，看日志里每 5 步打印的
|
||
# 「腕部轴向」涨到多少，取它的 1/3（要能区分"擦到孔壁"
|
||
# 与"台肩落面"）。
|
||
# 数据源话题：servo.ft_wrench_topic（默认
|
||
# /<sn>/ft_sensor_wrench，真机与仿真同名同类型同单位）。
|
||
contact_relief_mm: 0.5 # 判到接触后的**卸载回退** [mm] —— 别调成 0
|
||
# 为什么必须有这一步（2026-09-24 一整轮排查换来的）：
|
||
# 位置控制的臂撞上硬面时不会"刚碰到就停"，它会顶到
|
||
# 自己的**出力上限**才停住。仿真实测：0.5mm 的一步压在
|
||
# 工件上，腕部 FT 直接读到 **+244N**，而臂这一步只走了
|
||
# 0.09mm。也就是说"判到接触"的那一刻，臂上顶着的力
|
||
# 与 contact_axial_delta_n **没有关系**（阈值只决定
|
||
# 什么时候承认接触）。
|
||
# 而规是靠**夹持摩擦**抱住的：轴向传力上限 µ·N，两指
|
||
# 合计 ≈73N（µ=0.8、握力 46N）。244N 远超它 ⇒ 规被顶出
|
||
# 爪外。本次现象：S07 判到接触 → S08 一转 → 规掉地上
|
||
# → 报 E1，而 E1 的日志只说"通止规掉了"，看不到真因。
|
||
# 0.5mm 怎么来的：判到接触时真实过压量通常只有 0.0x mm
|
||
# （5e4 N/m 时 10N ≈ 0.2mm），回退 0.5mm 足够把力放
|
||
# 干净，而规尖仍在孔里（通端穿板 18mm），不会脱出。
|
||
# 放不干净会在日志里明确告警（残余 > contact_hold_n）。
|
||
contact_hold_n: 5.0 # 卸载后允许的残余轴向力 [N]
|
||
# 超了不失败，但会**大声报**：它说明"夹持摩擦力不够
|
||
# 承受这次接触"，接着 S08 一转就会掉件。
|
||
# 真修法只有两条：① 降峰值（调小 contact_step_mm 或
|
||
# 加大 contact_relief_mm）；② 加握力（改夹爪合拢力度/
|
||
# 指垫摩擦）。**不要**去调 contact_axial_delta_n。
|
||
contact_safe_delta: 2000.0 # 法向安全上限 [任意单位]：超了立刻抬起并中止(E2)
|
||
# 必须 > contact_tangential_delta，否则自检就报矛盾
|
||
|
||
# ---- S08 棘轮拧入（慢转 → 松爪 → 快回转 → 夹紧，循环）----
|
||
# 停止判据：切向力 > screw_shear_delta **且** 法向力突增 > screw_normal_delta。
|
||
screw_step_deg: 120.0 # 每次慢转多少度。
|
||
# **硬约束**：2×step + 2×余量(8°) ≤ 关节7 行程。
|
||
# Rizon4/10 的关节7 是 ±175° → 行程 350° → step ≤ 167°。
|
||
# 填 180° 会**必然**贴限位（慢转+回转=360°>350°），
|
||
# 测试 test_ratchet_step_of_180_never_fits_the_rizon_joint7_range
|
||
# 与 S08 开头的行程核算都会拦；120° 留足余量吸收
|
||
# "回转每轮比指令少走 4~5°"的亏欠。
|
||
screw_speed_deg_s: 30.0 # 慢转速度 [deg/s]
|
||
screw_back_speed_deg_s: 120.0 # 松爪后的空转回转速度 [deg/s]（爪里没东西，可以快）
|
||
screw_max_cycles: 12 # 最多几轮（12×120°=1440° = **4 整圈**，够 M6 螺纹规走完）。
|
||
# 必须满足 screw_max_cycles × screw_step_deg 是 360° 的整数倍：
|
||
# 否则规停在螺纹中间，S09 拧出也少转几分之一圈。
|
||
screw_normal_delta: 400.0 # 拧到位时的法向力突增 [任意单位]（螺纹到底/顶住）
|
||
screw_shear_delta: 600.0 # 拧到位的切向力增量 [任意单位]
|
||
screw_grip_force_pct: 50.0 # 夹紧力百分比 [%]（**只有契约后端生效**：仿真/新驱动）
|
||
screw_grip_speed_pct: 50.0 # 夹紧速度百分比 [%]（同上；真机 dh_pgea 驱动不支持这两个参数）
|
||
screw_release_wait_s: 0.3 # 松爪后等多久再回转 [s]（等爪真的张开，否则会带着规磨）
|
||
|
||
# ---- S09 反向拧出 ----
|
||
unscrew_extra_cycles: 1 # 比拧入多转几轮，确保螺纹完全脱开
|
||
|
||
# ---- 力判据怎么调（现场最常问）----
|
||
# 1) 先看量级：bash ~/ros2/scripts/run_tactile.sh --cli，手压一下看读数变化；
|
||
# 2) S07 的接触判据是"相对基线的增量"，基线在 S06 悬停时（还没碰工件）测；
|
||
# 3) 太灵敏（刚靠近就停）→ 调大 contact_tangential_delta；
|
||
# 不灵敏（压很深才停）→ 调小；同时看启动日志里的基线/实测值再定。
|
||
|
||
tactile:
|
||
backend: "orisys" # orisys | mock
|
||
# 传感器型号。必须与 devices/sensors/orisys/models/ 下的子目录名一致。
|
||
model: "dd03"
|
||
# 型号配置随 SDK 一起放：devices/sensors/orisys/models/<型号>/config.json
|
||
config_file: "/home/cc/ros2/devices/sensors/orisys/models/dd03/config.json"
|
||
# 用 by-path 稳定路径（插哪个 USB 口就认哪个口，不受 video 编号漂移影响）。
|
||
# 当前：3.4.1=左指 3.4.2=右指；若左右反了，互换两行即可。
|
||
left_video: "/dev/v4l/by-path/pci-0000:00:14.0-usb-0:3.4.1:1.0-video-index0"
|
||
right_video: "/dev/v4l/by-path/pci-0000:00:14.0-usb-0:3.4.2:1.0-video-index0"
|
||
cuda: false # 两路进程抢 GPU 上下文会互相拖慢；CPU 解算仅 ~9ms/帧，足够
|
||
isstitch: true
|
||
infer_backend: "cpu" # cupy | opencl | cpu；双路并行用 cpu 最稳
|
||
baseline_seconds: 2.0
|
||
filter_alpha: 0.25
|
||
motion_threshold: 0.0
|
||
target_fps: 30.0
|
||
publish_hz: 20.0
|
||
# 视触觉图像与力矢量场的发布频率。0 = 关闭（默认）。
|
||
# 三力走 30fps 的轻量队列；图像每帧几百 KB，单独走一条 maxsize=1 的队列，
|
||
# 打开它不会影响三力的时序。需要看形变图/矢量场时设成 5~10 即可。
|
||
image_hz: 0.0
|
||
rotation_config: "" # 左右共用；有分指文件则用下面两项
|
||
left_rotation: "" # 例：orisys SDK 的 rotation_config_0.json
|
||
right_rotation: ""
|
||
|
||
go_home:
|
||
pose: "home"
|
||
speed: 0.25
|
||
|
||
network:
|
||
# 2026-09-29 改线：两台臂 → 交换机 → USB-C 千兆网卡（ASIX AX88179）
|
||
# MAC 6c:1f:f7:d0:6f:17。内核现在叫 eth0（不再用 enx6c1ff7d06f17）。
|
||
# 本机 192.168.2.10 ↔ 063989(左89)=.101 / 063991(右91)=.100
|
||
# NM 配置名 robot-usb-lan（静态 .10/24，不当默认路由）。
|
||
# 说明：enp4s0 仍是办公网（192.168.1.x）；两台臂已不在 enp4s0 上。
|
||
prefer_iface: "eth0"
|
||
|
||
# 仿真平台（当前 Gazebo 尚未接线，详见 devices/sim/gazebo/README.md）
|
||
# 设计约束：仿真与真机共用同一套参数（servo.* / hole_detect.stage2.*），
|
||
# 只有物理引擎自身的参数放在这里，避免「仿真调好的值搬到真机不生效」。
|
||
sim:
|
||
# 这一段由 arm_sim_gazebo.sim_site 真正读取（此前是"声明了但没人读"，
|
||
# 改了毫无反应也不报错）。三个键的接线状况**不一样**，务必看清：
|
||
#
|
||
# world ✔ 已接线。优先级：命令行 --world > 这里 > 内置默认。
|
||
# 相对路径按**仓库根**解析。
|
||
# physics_enable ✘ 未接线。世界文件里的重力/步长是写死的。若这里写的值
|
||
# 与世界的实际状态不符，启动时会打印告警（不再静默）。
|
||
# joint_prefix ✘ 未接线。前缀由 urdf/arm.sim.urdf.xacro 的 arm_prefix
|
||
# 决定，且 config/controllers.yaml 里硬编码了 sim_joint1..7。
|
||
# 写了个不同的值会告警。要真改，三处必须一起改。
|
||
#
|
||
# 另外注意：`run_sim.sh` 走的仍是 mock 驱动（假数据 + RViz），不是物理仿真；
|
||
# 物理仿真入口是 `scripts/run_sim_gz.sh`（Gazebo）。两者互不影响。
|
||
platform: "gazebo"
|
||
world: "devices/sim/gazebo/worlds/assembly.world"
|
||
physics_enable: true
|
||
joint_prefix: "sim_" # 仿真关节前缀，必须与真实臂前缀区分
|