ros_contorl/devices/sim/gazebo

devices/sim/gazebo/ —— Gazebo 仿真平台（中文手册）

这份文档是仿真的"使用说明书 + 建模规范 + 避坑清单"。

读法建议：

• 只想跑起来 → 看 第 1 节 使用方法。
• 要加一个工件/夹具/机器人 → 看 第 2 节 建模要求 与 第 3 节 导入格式。
• 要改参数 → 看 第 4 节 配置方式（有一张"改哪里才真的生效"的表，先看它）。
• 改仿真之前必须看完 第 5 节 实测踩到的坑 —— 仿真里绝大多数错误都是静默的： 不报错、能出图、能查到 TF，结果却是错的。每条坑都写了"怎么发现"。

本文档只记录已经跑通的事实。没接线的东西会明确标 ✘，不要凭印象改成 ✔。

---

0. 先读这段：哪个"仿真"是真的

仓库里有两个都叫"仿真"的东西，很容易混：

入口	底层	有物理吗	用途
scripts/run_sim.sh	arm_bringup 的 mock 驱动（mock_arm_driver.py）	✘ 假数据 + RViz	验证话题/流程/UI，不耗资源
scripts/run_sim_gz.sh	Gazebo Fortress（真物理、真接触）	✔	验证接触/力控/几何、装配状态机

要验证装配的接触与力判据，只能用后者。 前者永远不会产生接触力。

判断当前在跑哪个：ros2 topic echo /clock --once —— 有 /clock 且时间在走，是 Gazebo。

---

1. 使用方法

1.1 装环境（只做一次）

bash scripts/install_sim_env.sh     # 建独立 conda 环境 ros_gz_sim（Gazebo Fortress + ROS 2 Humble）
bash scripts/build_sim.sh           # 编仿真用的包（--merge-install）

环境是独立的一份，与真机环境 ros_flexiv 隔离。理由与踩坑见 坑 3 和 scripts/install_sim_env.sh 顶部的长注释。

1.2 启动

bash scripts/run_sim_gz.sh                            # 无头（无显示器环境用这个）
bash scripts/run_sim_gz.sh --gui                      # 带 Gazebo GUI
bash scripts/run_sim_gz.sh --check                    # 只自检描述，不启动任何东西
bash scripts/run_sim_gz.sh --world /abs/other.world   # 换 world
bash scripts/run_sim_gz.sh --no-gripper --no-sensors  # 最省资源，只验运动学

首次启动约 10~20 秒（Gazebo 初始化 + spawn + 拉控制器）。启动日志末尾会打印一份 验收命令清单，照它做一遍就能确认仿真真的活了。

1.3 验收（★ 必做，别只看"有没有图像"）

ros2 topic echo /clock --once                  # 仿真时间在走
ros2 control list_controllers                  # 两个控制器 active
ros2 topic echo /joint_states --once           # 7 个 sim_joint
ros2 run tf2_ros tf2_echo world sim_tcp        # 末端位姿可查
ros2 topic echo /tactile/left/status --once    # 触觉三力在发
ign model -m workpiece -p                      # 工件在台面上
python3 scripts/check_sim_cameras.py           # ★ 相机的真正验收
python3 scripts/check_sim_scene.py             # ★ 场景摆放 + 孔检测几何验收
python3 scripts/check_sim_gripper.py           # ★ 夹爪的几何验收（开合行程）

机械臂应当在重力下保持站住。若它瘫下去或消失，说明描述有问题 —— 见 坑 1。

⚠️ check_sim_scene.py 是跑前检查（"现在这一轮能不能开始"），不是跑后检查： 一轮装配跑完之后场景必然是脏的 —— 放手时抓取锁释放、规掉在台面上， 落点可能磕到工件。所以跑完再查会报红，这是正常的。下一轮 run_gauge_assembly.sh --sim 开头的上料脚本会把 scene.yaml 里所有物体 复位回名义位姿（sim_load_gauge.py::_reset_scene_objects），届时就干净了。

这几条 check_*.py 才是验收标准，"有图像 / 服务有返回"证明不了什么。 它们做的判断分别是：

工具	它凭什么判"通过"	它防的是哪一类错
check_sim_cameras.py	把深度反投影到 world，要求命中已知平面（地面 z=0、台面 z=0.40）	深度单位差 1000 倍、frame_id 盖成本体系、内参是 gz 的错值、相机位姿朝向反了 —— 全都能出图
check_sim_scene.py	摆放自洽 + 像素门槛与真机一致 + 孔检测特征的像素值达标 + 手外相机完整看得见工件	物体悬空/穿模/嵌入、场景里的门槛与真机漂移、成像尺度不够、粗定位框不全
check_sim_gripper.py	命令开口 mm → 两指尖在 world 里的实际间距增加同样的 mm；两侧行程相等	耦合链套错一层、命令数组顺序错、毫米/米混用、适配器没起来（服务在、话题在，夹具就是不动）

1.4 停止

bash scripts/stop_sim_gz.sh      # 稳妥地停掉整套仿真

