orisys/ros_contorl
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
main
ros_contorl/devices/sim/gazebo/README.md
T
arm_dev c1cd862eea 真机落地前的仿真补齐：夹持/腕力/真值位姿，让仿真能验证 S07–S09 
这批是 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。
2026-09-28 10:17:06 +08:00

693 lines
39 KiB
Markdown
Raw Permalink Blame History

This file contains ambiguous Unicode characters
This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.
# devices/sim/gazebo/ —— Gazebo 仿真平台（中文手册）
> **这份文档是仿真的"使用说明书 + 建模规范 + 避坑清单"。**
>
> 读法建议：
> - 只想**跑起来** → 看 [第 1 节 使用方法](#1-使用方法)。
> - 要**加一个工件/夹具/机器人** → 看 [第 2 节 建模要求](#2-建模要求) 与 [第 3 节 导入格式](#3-导入格式)。
> - 要**改参数** → 看 [第 4 节 配置方式](#4-配置方式)（有一张"改哪里才真的生效"的表，**先看它**）。
> - **改仿真之前必须看完** [第 5 节 实测踩到的坑](#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
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](#坑-3conda-环境里-gz-插件路径缺-conda_prefixlib) 和 `scripts/install_sim_env.sh` 顶部的长注释。
### 1.2 启动
```bash
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 验收（★ 必做，别只看"有没有图像"）
```bash
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](#坑-1官方-flexiv_description-直接喂-gazebo-会被-sdformat-拒掉)。
> ⚠️ `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
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](#坑-1官方-flexiv_description-直接喂-gazebo-会被-sdformat-拒掉)）。
- 质量与真实值同量级即可，不必精确。装配里要紧的是**质量比**：
工件太重会让夹爪"压进去"的读数异常大（见 `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 落地判据：怎么知道模型建对了
```bash
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：
```yaml
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](#坑-2world-里声明了任何-plugin-都会挤掉默认系统)）。
### 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 改相机图幅会牵动三处（最常见的漏改）
```bash
# 想改 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`），所以应用层代码
不用为仿真改一行：
```bash
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`）
Reference in New Issue View Git Blame Copy Permalink