arm_dev
b29339e522
三块补齐，目标是"同一个流程节点，仿真/真机都不改代码"：
1. 真值孔位（truth_holes.py）：把 scene.yaml 与模型 SDF 里已知的孔几何
直接投影到相机像素坐标，替代 HoughCircles。perception.source 选：
yolo / truth / external —— truth 就走这条路。
为什么不用 Hough：仿真里孔是"方孔 + 贴片亮环"的近似（SDF 没有布尔运算），
靠图像门槛去凑 radus 区间很脆；而孔位本来就是场景里已知量，直接投影
既准又稳，还能当真实检测算法的对照基线。
2. 真值相机（sim_truth_perception.py）：同理，直接算物体位姿，供
visual_servo/filtered_point 用，绕开 ONNX。
3. 补 /home 与 set_named_pose 服务（sim_home_service.py）：真机上这两个
由 Flexiv 驱动提供，仿真里没有 —— 缺了 S01 直接超时。
4. sim_tactile_adapter 容错导入 ros_gz_interfaces：那一路只是"接触点"，
力本身走 geometry_msgs/Wrench。typesupport 加载不了时退化成
contact 恒 False + centroid 恒 0，三力判据（S07 用的）不受影响 ——
宁可用退化模式跑，也不要整个适配器起不来。
5. repo_root 解析（已装包要能找回源码仓库根，见 test_repo_root.py 的用例）
与 use_sim_time 向子 launch 的传递。
另：scripts/gen_sim_handeye.py 把 URDF 真值导出手眼文件（仿真不标定）。
Co-authored-by: Cursor <cursoragent@cursor.com>
147 lines
9.2 KiB
YAML
147 lines
9.2 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]
|
||
|
||
# ---------------------------------------------------------------------------
|
||
# 场景物体
|
||
# ---------------------------------------------------------------------------
|
||
# 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 里有提示交互），仿真里先立在座上，
|
||
便于确认夹爪能不能够到、开合范围够不够。
|