不要用 pkill -f gz：它会把调用它的那个 shell 一起杀掉（-f 匹配命令行， 而 pkill 自己的命令行里就含 gz），也会误杀同名的其他进程。

stop_sim_gz.sh 走两条路：

1. 进程组（主通道）：run_sim_gz.sh 用 setsid 把整栈起在自己的会话里， PGID 记在 log_sim/sim.pgid，停止时 kill -TERM -PGID 一次带走整组 —— 不依赖任何"进程名模式表"。
2. 模式扫描（兜底）：scripts/lib/sim_procs.sh 的点名表 + install_sim/ 兜底规则，收拾历史孤儿与 ign 自己 fork 出去的进程；杀完再扫一遍， 有残留就非零退出。

⚠️ 残留进程比"没停干净"严重得多，这是实测踩出来的：

• 残留的 sim_gripper_adapter 会和新适配器同时订阅 /joint_states、 同时发布 /gripper/status，服务调用还会被随机分给其中一个；
• 残留的 static_transform_publisher 发的是 latched /tf_static， 会把旧相机外参一直按在新仿真的 TF 树上。

症状一模一样：验收脚本读到"开口恒为 0 mm、指尖间距 0.03 mm"（真实值 127 mm）， 看起来就像夹爪换算写错了。所以 run_sim_gz.sh 现在会拒绝在残留状态下启动， check_sim_gripper.py 也会先查重名节点。

---

2. 建模要求

2.1 三条通用规矩（违反哪一条都会"看起来正常"）

规矩	为什么	违反了会怎样
单位必须是米	Gazebo 没有单位系统，SDF 里的数字就是米	用毫米建的 mesh 会变成 1000 倍大的怪物，或者小到看不见
模型原点放在底面中心	放置时 pose.z 就等于"放在台面上的高度"	原点在质心时物体一半埋进台面，或者悬空；sim_scene 会校验并报错
<model name> 必须与推理用的类别名一致	仿真里算出的坐标要与 YOLO 结果对齐	视觉算出的类别名与场景里的名字对不上，坐标静默错位

sim_scene.validate_scene() 会在启动时把上面第 1、2 条自动查一遍 （模型原点、是否穿台面/悬空/出界/互相嵌入），有问题的物体不会被 spawn 出去。

2.2 质量与惯性（<inertial>）

<inertial> 不是可选的。 缺了它 Gazebo 会给一个默认值，物体表现得像另一个东西； 更糟的是惯性为 0 或 NaN 时物体直接消失（见 坑 1）。

• 质量与真实值同量级即可，不必精确。装配里要紧的是质量比： 工件太重会让夹爪"压进去"的读数异常大（见 config/tactile.yaml 的复测记录）。
• inertia 用简化公式算（薄板、圆柱的公式够用），只要量级对。
• 参考 models/workpiece/model.sdf（铝板：mass=0.08）与 models/gauge/model.sdf（钢规：mass=0.02）。

2.3 <collision> 与 <visual> 的分工

	作用	建模要求
<collision>	决定物理：穿不穿、卡不卡、接触力多大	用简单几何体（box/cylinder），不要用 mesh —— mesh 碰撞在接触力计算上又慢又不稳
<visual>	决定"相机看见什么"	可以加入只为人眼/算法准备的细节（亮环、色块），它们不参与物理

这两个可以不一样，而且这个框架有意让它们不一样：

models/workpiece/model.sdf 的 collision 是"一块带三个方孔的平板"， 而 visual 额外加了两个亮环（φ14 外 / φ7 内）来模拟真机上的压入衬套 —— 因为真机的 HoughCircles 就是靠这两个亮环找孔的。若只按 collision 建视觉， 仿真里"孔检测"会因为看不见环而永远失败，而这不是算法问题。

⚠️ 由此带来一条约定：visual 的名字有意义，sim_scene.features_from_workpiece 按名字前缀（insert_*_ring / center_bore）去读出特征尺寸，用来算像素值。 改 visual 的名字必须同步改那个函数（有单测守着）。

2.4 接触参数（<surface>）：只在必要时改，且要写清理由

默认接触参数（kp 很大 = 刚性）对"位置控制 + 刚性夹持"是灾难： 夹爪把工件"压进去"的力会到几百牛，工件被弹飞。

models/gauge/model.sdf 因此对规单独放软了接触刚度（kp/kd 比台面软得多）， 注释里写明了原因。这是有意为之，不是随手抄的参数 —— 照抄前先想清楚你的物体 是不是也要被夹爪夹。

2.5 落地判据：怎么知道模型建对了

ign sdf -p models/<名>/model.sdf        # 先过语法
python3 scripts/check_sim_scene.py      # 再过摆放与成像

两条都过了，才认为"模型建好了"。不要以"Gazebo 里能看到它"为判据。

---

3. 导入格式

3.1 物料放置总表

