arm_dev
6922b18187
为什么
------
应用层只认 arm_msgs/srv/GripperExecute（开口毫米），真机与仿真必须同一套接口。
但 GN01 的 URDF 里 <mimic> 是**线性近似**：直接按线性系数下发 finger_width_joint，
"命令开 20 mm"在 Gazebo 里量出来是 21.75 mm —— 抓不住工件，而且看起来像算法问题。
实测（2026-09-24，Gazebo 内量两指尖间距）：
线性下发 20 mm → 实际 21.74 mm（偏差 1.74 mm，超 2 mm 抓取容差的前半步）
走 FK 反解 → 实际 20.00 mm（≤0.5 mm）
怎么改
------
- 新增 sim_gripper_kin.py：解析 URDF 正运动学，算出"主关节值 ↔ 真实开口 mm"的
单调曲线，反解用二分。曲线非单调/零行程时**主动报错**，不静默退化成线性。
- sim_gripper.py：开口换算全部走该曲线；容差统一改成毫米。
- sim_gripper_adapter.py：反馈改取**从动关节**（见下）；is_gripped 按毫米判定。
- 新增 check_sim_gripper.py：三层验收（服务/开口一致/指尖真实几何行程 + 对称性）。
踩到的坑
--------
- finger_width_joint 挂在无质量的 unused_finger_width_link 上，与手指**没有机械
连接**：命令到 0.100，它的 /joint_states 恒为 1.4e-08，6 个从动关节才准到
0.7854。反馈若取它，/gripper/status 会永远报"开口 0 mm"。
- 残留进程会伪造出"代码写错了"的现场：旧 sim_gripper_adapter 与新实例抢话题，
验收读到"指尖间距 0.03 mm"（真实 127 mm）；孤儿 static_transform_publisher
用 latched /tf_static 把旧相机外参按在新仿真上。
→ 新增 scripts/lib/sim_procs.sh；run_sim_gz.sh 用 setsid 独立进程组，
PGID 记进 log_sim/sim.pgid，stop 时整组 kill；启动前预检拒绝带病启动。
- 验收脚本自己也错过两次（把 TF 的米当毫米比 → 127 mm 显示成 0.13 mm；
用会转动指垫的中点漂移判对称 → 必然位移 16 mm）。判据改成"两侧各走多远"，
单位只在 _dist_mm 里换算一次。
验证
----
python3 -m pytest src/arm_sim_gazebo/test/ -q → 307 passed
python3 scripts/check_sim_gripper.py → 三层全过
第 2 层：请求/反馈开口最大偏差 < 0.5 mm
第 3 层：期望行程 vs 实际行程、左右行程差，均 < 1.5 mm
bash scripts/stop_sim_gz.sh --list → 整组列出（含 static tf）
文档
----
- 仿真手册：新增 §4.4（开口 mm 怎么换算）、坑 12（宽度关节不动、
反馈必须取从动关节）；停止一节改写成进程组语义。
- FRAMEWORK_RULES：新增 R4.11（残留进程会伪造现场）、R4.12（验收判据要挑
与实现无关的物理量）；R4.9 表补夹爪；清单补 13/14 两条。
- HANDBOOK：新增 §3.4「在仿真里开合夹爪」，补 §4.4 的两条新教训。
Co-authored-by: Cursor <cursoragent@cursor.com>
546 lines
31 KiB
Markdown
546 lines
31 KiB
Markdown
# 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_*.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` | ✔ 适配器每次启动都读 |
|
||
| 装配的悬高/速度 | **复用真机那套**（`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`），再按耦合关系反推开口。
|
||
|
||
---
|
||
|
||
## 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`）
|