arm_dev
c1cd862eea
这批是 09-24 那轮仿真工作的收尾，一直没提交。内容按目的分四块： 1) 仿真真能「夹住」工件 sim_grasp_lock：给规与夹爪之间加接触锁（默认关，run_gauge_assembly --sim 才开）。 没有它时仿真里规只靠摩擦，S08 棘轮一转就滑出指间，于是把「仿真不真」误读成 「力判据不对」。新增 test_sim_grasp_lock.py。 2) 腕部力传感器进仿真 sim_wrist_ft（+ config/wrist_ft.yaml）：S07 的首选判据是腕部 FT 的轴向突变， 而仿真里以前根本没有腕部力，S07 只能退到触觉兜底，两套判据从没在同一场景下比过。 现在两条通道都在，接触判定可对照。 3) 真值位姿可查（不再靠猜） sim_live_pose / sim_pose_service：把 Gazebo 里刚体的实测位姿发布出来， stage2 的真值路径按**实测**位姿算孔位，而不是按场景文件里的标称位姿。 顺带把真值孔位的推导（truth_holes）独立出来并补测。 4) 工具几何成为可配置项 config/sim_tool_geometry.yaml + stage2 的 tool_tip_below_flange_m 启动参数： 仿真里规会在指间下滑，工具长每次装夹都不同（实测见过 243/254mm）， 悬停高度与 S07 下探预算必须按实测值算，不能再写死。 应用层同时跟进：gauge_fsm / gauge_config 把「悬停时工具尖端入没入工件」的判据 （hover_z_guard）与拖件判定（drag_verdict）收成纯函数并补测；stage3 搜孔、 site 导出、run_gauge_assembly 与若干文档同步。 新增 test_gauge_exit_lift / test_gauge_gripper_client / test_node_handler_collisions。 测试：全仓库 1085 passed。
199 lines
13 KiB
YAML
199 lines
13 KiB
YAML
# =============================================================================
|
||
# devices/sim/gazebo/config/scene.yaml
|
||
#
|
||
# 仿真场景：**谁摆在台面上的哪里**。
|
||
#
|
||
# 为什么单独一份文件，而不是把位姿写进模型或 world：
|
||
# 模型描述"这个东西长什么样"，场景描述"它现在在哪"。
|
||
# 分开之后，同一个模型可以在场景里出现多次、可以在不改模型的情况下换位置，
|
||
# 也才能在真机现场对照着调整（"现场工件是摆在偏左还是偏右？"）。
|
||
# world 文件里只放**固定不动**的东西（地面、台子、眼在手外的相机）。
|
||
#
|
||
# ⚠️ 位姿是**绝对**坐标（世界系、米），不是相对台面的偏移。
|
||
# 仿真世界没有"台面坐标系"这种东西，写相对量只会多一层换算、多一个出错点。
|
||
# 下面的 z 值都是"物体的原点"。本框架约定：模型的局部原点 = 它**底面的中心**
|
||
# （工件的板底、通止规的规尖），所以"放在台面上"就是 z = 台面高度。
|
||
# 这条约定让场景文件读起来最直观，代价是建模时必须把原点设在底部 ——
|
||
# 用 SolidWorks/Blender 导出网格前先把原点挪到底面中心。
|
||
# =============================================================================
|
||
|
||
# 场景模型所在目录（相对仓库根）。每个子目录一个模型：<名字>/model.sdf + model.config
|
||
models_dir: devices/sim/gazebo/models
|
||
|
||
# ---------------------------------------------------------------------------
|
||
# 台面几何 —— 用于校验"模型确实摆在台面上、没悬空也没穿模"
|
||
# ---------------------------------------------------------------------------
|
||
# 这些值必须与 worlds/assembly.world 里的 work_table 一致。
|
||
# 不一致时脚本会报错（scripts/check_sim_scene.py），而不是让你在现场猜。
|
||
# 台面：pose(0.55,0,0) + link top 在 z=0.375 + box 厚 0.05 → 顶面 0.40
|
||
# box 尺寸 0.8(x) × 1.2(y)
|
||
table_top_z: 0.40
|
||
table_bounds:
|
||
x: [0.15, 0.95]
|
||
y: [-0.60, 0.60]
|
||
|
||
# ---------------------------------------------------------------------------
|
||
# 作业条件 —— 校验"仿真里的成像几何与真机一致"
|
||
# ---------------------------------------------------------------------------
|
||
# 二阶段的孔检测是**像素门槛**（hough_min_radius / hole_distance_min …），
|
||
# 它们只在"相机到工件表面距离"合适时才有意义。所以这个距离必须在仿真里
|
||
# 显式复现，不能靠"看起来差不多"。
|
||
#
|
||
# ⚠️ 真机与仿真的**悬停高度不一样**，别照抄：
|
||
# 真机 site.yaml：robots[].servo.hole_hover_z_m = 0.23（法兰相对基座）
|
||
# 仿真需要：约 0.56（法兰相对世界，算下来见下）
|
||
# 原因是两者的"基座与台面的相对关系"不同：真机是机器人装在台面同一高度上
|
||
# （法兰比台面高 23cm 就等于离工件 23cm），而仿真里机械臂站在地面、
|
||
# 台面在 0.40m 高。**相机到工件的距离**才是决定成像的东西，绝对高度不是。
|
||
# 这个差异目前是**已知且未解决**的（要让两套参数完全通用，得把仿真的台面
|
||
# 降到与真机一致，那会连带改 D435 的位姿和相机验收基线）——
|
||
# 详见 README 的"仿真 ≠ 真机"一节，那里写了决策点与影响面。
|
||
operating:
|
||
# 用哪台相机做孔检测（决定用哪组内参算像素尺度）
|
||
camera: sim_d405
|
||
# 哪个物体是"工件" —— 相机到工件的距离以**它的上表面**为基准。
|
||
# 这里写的是物体的 name（不是模型名）。
|
||
part_object: workpiece
|
||
# 相机光心到工件**上表面**的距离 [m]。
|
||
# 这个值不是量出来的，是**两条约束夹出来的**：
|
||
#
|
||
# (a) 像素门槛（来自 hole_detect.yaml，见下面 gates_px）
|
||
# 亮环 φ14（半径 7mm）要落在 10~30px → 距离 ∈ [0.104, 0.313]
|
||
# 两孔间距 58mm 要 ≥ 90px → 距离 ≤ 0.288
|
||
# 取中段 ⇒ 约 0.17~0.25 都行。
|
||
#
|
||
# (b) 机械臂够得着（site.yaml 的 servo.workspace_xyz_max 是 z=0.65）
|
||
# 仿真里法兰高度 = 工件上表面(0.412) + 本值 + 相机伸出量(0.05)
|
||
# 本值 0.17 → 法兰 0.632，离上界还有 18mm 余量。
|
||
# 本值 0.22 → 法兰 0.682，**超出工作空间**，二阶段会直接拒绝执行。
|
||
#
|
||
# 两条约束交出来的取值，恰好也解释了真机的数：
|
||
# 真机 hole_hover_z_m=0.23（法兰相对基座）、相机比法兰靠下 0.05、
|
||
# 台面与基座同一水平面、板上表面 0.012 ⇒ 相机到工件 = 0.23-0.05-0.012
|
||
# = 0.168 ≈ 0.17。所以 0.17 不是凑出来的，而是与真机的悬停高度自洽的。
|
||
camera_to_part_m: 0.17
|
||
# 相机相对法兰沿工具轴的伸出量 [m]。arm.sim.sensors.xacro 里 d405_z=0.05。
|
||
# 竖直朝下装配时工具轴朝下，所以相机比法兰更靠近工件这么多。
|
||
camera_offset_below_flange_m: 0.05
|
||
|
||
# ---------------------------------------------------------------------------
|
||
# 孔检测的像素门槛 —— 用于在**启动时**就算出"仿真里这些特征能否被检测到"
|
||
# ---------------------------------------------------------------------------
|
||
# ⚠️ 这里的值必须与真机的检测配置一致：
|
||
# src/arm_application/config/hole_detect.yaml
|
||
# config/site.yaml 里 hole_detect.* 对它的覆盖（site.yaml 优先）
|
||
# 抄一份的原因：那些门槛是"像素"量，只有在仿真侧算出像素值才能提前发现
|
||
# "孔间距不够/环太小"，不必等上真机。代价是两份数字可能漂移 ——
|
||
# 所以 scripts/check_sim_scene.py 会读那两个文件做**交叉核对**，
|
||
# 不一致直接报错。改门槛时改真机那份，然后同步到这里（脚本会提醒）。
|
||
#
|
||
# 键名必须与模型里 visual 的名字约定对应（见 sim_scene.features_from_workpiece）：
|
||
# insert_ring_radius 嵌件亮环半径（模型里 insert_*_ring 的 radius）
|
||
# hole_spacing 两孔中心距（两个 insert_*_ring 的 pose 距离）
|
||
#
|
||
# 半径区间取 [6, 30]：这是 config/site.yaml **实际生效**的值
|
||
# （site.yaml 覆盖了包内 hole_detect.yaml 的 10/30；生效规则是 site 优先，
|
||
# 见 hole_detector.config_from_site）。脚本会读那两个文件做交叉核对，
|
||
# 不一致直接失败 —— 所以这里永远等于真机在用的门槛，不需要人工同步。
|
||
gates_px:
|
||
insert_ring_radius: [6, 30]
|
||
hole_spacing: [90, 700]
|
||
|
||
# ---------------------------------------------------------------------------
|
||
# 世界模型位姿回读 —— 仿真里唯一能发现"物体被撞走/掉下去了"的手段
|
||
# ---------------------------------------------------------------------------
|
||
# 为什么需要它（2026-09-24 花了一整轮排查换来的，值得完整读一遍）：
|
||
# `perception.source=truth` 是**纯几何**的：它按下面 objects 里的**名义位姿**
|
||
# 把孔投到像素上，**从不问 Gazebo 物体现在到底在哪**。
|
||
# 于是工件一旦被碰走，"真值"就开始撒谎，而且每一处现象都指向别的原因：
|
||
#
|
||
# 实测：工件掉进台面通槽、落到地面（z≈0），而 scene.yaml 仍写着 z=0.40。
|
||
# S05 拿"名义孔位"的像素去查**真实**深度图 → 孔位处看到的是槽底/地面
|
||
# （0.6m，而工件面其实只有 0.21m）→ 三角化出 k1/k2 间距 169mm
|
||
# （真值 58mm）→ 被间距门槛丢掉 → 精拍连丢 9 拍 → S05 超时 ABORT。
|
||
# 现场现象像"检测坏了 / 标定坏了 / 阈值调高了"，全都不是。
|
||
# 真正的信息只有一句："**工件现在不在 scene.yaml 说的位置**。"
|
||
#
|
||
# Gazebo 自己会把每个模型的实际位姿发在 /world/<world>/pose/info
|
||
# （gz.msgs.Pose_V），桥成 tf2_msgs/TFMessage 之后，漂移监视器
|
||
# （sim_truth_perception）就能把"名义位姿 vs 实际位姿"直接对比出来并**指名报错**。
|
||
#
|
||
# 话题名里的 world 名必须与 worlds/assembly.world 的文件名一致（`assembly`）。
|
||
pose_info:
|
||
gz_topic: "/world/assembly/pose/info"
|
||
ros_topic: "/sim/world_pose_info"
|
||
# 类型固定这两条：ros_gz_bridge 原生支持 Pose_V ↔ TFMessage（实测确认，
|
||
# 见 ros_gz_sim_demos 的 tf_bridge.launch.py）。写错类型不会有任何报错，
|
||
# 只会"桥接器起来了但一个消息都没有" —— 所以别改。
|
||
ros_type: "tf2_msgs/msg/TFMessage"
|
||
gz_type: "gz.msgs.Pose_V"
|
||
# 漂移判据：**两个**阈值，别合成一个（2026-09-24 的教训）。
|
||
#
|
||
# drift_tol_m（2mm）= **告警**线：真值几何已经跟着**实测**位姿走
|
||
# （见 arm_sim_gazebo/sim_live_pose.py），所以 2mm 不再意味着"几何错了"，
|
||
# 只意味着"这个场景跟 scene.yaml 写的不一样了"。而 2~3mm 是**天天都有**的：
|
||
# spawn 落地就偏 2.6mm，工件骑在通槽上静置十几分钟还会再挪 1.9mm。
|
||
# 2mm 的来历：真值投影在相机到工件 0.17m 处，fx≈447 ⇒ 1mm 位姿误差 ≈ 2.6px，
|
||
# 而 S05 的稳定判据是 2px —— 2mm 是"开始影响二阶段"的量级。
|
||
# yaw 取 2°：两孔间距 58mm，绕中心转 2° 会让孔横移 1mm（约 2.6px），同量级。
|
||
drift_tol_m: 0.002
|
||
drift_tol_deg: 2.0
|
||
# drift_stop_m 才是**停发**线：到这个量级说明工件掉了/被搬走了，场景前提没了
|
||
# （再发点等于指挥机械臂去装一个不存在的工件）。20mm 的来历：工件一半宽 70mm、
|
||
# 掉下台面是 400mm 量级，而"顺手碰一下"是几毫米 —— 20mm 两边都分得开。
|
||
drift_stop_m: 0.02
|
||
# 启动复位：开局把被盯的物体 set_pose 回名义位姿（仿真专有，默认关，这里开）。
|
||
# 为什么还要它：上面两条线是"容差"，这一条是**消除源头** ——
|
||
# 物体是"在名义位姿上方 spawn、再落下来"的，落地那 2.6mm 偏差纯属 spawn
|
||
# 的落点问题（实测 set_pose 回名义位姿后它就停在那儿不动）。
|
||
# 只在开局 20s 窗口内、且只做一次 —— 运行中的漂移必须报错，不能悄悄搬回去
|
||
# （那会抹掉"谁把它撞歪了"的现场，还可能把工件从爪里拽走）。
|
||
# 实现见 sim_truth_perception._maybe_startup_reset（含完整理由）。
|
||
reset_on_start: true
|
||
|
||
# ---------------------------------------------------------------------------
|
||
# 场景物体
|
||
# ---------------------------------------------------------------------------
|
||
# name = 进 Gazebo 后的实体名（也是 `ign model -m <name>` 用的名字）。
|
||
# model = models_dir 下的子目录名 / model.config 里的 <name>。
|
||
# yaw = 绕世界 Z 的旋转 [rad]。工件的 yaw 决定两个孔的连线方向，
|
||
# 而两孔连线方向会直接影响 stage2 的 k1/k2 判定（k1_label: left/right）
|
||
# —— 换 yaw 后要重新确认哪边是"画面里的左孔"。
|
||
objects:
|
||
- name: workpiece
|
||
model: workpiece
|
||
# 摆在工作台长度方向的中间偏左（靠近机械臂一侧）：
|
||
# 太靠外（x 大）→ 手臂要伸直，接近工作空间边缘，柔顺下压时余量小
|
||
# 太靠里（x 小）→ 会被手臂本体挡住 D435 的视线（粗定位就瞎了）
|
||
# x=0.52 是两者折中；校验脚本会把工件四角反投影到 D435 画面里确认没被裁掉。
|
||
pose: {x: 0.52, y: 0.00, z: 0.40, yaw: 0.0}
|
||
note: >
|
||
工件。z=0.40 = 台面顶面，板底贴台面。
|
||
工件**上表面**在 0.40+0.012=0.412，二阶段的对孔基准面就是它。
|
||
|
||
- name: gauge_stand
|
||
model: gauge_stand
|
||
# 与规**同心**：规座就在规的下面，规插在它的方孔里。
|
||
# 座高 32mm，所以规的 z = 台面 + 32 - 30（通端长 30）= 0.402，
|
||
# 也就是"台肩正好坐在座顶面上、规尖离台面 2mm"。
|
||
# 这个 30mm 不是随手写的：它来自 gauge/model.sdf 里通端的长度，
|
||
# 改模型时必须一起改（改错了的表现是规穿进台面或悬在座里没坐实）。
|
||
pose: {x: 0.30, y: 0.28, z: 0.40, yaw: 0.0}
|
||
note: >
|
||
规座（工装，静态）。**必须在规之前 spawn**（objects 里的顺序就是 spawn 顺序）：
|
||
否则规先落到台面上、规座再出现在它身体里 —— 两个刚体互相嵌入，
|
||
求解器会把规弹飞，而现象看起来像"规自己会跳"。
|
||
|
||
- name: gauge
|
||
model: gauge
|
||
# 立在规座里、工件旁边偏机械臂一侧，方便夹爪从上往下夹住夹持段。
|
||
# y=0.28 是避开工件与 D435 视线的一侧（工件在 y=0 附近）。
|
||
# z 见 gauge_stand 的说明：0.402 = 座顶(0.432) - 通端长(0.030)。
|
||
pose: {x: 0.30, y: 0.28, z: 0.402, yaw: 0.0}
|
||
note: >
|
||
通止规。规尖在 z=0.402（原点在规尖），夹持段在 0.435~0.480。
|
||
本阶段它只是"插在规座里立着"；把它夹起来是 P6 的活（P6 会用
|
||
GripperExecute 指令夹爪开到 ~35mm、伸到夹持段、再闭合到 ~29mm）。
|
||
真机上规是**人工放进夹爪**的（P6 里有提示交互），仿真里先立在座上，
|
||
便于确认夹爪能不能够到、开合范围够不够。
|