类型	放哪	格式	命名要求	谁读它
世界文件	worlds/	.world / .sdf	<场景名>.world	site.yaml → sim.world
机器人描述	urdf/	.urdf / .xacro	关节前缀必须与真机不同（仿真用 sim_）	gazebo_sim_launcher
场景物体	models/<模型名>/	<模型名>.sdf + <模型名>.config	目录名 = <model name> = 推理类别名	config/scene.yaml → 启动时 spawn
网格文件	models/<模型名>/meshes/	.stl / .dae	单位必须是米	被 SDF 的 <mesh><uri> 引用
相机声明	config/cameras.yaml	YAML	ros_ns 必须等于应用的 camera_name	桥接 + 适配器 + 静态 TF
场景布局	config/scene.yaml	YAML	—	启动时 spawn + 像素门槛校验
触觉标定	config/tactile.yaml	YAML	话题名必须与 URDF 里的 <topic> 一致	sim_tactile_adapter
控制器	config/controllers.yaml	YAML	控制器名与真机一致	ros2_control

sensor_plugins/ 目前是空的（只有 .gitkeep）。触觉走的是 Gazebo 内置的 force_torque + contact 传感器 + 本框架的适配器，不需要自己写 C++ 插件。 那个目录留给"以后真要写插件"的场景。

3.2 一个模型目录长什么样

models/workpiece/
├── model.sdf       # 几何 + 惯性 + 接触参数
└── model.config    # 元数据：<name>、版本、指向 model.sdf、描述

model.config 里的 <name> 必须等于目录名，也必须等于 YOLO 的类别名。 models/workpiece/model.config 里写了这条约定，照它抄。

3.3 加一个场景物体：改 scene.yaml 一处即可

物体不在 world 文件里，而在 config/scene.yaml 的 objects 列表里， 启动时由 ros_gz_sim create 动态 spawn：

objects:
  - name: workpiece          # Gazebo 里的实体名（也是 ign model -m 的名字）
    model: workpiece         # models/ 下的目录名
    pose: {x: 0.52, y: 0.0, z: 0.40, yaw: 0.0}   # z = 台面顶面高度（原点在底面）
    note: 为什么摆这儿 —— 请写清，否则半年后没人敢动

这么设计的原因：world 文件是"环境"（地面、台面、重力、固定相机）， 场景是"这一批活要做什么"（工件在哪、规座在哪）。混在一起会导致 "为了挪一下工件而去改 world 文件"，而 world 文件一旦改错（比如丢了 UserCommands 系统）会让所有模型都 spawn 不进去（见 坑 2）。

3.4 URDF → Gazebo：中间隔着 sdformat，会丢东西

flexiv_description 的 xacro 展开后交给 sdformat 转成 SDF。这个转换是有损的：

会被丢/被改的	后果	本框架的应对
<mimic> 标签	夹爪的从动指节不再跟随 → 夹爪"驱动了但不动"	urdf_normalize 把 mimic 关系记下来，改成把全部 7 个夹爪关节都接入 ros2_control，由控制器按记录的关系驱动
缺 <inertial> 的 link	惯性 NaN → 物体消失	启动时补默认惯性
零质量的 link	惯性叠加出 NaN	同上，一律归一化
世界 link（无质量的根 link）	整棵子树被静默丢掉	D435 因此必须放在 world 文件里，不能挂在 URDF 的 world link 上

这些都不是"理论上可能"，是实测踩过的。归一化逻辑在 src/arm_sim_gazebo/arm_sim_gazebo/urdf_normalize.py（37 项单测）。 单独排查：gazebo_sim_launcher --check。

---

4. 配置方式

4.1 ★ 改哪里才真的生效（先看这张表）

想改什么	改哪里	真的生效吗
用哪个世界	site.yaml → sim.world，或命令行 --world	✔ 优先级：--world > site.yaml > 内置默认
场景里有哪些物体、摆在哪	config/scene.yaml	✔ 启动时 spawn，并做摆放/像素校验
相机的视场/坐标系/话题	config/cameras.yaml，并且同步 SDF 里对应的 horizontal_fov	✔ 启动时自动比对，不一致告警
相机图幅（影响像素尺度）	config/cameras.yaml 的 width/height 且 SDF 的 image_width/height	✔ 启动时自动比对（这是最容易漏的一处，见 4.3）
触觉标度/轴向/基线	config/tactile.yaml	✔ 适配器每次启动都读
感知来源（一阶段目标点 + 二阶段孔位）	site.yaml → perception.source	✔ 仿真那台写 truth：一阶段由 sim_truth_perception 按场景位姿算目标点，二阶段由 arm_sim_gazebo.truth_holes 按场景几何投影孔位（见 §5 坑 13）
装配的悬高/速度	复用真机那套（servo.* / hole_detect.stage2.*）	✔ 有意共用
物理开关	site.yaml → sim.physics_enable	✘ 未接线。要真关，改 worlds/assembly.world 的 <gravity>
关节前缀	site.yaml → sim.joint_prefix	✘ 未接线。改要动三处：urdf/arm.sim.urdf.xacro 的 arm_prefix、config/controllers.yaml 的 sim_joint1..7、本项

设计约束：仿真与真机必须共用同一套 site.yaml 参数。任何"仿真专用参数"都是 未来"仿真里调好的值搬到真机不生效"的隐患。只允许物理引擎自身的参数 （重力、步长）放在 sim.* 下。

