仿真场景：台面开 90mm 通槽，让工件的贯通孔真能穿过
现象（非常隐蔽，容易被误判成阈值问题）：
通止规的通端长 30mm，工件是 12mm 厚板、孔是**贯通孔** —— 真机上通端要
穿过板再伸出去 18mm，靠**台肩落到工件上表面**那一下判"到底了"。
但台面原本是一整块、工件平放在上面 → 孔下方被堵死，规只能进 12mm 就顶
在台面上。于是 S07 一路压到上限都等不到台肩落面的大接触力，报
"下探 N mm 都没触发接触判据"然后 ABORT。
力也不是完全没有（顶台面时切向读 30~45N），所以看起来像"阈值调太高"。
修法：台面沿 Y 开一道 90mm 宽通槽（|y| ≤ 45mm），南北两块各留 5mm 承压面
（工件 100mm 宽，跨在槽上；板是刚体只受重力，够用）。桌腿视觉同步分两块，
别把通槽在画面上堵回去。槽**只开在工件孔位所在的 Y 带**，台面高度一点没动
→ 相机距离、孔检测像素门槛、site 的悬停高度全都不用改。
顺带：contact_max_depth_mm 25 → 35。判"到底了"的是台肩，而台肩在规尖上方
30mm（通端长 30），25mm 的上限**根本到不了**，只会白报超时。
35 = 30（台肩落面）+ 5（对位残差/板厚公差余量）。
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
arm_dev
+138
-3
@@ -55,7 +55,9 @@ robot:
|
||||
handeye_waypoints: "/home/USER/ros2/handeye/poses/89_eye_in_hand.yaml"
|
||||
handeye_workdir: "/home/USER/ros2/handeye/tmp/89"
|
||||
# 左臂跟踪高度（只在 --arm 89 时生效，改 91 的 servo 不影响这里）
|
||||
servo:
|
||||
# 锚点 `&servo_left`：下面的"仿真台"条目直接引用它 —— 仿真建的就是左臂，
|
||||
# 悬停/锁高必须与 89 **一模一样**，抄两份迟早漂移。
|
||||
servo: &servo_left
|
||||
ee_z_m: 0.35 # 跟踪锁高：法兰相对基座 Z [m]
|
||||
min_ee_z_m: 0.20
|
||||
safe_z_m: 0.45
|
||||
@@ -67,6 +69,67 @@ robot:
|
||||
# 法兰 Z [m]；要兼顾：够低拍清 k1/k2 螺纹孔，够高通止规尖不碰工件。
|
||||
# 调试：bash ~/ros2/scripts/run_stage2_locate.sh --arm 89 --hover-only
|
||||
hole_hover_z_m: 0.23 # ← 二阶段悬停高度调这一行
|
||||
# >>> 仿真那台臂（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/USER/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）。
|
||||
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（会撞工件）
|
||||
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]
|
||||
# 仿真不跑推理：感知结果由 sim_truth_perception 按已知位姿直接给出。
|
||||
# 原因：ros_gz_sim 环境里没有 torch/ultralytics，且本机离线装不上 ——
|
||||
# perception_node 一启动就死，表现为"一阶段永远不锁定"。
|
||||
# 详见 devices/sim/gazebo/README.md「仿真里哪些模块用真值替代」。
|
||||
perception:
|
||||
source: "truth"
|
||||
- sn: "Rizon4-XXXXXX"
|
||||
arm: right
|
||||
ip: "192.168.2.XXX"
|
||||
@@ -191,6 +254,33 @@ cameras:
|
||||
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: "<序列号>"
|
||||
@@ -451,6 +541,34 @@ hole_detect:
|
||||
# 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"
|
||||
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
|
||||
#
|
||||
# 这一段全是**会动的量**，也是现场最常改的地方。改完不必改代码：
|
||||
@@ -492,7 +610,11 @@ gauge_assembly:
|
||||
# 前提是手外相机做过 eye-to-hand 标定：handeye/results/<相机>_eye_to_hand.yaml。
|
||||
# 没有那个文件时 S03 会明确报"缺哪个文件、怎么生成"并回 S04，不做假装成功的反投影。
|
||||
coarse_enabled: true
|
||||
coarse_camera: "d435" # 手外相机命名空间（话题 /d435/...）
|
||||
# 手外相机名：**以 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 # 等手外相机出图的最长秒数（相机刚起来要一两秒）
|
||||
@@ -502,7 +624,14 @@ gauge_assembly:
|
||||
# 跟踪抖动是零均值的，相邻帧之差会在画面明明在晃时判成稳定，拍出糊图。
|
||||
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 重拍（不是硬往下走）
|
||||
@@ -511,7 +640,13 @@ gauge_assembly:
|
||||
# ---- S07 柔顺接触：步进下压，切向力突变即停 ----
|
||||
contact_speed_m_s: 0.02 # 每步下压速度 [m/s]；这一步必须慢，快了就是一次冲击
|
||||
contact_step_mm: 0.5 # 每步下压量 [mm]（越大越快，越小越不会过冲）
|
||||
contact_max_depth_mm: 25.0 # 最多下探多少 [mm]；到了还没接触判失败（不会一直压）
|
||||
contact_max_depth_mm: 35.0 # 最多下探多少 [mm]；到了还没接触判失败（不会一直压）
|
||||
# 为什么是 35 而不是 25：判"到底了"的是**台肩落到工件
|
||||
# 上表面**那一下，而台肩在规尖上方 30mm（通端长 30，
|
||||
# 见 models/gauge/model.sdf）。工件板厚 12mm，所以要
|
||||
# 下压 30mm 台肩才落面 —— 25mm 的上限根本到不了，
|
||||
# 只会报"下探 25mm 都没触发接触判据"然后 ABORT。
|
||||
# 35 = 30（台肩落面）+ 5（对位残差/工件厚度公差余量）。
|
||||
contact_tangential_delta: 300.0 # 接触判据：两路切向任一相对基线增量超过它 [任意单位]
|
||||
contact_safe_delta: 2000.0 # 法向安全上限 [任意单位]：超了立刻抬起并中止(E2)
|
||||
# 必须 > contact_tangential_delta，否则自检就报矛盾
|
||||
|
||||
@@ -115,8 +115,29 @@
|
||||
<pose>0.55 0 0 0 0 0</pose>
|
||||
<link name="top">
|
||||
<pose>0 0 0.375 0 0 0</pose>
|
||||
<collision name="collision">
|
||||
<geometry><box><size>0.8 1.2 0.05</size></box></geometry>
|
||||
<!-- ===================================================================
|
||||
台面沿 Y 留一道 90mm 宽的通槽（|y| ≤ 45mm），台面分成南北两块。
|
||||
|
||||
为什么必须留这道槽（实测踩过，现象非常隐蔽）：
|
||||
通止规的**通端**长 30mm，而工件是 12mm 厚的板、板上的孔是
|
||||
**贯通孔** —— 真机上规的通端要穿过板再伸出去 18mm，靠**台肩**
|
||||
落到工件上表面来判"到底了"。
|
||||
但台面原本是一整块，工件平放在上面 → 孔的下方被台面堵死，
|
||||
规只能进去 12mm 就顶在台面上。于是：
|
||||
S07 一路下压到上限（25mm）都等不到台肩落面的大接触力，
|
||||
报"下探 25mm 都没触发接触判据"然后 ABORT。
|
||||
力也不是完全没有（顶台面时切向读 30~45N），所以看起来像
|
||||
"阈值调太高"，很容易被误判成传感器/阈值问题。
|
||||
|
||||
槽只开在工件孔位所在的 Y 带（孔在 y = ±29mm、φ7 见方），
|
||||
不抬高工件、不动台面高度 → 相机距离、孔检测像素门槛、
|
||||
site.yaml 里的悬停高度全都不用改。
|
||||
南北两块各留 y ∈ [±45, ±600]mm，工件（100mm 宽）跨在槽上，
|
||||
两侧各有 5mm 承压面 —— 板是刚体、只受重力，够用。
|
||||
=================================================================== -->
|
||||
<collision name="collision_south">
|
||||
<pose>0 -0.3225 0 0 0 0</pose>
|
||||
<geometry><box><size>0.8 0.555 0.05</size></box></geometry>
|
||||
<surface>
|
||||
<friction>
|
||||
<ode><mu>0.8</mu><mu2>0.8</mu2></ode>
|
||||
@@ -131,19 +152,56 @@
|
||||
</contact>
|
||||
</surface>
|
||||
</collision>
|
||||
<visual name="visual">
|
||||
<geometry><box><size>0.8 1.2 0.05</size></box></geometry>
|
||||
<collision name="collision_north">
|
||||
<pose>0 0.3225 0 0 0 0</pose>
|
||||
<geometry><box><size>0.8 0.555 0.05</size></box></geometry>
|
||||
<surface>
|
||||
<friction>
|
||||
<ode><mu>0.8</mu><mu2>0.8</mu2></ode>
|
||||
</friction>
|
||||
<contact>
|
||||
<ode>
|
||||
<kp>1e7</kp>
|
||||
<kd>1e4</kd>
|
||||
<max_vel>0.01</max_vel>
|
||||
<min_depth>0.001</min_depth>
|
||||
</ode>
|
||||
</contact>
|
||||
</surface>
|
||||
</collision>
|
||||
<visual name="visual_south">
|
||||
<pose>0 -0.3225 0 0 0 0</pose>
|
||||
<geometry><box><size>0.8 0.555 0.05</size></box></geometry>
|
||||
<material>
|
||||
<ambient>0.45 0.4 0.35 1</ambient>
|
||||
<diffuse>0.55 0.5 0.42 1</diffuse>
|
||||
</material>
|
||||
</visual>
|
||||
<visual name="visual_north">
|
||||
<pose>0 0.3225 0 0 0 0</pose>
|
||||
<geometry><box><size>0.8 0.555 0.05</size></box></geometry>
|
||||
<material>
|
||||
<ambient>0.45 0.4 0.35 1</ambient>
|
||||
<diffuse>0.55 0.5 0.42 1</diffuse>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
<!-- 四条腿，纯视觉：碰撞只保留台面，省掉不必要的接触计算 -->
|
||||
<!-- 桌腿：纯视觉（碰撞只保留台面，省掉不必要的接触计算）。
|
||||
和台面一样沿 Y 分成两块，别把上面那道通槽在画面上堵回去 ——
|
||||
否则"孔是通的"这件事在预览里看不出来，容易被当成模型没改。 -->
|
||||
<link name="legs">
|
||||
<pose>0 0 0.175 0 0 0</pose>
|
||||
<visual name="visual">
|
||||
<geometry><box><size>0.7 1.1 0.35</size></box></geometry>
|
||||
<visual name="visual_south">
|
||||
<pose>0 -0.325 0 0 0 0</pose>
|
||||
<geometry><box><size>0.7 0.45 0.35</size></box></geometry>
|
||||
<material>
|
||||
<ambient>0.3 0.3 0.3 1</ambient>
|
||||
<diffuse>0.35 0.35 0.35 1</diffuse>
|
||||
</material>
|
||||
</visual>
|
||||
<visual name="visual_north">
|
||||
<pose>0 0.325 0 0 0 0</pose>
|
||||
<geometry><box><size>0.7 0.45 0.35</size></box></geometry>
|
||||
<material>
|
||||
<ambient>0.3 0.3 0.3 1</ambient>
|
||||
<diffuse>0.35 0.35 0.35 1</diffuse>
|
||||
|
||||
Reference in New Issue
Block a user