ros_contorl/devices/sim/gazebo/config/scene.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: 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 里有提示交互），仿真里先立在座上，
      便于确认夹爪能不能够到、开合范围够不够。