✘ 的两个键由 arm_sim_gazebo.sim_site.unwired_warnings() 守着：写的值与实际状态 不一致时启动会告警。不要让它们重回静默。

4.2 配置文件之间的分工

config/site.yaml          ← 站点真相：真机有哪些设备、序列号、阈值（仿真与真机共用）
   │
   ├── devices/sim/gazebo/config/cameras.yaml   ← "Gazebo 里有什么相机、桥到哪个 ROS 名字"
   ├── devices/sim/gazebo/config/scene.yaml     ← "这一批活：工件/规座摆在哪"（含像素门槛）
   ├── devices/sim/gazebo/config/tactile.yaml   ← 仿真的力标定口径与轴向映射
   └── devices/sim/gazebo/config/controllers.yaml ← 控制器（名字与真机一致）

一条原则：能派的就别抄。 相机位姿从 world 文件里解析（不在 cameras.yaml 重复写）、 内参从 horizontal_fov 与图幅算出（不手抄 fx/fy）—— 抄一份就多一处能漂移的地方。

唯一的例外是 scene.yaml 里的 gates_px：像素门槛本来只在真机的 hole_detect.yaml/site.yaml 里，但仿真要在启动时就判"能不能看见"， 所以抄了一份。为了不让它漂移，check_sim_scene.py 会读那两个文件做交叉核对， 不一致直接判失败。抄一份可以，但必须有人守着。

4.3 改相机图幅会牵动三处（最常见的漏改）

# 想改 D405 的分辨率，三处必须同时改：
devices/sim/gazebo/config/cameras.yaml        # width / height（内参由此算出）
devices/sim/gazebo/urdf/arm.sim.sensors.xacro # image_width / image_height（真正渲染的）

第三处是下游：hole_detect.yaml 的像素门槛（hough_min_radius、hole_distance_min） 只在特定像素尺度下才对。仿真用了 640x480（fx=337）而不是真机的 848x480（fx=447）时， 58mm 的两孔间距成像只剩 85px，低于 hole_distance_min=90 —— 孔检测在仿真里根本不可能通过，而报出来的现象会像是"算法不行"。

启动时会比对 cameras.yaml 与 SDF 的视场和图幅，不一致会告警并说明影响。

---

4.4 夹爪：仿真里怎么开合，口径是什么

真机和仿真的服务接口是同一套（arm_msgs/srv/GripperExecute），所以应用层代码 不用为仿真改一行：

ros2 service call /gripper/open     arm_msgs/srv/GripperExecute      # 全开
ros2 service call /gripper/close    arm_msgs/srv/GripperExecute      # 全合
ros2 service call /gripper/move     arm_msgs/srv/GripperExecute "{command: {command: move, position: 20.0}}"   # 开 20 mm
ros2 topic echo /gripper/status --once                               # 开口、是否夹住、是否在动

口径（唯一容易搞错的地方）：position 是开口毫米，而且是真的毫米。

仿真夹爪是官方 GN01，它的 URDF 用一条线性 <mimic> 耦合把 6 个从动关节挂在 "宽度关节"上。线性近似在线性上不准：命令 20 mm，两指实际相距 21.75 mm —— 抓 20 mm 的工件会空出 1.75 mm，没有接触力，触觉那一步就废了。

所以适配器不按线性换算，而是用 URDF 正运动学反解（sim_gripper_kin.py）： 命令 20 mm → 解出使"两指间距 == 20 mm"的关节值。这个正运动学与 Gazebo 的 实测吻合到 0.01 mm（0~100 mm 全程），所以"开口 20 mm"在仿真里是物理上真的 20 mm。

顺带两个必然的推论，看到别当 bug：

现象	原因
启动日志说行程是 99.93 mm 而不是 100.0	上游限位写成 0~0.100 m，线性耦合让指尖只走了 99.93 mm。日志报实测值，否则"全开时开口读数与几何对不上"会变成一个查不出的矛盾
合拢时两个 *_finger_tip frame 相距 27.09 mm	tip frame 不在夹持面上，且随指垫转动。别拿它当开口，要用它的变化量（变化量与开口 1:1）

改夹爪的耦合关系时：devices/sim/gazebo/config/controllers.yaml 的 joints: 顺序、 归一化时删下来的 <mimic> 表、URDF 限位三者必须一致，sim_gripper.py 会在启动时 逐条校验并明确报错（顺序错不会报错，只会把角度发给别的关节）。 验收不看"夹爪动了没有"，看几何：python3 scripts/check_sim_gripper.py。

---

5. 实测踩到的坑

这些坑的共同特征是：不报错。话题在、图像有、TF 查得到、create 打印成功，但结果是错的。 所以每条都写了"怎么发现"。

坑 1：官方 flexiv_description 直接喂 Gazebo 会被 sdformat 拒掉

7 轴本体缺 <inertial> 包裹；夹爪有零质量 link 导致惯性叠加出 NaN。 只有 Gazebo 会暴露，RViz/MoveIt 完全正常。

