真机落地实测修的三处：参数类型不一致导致节点启动即崩、权重目录判据、相机日志
1) site.yaml 的整数把节点写死（ros_node.TolerantNode）
现场 site.yaml 写 `ae_limit_us: 8000`（YAML 整数），脚本按 `-p ae_limit_us:=8000`
传参，而节点 declare 的是 double —— rclpy 严格比对类型，节点**启动即崩**：
InvalidParameterTypeException: Trying to set parameter 'ae_limit_us'
to '8000' of type 'INTEGER', expecting type 'DOUBLE'
现场看到的只是「顶置 D435 起不来」，报错里一个字都没提 site.yaml，而
`8000` 与 `8000.0` 在运维眼里是同一个数。同一族的雷还有 width 848.0→int、
emitter_enabled 1→bool、带不带引号的序列号。
修法是新增应用层统一基类 TolerantNode：在**声明阶段**把覆盖值的类型按
声明类型收敛（异常就发生在声明阶段，所以 `float(...)` 这种事后 cast 救不了）。
全部应用节点改继承它；补 test_ros_node.py。
2) 权重目录判据（weights_layout）
模型文件「在不在、名字对不对」以前散在几处各判一半，缺文件时的现象是
推理静默退回随机权重。收成 weights_layout.py 一条判据 + scripts/weights.py 调用。
3) 相机日志：USB 降速要说清怎么办
realsense_color_pub 协商到 USB 2.1 / 480Mbps 时只打一行速率，没人知道该做什么；
现在直接给可执行的处置（换线/换口/SuperSpeed 不复用）。realsense_view 同步。
4) stage2_locate：接上 hover_z_guard 与 tool_tip_below_flange_m
悬停高度必须让工具尖端留在工件面上方（09-24 的教训：悬停时尖端已钻进工件
25mm，S05 横向对准把工件拖走 92mm）。step2 侧也按同一判据兜住，且工具长
允许由启动参数覆盖（仿真上料后实测得到）。
5) config/site.example.yaml 与 site.yaml 重新同步
上一批给夹爪加了 auto_init: false，模板没跟着重跑，TestExampleParity 因此失败。
重跑 scripts/gen_site_example.py 同步（模板是生成物，不手改）。
测试：全仓库 1085 passed（此前 1 failed）。
This commit is contained in:
arm_dev
+136
-21
@@ -49,6 +49,15 @@ robot:
|
||||
sn: "<序列号>"
|
||||
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/USER/ros2/src/arm_control/config/named_poses_89.yaml"
|
||||
paths:
|
||||
handeye_file: "/home/USER/ros2/handeye/results/89_eye_in_hand.yaml"
|
||||
@@ -69,6 +78,12 @@ robot:
|
||||
# 法兰 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，
|
||||
# 对"轻碰"更灵敏，但对负载变化更敏感）。
|
||||
# >>> 仿真那台臂（Gazebo）也登记成 robots[] 里的一台 <<<
|
||||
# 为什么必须登记：应用是按"当前臂"的名字去查 frame / joint / 相机话题的
|
||||
#（`sim_env.sh` 注释里说的"仿真与真机共用同一套 site.yaml"）。仿真的 TF 与关节
|
||||
@@ -114,22 +129,61 @@ robot:
|
||||
# 一阶段跟踪高 = 悬停 + 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
|
||||
ee_z_m: 0.68 # 一阶段跟踪锁高（法兰，世界系）
|
||||
hole_hover_z_m: 0.632 # 二阶段悬停高：= 0.412 + 0.17 + 0.05
|
||||
safe_z_m: 0.75 # 安全抬升/复位高度（高于跟踪高）
|
||||
z_ref_m: 0.632 # 没有测量时的兜底 Z：绝不能是 0.22（会撞工件）
|
||||
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.75]
|
||||
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-XXXXXX"
|
||||
arm: right
|
||||
ip: "192.168.2.XXX"
|
||||
@@ -658,32 +712,93 @@ gauge_assembly:
|
||||
# 于是 S07 一路压到顶都等不到"台肩落面"那一下，报"没触发接触判据"，
|
||||
# 看起来像力阈值标错了 —— 实际是几何错。真机同参数同结果。
|
||||
# S07 启动时会把这个差值算出来印在日志里（负值会明确标注"已经插进工件了"）。
|
||||
tool_tip_below_flange_m: 0.243 # 工具尖端（规尖）在法兰下方的距离 [m]
|
||||
# 怎么量/怎么算（这个 0.243 的来历）：
|
||||
# TCP（两指中点）在法兰下方 0.200
|
||||
# + 规尖在 TCP 下方 0.043
|
||||
# 为什么是 0.043 而不是规"夹持段中心到尖端"的 0.0555：
|
||||
# 规尖要对齐的是**指腹接触面**，而指腹接触面不在
|
||||
# TCP 原点上 —— URDF 里 inner_finger_pad_joint
|
||||
# z=0.0235、finger_tcp_joint z=0.0360（同父链节
|
||||
# 同方向）→ 指腹中心比 TCP 高 12.5mm。
|
||||
# 所以 0.0555 − 0.0125 = 0.043。
|
||||
# ⚠️ 换夹爪/换规必须重量（量法：夹住规之后用
|
||||
# TF 直接读 flange→规尖 的 z 差，别靠推）。
|
||||
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。
|
||||
#（孔就在上表面上）。仿真里实测是 0.412。
|
||||
# 只在"stage2 还没跑就想知道几何对不对"时才需要手填。
|
||||
contact_tangential_delta: 300.0 # 接触判据：两路切向任一相对基线增量超过它 [任意单位]
|
||||
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: 180.0 # 每次慢转多少度；要 ≤ 预绕角（否则第一轮就贴限位）
|
||||
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×180°=2160°，够 M6 螺纹规走完）
|
||||
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 # 夹紧力百分比 [%]（**只有契约后端生效**：仿真/新驱动）
|
||||
|
||||
Reference in New Issue
Block a user