From d32e6ae53fbf861ca96b902ee201fa615dcfbab3 Mon Sep 17 00:00:00 2001 From: arm_dev Date: Wed, 23 Sep 2026 17:13:00 +0800 Subject: [PATCH] =?UTF-8?q?P5c-6=20=E5=9C=BA=E6=99=AF=E7=89=A9=E6=96=99?= =?UTF-8?q?=E8=90=BD=E5=9C=B0=20+=20Gazebo=20=E4=B8=AD=E6=96=87=E6=89=8B?= =?UTF-8?q?=E5=86=8C=EF=BC=9A=E5=B7=A5=E4=BB=B6/=E9=80=9A=E6=AD=A2?= =?UTF-8?q?=E8=A7=84=E6=A8=A1=E5=9E=8B=E3=80=81=E5=9C=BA=E6=99=AF=E6=A0=A1?= =?UTF-8?q?=E9=AA=8C=E3=80=81=E4=B8=A4=E5=A4=84=E9=9D=99=E9=BB=98=E5=A4=B1?= =?UTF-8?q?=E6=95=88=E4=BF=AE=E5=A4=8D?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 一、场景物料（新增） devices/sim/gazebo/models/workpiece/ 带三个孔的铝板。碰撞体是"带方孔的平板"， 视觉额外加两个 φ14/φ7 亮环 —— 因为真机的 HoughCircles 就是靠这两个亮环找孔， 只按碰撞体建视觉会让"孔检测"在仿真里永远失败，而那看起来像算法问题。 devices/sim/gazebo/models/gauge/ 通止规：通端/套环/握柄/止端四段，各段异色 便于肉眼判断方向。接触刚度刻意比台面软 —— 位置控制器会"刚性压进去"， 用默认刚度会被弹飞（注释里写了原因，不是随手抄的参数）。 devices/sim/gazebo/models/gauge_stand/ 规座（static）。规直接站在台面上会立刻倒， 加个方孔底座把它立住。 三个模型都是 model.sdf + model.config，原点在**底面中心**，单位米。 二、场景配置与校验（新增） config/scene.yaml 物体不再写死在 world 里，改为 scene.yaml + 启动时动态 spawn。 理由：world 是"环境"（地面/台面/相机/系统插件），场景是"这一批活做什么"。 动 world 的风险面太大 —— 它只要丢一个系统声明，所有模型都 spawn 不进去（见旧坑 2）。 含 operating（相机到工件的距离等作业条件）与 gates_px（像素门槛）。 sim_scene.py 摆放校验：从 SDF **逐碰撞体**解析 AABB（不抄尺寸，避免漂移）， 检查原点在底面 / 不穿台面 / 不出界 / 物体之间不嵌入。 重叠判定必须逐碰撞体三维判，不能拿总包围盒相交 —— 正常装配里本来就有嵌套（规立在规座方孔里），粗判会误报， 而误报的后果是人把检查整个关掉，之后真重叠也抓不到。 另有成像几何：把亮环/孔间距按相机距离换算成像素，与门槛比对。 scripts/check_sim_scene.py 验收工具（退出码可用）：摆放 + 门槛漂移 + 成像几何 + 悬停高度 vs 工作空间 + 手外相机是否**完整**看得见工件 + Gazebo 实际位姿。 三、修掉两处静默失效（都是"看起来一切正常"） 1) parse_spawned_poses 一直是**死代码**：它按 "x: N / y: N / z: N" 解析， 而 Fortress 实际输出是方括号两行（XYZ / RPY）。对不上 → 永远返回空 → 调用方按设计"降级成跳过位姿核对" → 位姿检查**从来没跑过**， 而日志只说"跳过"，看起来像正常降级。 现在两种格式都支持，并用真实抓下来的输出样本做回归测试。 教训：宽容降级会把"没检查"伪装成"检查通过"。 2) stop_sim_gz.sh 的模式表里漏了 robot_state_publisher（而注释里写着它有覆盖）： 脚本报"已停止"，它却还在发上一份 /robot_description 与 TF， 下一次启动就会"改了描述却不生效"。 顺带修掉同源的两个问题：awk 用 -v 传模式导致它匹配到自己、每次复查都多报 一个空命令行的幽灵 PID；以及"杀的时候"和"复查的时候"两份扫描实现会各自漂移。 现在共用同一个 scan_pids()，模式改用环境变量传递，并在杀完之后**再扫一遍**： 有残留就报出来并非零退出 —— 下次再有漏项，不再静默。 实测：--list 列出全套 9 个进程；停止后 rc=0 且 pgrep 干净。 四、相机：D405 分辨率对齐真机 仿真的 D405 此前渲染 640x480，而真机跑 848x480（fx 337 而非 447）。后果是 58mm 的两孔间距成像只剩 85px，低于 hole_distance_min=90 —— 孔检测在仿真里根本不可能通过，而现象看起来像"算法不行"。 已把 xacro 与 cameras.yaml 一起改成 848x480，并把启动时的比对从"只比视场" 扩成"视场 + 图幅"（sim_cameras.check_camera_consistency）， 不一致时说明它对像素门槛的影响。 五、文档 devices/sim/gazebo/README.md 重写为中文手册：使用方法 / 建模要求 / 导入格式 / 配置方式（含"改哪里才真的生效"的接线表）/ 11 条实测坑（每条都有"怎么发现"）。 manifest.yaml 的进度、assets、description、坑清单与本轮状态对齐。 scripts/README.md 补两个验收工具与停止脚本；根 README 补仿真入口与手册链接。 cameras.yaml 里一处过期的函数名（check_fov_consistency）修正。 六、测试 新增 test_sim_scene.py（79 条，此前 sim_scene.py 完全没有单测 —— 这正是 位姿解析死掉却无人发现的原因）与 test_check_sim_scene.py（21 条）， 含三条回归：world_boxes 的偏移碰撞体不能被对称化放大（假重叠）、 center_bore 不能被 _bore 后缀吃掉、Fortress 位姿输出格式。 全量：src/arm_sim_gazebo + arm_interfaces + arm_application 共 515 通过。 同时附上 config/site.example.yaml 的重新生成（此前 site.yaml 的注释改动没同步， 它的 parity 测试是红的）。 Co-authored-by: Cursor --- README.md | 4 +- config/site.example.yaml | 13 +- devices/sim/gazebo/README.md | 536 ++++++++-- devices/sim/gazebo/config/cameras.yaml | 10 +- devices/sim/gazebo/config/scene.yaml | 146 +++ devices/sim/gazebo/manifest.yaml | 61 +- devices/sim/gazebo/models/gauge/model.config | 28 + devices/sim/gazebo/models/gauge/model.sdf | 193 ++++ .../gazebo/models/gauge_stand/model.config | 22 + .../sim/gazebo/models/gauge_stand/model.sdf | 107 ++ .../sim/gazebo/models/workpiece/model.config | 30 + devices/sim/gazebo/models/workpiece/model.sdf | 325 ++++++ devices/sim/gazebo/urdf/arm.sim.sensors.xacro | 30 +- devices/sim/gazebo/worlds/assembly.world | 34 +- scripts/README.md | 5 + scripts/check_sim_scene.py | 438 ++++++++ scripts/stop_sim_gz.sh | 155 +++ .../arm_sim_gazebo/sim_cameras.py | 78 +- .../arm_sim_gazebo/sim_scene.py | 851 ++++++++++++++++ .../launch/gazebo_sim.launch.py | 120 ++- .../test/test_check_sim_scene.py | 407 ++++++++ src/arm_sim_gazebo/test/test_sim_cameras.py | 138 ++- src/arm_sim_gazebo/test/test_sim_scene.py | 939 ++++++++++++++++++ 23 files changed, 4489 insertions(+), 181 deletions(-) create mode 100644 devices/sim/gazebo/config/scene.yaml create mode 100644 devices/sim/gazebo/models/gauge/model.config create mode 100644 devices/sim/gazebo/models/gauge/model.sdf create mode 100644 devices/sim/gazebo/models/gauge_stand/model.config create mode 100644 devices/sim/gazebo/models/gauge_stand/model.sdf create mode 100644 devices/sim/gazebo/models/workpiece/model.config create mode 100644 devices/sim/gazebo/models/workpiece/model.sdf create mode 100644 scripts/check_sim_scene.py create mode 100755 scripts/stop_sim_gz.sh create mode 100644 src/arm_sim_gazebo/arm_sim_gazebo/sim_scene.py create mode 100644 src/arm_sim_gazebo/test/test_check_sim_scene.py create mode 100644 src/arm_sim_gazebo/test/test_sim_scene.py diff --git a/README.md b/README.md index 6f7a9cf..207cd3d 100644 --- a/README.md +++ b/README.md @@ -53,6 +53,7 @@ bash ~/ros2/scripts/run_visual_servo_rt.sh --arm 89 --control | 双爪 ROS | `scripts/run_dual_gripper.sh` | | 入孔检测 | `scripts/run_hole_detect.sh --arm … --preview` | | Orisys 触觉 | `scripts/run_tactile.sh [--cli]` | +| Gazebo 仿真 | `scripts/run_sim_gz.sh [--gui]`（停：`scripts/stop_sim_gz.sh`） | | 看配置 | `scripts/show_site_config.sh` | 89 与 91 **各有** 手眼、相机序列号、锁高、named poses，禁止混用。 @@ -70,12 +71,13 @@ bash ~/ros2/scripts/run_visual_servo_rt.sh --arm 89 --control | [docs/GRIPPER.md](docs/GRIPPER.md) | PGEA + RGI | | [docs/HOLE_DETECT.md](docs/HOLE_DETECT.md) | 入孔检测 | | [docs/TACTILE.md](docs/TACTILE.md) | Orisys 视触觉 | +| [devices/sim/gazebo/README.md](devices/sim/gazebo/README.md) | Gazebo 仿真手册（使用方法 / 建模要求 / 导入格式 / 配置方式 / 避坑清单） | | [docs/INTERFACES.md](docs/INTERFACES.md) | ROS / 脚本接口 | | [docs/TRAJECTORIES.md](docs/TRAJECTORIES.md) | 轨迹回放 | ## 上传 git 时 -- **要提交**：`src/` `scripts/` `docs/` `config/site.example.yaml` `models/README.md` `models/**/*.labels.yaml` `handeye/` +- **要提交**：`src/` `scripts/` `docs/` `devices/` `config/site.example.yaml` `models/README.md` `models/**/*.labels.yaml` `handeye/` - **不要提交**：`config/site.yaml`（含本机 IP/序列号）、`handeye/tmp/`（临时产物）、`build/` `install/` `log/`、YOLO `.pt/.onnx/.engine`、`thirdparty/` `vendor/` - 规则已写在 `.gitignore` diff --git a/config/site.example.yaml b/config/site.example.yaml index e168114..f9ae25c 100644 --- a/config/site.example.yaml +++ b/config/site.example.yaml @@ -111,11 +111,15 @@ paths: # bash ~/ros2/scripts/export_yolo_trt.sh /home/USER/ros2/models/yolo/detect_best_0.onnx 640 # 4) yolo.imgsz 必须等于 ONNX 输入边长（当前 640） # 5) 改权重/imgsz 后重启：bash ~/ros2/scripts/run_visual_servo_rt.sh [--control] + # 6) 换完确认标签没丢：bash ~/ros2/scripts/weights.sh doctor --check-engines # 当前：detect_best_0.onnx（2026-09-04 从 ~/下载 移入，640 输入） yolo_weights: "/home/USER/ros2/models/yolo/detect_best_0.onnx" - # 备选模型：直接换成 models/yolo/ 下的其他 .onnx / .pt 绝对路径即可。 - # 换完建议确认标签没丢：bash ~/ros2/scripts/weights.sh doctor --check-engines - # 入孔 YOLO（backend=yolo 时把孔当框）。two_stage 用上面的工件权重。 + # 换模型：把 models/yolo/ 下别的 .onnx / .pt 绝对路径填进来即可。 + # 未在用的模型已移出仓库（体积大且干扰 `weights.sh list`），要恢复见 models/README.md。 + # 入孔 YOLO（backend=yolo 时把孔当框）。two_stage 用下面的工件权重。 + # 原先指向 hole_insert/best.pt —— 那个文件不存在（悬空引用）。这里改成实际存在的 + # detect_best_1.onnx，这样 backend: yolo 至少是能跑的。查悬空引用： + # bash ~/ros2/scripts/weights.sh doctor hole_weights: "/home/USER/ros2/models/hole_insert/detect_best_1.onnx" # 下面是缺省（未匹配到 robots[] 时）。真机以 robots[].paths 为准：89 / 91 各一份。 handeye_file: "/home/USER/ros2/handeye/results/89_eye_in_hand.yaml" @@ -339,6 +343,7 @@ gripper: # bash ~/ros2/scripts/run_hole_detect.sh --arm 89 --preview hole_detect: backend: "two_stage" # two_stage | yolo | hough + # backend=yolo 时用。原为 hole_insert/best.pt（文件不存在），已改为实际存在的模型。 weights: "/home/USER/ros2/models/hole_insert/detect_best_1.onnx" # 二阶段工件检测权重（YOLO 先框出工件，再 Hough 找 k1/k2 螺纹孔） workpiece_weights: "/home/USER/ros2/models/hole_insert/detect_best_1.onnx" @@ -491,7 +496,7 @@ sim: # 决定，且 config/controllers.yaml 里硬编码了 sim_joint1..7。 # 写了个不同的值会告警。要真改，三处必须一起改。 # - # 另外注意：`run_sim.sh` 走的是 mock 驱动（假数据 + RViz），**不是**物理仿真； + # 另外注意：`run_sim.sh` 走的仍是 mock 驱动（假数据 + RViz），不是物理仿真； # 物理仿真入口是 `scripts/run_sim_gz.sh`（Gazebo）。两者互不影响。 platform: "gazebo" world: "devices/sim/gazebo/worlds/assembly.world" diff --git a/devices/sim/gazebo/README.md b/devices/sim/gazebo/README.md index 29c0622..074e32a 100644 --- a/devices/sim/gazebo/README.md +++ b/devices/sim/gazebo/README.md @@ -1,148 +1,464 @@ -# devices/sim/gazebo/ —— Gazebo 仿真平台 +# devices/sim/gazebo/ —— Gazebo 仿真平台（中文手册） -> 这份文档记录的是**已经跑通的事实**和**实测踩到的坑**。仿真里绝大多数错误都是 -> 静默的（不报错、能出图、能查到 TF，结果却是错的），所以下面每条坑都写了 -> **怎么验**。改仿真之前先把"坑"那一节看完。 +> **这份文档是仿真的"使用说明书 + 建模规范 + 避坑清单"。** +> +> 读法建议： +> - 只想**跑起来** → 看 [第 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**（真物理、真接触） | ✔ | 验证接触/力控/几何 | +| `scripts/run_sim_gz.sh` | **Gazebo Fortress**（真物理、真接触） | ✔ | 验证接触/力控/几何、装配状态机 | **要验证装配的接触与力判据，只能用后者。** 前者永远不会产生接触力。 +判断当前在跑哪个：`ros2 topic echo /clock --once` —— 有 `/clock` 且时间在走，是 Gazebo。 + --- -## 第一部分：现状（截至 P5c-4，2026-09-23） +## 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 # ★ 场景摆放 + 孔检测几何验收 +``` + +机械臂应当**在重力下保持站住**。若它瘫下去或消失，说明描述有问题 —— +见 [坑 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` | 摆放自洽 + 像素门槛与真机一致 + 孔检测特征的像素值达标 + 手外相机**完整**看得见工件 | 物体悬空/穿模/嵌入、场景里的门槛与真机漂移、成像尺度不够、粗定位框不全 | + +### 1.4 停止 + +```bash +bash scripts/stop_sim_gz.sh # 稳妥地停掉整套仿真 +``` + +**不要用 `pkill -f gz`**：它会把调用它的那个 shell 一起杀掉（`-f` 匹配命令行， +而 `pkill` 自己的命令行里就含 `gz`），也会误杀同名的其他进程。`stop_sim_gz.sh` +按进程树精确识别，排除了自己和自己的祖先进程。 + +--- + +## 2. 建模要求 + +### 2.1 三条通用规矩（违反哪一条都会"看起来正常"） + +| 规矩 | 为什么 | 违反了会怎样 | +|------|--------|--------------| +| **单位必须是米** | Gazebo 没有单位系统，SDF 里的数字就是米 | 用毫米建的 mesh 会变成 1000 倍大的怪物，或者小到看不见 | +| **模型原点放在底面中心** | 放置时 `pose.z` 就等于"放在台面上的高度" | 原点在质心时物体一半埋进台面，或者悬空；`sim_scene` 会校验并报错 | +| **`` 必须与推理用的类别名一致** | 仿真里算出的坐标要与 YOLO 结果对齐 | 视觉算出的类别名与场景里的名字对不上，坐标静默错位 | + +`sim_scene.validate_scene()` 会在启动时把上面第 1、2 条自动查一遍 +（模型原点、是否穿台面/悬空/出界/互相嵌入），有问题的物体**不会**被 spawn 出去。 + +### 2.2 质量与惯性（``） + +**`` 不是可选的。** 缺了它 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 `` 与 `` 的分工 + +| | 作用 | 建模要求 | +|---|------|----------| +| `` | 决定物理：穿不穿、卡不卡、接触力多大 | 用**简单几何体**（box/cylinder），不要用 mesh —— mesh 碰撞在接触力计算上又慢又不稳 | +| `` | 决定"相机看见什么" | 可以加入**只为人眼/算法准备**的细节（亮环、色块），它们不参与物理 | + +**这两个可以不一样，而且这个框架有意让它们不一样**： + +`models/workpiece/model.sdf` 的 collision 是"一块带三个方孔的平板"， +而 visual 额外加了两个**亮环**（φ14 外 / φ7 内）来模拟真机上的压入衬套 —— +因为真机的 HoughCircles 就是靠这两个亮环找孔的。若只按 collision 建视觉， +仿真里"孔检测"会因为看不见环而永远失败，而这不是算法问题。 + +> ⚠️ 由此带来一条约定：**visual 的名字有意义**，`sim_scene.features_from_workpiece` +> 按名字前缀（`insert_*_ring` / `center_bore`）去读出特征尺寸，用来算像素值。 +> 改 visual 的名字必须同步改那个函数（有单测守着）。 + +### 2.4 接触参数（``）：只在必要时改，且要写清理由 + +默认接触参数（`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` | 目录名 = `` = 推理类别名 | `config/scene.yaml` → 启动时 spawn | +| 网格文件 | `models/<模型名>/meshes/` | `.stl` / `.dae` | **单位必须是米** | 被 SDF 的 `` 引用 | +| 相机声明 | `config/cameras.yaml` | YAML | `ros_ns` 必须等于应用的 `camera_name` | 桥接 + 适配器 + 静态 TF | +| 场景布局 | `config/scene.yaml` | YAML | — | 启动时 spawn + 像素门槛校验 | +| 触觉标定 | `config/tactile.yaml` | YAML | 话题名必须与 URDF 里的 `` 一致 | `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 # 元数据：、版本、指向 model.sdf、描述 +``` + +`model.config` 里的 `` **必须等于目录名**，也必须等于 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。**这个转换是有损的**： + +| 会被丢/被改的 | 后果 | 本框架的应对 | +|---------------|------|--------------| +| `` 标签 | 夹爪的从动指节不再跟随 → 夹爪"驱动了但不动" | `urdf_normalize` 把 mimic 关系**记下来**，改成把全部 7 个夹爪关节都接入 `ros2_control`，由控制器按记录的关系驱动 | +| 缺 `` 的 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` 的 `` | +| 关节前缀 | `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 的视场**和**图幅，不一致会告警并说明影响。 + +--- + +## 5. 实测踩到的坑 + +这些坑的共同特征是：**不报错**。话题在、图像有、TF 查得到、`create` 打印成功，但结果是错的。 +所以每条都写了"怎么发现"。 + +### 坑 1：官方 `flexiv_description` 直接喂 Gazebo 会被 `sdformat` 拒掉 + +7 轴本体缺 `` 包裹；夹爪有零质量 link 导致惯性叠加出 NaN。 +**只有 Gazebo 会暴露，RViz/MoveIt 完全正常。** + +→ 启动时归一化（`urdf_normalize.py`）。单独排查：`gazebo_sim_launcher --check`。 + +### 坑 2：world 里声明了任何 `` 都会挤掉默认系统 + +world 里只要有一个 ``，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` 字段，看起来"应该有"。 + +→ 三向力改由 `` 提供（挂在指尖关节上，读该关节传递的约束力）。 +接触布尔仍用 contact 传感器（它只有这个用途）。 + +> 附带一个教训：**光看消息定义判断不了运行时会填什么**，必须 `ign topic -e` 实测。 + +### 坑 8：`forcetorque-system` 也必须显式声明 + +与坑 2 同源。只声明了 `contact-system` 时，`force_torque` 传感器**话题根本不出现**， +而 URDF 里写得好好的。 + +→ `assembly.world` 里显式声明 `libignition-gazebo6-forcetorque-system.so`。 + +### 坑 9：`sdformat` 用**占位值**填它不认识/缺失的东西 + +这是坑 1 的"隐藏变体"，也是**验收检查本身**最容易失效的地方： + +| 你写的 | `sdformat` 的输出 | +|--------|-------------------| +| 有 `` 但没 `` | `__default__` | +| 有 `` 但没 `` | `__default_topic__` | +| `` 写在 `` 里而不是 `` 里 | 话题跑到默认作用域，订阅方永远收不到 | + +于是"检查这个字段是不是空"的代码**永远通过** —— 因为它不是空的，它是个占位字符串。 + +→ `robot_description.check_tactile_collisions()` 现在显式判这两个占位值 +（`__default__` / `__default_topic__`），并检查 `` 的位置对不对。 +有负向单测守着（`test_robot_description.py`）。 + +> 通用教训：**校验代码自己会静默失效**。凡是"检查 X 是否为空"的地方， +> 都要问一句"它会不会被填成一个假值"。 + +### 坑 10：`` 标签被 `sdformat` 丢掉 + +夹爪的从动指节在 URDF 里靠 `` 定义跟随关系，而 `sdformat` 转换时**把它丢了**。 + +症状是"夹爪控制器 active，但手指不动" —— 控制器在驱动一个不存在的关节。 + +→ `urdf_normalize` 在转换前把 mimic 关系**记录成一张表**（`mimic_relations()`）， +去掉标签，并把全部 7 个夹爪关节都接入 `ros2_control`，由适配器按关系驱动。 + +> 顺带：从动关节原本 `effort="0"`，归一化时一并改成可驱动的值， +> 否则它在控制器里是个"只能看不能动"的关节。 + +### 坑 11：`gz model -m <名> -p` 的输出格式没有稳定契约 + +不同版本的文本格式不同。验收脚本因此**宽松解析**，解析不出来时降级成 +"跳过位姿核对"，而不是报一个假错。 + +> 这是有意的取舍：**宁可漏报，不可误报**。误报会让人不再相信工具。 + +--- + +## 6. 现状与进度 + +### 已就绪（端到端验证过） | 部分 | 位置 | 说明 | |------|------|------| -| 运行环境 | `scripts/install_sim_env.sh` / `sim_env.sh` | 独立 conda 环境 `ros_gz_sim`（Gazebo Fortress 6.16 + ROS 2 Humble），与真机环境 `ros_flexiv` 隔离 | -| 世界 | `worlds/assembly.world` | 地面 + 工作台 + D435 + 通止规标记位 | -| 机器人描述 | `urdf/*.xacro` | 复用官方 `flexiv_description`，启动时**归一化**后喂 Gazebo | -| 控制器 | `config/controllers.yaml` | 控制器名与真机一致（`rizon_arm_controller` + `joint_state_broadcaster`） | -| 启动器 | `src/arm_sim_gazebo` 的 `gazebo_sim_launcher` | 起 Gazebo → 发归一化 URDF → spawn → 拉起控制器 | -| 相机 | `config/cameras.yaml` | 两台相机的**唯一真相**：话题、坐标系、视场 | +| 运行环境 | `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` | 实测开合 27→127mm | ### 还没做 | 部分 | 缺什么 | 影响 | |------|--------|------| -| `models/` | 工件（k0/k1/k2）与通止规模型 | 装配任务（P6）跑不起来 | -| `sensor_plugins/` | Orisys 三向力插件 | **无法验证力控**（S07 接触、S08 拧紧判据） | - -> **Orisys 视触觉没有现成的 Gazebo 插件。** 3 路切向力 + 1 路法向力 + 深度图 + -> 力矢量图这套数据必须自己写插件。建议做法：订阅 Gazebo 的接触传感器数据， -> 按接触深度与相对滑动速度合成一组"类触觉"读数，再按真实传感器的量纲发布到 -> 同样的话题上（`/tactile/{left,right}/status`，`arm_msgs/TactileStatus`）。 -> 在它做出来之前，仿真里跑 P6 只能验证**几何与流程**，力判据要等真机。 +| 视触觉图像 | 仿真的深度图/力矢量图只能是**占位**（空/NaN） | `TactileStatus` 的图字段在仿真里不可信；三力是可信的 | +| 夹具/料框 | 目前只有工件、规、规座 | 上料流程（若有）没法验 | +| 夹爪服务适配器 | `~/open|~/close|~/move` 服务（mm ↔ 弧度换算） | 仿真里目前只能直接发关节指令开合，无法走真机那套服务接口 | --- -## 第二部分：日常怎么用 +## 7. 有意保留的规格差异（不是 bug，但会让人困惑） -### 启动 - -```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 --no-gripper --no-sensors # 最省资源，只验运动学 -``` - -首次启动约需 10~20 秒（Gazebo 初始化 + spawn + 拉控制器）。 - -### 验收（★ 必做，别只看"有没有图像"） - -```bash -ros2 topic echo /clock --once # 仿真时间在走 -ros2 control list_controllers # 两个控制器 active -ros2 topic echo /joint_states --once # 7 个 sim_joint -python3 scripts/check_sim_cameras.py # ★ 相机的真正验收 -``` - -**`check_sim_cameras.py` 才是相机的验收标准。** 它把深度反投影到 world， -检查落点是否命中已知平面（地面 z=0、台面 z=0.40）。"有图像"证明不了什么 —— -详见下面的坑 4 与坑 5。 - -### 放置格式与命名要求 - -| 类型 | 放哪 | 格式 | 命名要求 | -|------|------|------|----------| -| 世界文件 | `worlds/` | `.world` / `.sdf` | `<场景名>.world`，如 `assembly.world` | -| 机器人模型 | `urdf/` | `.urdf` / `.xacro` | 关节前缀必须与真机不同（仿真用 `sim_`） | -| 工件/夹具网格 | `models/<模型名>/` | `<模型名>.sdf` + `meshes/` | **模型名必须与推理用的类别名一致**，如 `workpiece` | -| 传感器插件 | `sensor_plugins/` | `.so` 源码 + `CMakeLists.txt` | 插件类名以 `Orisys` 开头 | -| 网格文件 | `models/<模型名>/meshes/` | `.stl` / `.dae` | **单位必须是米**。用毫米建模的必须先缩放到米 | -| 相机声明 | `config/cameras.yaml` | YAML | `ros_ns` 必须等于应用启动时的 `camera_name` | - -### 改配置：改哪里才会真正生效 - -| 想改什么 | 改哪里 | 真的生效吗 | -|----------|--------|------------| -| 用哪个世界 | `site.yaml` → `sim.world`，或命令行 `--world` | ✔ 已接线（优先级：`--world` > `site.yaml` > 内置默认） | -| 相机的视场/坐标系/话题 | `config/cameras.yaml`，**并且**同步 SDF 里对应的 `horizontal_fov` | ✔ 启动时自动比对，不一致会告警 | -| 物理开关 | `site.yaml` → `sim.physics_enable` | ✘ **未接线**。要真关，改 `worlds/assembly.world` 里的 `` | -| 关节前缀 | `site.yaml` → `sim.joint_prefix` | ✘ **未接线**。前缀由 `urdf/arm.sim.urdf.xacro` 的 `arm_prefix` 决定，且 `config/controllers.yaml` 硬编码了 `sim_joint1..7` —— 三处要一起改 | -| 仿真里的悬高/速度 | **复用真机那套参数**（`servo.*` / `hole_detect.stage2.*`） | ✔ | - -> **设计约束**：仿真与真机必须共用同一套 `site.yaml` 参数。任何"仿真专用参数"都是 -> 未来"仿真调好的值搬到真机不生效"的隐患。只允许物理引擎自身的参数（重力、步长） -> 放在 `sim.*` 下。 -> -> ✘ 标记的两个键由 `arm_sim_gazebo.sim_site.unwired_warnings()` 守着：只要你写的值 -> 和实际状态不一致，启动时就会告警。**不要让它们重回静默。** +| 项 | 真机 | 仿真 | 后果 | +|----|------|------|------| +| 镜头畸变 | 有（`d` 非 0） | **无**（`d` 全 0、`R` 单位阵） | "仿真里用畸变矫正调出来的参数"搬到真机仍需重调 | +| D435 图幅 | 1280x720 | 640x480 | 像素尺度不同，`hole_detect` 的像素门槛搬过去要重算 | +| 触觉单位 | Orisys 形变任意单位（非牛顿） | 牛顿 × `force_scale` | 见 `config/tactile.yaml` 头部的"单位口径"；选错**不报错**，只是阈值不触发或一碰就炸 | +| 触觉图像 | 有真实深度图/力矢量图 | 占位 | 下游不用改代码，但图内容不可信 | --- -## 第三部分：实测踩到的坑（都属于"静默失效"） - -这些坑的共同特征是：**不报错**。话题在、图像有、TF 查得到、`create` 打印成功， -但结果是错的。所以每条都配了"怎么发现"。 - -| # | 坑 | 症状 | 怎么发现 / 怎么修 | -|---|----|------|-------------------| -| 1 | 官方 `flexiv_description` 直接喂 Gazebo 会被 `sdformat` 拒掉：7 轴本体缺 `` 包裹；夹爪有零质量 link 导致惯性叠加出 NaN | 只有 Gazebo 拒绝，RViz/MoveIt 完全正常 | 启动时**归一化**（`urdf_normalize.py`）。单独排查：`gazebo_sim_launcher --check` | -| 2 | world 里只要声明了**任何一个** ``，就会挤掉默认的 Physics / UserCommands / SceneBroadcaster | 少了 UserCommands → `/world/<名>/create` 服务不存在 → 模型根本没进 Gazebo，而 `create` 仍打印 `OK creation of entity`；控制器永远起不来 | 启动时会检查并告警。修法：在 world 里显式声明这三个系统 | -| 3 | 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` 负责 | -| 4 | **gz 发的 `camera_info` 是错的**：恒为 `fx=fy=277, cx=160, cy=120`（它的默认 60° 视场 + 半分辨率主点），而**渲染**用的是配置的 87° 视场 | 照抄它做三维反投影，点会偏**半米量级**；单目预览完全正常 | 用已知地面平面做最小二乘反解内参，得到 `fy=337.2 / cy=239.5`，与 `640/(2·tan(87°/2))=337.2`、`480/2=240` 吻合（残差中位 0.2mm）。已改为由 `sim_cameras.intrinsics()` 按 `cameras.yaml` 的视场**重算并覆盖** | -| 5 | **相机位姿的 `rpy` 顺序**：URDF 相机是"X 朝前"约定，**视线是本体 X 轴**；而 `rpy` 复合顺序是 `Rz·Ry·Rx`，"先俯 50° 再偏航 180°"会把视线翻到**背面** | D435 背对台面、只看得见地面。什么都不报，**而且几何自洽**（点确实落在真实地面上），只查数值发现不了 | 用 `scripts/check_sim_cameras.py` 检查"该看见的平面有没有看见"。已修正 + 加回归测试 | - -关于坑 4：代价是 `cameras.yaml` 的 `horizontal_fov` 必须与 SDF 真正渲染用的值一致。 -启动时会自动比对并告警（`sim_cameras.check_fov_consistency`）。 - -关于相机还有两条**规格差异**，是有意保留的（不是 bug，但会让人困惑）： - -- **畸变**：gz 不模拟镜头畸变（`d` 全 0，`R` 单位阵）。所以"仿真里用畸变矫正 - 调出来的参数"搬到真机仍需重调。 -- **分辨率**：真机 D405 是 848x480、D435 是 1280x720，仿真建模用 640x480。 - 对"算法能不能跑通"没影响（内参从 `cameras.yaml` 自洽推导）， - 但"仿真里调好的像素阈值搬到真机"会因像素尺度不同而需要重调。 - ---- - -## 相关文件一览 +## 8. 文件一览 ``` devices/sim/gazebo/ -├── manifest.yaml # 设备清单：进度、配置声明、放置要求 +├── manifest.yaml # 设备清单：进度、配置声明、放置要求（与本文档同源） ├── README.md # 本文档 ├── config/ -│ ├── cameras.yaml # ★ 两台相机的唯一真相（话题/坐标系/视场） -│ └── controllers.yaml # 控制器配置（名字与真机一致） -├── urdf/ # 仿真用机器人描述（含 ros2_control 与传感器） -├── worlds/assembly.world # 场景 -├── models/ # ✘ 待做：工件/通止规/夹具 -└── sensor_plugins/ # ✘ 待做：Orisys 触觉插件 +│ ├── 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/check_sim_cameras.py`（相机验收）。 +配套代码：`src/arm_sim_gazebo/`（启动器、描述归一化、相机适配器、触觉适配器、场景逻辑） +配套脚本：`scripts/run_sim_gz.sh`（入口）、`scripts/stop_sim_gz.sh`（停止）、 +`scripts/check_sim_cameras.py`（相机验收）、`scripts/check_sim_scene.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`） diff --git a/devices/sim/gazebo/config/cameras.yaml b/devices/sim/gazebo/config/cameras.yaml index a8f0730..4d81fba 100644 --- a/devices/sim/gazebo/config/cameras.yaml +++ b/devices/sim/gazebo/config/cameras.yaml @@ -36,7 +36,7 @@ # 因此 sim_camera_adapter 用下面这份 horizontal_fov **重算**并覆盖内参。 # # 所以每台相机必须声明 horizontal_fov，且**必须与 SDF 里真正渲染用的值一致**。 -# 它有两个来源，必须同步（启动时会自动比对并告警，见 sim_cameras.check_fov_consistency）： +# 它有两个来源，必须同步（启动时会自动比对并告警，见 sim_cameras.check_camera_consistency）： # 腕上相机 → devices/sim/gazebo/urdf/arm.sim.sensors.xacro 的 horizontal_fov # 世界相机 → devices/sim/gazebo/worlds/assembly.world 里该 的 horizontal_fov # ============================================================================= @@ -59,7 +59,13 @@ cameras: # 仿真的传感器也按这个建，这样两边的像素尺度接近。 # 内参由它和图像尺寸算出：fx = fy = W / (2*tan(hfov/2))，cx=W/2，cy=H/2。 horizontal_fov: 1.5184 - width: 640 + # 图像尺寸必须与真机一致：真机 D405 跑 848x480@30（site.yaml 的 camera.width/height）。 + # 这**不是**显示偏好 —— 阶段二的孔检测在原始分辨率图上跑 HoughCircles， + # hole_detect.yaml 的像素门槛（hough_min_radius / hole_distance_min）只在这个 + # 尺度下才对。曾经用 640x480（fx 337 而非真机 447），58mm 的两孔间距 + # 成像只剩 85px，低于 hole_distance_min=90 → 孔检测在仿真里根本不可能通过。 + # 启动时会比对这里与 xacro 宏默认值，不一致会告警（sim_cameras）。 + width: 848 height: 480 note: 仿真里由 sim_link7 上的 d405_rgbd 传感器提供 diff --git a/devices/sim/gazebo/config/scene.yaml b/devices/sim/gazebo/config/scene.yaml new file mode 100644 index 0000000..bae44fc --- /dev/null +++ b/devices/sim/gazebo/config/scene.yaml @@ -0,0 +1,146 @@ +# ============================================================================= +# 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 ` 用的名字）。 +# model = models_dir 下的子目录名 / model.config 里的 。 +# 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 里有提示交互），仿真里先立在座上， + 便于确认夹爪能不能够到、开合范围够不够。 diff --git a/devices/sim/gazebo/manifest.yaml b/devices/sim/gazebo/manifest.yaml index 5517ceb..ee6204b 100644 --- a/devices/sim/gazebo/manifest.yaml +++ b/devices/sim/gazebo/manifest.yaml @@ -3,7 +3,7 @@ # # Gazebo 仿真平台清单。 # -# 进度（截至 P5c-4，如实记录，不要凭印象改）： +# 进度（截至 P5c-6，如实记录，不要凭印象改）： # ✔ 环境 独立 conda 环境 ros_gz_sim（Gazebo Fortress + ROS 2 Humble） # 安装见 scripts/install_sim_env.sh，激活见 scripts/sim_env.sh # ✔ 世界文件 worlds/assembly.world（sdformat 校验通过） @@ -14,8 +14,12 @@ # 起 Gazebo → 发归一化 URDF → spawn → 拉起两个控制器（active） # 入口脚本 scripts/run_sim_gz.sh（此前缺失，被 install_sim_env.sh # 与 build_sim.sh 的提示语引用，照做会报文件不存在） -# ✘ 场景模型 models/ 还是空的（工件/通止规模型未做） -# ✘ 触觉插件 sensor_plugins/ 还是空的（Orisys 三向力插件未做） +# ✔ 场景模型 models/ 已有工件（带检测亮环的孔板）、通止规、规座 +# 布局在 config/scene.yaml，启动时动态 spawn 并做摆放/几何校验 +# ✔ 触觉 **不需要自定义插件**：用 Gazebo 内置的 force_torque（三向力） +# + contact（接触布尔）传感器，由 sim_tactile_adapter 合成 +# arm_msgs/TactileStatus。标定与轴向映射在 config/tactile.yaml +# （轴向是实测的，不是猜的）。视触觉图像只能是占位。 # ✔ 相机出图 D405/D435 已注册并桥接到 ROS，且**几何正确性已验证** # 桥接+适配：config/cameras.yaml → sim_cameras（纯逻辑： # 话题映射 / 深度换算 / 内参，单测覆盖） @@ -24,7 +28,9 @@ # 实测：d435 台面 11.9% / 地面 75.3%；d405 台面 37.8% / 地面 54.0% # ✔ 配置接线 sim.world 现在真的从 config/site.yaml 读（此前硬编码，改了无效） # -# 所以：D405/D435 可用，触觉还要等 P5c-5，装配状态机（P6）依赖它。 +# 所以：相机、触觉三力、场景物体都可用，装配状态机（P6）可以在这里验几何与接触。 +# 仍然做不到的：视触觉的**图像**（深度图/力矢量图只有占位），以及走真机那套 +# 夹爪服务接口（仿真里目前直接发关节指令）。 # # 实测踩到的坑（都属于"静默失效"，后续改仿真务必先看这几条）： # 1. 官方 flexiv_description 直接喂 Gazebo 会被 sdformat 拒掉：7 轴本体缺 @@ -49,6 +55,22 @@ # d435 就踩了这个坑：相机背对台面、只看得见地面，却什么都不报， # 而且几何自洽（点确实落在真实地面上），只查数值发现不了。 # 已修正并加回归测试（test_robot_description 的 d435 位姿用例）。 +# 6. 接触传感器的消息是**事件式**的：只在接触时发，离开时不发。写成 +# "收到=接触/没收到=不管"会让 contact 永远停在 True。判据必须是 +# "最近 contact_timeout_s 内收到过"。 +# 7. **contact-system 发的 Contacts 消息里没有力**（wrenches 恒空，实测确认）。 +# 消息结构里有这个字段，看起来"应该有"—— 这是最耗时的一个坑。三向力改由 +# force_torque 传感器提供；且它所需的 forcetorque-system 也必须像坑 2 那样 +# 在 world 里显式声明，否则话题根本不出现。 +# 8. **sdformat 会用占位值填缺失的东西**： 缺 → "__default__"， +# 缺 → "__default_topic__"。于是"检查字段是否为空"的校验代码永远通过。 +# check_tactile_collisions 已显式判这两个占位值（有负向单测）。 +# 通用教训：校验代码自己会静默失效。 +# 9. ** 标签会被 sdformat 丢掉**：夹爪从动指节不再跟随（控制器 active +# 但手指不动）。解法是在归一化时记录 mimic 关系、去掉标签、把全部 7 个夹爪 +# 关节都接入 ros2_control（从动关节原先 effort=0，也一并修掉）。 +# 10. `ign model -m <名> -p` 的文本输出没有稳定契约，验收脚本按"宽松解析 + +# 解析不出来就跳过"处理 —— 宁可漏报，不可误报。 # ============================================================================= id: gazebo @@ -63,11 +85,14 @@ description: > 物理仿真平台。环境、世界、机器人描述、控制器与启动器均已就绪并端到端验证 （起 Gazebo → spawn → 两个控制器 active → 机械臂在重力下站住）。 D405/D435 已桥接到 ROS，且几何正确性有自动验收（scripts/check_sim_cameras.py）。 - 场景模型（工件/通止规）与 Orisys 触觉插件尚未实现； + 场景物体（工件/通止规/规座）在 devices/sim/gazebo/models/，布局在 config/scene.yaml， + 摆放与成像几何有自动验收（scripts/check_sim_scene.py）。 + 触觉用 Gazebo 内置的 force_torque + contact 传感器，由 sim_tactile_adapter + 合成 arm_msgs/TactileStatus；轴向映射是实测的（config/tactile.yaml）。 + 视触觉的**图像**（深度图/力矢量图）在仿真里只能是占位，不可信。 sim.world 已真正从 config/site.yaml 读取（physics_enable / joint_prefix 尚未接线， 写了不一致的值会告警而非静默忽略，见文件头 config_schema）。 - 注意：Orisys 视触觉没有现成 Gazebo 插件，自定义插件做出来之前， - 仿真里无法验证通止规装配的 S07 接触与 S08 拧紧力判据。 + 手册见 devices/sim/gazebo/README.md（含建模要求、导入格式、配置方式、避坑清单）。 # --- 物料放置要求 ---------------------------------------------------------- assets: @@ -81,11 +106,18 @@ assets: - path: models required: false what: 工件 / 夹具 / 通止规模型 - format: 每个模型一个子目录，内含 <模型名>.sdf 与 meshes/ + format: > + 每个模型一个子目录，内含 model.sdf 与 model.config（两个文件名都是固定的， + Gazebo 按这个约定查找）。原点必须在**底面中心**，这样 scene.yaml 里的 + pose.z 就等于"放在台面上的高度"。碰撞体用 box/cylinder 等简单几何， + 视觉可以额外加只服务于算法的细节（如工件上的检测亮环）。 naming: > - 模型名必须与推理用的类别名一致（如 workpiece），否则仿真里算出的坐标 - 与推理结果对不上。网格单位必须是米，用毫米建模的必须先缩放。 - where_used: Gazebo 场景加载 + 目录名 = SDF 里的 = model.config 里的 = 推理用的类别名 + （如 workpiece），四者必须一致，否则仿真里算出的坐标与推理结果对不上。 + 网格单位必须是米，用毫米建模的必须先缩放。 + where_used: > + 由 config/scene.yaml 的 objects[].model 引用，启动时动态 spawn； + 摆放合法性与成像几何由 scripts/check_sim_scene.py 验收。 - path: urdf required: false @@ -96,10 +128,13 @@ assets: - path: sensor_plugins required: false - what: 传感器插件（含 Orisys 触觉自定义插件） + what: 传感器插件（自定义 C++ 插件用；当前**为空**） format: 插件源码 + CMakeLists.txt naming: 插件类名以 Orisys 开头 - where_used: 仿真传感器数据发布 + where_used: > + 目前没有使用者：触觉走 Gazebo 内置的 force_torque + contact 传感器， + 由 sim_tactile_adapter（Python）合成 TactileStatus，无需自研插件。 + 保留此目录用于"以后真需要写插件"的场景（如自研视触觉弹性体成像）。 # --- 适配器 ---------------------------------------------------------------- driver: diff --git a/devices/sim/gazebo/models/gauge/model.config b/devices/sim/gazebo/models/gauge/model.config new file mode 100644 index 0000000..d76ebb2 --- /dev/null +++ b/devices/sim/gazebo/models/gauge/model.config @@ -0,0 +1,28 @@ + + + + gauge + 1.0 + model.sdf + + + ros2 通止规装配项目 + + + + + 螺纹通止规：通端 φ6.0×30mm（朝下，要能穿过 φ7 孔）、 + 夹持段 φ30×45mm（夹爪夹这里）、止端 φ8.0×8mm（朝上，应通不过 φ7 孔）。 + 总长 86mm，钢件，约 0.265kg。 + + 螺纹不建模（旋入靠状态机的 screw_pitch_mm 做运动学同步，不是螺纹几何）， + 接触刚度故意给软（kp=5e4）以匹配位置控制夹爪 —— 两处原因都写在 model.sdf 文件头。 + + diff --git a/devices/sim/gazebo/models/gauge/model.sdf b/devices/sim/gazebo/models/gauge/model.sdf new file mode 100644 index 0000000..8095060 --- /dev/null +++ b/devices/sim/gazebo/models/gauge/model.sdf @@ -0,0 +1,193 @@ + + + + + + false + false + + + + + + 0.265 + + 1.784e-04 + 0 + 0 + 1.784e-04 + 0 + 2.980e-05 + + + + + + 0 0 0.015 0 0 0 + 0.0030.030 + + 0.250.25 + + + 5e4 + 5e2 + 0.01 + 0.0005 + + + + + + + + 0 0 0.0315 0 0 0 + 0.0080.003 + + 0.250.25 + + 5e45e20.010.0005 + + + + + + + 0 0 0.0555 0 0 0 + 0.0150.045 + + 0.40.4 + + 5e45e20.010.0005 + + + + + + + 0 0 0.082 0 0 0 + 0.0040.008 + + 0.250.25 + + 5e45e20.010.0005 + + + + + + + + 0 0 0.015 0 0 0 + 0.0030.030 + + 0.08 0.08 0.09 1 + 0.12 0.12 0.13 1 + 0.1 0.1 0.1 1 + + + + 0 0 0.0315 0 0 0 + 0.0080.003 + + 0.7 0.7 0.68 1 + 0.8 0.8 0.78 1 + + + + + 0 0 0.0555 0 0 0 + 0.0150.045 + + 0.75 0.75 0.72 1 + 0.85 0.85 0.82 1 + 0.3 0.3 0.3 1 + + + + + 0 0 0.082 0 0 0 + 0.0040.008 + + 0.6 0.05 0.05 1 + 0.8 0.08 0.08 1 + + + + + + diff --git a/devices/sim/gazebo/models/gauge_stand/model.config b/devices/sim/gazebo/models/gauge_stand/model.config new file mode 100644 index 0000000..53f9fdb --- /dev/null +++ b/devices/sim/gazebo/models/gauge_stand/model.config @@ -0,0 +1,22 @@ + + + + gauge_stand + 1.0 + model.sdf + + + ros2 通止规装配项目 + + + + + 通止规座：60×60×32mm 方块、中心 7×7 贯通方孔，静态（工装，不该被推走）。 + + 存在的唯一理由：让通止规能立住。规的支撑面只有 φ6，高 86mm， + 直接立在台面上会在 0.2 秒内倒掉（实测 RPY 变成躺平）—— 详见 model.sdf 文件头。 + 规座与规的关系是"插在孔里"，属于正常嵌套，不是碰撞重叠。 + + diff --git a/devices/sim/gazebo/models/gauge_stand/model.sdf b/devices/sim/gazebo/models/gauge_stand/model.sdf new file mode 100644 index 0000000..ff399e3 --- /dev/null +++ b/devices/sim/gazebo/models/gauge_stand/model.sdf @@ -0,0 +1,107 @@ + + + + + + true + false + + + + + + 0 -0.01675 0.016 0 0 0 + 0.060 0.0265 0.032 + 0.60.6 + + + 0 0.01675 0.016 0 0 0 + 0.060 0.0265 0.032 + 0.60.6 + + + -0.01675 0 0.016 0 0 0 + 0.0265 0.007 0.032 + 0.60.6 + + + 0.01675 0 0.016 0 0 0 + 0.0265 0.007 0.032 + 0.60.6 + + + + 0 -0.01675 0.016 0 0 0 + 0.060 0.0265 0.032 + + 0.25 0.28 0.32 1 + 0.33 0.36 0.40 1 + + + + 0 0.01675 0.016 0 0 0 + 0.060 0.0265 0.032 + + 0.25 0.28 0.32 1 + 0.33 0.36 0.40 1 + + + + -0.01675 0 0.016 0 0 0 + 0.0265 0.007 0.032 + + 0.25 0.28 0.32 1 + 0.33 0.36 0.40 1 + + + + 0.01675 0 0.016 0 0 0 + 0.0265 0.007 0.032 + + 0.25 0.28 0.32 1 + 0.33 0.36 0.40 1 + + + + + + diff --git a/devices/sim/gazebo/models/workpiece/model.config b/devices/sim/gazebo/models/workpiece/model.config new file mode 100644 index 0000000..0206c87 --- /dev/null +++ b/devices/sim/gazebo/models/workpiece/model.config @@ -0,0 +1,30 @@ + + + + workpiece + 1.0 + model.sdf + + + ros2 通止规装配项目 + + + + + 通止规装配的工件：140×100×12mm 板，中心 φ40 光孔（会被检测的 max_radius 滤掉）， + 两侧各一个 φ7 光孔、孔心间距 58mm、孔口带 φ14 亮环（二阶段 HoughCircles 检测的目标）。 + + 详细说明（尺寸怎么反推出来的、哪些是近似）见 model.sdf 文件头与 + devices/sim/gazebo/README.md 的"仿真 ≠ 真机"一节。 + + diff --git a/devices/sim/gazebo/models/workpiece/model.sdf b/devices/sim/gazebo/models/workpiece/model.sdf new file mode 100644 index 0000000..5f1541a --- /dev/null +++ b/devices/sim/gazebo/models/workpiece/model.sdf @@ -0,0 +1,325 @@ + + + + + + + false + false + + + + + + 0.454 + + 3.804e-04 + 0 + 0 + 7.404e-04 + 0 + 1.110e-03 + + + + + + + + 0 -0.035 0.006 0 0 0 + 0.140 0.030 0.012 + 0.60.6 + + + + + -0.045 -0.01175 0.006 0 0 0 + 0.050 0.0165 0.012 + 0.60.6 + + + + 0.045 -0.01175 0.006 0 0 0 + 0.050 0.0165 0.012 + 0.60.6 + + + + + -0.05125 0 0.006 0 0 0 + 0.0375 0.007 0.012 + 0.60.6 + + + + -0.02275 0 0.006 0 0 0 + 0.0055 0.007 0.012 + 0.60.6 + + + 0.02275 0 0.006 0 0 0 + 0.0055 0.007 0.012 + 0.60.6 + + + 0.05125 0 0.006 0 0 0 + 0.0375 0.007 0.012 + 0.60.6 + + + + + -0.045 0.01175 0.006 0 0 0 + 0.050 0.0165 0.012 + 0.60.6 + + + 0.045 0.01175 0.006 0 0 0 + 0.050 0.0165 0.012 + 0.60.6 + + + + + 0 0.035 0.006 0 0 0 + 0.140 0.030 0.012 + 0.60.6 + + + + + 0 -0.035 0.006 0 0 0 + 0.140 0.030 0.012 + + 0.35 0.36 0.38 1 + 0.42 0.43 0.45 1 + + + + -0.045 -0.01175 0.006 0 0 0 + 0.050 0.0165 0.012 + + 0.35 0.36 0.38 1 + 0.42 0.43 0.45 1 + + + + 0.045 -0.01175 0.006 0 0 0 + 0.050 0.0165 0.012 + + 0.35 0.36 0.38 1 + 0.42 0.43 0.45 1 + + + + -0.05125 0 0.006 0 0 0 + 0.0375 0.007 0.012 + + 0.35 0.36 0.38 1 + 0.42 0.43 0.45 1 + + + + -0.02275 0 0.006 0 0 0 + 0.0055 0.007 0.012 + + 0.35 0.36 0.38 1 + 0.42 0.43 0.45 1 + + + + 0.02275 0 0.006 0 0 0 + 0.0055 0.007 0.012 + + 0.35 0.36 0.38 1 + 0.42 0.43 0.45 1 + + + + 0.05125 0 0.006 0 0 0 + 0.0375 0.007 0.012 + + 0.35 0.36 0.38 1 + 0.42 0.43 0.45 1 + + + + -0.045 0.01175 0.006 0 0 0 + 0.050 0.0165 0.012 + + 0.35 0.36 0.38 1 + 0.42 0.43 0.45 1 + + + + 0.045 0.01175 0.006 0 0 0 + 0.050 0.0165 0.012 + + 0.35 0.36 0.38 1 + 0.42 0.43 0.45 1 + + + + 0 0.035 0.006 0 0 0 + 0.140 0.030 0.012 + + 0.35 0.36 0.38 1 + 0.42 0.43 0.45 1 + + + + + + -0.029 0 0.010 0 0 0 + 0.0070.001 + + + 0.85 0.85 0.82 1 + 0.95 0.95 0.90 1 + 0.2 0.2 0.2 1 + + + + -0.029 0 0.0075 0 0 0 + 0.00350.007 + + + 0.02 0.02 0.02 1 + 0.03 0.03 0.03 1 + + + + + 0.029 0 0.010 0 0 0 + 0.0070.001 + + 0.85 0.85 0.82 1 + 0.95 0.95 0.90 1 + 0.2 0.2 0.2 1 + + + + 0.029 0 0.0075 0 0 0 + 0.00350.007 + + 0.02 0.02 0.02 1 + 0.03 0.03 0.03 1 + + + + + + 0 0 0.007 0 0 0 + 0.0200.010 + + 0.02 0.02 0.02 1 + 0.03 0.03 0.03 1 + + + + + + diff --git a/devices/sim/gazebo/urdf/arm.sim.sensors.xacro b/devices/sim/gazebo/urdf/arm.sim.sensors.xacro index c7b7f74..c32278c 100644 --- a/devices/sim/gazebo/urdf/arm.sim.sensors.xacro +++ b/devices/sim/gazebo/urdf/arm.sim.sensors.xacro @@ -47,11 +47,39 @@ d405_roll:=0.0 d405_pitch:=0.0 d405_yaw:=0.0 - image_width:=640 + image_width:=848 image_height:=480 image_hz:=30 "> + + - - true - 0.15 0.35 0.755 0 0 0 - - - 0.040.004 - - 0.1 0.6 0.9 1 - 0.1 0.6 0.9 1 - - - -