→ 启动时归一化（urdf_normalize.py）。单独排查：gazebo_sim_launcher --check。

坑 2：world 里声明了任何 <plugin> 都会挤掉默认系统

world 里只要有一个 <plugin>，Gazebo 就不再自动加载默认的 Physics / UserCommands / SceneBroadcaster。

少了 UserCommands 的后果最阴：/world/<名>/create 服务不存在 → 模型根本没进 Gazebo，而 create 仍然打印 OK creation of entity， 控制器永远起不来。

→ 这三个系统必须在 world 里显式声明（assembly.world 里已有）。启动时会检查并告警。

坑 3：conda 环境里 gz 插件路径缺 $CONDA_PREFIX/lib

ros_gz_sim 的 launch 只拼 LD_LIBRARY_PATH，而 conda 激活不往那里加。 症状是 gz_ros2_control 报 couldn't find shared library，而库其实就在那儿（ldd 全齐）。

→ 必须在 ros2 launch 之前设 GZ_SIM_SYSTEM_PLUGIN_PATH；资源路径同理（否则 mesh 全加载不出来）。两件事都由 scripts/sim_env.sh 负责 —— 不要绕过它直接 ros2 launch。

坑 4：gz 发的 camera_info 是错的

恒为 fx=fy=277, cx=160, cy=120（它的默认 60° 视场 + 半分辨率主点）， 而渲染用的是配置的 87° 视场与 640x480。

照抄它做三维反投影，点会偏半米量级；而单目预览完全正常。

→ 用已知地面平面最小二乘反解内参，得 fy=337.2 / cy=239.5，与 640/(2·tan(87°/2))=337.2、480/2=240 吻合（残差中位 0.2mm）。 已改为由 sim_cameras.intrinsics() 按 cameras.yaml 的视场重算并覆盖。

代价：cameras.yaml 的 horizontal_fov 必须与 SDF 真正渲染用的值一致， 启动时自动比对告警（sim_cameras.check_camera_consistency）。

坑 5：相机位姿的 rpy 复合顺序

URDF 相机是"X 朝前"约定，视线是本体 X 轴；而 rpy 的复合顺序是 Rz·Ry·Rx。 "先俯 50° 再偏航 180°"会把视线翻到背面。

D435 就这么翻过：背对台面、只看得见地面。什么都不报，而且几何自洽 （点确实落在真实地面上），只查数值发现不了。

→ 用 scripts/check_sim_cameras.py 检查"该看见的平面有没有看见"。已修正 + 加回归测试。

坑 6：接触传感器只发"接触"，不发"离开"

Gazebo 的 contact 是事件式的：接触时发消息，离开时一条都不发。

若写成"收到消息=接触 / 没收到=不管"，那么离开工件后 contact 会永远停在 True。

→ 判据必须是"最近 contact_timeout_s 内收到过消息"（config/tactile.yaml）。 config/tactile.yaml 里对这点有专门说明。

坑 7：contact-system 发的 Contacts 消息里没有力

ros_gz_interfaces/msg/Contact 的 wrenches 字段在 Fortress 的默认 contact-system 下永远是空的（实测 ign topic -e -t /tactile/left/raw -n 1， 只有几何名与接触点，没有任何力）。

这是这套链路里最耗时的一个坑：消息结构里有 wrenches 字段，看起来"应该有"。

→ 三向力改由 <sensor type="force_torque"> 提供（挂在指尖关节上，读该关节传递的约束力）。 接触布尔仍用 contact 传感器（它只有这个用途）。

附带一个教训：光看消息定义判断不了运行时会填什么，必须 ign topic -e 实测。

坑 8：forcetorque-system 也必须显式声明

与坑 2 同源。只声明了 contact-system 时，force_torque 传感器话题根本不出现， 而 URDF 里写得好好的。

→ assembly.world 里显式声明 libignition-gazebo6-forcetorque-system.so。

坑 9：sdformat 用占位值填它不认识/缺失的东西

这是坑 1 的"隐藏变体"，也是验收检查本身最容易失效的地方：

你写的	sdformat 的输出
有 <contact> 但没 <collision>	__default__
有 <contact> 但没 <topic>	__default_topic__
<topic> 写在 <sensor> 里而不是 <contact> 里	话题跑到默认作用域，订阅方永远收不到

于是"检查这个字段是不是空"的代码永远通过 —— 因为它不是空的，它是个占位字符串。

→ robot_description.check_tactile_collisions() 现在显式判这两个占位值 （__default__ / __default_topic__），并检查 <topic> 的位置对不对。 有负向单测守着（test_robot_description.py）。

通用教训：校验代码自己会静默失效。凡是"检查 X 是否为空"的地方， 都要问一句"它会不会被填成一个假值"。

坑 10：<mimic> 标签被 sdformat 丢掉

夹爪的从动指节在 URDF 里靠 <mimic> 定义跟随关系，而 sdformat 转换时把它丢了。

症状是"夹爪控制器 active，但手指不动" —— 控制器在驱动一个不存在的关节。

→ urdf_normalize 在转换前把 mimic 关系记录成一张表（mimic_relations()）， 去掉标签，并把全部 7 个夹爪关节都接入 ros2_control，由适配器按关系驱动。

顺带：从动关节原本 effort="0"，归一化时一并改成可驱动的值， 否则它在控制器里是个"只能看不能动"的关节。

坑 11：gz model -m <名> -p 的输出格式没有稳定契约

不同版本的文本格式不同。验收脚本因此宽松解析，解析不出来时降级成 "跳过位姿核对"，而不是报一个假错。

这是有意的取舍：宁可漏报，不可误报。误报会让人不再相信工具。

---

坑 12：夹爪的"宽度关节"在 Gazebo 里根本不动

GN01 的描述里有一条 finger_width_joint，注释说它"本应是 prismatic，但为了配合 mimic 只能是 revolute"，挂在 unused_finger_width_link 上（那个 link 的注释写着 "cannot have 0 mass" —— 纯粹为了 URDF 合法）。它和手指没有任何机械连接。

实测（2026-09-24）：把它命令到 0.100，它的 /joint_states 值恒为 ~0 （1.4e-08），而 6 个从动关节准确到位（0.7854 = 9.404×0.100−0.155）。

于是有两件事必须做对：

1. 命令数组里仍然要带上它（controllers.yaml 的 joints: 含它，少一个会错位）；
2. 反馈绝不能取它 —— 否则 /gripper/status 会永远报"开口 0 mm"， 而夹爪明明开着。这是最典型的一类静默失效：服务返回成功、状态有数据、 就是数为错。

适配器的做法：反馈取"与主关节直接相连、|multiplier| 最大的那个从动关节" （GN01 是 left_outer_knuckle_joint），再按耦合关系反推开口。

坑 13：二阶段的孔，在仿真里"每拍都丢"

现象（log_sim/gauge_flow.log）：

一阶段已锁定 — 停跟踪、钉住当前关节，直接进二阶段
[pipeline] === S05_snap ===
精拍 5帧中值图  L=(265.2,320.4) R=(529.7,288.8)
WARN  两孔基座间距 107.9mm 不像 k1/k2（期望 32–85mm），丢弃本拍
...（同样一行重复 8 次）
ERROR S05 精拍定位失败 code=4

一阶段（S04）完全正常：真值目标锁定、跟踪到位、悬停到 0.632m。所以看起来像 "孔检测的门槛配错了"。真因有两段，第二段才是关键：

1. 工件检测模型在合成图上出不了框（实测最高分 0.017，见 models/ 的说明）→ hole_detect_hough 拿不到 ROI，于是按调用方的允许退到整幅画面当 ROI。 这一步本身是设计好的退路（detect_holes(allow_full_frame_roi=True)）， 真机上"权重不匹配但孔看得见"时就靠它。
2. 整幅画面里还站着夹爪和通止规（它们就在工件上方、比工件更近）。 HoughCircles 在它们身上找到一对"看起来对称的圆"，间距 108mm —— 真值 58mm。于是每一拍都被间距门槛丢掉，丢满 8 拍超时。

为什么不能靠"把门槛放宽"解决：107.9mm 离 58mm 不算远，放宽一点就会让这对 假孔通过，然后机械臂去对准一个错的点 —— 症状推迟到 S07 才爆（拧不进 / 掉件重放），比现在难查得多。宁可现在"每拍都丢"。

修法：仿真里不要用检测器，用真值孔位 —— 与一阶段的 sim_truth_perception 同一套思路（都是"场景里本来就知道，何必去推"）：

孔世界点 = scene.yaml 的物体位姿 + 模型 <visual name="*_ring"> 的局部位置
孔像素   = K · T_cam_world · 孔世界点     （T 由 TF 查，K 来自 camera_info）

• 实现在 arm_sim_gazebo/truth_holes.py：纯几何，没有 ROS 依赖， 所以在没有 Gazebo 的机器上也能测（test_truth_holes.py，13 条）。
• 由 stage2_locate_node 在 perception.source == truth 时接管 _detect_holes_img / _best_bbox（gauge_assembly_node 继承这两个方法， 所以 S05 的整条链一起生效）。
• 真机（source: yolo）那条路一行都没改。
• 为什么用 perception.source 而不是新加一个"仿真孔位"开关：这个问题本来就是 "谁产生视觉测量"，一阶段的目标点与二阶段的孔位只是它的两个消费者 —— 拆成两个开关，迟早出现"一阶段走真值、二阶段走模型"的半仿状态。

顺带被这条坑钉住的两个行为：

• 孔在相机背后时真值路径返回"不可用"而不是硬给一个点（给伺服幻影目标 会让机械臂朝错误方向走，看起来像"标定坏了"）；
• 模型里嵌件亮环不是正好 2 个时直接报"有几个环"，不猜。

验收：

python3 -m pytest src/arm_sim_gazebo/test/test_truth_holes.py -q

---

坑 14：上一轮把工件推走了 → 下一轮 S04 报"一个视觉目标点都没收到"

现象（/tmp/pipeline_stage1.log）：

[sim_truth_perception-1] [ERROR] 仿真物体 'workpiece' 已经不在 scene.yaml 说的地方了 —— **停止发真值点**
        名义 (x,y,z)=(0.5200,0.0000,0.4000) yaw=0.0°
        实际 (x,y,z)=(0.4923,-0.0717,0.4000) yaw=8.3°
        偏差 |Δp|=76.9mm、姿态差 8.3°（容差 2.0mm / 2.0°）

然后下一轮 gauge_assembly 报「一阶段 150s 没锁定：一个视觉目标点都没收到」， 看起来像"真值节点没起来 / 场景名读不出来"。

这条不是 bug，是两条正确行为叠在一起：

1. 真值感知是纯几何的 —— 它按 scene.yaml 的名义位姿算目标点与孔位。 物体一挪，算出来的就是假测量，而假测量不会报错：现象会是 「S05 两孔间距 169mm（真值 58mm）」或「S07 一路下压不接触」， 看起来像检测/标定/阈值坏了。所以 sim_truth_perception 有漂移看门狗。 它不是一条线，是两条（2026-09-24 改，判据是纯函数 pose_deviation()， 有单测；看门狗本身会自检"话题一帧都没收到"）：
   • pose_info.drift_tol_m（2mm）→ 告警，但照发点：真值几何已经跟着 实测位姿算了（见下面 §5.1），2mm 只是"这场戏跟 scene.yaml 写的不一样了"；
   • pose_info.drift_stop_m（20mm）→ 停发：到这个量级说明工件掉了/被搬走了， 场景前提没了，再发点等于指挥机械臂去装一个不存在的工件。
2. 谁把工件推走的：上一轮 S07 判到接触的那一刻臂上顶着几十牛顿 （位置控制的臂撞硬面不会在"刚碰到"停住），若卸载没生效，S08 再一转， 工件就被推走/推歪了。所以第 1 条其实是在替你报告真因。

修法：把场景搬回去。scripts/sim_load_gauge.py 开头现在会复位 scene.yaml 里所有物体（不止通止规）—— 所以正常情况下重跑一次就恢复正常， 不必重启仿真。若复位失败（比如工件卡在爪里），看门狗会继续拒发点， 那时重启仿真（bash scripts/run_sim_gz.sh）让它按场景重新 spawn。

排查顺序（按物理可信度）：

① 看 sim_truth_perception 的日志有没有"不在 scene.yaml 说的地方了"
   ①a 偏差 ~2mm 级 → 只是告警，点照发，不用管（这是落地/蠕动的常态）
   ①b 偏差 >20mm   → 场景前提没了：复位（重跑 sim_load_gauge.py）或重启仿真；
                     顺手查"谁推的它"（S07 的卸载有没有生效：日志里那行
                     "接触后卸载：回退 X mm … 残余 Y N"）
② 没有 → 才轮到查"真值节点起没起来 / 场景名对不对"

5.1 真值口径：几何按实测位姿算，名义位姿只用来报"物体被搬走了"

（2026-09-24 定；实现 arm_sim_gazebo/sim_live_pose.py，谁要谁订阅。）

真值路径最早是"按 scene.yaml 的名义位姿算目标点与孔位"，也就是假设物体没动过。 实测这个假设站不住，而且偏差是安静的：

现象	真因
S05 算出的两孔间距 169mm（真值 58mm）	工件掉进 90mm 通槽、落到地面（z≈0.6m），而真值仍按名义位姿把孔投到像素上
S07 一路下压不接触	孔位差 2~3mm ⇒ 规尖（φ6）对不上孔（φ7，单边 0.5mm 余量），压在工件面上而不是进孔

而 2~3mm 是天天都有的：物体是"在名义位姿上方 spawn、再落下"的，落地就偏 2.6mm；工件骑在台面通槽上静置十几分钟还会再挪 1.9mm。

所以现在的口径是：

1. 算几何用实测位姿（/world/<world>/pose/info → LiveScenePose）。 真值感知的目标点、二阶段定位的孔位都走这一条。这也更接近真机 —— 真机上的视觉也是"量到哪就是哪"，不会拿图纸坐标去算。
2. 名义位姿只做两件事：判"物体还在不在该在的地方"（上面那两条线）， 和开局复位。
3. 开局复位（pose_info.reset_on_start: true）：只在节点启动后 20s 窗口内、 只做一次，把被盯的物体 set_pose 回名义位姿 —— 专门消掉 spawn 落地的 2~3mm。运行中的漂移必须报错，不能悄悄搬回去（那会抹掉"谁把它撞歪了" 的现场，还可能把工件从爪里拽走）。实现见 sim_truth_perception._maybe_startup_reset 与 sim_pose_service.set_model_pose_yaw。

⚠️ 复位/搬运用的是 Gazebo 的 `set_pose` 服务，命令行调用时 `--reqtype` 必须写
   `ignition.msgs.Pose`（写 `gz.msgs.Pose` 会报 "Unable to create request"，且不说是哪错了）。
   实现只有一份：`arm_sim_gazebo/sim_pose_service.py`。

---

6. 现状与进度

已就绪（端到端验证过）

部分	位置	说明
运行环境	scripts/install_sim_env.sh / sim_env.sh	独立 conda 环境 ros_gz_sim（Gazebo Fortress 6.16 + ROS 2 Humble）
世界	worlds/assembly.world	地面 + 工作台（顶面 z=0.40）+ D435（世界相机）
机器人描述	urdf/*.xacro	复用官方 flexiv_description，启动时归一化
控制器	config/controllers.yaml	控制器名与真机一致
启动器	src/arm_sim_gazebo 的 gazebo_sim_launcher	起 Gazebo → 发归一化 URDF → spawn → 拉控制器 → spawn 场景
相机	config/cameras.yaml + sim_camera_adapter	D405/D435 桥接到 ROS，内参/单位/frame 都已校正
触觉	config/tactile.yaml + sim_tactile_adapter	force_torque 三向力 + contact 布尔 → arm_msgs/TactileStatus
场景物体	models/ + config/scene.yaml	工件（带亮环的孔板）、通止规、规座，可校验摆放
夹爪	7 关节接入 ros2_control + sim_gripper_adapter	开口走毫米制（FK 反解，见 §4.4）；实测合拢→全开 27.09→127.02 mm，行程 99.93 mm；验收 check_sim_gripper.py

还没做

部分	缺什么	影响
视触觉图像	仿真的深度图/力矢量图只能是占位（空/NaN）	TactileStatus 的图字段在仿真里不可信；三力是可信的
夹具/料框	目前只有工件、规、规座	上料流程（若有）没法验
夹爪"夹住"判据	is_gripped 是推断（指令到合拢位但实际没跟上），不是测量	真机有 DH 状态字；仿真里最终仍应看触觉力

---

7. 有意保留的规格差异（不是 bug，但会让人困惑）

项	真机	仿真	后果
镜头畸变	有（d 非 0）	无（d 全 0、R 单位阵）	"仿真里用畸变矫正调出来的参数"搬到真机仍需重调
D435 图幅	1280x720	640x480	像素尺度不同，hole_detect 的像素门槛搬过去要重算
触觉单位	Orisys 形变任意单位（非牛顿）	牛顿 × force_scale	见 config/tactile.yaml 头部的"单位口径"；选错不报错，只是阈值不触发或一碰就炸
触觉图像	有真实深度图/力矢量图	占位	下游不用改代码，但图内容不可信
夹爪型号	DH PGEA-100-20-W（site.yaml 的 stroke_mm = 20 mm）	官方 GN01（行程 99.93 mm）	同一个"开 8 mm"两边几何都对，但"全开"对应的开口差 5 倍 —— 别把仿真的全开当真机的全开（启动日志会打出来）
夹爪指令路径	DH 走 USB-RS485 Modbus（IPC）	JointGroupPositionController + sim_gripper_adapter	上层服务名/消息类型一致（GripperExecute），所以算法不用改

---

8. 文件一览

devices/sim/gazebo/
├── manifest.yaml            # 设备清单：进度、配置声明、放置要求（与本文档同源）
├── README.md                # 本文档
├── config/
│   ├── cameras.yaml         # ★ 两台相机的唯一真相（话题/坐标系/视场/图幅）
│   ├── controllers.yaml     # 控制器配置（名字与真机一致）
│   ├── scene.yaml           # ★ 场景布局（工件/规/规座）+ 像素门槛
│   └── tactile.yaml         # ★ 触觉标定：单位口径、轴向映射（实测）、基线策略
├── urdf/
│   ├── arm.sim.urdf.xacro        # 顶层：拼装 + 前缀
│   ├── arm.sim.gazebo.xacro      # Gazebo 扩展（接触/摩擦参数）
│   ├── arm.sim.ros2_control.xacro# 控制器接口（7 轴 + 7 个夹爪关节）
│   └── arm.sim.sensors.xacro     # D405 + 触觉传感器（contact + force_torque）
├── worlds/assembly.world    # 环境：地面 + 台面 + D435 + 显式声明的系统插件
├── models/
│   ├── workpiece/           # 工件：带三个孔的铝板 + 两个检测用亮环
│   ├── gauge/               # 通止规：通端/套环/握柄/止端，接触参数刻意放软
│   └── gauge_stand/         # 规座：static，把规立住
└── sensor_plugins/          # （空）触觉走内置传感器 + 适配器，不需要自定义插件

配套代码：src/arm_sim_gazebo/（启动器、描述归一化、相机适配器、触觉适配器、场景逻辑） 配套脚本：scripts/run_sim_gz.sh（入口）、scripts/stop_sim_gz.sh（停止）、 scripts/lib/sim_procs.sh（仿真进程识别）、scripts/check_sim_cameras.py（相机验收）、 scripts/check_sim_scene.py（场景验收）、scripts/check_sim_gripper.py（夹爪验收） 单测：src/arm_sim_gazebo/test/（test_urdf_normalize / test_sim_cameras / test_sim_site / test_sim_tactile / test_robot_description / test_check_sim_scene / test_sim_gripper / test_sim_gripper_kin）