arm_dev
55e5f8f20b
`--arm 89 --sim` 是个**合法且有意的**用法（run_gauge_assembly.sh 里
_HAD_ARM 那段写着"想用仿真核对某台真机的点位时会这么用"），但手眼真值导出
传的是 ${ROBOT_SN} —— 于是帧名成了 Rizon4-063989_flange，而仿真 URDF 里是
sim_flange。导出先干等 20s，再报"仿真栈起来了吗？"，把人引去重启一个明明在
跑的仿真。文档 §7 里写的就是这条命令，所以照着敲 100% 失败。
真值必须按**仿真台**的帧名导出，与"用哪台臂的点位"无关 → 固定用仿真台别名
（SIM_ARM_ALIAS 可覆盖，默认 sim）。
顺带把 §7 那组验收命令补成真能跑的：
· 以前只写"先编"，没说**怎么起仿真**，也没说**起之前要清干净** ——
这次就是在这两步上栽的，新增 §6.5 把三步（编 / 清 / 起）写全；
· "arm_application 子集 454 passed"配的那条裸 `PYTHONPATH=... pytest`
命令实测已经收集阶段就 error（导入不到 ament），改成走 run_unit_tests.sh
并更新数字；总测试数 1118 → 1128。
验证：`bash scripts/run_gauge_assembly.sh --arm 89 --sim --dry-run` 现在
state=DONE ok=True 2/2 孔、退出码 0（修改前必然退出码 1）。
Co-authored-by: Cursor <cursoragent@cursor.com>
404 lines
27 KiB
Markdown
404 lines
27 KiB
Markdown
# 通止规装配（P6 看板流程）
|
||
|
||
> 一句话：**把通止规（螺纹规）从"拿着"做到"拧进 k1、拧出、拧进 k2、拧出、回 home"**，
|
||
> 全过程由一台状态机盯着，任何一步不满足判据就停在那里、说清原因，而不是"接着往下走"。
|
||
>
|
||
> 入口：`bash ~/ros2/scripts/run_gauge_assembly.sh --arm 89`
|
||
> 配置：`config/site.yaml` → `gauge_assembly:` 段（**每个键都有中文注释，先读那一段**）
|
||
> 代码：`src/arm_application/arm_application/gauge_*.py`（纯逻辑）+ `gauge_assembly_node.py`（ROS 壳）
|
||
|
||
---
|
||
|
||
## 1. 先跑干跑（不发任何运动指令）
|
||
|
||
```bash
|
||
source ~/ros2/scripts/env.sh
|
||
bash ~/ros2/scripts/run_gauge_assembly.sh --arm 89 --dry-run
|
||
```
|
||
|
||
干跑会：S00 自检全跑一遍 → S01…S11 每步只打印"我打算做什么" → 最后打印
|
||
`[gauge] /gauge/result`。**干跑不会给臂/夹爪发任何指令**，可以随时按，
|
||
用来确认"编排对不对、配置读到没有、缺哪些服务"。
|
||
|
||
真跑（会动臂）：
|
||
|
||
```bash
|
||
bash ~/ros2/scripts/run_gauge_assembly.sh --arm 89
|
||
```
|
||
|
||
前置状态（脚本不会替你开）：
|
||
|
||
| 要什么 | 命令 |
|
||
|--------|------|
|
||
| 臂的伺服 + ros2_control | `bash ~/ros2/scripts/run_real.sh --arm 89` |
|
||
| 腕部 D405（S05 要拍照） | `bash ~/ros2/scripts/run_camera_d405.sh --arm 89 --preview` |
|
||
| 夹爪 | `bash ~/ros2/scripts/run_gripper.sh --arm 89` |
|
||
| 视触觉（S06 悬停时测基线） | `bash ~/ros2/scripts/run_tactile.sh` |
|
||
| 手外 D435（只有 S03 用，可选） | `bash ~/ros2/scripts/run_camera_d435.sh --preview` |
|
||
|
||
缺了会怎样：**S00 自检会逐项报出来**，缺"必检项"就直接停（exit 1，不发指令）。
|
||
S00 的清单与"缺了会怎样"在 `gauge_boot.py` 里是全表声明的，不在脚本里拼。
|
||
|
||
---
|
||
|
||
## 2. 状态机（`gauge_fsm.py` 是唯一权威）
|
||
|
||
```
|
||
S00 ─boot_ok─▶ S01 ─pose_ok─▶ S02 ─grip_ok─▶ S04 ─track_locked─▶ S05 ─snap_ok─▶ S06
|
||
(joint_limit 也进 S02) │ (hover_k)
|
||
│ track_lost align_out │ │ align_ok
|
||
▼ │ ▼
|
||
S03 ─coarse_ok─▶ S05 ◀────────┘ S07 ─contact_ok─▶ S08
|
||
│ (snap) ▲ │
|
||
coarse_fail ─▶ S04 │ screw_done
|
||
│ ▼
|
||
E1 掉件：S08 ─gauge_dropped─▶ S01 ─RELOADED─▶ S10 ─pose_ok─▶ S06 S09 ─unscrew_done─┐
|
||
▲ （最后一孔？） │
|
||
└── 还有孔：S10 = 回该孔记录位姿 │ │
|
||
▼ ▼
|
||
S10 ───────▶ S11 ─pose_ok─▶ DONE
|
||
```
|
||
|
||
注：S05 `snap_unstable` 是**自环**（继续等稳定），S08 掉件那一路是 E1；
|
||
S09 不做掉件判据（规正在往外退，"掉了"和"拧出来了"分不出来）。
|
||
|
||
**E1 掉件后"哪个孔"**（这条是看板定的，容易误会，单独说）：
|
||
|
||
- 掉件后孔序号**前进一格** → 该孔**作废**，直接去做下一个孔（掉件多半意味着螺纹/夹持已经
|
||
不对劲，原地重来风险更大）；结果里会留一条 note，请人去检查夹持与螺纹。
|
||
- 在**最后一个孔**掉件时序号被夹住不再前进 → 表现为"重试本孔"（否则就没孔可做了）。
|
||
- 想让"掉件后重试本孔"，改 `gauge_fsm.py` 里 `(SCREW_IN, GAUGE_DROPPED)` 的落点
|
||
与 `handle()` 里推进序号那一段，同时改 `test_gauge_fsm.py` 的两条掉件测试。
|
||
|
||
| 状态 | 干什么 | 过了判据 | 不过 |
|
||
|------|--------|----------|------|
|
||
| S00 | 自检：臂/相机/夹爪/触觉/算法都在且**有真数据** | `boot_ok` | `boot_fail` → 中止，不打任何指令 |
|
||
| S01 | 回 `home` → 切工具系 → **逆时针预绕到极限角**（为棘轮留回转行程） | `pose_ok` | `pose_fail` → 中止；撞关节限位**不算失败**（`joint_limit` → 继续） |
|
||
| S02 | 夹持确认（看夹爪状态字，**当前默认跳过**） | `grip_pass` / `grip_skipped` | `grip_fail` → 中止 |
|
||
| S04 | 起视觉跟踪（一阶段伺服） | `track_locked` | `track_lost` → **回 S03 粗定位兜底** |
|
||
| S03 | 手外相机粗定位 → 悬停到工件上方 `coarse_hover_m` | `coarse_ok` → 回 S05 | `coarse_fail` → 回 S04 继续跟踪 |
|
||
| S05 | 等画面**稳定**再精拍定位（判据：到均值最远距离 ≤ `snap_tol_px`） | `snap_ok` | 抖动 → `snap_unstable` → 留在 S05 重来 |
|
||
| S06 | 正悬停在孔上方 `servo.hole_hover_z_m`；偏差 > `align_tol_mm` 回 S05 | `align_ok` | 超差 → `align_out` → 回 S05 重拍 |
|
||
| S07 | 测力基线 → 步进下压（每步 `contact_step_mm`）→ 切向突变即停 | `contact_ok` | 法向力 > `contact_safe_delta` → **抬起 + 中止(E2)**；下探超 `contact_max_depth_mm` → 中止 |
|
||
| S08 | 棘轮：慢转 `screw_step_deg` → 松爪 → 快回转 → 夹紧，最多 `screw_max_cycles` 轮 | `screw_done` | 掉件 → `gauge_dropped` → **E1**；力矩到位 → `screw_stop` → S09 |
|
||
| S09 | 反向拧出（比拧入多 `unscrew_extra_cycles` 轮） | `unscrew_done` → S11（最后一孔）或 S10 | 失败 → 中止（此阶段不判掉件） |
|
||
| S10 | 回该孔记录位姿，重复 S06–S09（下一孔） | `pose_ok` → S06 | 超时 → 中止 |
|
||
| S11 | 回 home + 开爪，收尾 | `pose_ok` → **DONE** | 超时 → 中止 |
|
||
| **E1** | 掉件：**立即停转**（不补转），→ S01 回 home + 开爪，等人工把规塞回爪里，发 `/gauge/reload_ok` 后 → S10「换下一个孔」 | `reloaded` | 超时 `reload_timeout_s` → 中止；累计超过重放上限 → 中止并留言 |
|
||
| **E2** | 任何中止：先退到安全高度（`_safe_retract`），再报错退出 | — | — |
|
||
|
||
**状态机是"数据"不是"代码"**：`TRANSITIONS` 是一张 `(状态, 事件) → 下一状态` 的表，
|
||
节点里每个 `_step_*` 只允许返回表里登记的合法事件。
|
||
`test_gauge_step_events.py` 会**读节点源码的 AST**，把每个 `_step_*` 可能返回的事件
|
||
逐个问状态机"合法吗" —— 节点和转移表是两份数据，漏登记要在 pytest 里炸，
|
||
而不是在真机上跑到那一步才炸。
|
||
|
||
---
|
||
|
||
## 3. 代码分工（改哪一块，找哪个文件）
|
||
|
||
| 文件 | 管什么 | 有测试吗 |
|
||
|------|--------|----------|
|
||
| `gauge_fsm.py` | 状态/事件/转移表/上下文（**不改 ROS**，纯数据） | `test_gauge_fsm.py` |
|
||
| `gauge_kin.py` | URDF 关节限位解析、预绕角计算、**实测绕工具 Z 转了多少度** | `test_gauge_kin.py` |
|
||
| `gauge_config.py` | `site.yaml → gauge_assembly` 段的解析与校验（键名/单位/类型/交叉矛盾） | `test_gauge_config.py` |
|
||
| `gauge_gripper.py` | 夹爪后端选择（契约 vs 真机 Trigger）+ 状态字→"夹住没/掉了没" | `test_gauge_gripper.py` |
|
||
| `gauge_boot.py` | S00 自检清单与报告（必检/选检、缺了会怎样） | `test_gauge_boot.py` |
|
||
| `gauge_assembly_node.py` | ROS 壳：发指令、读反馈、把事件交给状态机 | 间接（见上表） |
|
||
| `launch/gauge_assembly.launch.py` | 只做一件事：发布手眼静态 TF（S05 的前提） | — |
|
||
| `scripts/run_gauge_assembly.sh` | 环境 + 参数 + 前置说明 | — |
|
||
|
||
设计原则：**能离线测的逻辑一律不放 ROS 里**。所以状态机、限位、配置、夹爪判据
|
||
都可以在没有机器人、没有 Gazebo 的机器上跑（`pytest src/arm_application/test -q`）。
|
||
|
||
---
|
||
|
||
## 4. 话题 / 服务契约
|
||
|
||
| 名字 | 类型 | 方向 | 说明 |
|
||
|------|------|------|------|
|
||
| `/gauge/state` | `std_msgs/String` | 出 | 每次转移都发一条：`#序号 状态 --事件--> 状态` |
|
||
| `/gauge/progress` | `std_msgs/String` | 出 | 当前进度（第几孔 / 第几轮棘轮），给人看的 |
|
||
| `/gauge/result` | `std_msgs/String` | 出 | 结束时的结论（JSON：`ok`/`state`/`holes_done`/`failures`/`fsm` 轨迹…） |
|
||
| `--result-file`（默认 `/tmp/gauge_assembly_Rizon4-<SN>.json`） | 文件 | 出 | **同一份结论落盘**。`ros2 launch` 的退出码恒为 0（R6.9），所以脚本/CI 靠这个文件判成败 |
|
||
| `/gauge/reload_ok` | `std_msgs/Bool` | 入 | **E1 人工确认**：掉件后把规塞回爪里，发一条 `true` |
|
||
| `/gripper/open` `/gripper/close` | `arm_msgs/srv/GripperExecute` 或 `std_srvs/srv/Trigger` | 出 | 两种后端自动识别，见 `docs/GRIPPER.md` |
|
||
| `…/joint_states`、`…/tactile/*`、`…/color/image_raw` | 标准 | 入 | 全部来自既有的相机/触觉/驱动节点，本流程**不自己开设备** |
|
||
|
||
人工处理 E1（终端 2）：
|
||
|
||
```bash
|
||
ros2 topic pub --once /gauge/reload_ok std_msgs/msg/Bool "{data: true}"
|
||
```
|
||
|
||
---
|
||
|
||
## 5. 怎么调参（顺序很重要）
|
||
|
||
1. **先标定手眼**（否则 S05 拍出来的坐标是错的，后面全白做）：
|
||
见 `docs/HANDEYE.md`。腕部相机要有 eye-in-hand 结果文件，S03 才用得上 eye-to-hand。
|
||
2. **再看触觉量级**：`bash ~/ros2/scripts/run_tactile.sh --cli`，手压一下看读数变化。
|
||
所有力判据（`contact_*_delta`、`screw_*_delta`）都是"相对基线的增量"，单位是任意单位。
|
||
3. **再确认 S01 能转**：启动日志里会打印"预绕 150°（方向=逆时针，限位余量 3°）"，
|
||
以及**实测绕工具 Z 转了多少度**。实测与目标差 >5° 会中止 —— 因为后面所有
|
||
"转 XX 度"都建立在这条假设上，假设不成立时继续跑只会拧坏东西。
|
||
4. **最后调力阈值**：太灵敏（刚靠近就停）→ 调大 `contact_tangential_delta`；
|
||
不灵敏（压很深才停）→ 调小。
|
||
|
||
配置读错会怎样：拼错键名 → **直接报错并给最相近的候选名**；单位/类型不对 →
|
||
报"哪一行、期望什么、现在什么"；交叉矛盾（如 `contact_safe_delta ≤ contact_tangential_delta`）
|
||
→ 启动就报。**不会静默退回默认值。**
|
||
|
||
---
|
||
|
||
## 6. 还没做 / 未验证（照实说）
|
||
|
||
| 项 | 状态 |
|
||
|----|------|
|
||
| S02 夹持确认 | 预留（`grip_check: false`）。现在没有"规确实在爪里"的**可信**判据，靠触觉猜会误判 |
|
||
| S03 手外粗定位 | 需要 eye-to-hand 标定文件；没有就明确报缺哪个文件、怎么生成，并回 S04 |
|
||
| S07 接触判据 | 阈值**未在真机上标定**（仿真里做过通路验证）。⚠️ 见下面「S07 为什么在仿真里过不去」 |
|
||
| S08/S09 力矩到位判据 | 同上，属现场标定项 |
|
||
| 真机全流程 | **未跑过**（本仓库当前无真机）。干跑 / 仿真 / 单测已过，见 §7 |
|
||
| 仿真全流程 | ✅ **已跑通**（S00→S11、两个孔、DONE，连着两次；基线见 §7.1） |
|
||
|
||
### S07 为什么在仿真里过不去（2026-09-24 查到根因，别再往阈值上调）
|
||
|
||
**先说结论：不是阈值调高了，是「通止规靠夹持摩擦驱动」这件事本身决定了
|
||
轴向力传不到指上。** 要动的是判据**用哪个传感器**，不是阈值大小。
|
||
|
||
排查过程（先走过一次弯路，记下来免得重走）：S07 在 25mm 处 ABORT，
|
||
报「下探 25mm 都没触发接触判据」。**第一反应是阈值标高了**，于是：
|
||
25→35mm 下探预算、台面开 90mm 通槽让贯通孔真能穿过 —— 都做了，都该做
|
||
（确实是独立的 bug），但**没解决 S07**。真实因果链是：
|
||
|
||
> 补记（2026-09-24 晚）：那道通槽后来从 90mm **收窄到 36mm** —— 90mm 比工件
|
||
> 还宽（工件 y 向只有 100mm），工件会掉进槽里，而真值感知又按名义位姿报孔位，
|
||
> 于是 S05 三角化出 169mm 的假孔距。见 `devices/sim/gazebo/worlds/assembly.world`
|
||
> 里 `work_table` 的注释与 `scripts/check_sim_scene.py` 的承压宽度检查。
|
||
|
||
| # | 实测到的 | 日志证据 |
|
||
|---|---------|---------|
|
||
| 1 | 悬停时规尖已在工件面下 **23mm** | `悬停时尖端离面 -23.0mm`（工具几何自检） |
|
||
| 2 | 台肩本应在"再下压 7mm"时落面，但切向读数**全程 0.0** | `下压 4.49~17.97mm 切向 (-0.0, -0.0)` |
|
||
| 3 | 读数恒 0 ⇒ 规**在爪里打滑**了：没跟着往下走，而是相对爪往上溜 | —— |
|
||
| 4 | 溜到 ~20mm 时规**顶到掌心**（变成刚性接触）→ 切向力突跳到 **225.9** | `下压 19.91mm 切向 (225.9, 56.8)` |
|
||
| 5 | 阈值 300 差一点没够 → 判据不触发 → 臂继续压到 25mm 顶死 | `连续 5 步实测零位移` |
|
||
|
||
关键一环是**为什么读数恒 0**：规靠指垫摩擦夹持，轴向力要经摩擦传给指。
|
||
摩擦上限 = µ·N，`gauge/model.sdf` 的夹持段 µ=0.25、实测握力 N≈46N →
|
||
单指 ~11.5N、两指 ~23N。而台肩一旦落面，接触刚度 5e4 N/m 意味着**过压
|
||
0.5mm 就是 25N** —— 一步 0.5mm 的步进就让轴向力越过摩擦上限，规打滑。
|
||
所以"台肩落面"那一下的力**根本传不到手指触觉上**，读数停在 0 是对的。
|
||
|
||
**真机同理会这样**（不是仿真特有）：只要工具是靠夹持摩擦抱住的，轴向力
|
||
就传不到指垫。**正确修法是换判据来源**，三选一（按可信度排序）：
|
||
|
||
1. **用法兰/腕部的力扭矩传感器**（本工作区的 Orisys 就在这一层）—— 轴向
|
||
力直接落在它上面，不经摩擦，物理上最干净。**推荐这条。**
|
||
2. 用**位置停滞**判"到底了"：连续若干步实测零位移即视为到位。现成的
|
||
`descend_progress` / `DESCEND_STALL_STEPS` 已经能测出来，只是现在把它
|
||
当**错误**报，改成"当作到达信号"即可。缺点是没有力的大小、区分不出
|
||
"顶到台面"和"顶到异物"。
|
||
3. 仍用指垫触觉，但阈值要按 **µ·N 反推**（≈23N 量级），且必须把夹持段
|
||
摩擦系数与握力一起标进 `site.yaml`。**最不推荐**：换了规/换了指垫就得重标。
|
||
|
||
**已落地（2026-09-24 当天实现并在仿真里跑通到 S07 判接触）**：
|
||
|
||
S07 的**首选判据已改成腕部 FT 轴向突变**（`gauge_fsm.wrench_touch`），触觉只作
|
||
兜底。真机用 Flexiv 的 `/＜sn＞/ft_sensor_wrench`、仿真用挂在 `joint7` 的
|
||
`force_torque` 传感器（`devices/sim/gazebo/config/wrist_ft.yaml`），两边都是
|
||
**牛顿 + 法兰/link7 系**，所以判据公式完全一致。
|
||
|
||
实测（仿真，k1 孔）：
|
||
|
||
```
|
||
S07 基线：切向 (-1.5, -22.5)、法向 53.9；腕部轴向 -21.82N（活，话题 /sim/ft_sensor_wrench）
|
||
S07 工具几何：悬停法兰 z=0.6320、工具尖端在法兰下方 245.0mm → 尖端 z=0.3870
|
||
S07 接触（腕部 FT）：轴向 +48.37N 相对基线 -21.82N 超过 +8.0N（下压 5.39mm）
|
||
```
|
||
|
||
下压 5.39mm 与几何自检算出来的"台肩还差 5.0mm 落面"吻合到亚毫米 ——
|
||
这既证明判据有效，也证明**工具几何配置是对的**。
|
||
|
||
**改判据之后暴露出的两个"静默假通过"**（都已修，见 `FRAMEWORK_RULES.md` §8）：
|
||
|
||
1. **卸载回退一步没走，却报"已放干净"**。判到接触的那一刻臂上顶着的是
|
||
"出力上限"量级的力（实测 ~70N，与阈值无关），必须先回退 `contact_relief_mm`
|
||
再交给 S08。但回退量 0.5mm 与 `_goto` 的到位容差写成了**同一个量级**，
|
||
浮点回减后 `dist < tol` 当场成立 ⇒ `_goto` 一步没走就返回 True；
|
||
而残余力判据比的是"回退前 vs 回退后"两个**相同**的读数 ⇒ 报"残余 0.0N"。
|
||
结果 S08 带着 70N 去转（夹持摩擦上限 µ·N 两指合计 ≈73N）→ 掉件(E1)。
|
||
现在：容差由 `gauge_fsm.relief_tol_m(回退量)` 推导（**保证严格小于回退量**，
|
||
有单测），残余力按**基线**算而不是按回退前后之差算。
|
||
2. **S08 第 2 轮就报"通止规掉了"，爪里其实好好的**。掉件判据原来吃
|
||
`GripperStatus.is_gripped` 的**瞬时翻转**，而它是**推断值**（"指令到合拢侧 +
|
||
实际开口被顶住"）；S08 会在"合到某开口/全合"之间发指令，某一帧的指令落到与
|
||
实际开口同一档 ⇒ 翻 False ⇒ 假 E1，然后整条流程去等人工"放好通止规"。
|
||
现在判据改看**开口塌陷**（`GripperClient.dropped()`：夹住时的开口 − 当前开口
|
||
≥ 5mm）—— 真掉件是两指合到底，几十毫米的变化，不会被指令切换带偏。
|
||
3. **S09 第一步就 ABORT，报"期望 +338.9°、实测 +26.0°"**，而现场什么都没坏。
|
||
棘轮把上游给的**目标角（绝对值）**当**增量**用了：关节在 `+158.9°`、目标
|
||
`+338.9°`（故意越限，好让下一步有余量）被算成"要转 338.9°"，于是"让余量"
|
||
退到 `−180°`（比限位 `−175°` 还低）⇒ 回旋校验不过 ⇒ ABORT。
|
||
现在全部过 `gauge_fsm.ratchet_delta_rad()` 折算（见 `FRAMEWORK_RULES.md` R2.9）。
|
||
4. **悬停高度按 `site.yaml` 的工具长算，S05 还是把工件推走**（迭代用满、残差
|
||
恒定 ~11mm，日志却让你查手眼/相机）。真因是仿真里规合爪后**在指间下滑**到
|
||
落座位置，实际工具长比标称长十几毫米 ⇒ 悬停时规尖已经插进工件 ⇒ 闭环追的是
|
||
"被自己推着走的工件"。现在：上料脚本**实测**工具长并落盘
|
||
`devices/sim/gazebo/config/sim_tool_geometry.yaml`，`run_gauge_assembly.sh --sim`
|
||
当启动参数覆盖标称值（`site.yaml` 里只是兜底）；S05 另有 `drag_verdict`
|
||
看门狗，工件被推走立刻停并明确报 code=6，不再让你去查手眼。
|
||
|
||
250|---
|
||
|
||
## 6.5 仿真跑之前：三件事（顺序别换）
|
||
|
||
第 7 节的验收命令**都要求仿真已经在跑**，而这里以前只写了"先编"，没写"怎么起"
|
||
和"起之前要清干净"——2026-09-28 实测就是在这两步上栽的。
|
||
|
||
```bash
|
||
# 1) 编（改过 src/ 之后必须；仿真 install_sim/ 是拷贝，不是 symlink —— 坑 2）
|
||
bash ~/ros2/scripts/build_sim.sh
|
||
|
||
# 2) 清干净：上一轮的仿真进程、以及**在真机域上跑着的 mock 栈**
|
||
bash ~/ros2/scripts/stop_sim_gz.sh # 仿真栈（进程组 + install_sim/ 兜底）
|
||
bash ~/ros2/scripts/stop_real.sh --list # 真机/mock 侧（应为空）
|
||
|
||
# 3) 起仿真（默认无头；要看画面加 --gui）
|
||
bash ~/ros2/scripts/run_sim_gz.sh
|
||
```
|
||
|
||
第 2 步里的 `stop_real.sh` **不是可选的礼貌动作**，实测踩过一次：
|
||
|
||
> `run_sim.sh`（mock 栈）名为 sim，用的却是**真机的 `install/`**、节点名与真机
|
||
> 完全一样，而且它当时**不管 `ROS_DOMAIN_ID`** —— 调用者 shell 里带着 77
|
||
> （之前跑过仿真）就落到**仿真的域**上。于是 mock 的 `joint_state_publisher`
|
||
> 与 Gazebo 争 `/joint_states`：**控制器 `feedback` 与 `reference` 完全一致**
|
||
> （控制器没坏），TF 却显示末端**一动不动**。
|
||
>
|
||
> 表现是"仿真上料失败、臂摆不到竖直位姿"，顺着现象查会一路查到 IK / TF / 时钟上。
|
||
> 现在两侧的域都钉死了（`run_sim.sh` → 真机域 0，`run_sim_gz.sh` → 仿真域 77，
|
||
> 见 `scripts/lib/ros_domain.sh`），但**残留进程本身**仍要靠第 2 步清掉。
|
||
|
||
## 7. 验收命令（都是真跑过的）
|
||
|
||
```bash
|
||
# 1) 纯逻辑层（不需要机器人）
|
||
bash ~/ros2/scripts/run_unit_tests.sh src/arm_application
|
||
# 真机环境：557 passed（`onnx` 齐）；仿真环境会少几项 —— 缺 onnx 的用例 skip，
|
||
# 不伪装成失败（R5.10）。
|
||
# 别用裸 `PYTHONPATH=src/... python3 -m pytest`：那样导入不到 ament/ROS、
|
||
# 收集阶段就报 error，看起来像"测试挂了"，其实只是环境没装 —— 用上面这个
|
||
# 运行器（它负责切 conda、按包拼 PYTHONPATH）。
|
||
|
||
# 2) 干跑全流程：S00→S01→…→S11→DONE，脚本退出码 0，结论 ok=true
|
||
bash ~/ros2/scripts/run_gauge_assembly.sh --arm 89 --sim --dry-run
|
||
|
||
# 3) 仿真里不干跑（缺 /home / 相机 / 触觉 / 夹爪状态）：S00 拦住，
|
||
# 脚本退出码 1，结论 ok=false、failures 逐条列出"缺了会怎样"，不发任何运动指令
|
||
bash ~/ros2/scripts/run_gauge_assembly.sh --arm 89 --sim
|
||
```
|
||
|
||
### 7.1 仿真全流程第一次真跑通（2026-09-24，留作基线）
|
||
|
||
```
|
||
S00_boot → S01_home_extreme → S02_grip_check → S04_track → pre_lift → stage1_track
|
||
→ S05_snap → hover → snap_coarse_0 → snap_fine_0 → align_move_0 → snap_fine_1
|
||
→ align_move_1 → snap_fine_2 → aligned（k1 残差 0.05mm）→ descend_above_hole → hold
|
||
→ S06_hover_k1 → S07_contact（腕部 FT +213N 判接触，下压 31.6mm；卸载回 0N）
|
||
→ S08_screw_in_k1（12 轮）→ S09_unscrew_k1 → 竖直提出 Z 0.627→0.707
|
||
→ S10_to_k2 → S06_hover_k2 → S07_contact → S08_screw_in_k2（12 轮）
|
||
→ S09_unscrew_k2 → 竖直提出 Z 0.638→0.707 → S11_reset --pose_ok--> DONE
|
||
|
||
通止规装配结束：state=DONE ok=True 已装 2/2 孔 掉件重放 0 次
|
||
```
|
||
|
||
**连着跑了两次都是 DONE**（第二次 `24` 次棘轮 = 12×2 孔，`relocations=0`），
|
||
说明场景复位是干净的、不残留上一轮的状态。落盘结论：
|
||
`/tmp/gauge_assembly_sim.json`（`ok=true`、`holes_done=2/2`、`residual_mm≈0.02`、
|
||
k1/k2 孔距 58mm —— 之前那个 169mm 是工件掉进 90mm 通槽后的假孔距）。
|
||
|
||
跑之前**必须**先编：`bash scripts/build_sim.sh`（坑 2）。
|
||
|
||
> 退出码是**脚本**给的：脚本读节点落盘的结论文件（`ros2 launch` 自己的退出码
|
||
> 恒为 0，见 R6.9）。
|
||
|
||
---
|
||
|
||
## 8. 踩过的坑（都是真发生过的，别再犯）
|
||
|
||
1. **四元数换算被抄成了四份，而且已经互相抄歪了。**
|
||
`pipeline/stage2/stage3/visual_servo` 四个 launch 各有一份 `_quat_from_R`，
|
||
退化分支（旋转 180°/90°）的公式各不相同 —— 意味着不同 launch 发布的同一个
|
||
手眼 TF 可能是**不同**的。现在全部收口到 `handeye_lib.read_transform_yaml`，
|
||
并加了回环测试 + "launch 里不许再自己实现四元数"的守卫测试。
|
||
2. **两个环境的"生效方式"不一样，"改了没生效"是幽灵问题。**
|
||
真机 `build.sh` 用 `--symlink-install`（软链，改源码立即生效）；
|
||
仿真 `build_sim.sh` **有意不用**它（仿真 conda 的 setuptools 旧，不认 `--editable`），
|
||
所以 `install_sim/` 里是**拷贝** —— **改完源码必须重编，否则跑的还是旧代码**，
|
||
而且不会有任何提示。判据一句话：
|
||
|
||
```bash
|
||
diff src/arm_application/arm_application/<改过的文件> \
|
||
install_sim/arm_application/lib/python3*/site-packages/arm_application/<同名文件>
|
||
```
|
||
|
||
不一致就 `bash scripts/build_sim.sh`（只重编 `arm_application` 更快：
|
||
`bash scripts/build_sim.sh` 之后看日志里 `Finished <<< arm_application`）。
|
||
3. **`os._exit()` 会把异常吞掉。** 节点入口一开始用 `os._exit` 退出，导致
|
||
"进程退出码 1、日志里什么都没有"。现在 `main()` 里 `traceback.print_exc()` 先打印。
|
||
4. **action 的"期望类型"必须写流程真正会调的类型。**
|
||
S00 曾把 `arm_jtc` 的期望类型写成 `arm_msgs/FollowJointTrajectory`，
|
||
而本流程直接走 `ros2_control` 的 JTC（`control_msgs/FollowJointTrajectory`），
|
||
于是自检永远说"类型不对"。自检的期望类型和代码里 `ActionClient` 用的是同一个常量。
|
||
5. **测试红一片时，人就不看测试了。** 仿真环境没装 `onnx`（只有真机 conda 有），
|
||
14 个用例常年报红。现在缺依赖的用例 `skip`（`importorskip`），
|
||
让"红"只表示真出问题。
|
||
6. **棘轮步长 180° 配 ±175° 的关节 7：必然贴限位。**（2026-09-24，S08 第 3 轮 ABORT）
|
||
棘轮一个来回（慢转 + 回转）要 `2×step` 的行程：`2×180 = 360°`，而关节 7 的
|
||
行程只有 `350°`（±175°）—— 关节**必然**有一头贴死，而"让余量"也腾不出空间
|
||
（腾一步还是 360°）。日志表现是"贴限位 → 让余量 → 还是贴限位"，
|
||
**真因（配置）藏在最后一行**，白烧 3 轮节拍才炸。
|
||
|
||
更隐蔽的一半：每轮回转实测只走 175.7 / 174.8°（速度/加减速限制），**每轮亏
|
||
4~5°**；这点亏欠只能靠余量吃，余量给 0 时关节均位会在 7~8 轮后累计外漂、
|
||
再也转不回来。
|
||
|
||
现在的口径（`FRAMEWORK_RULES.md` R2.10）：`2×step + 2×余量 ≤ 行程`，
|
||
出厂取 `screw_step_deg: 120`（12 轮 × 120° = 1440° = 4 整圈，仍是 360° 的
|
||
整数倍）；S08 开头用 `/robot_description` 里的**真实限位**算一次
|
||
（`gauge_fsm.ratchet_travel_ok`），不通过直接 ABORT 并打印"改哪个键、改成多少"。
|
||
出厂值本身有回归锚点：`test_shipped_site_yaml_ratchet_step_fits_every_supported_arm`
|
||
会拿 `flexiv_description` 里每台支持臂的 joint7 限位去算。
|
||
7. **棘轮的"转向"不能交给"谁更短"。**（同日，S08 第 3 轮 ABORT 的**另一半**）
|
||
棘轮原来是"目标角 → 最近一圈的增量"。步长正好 180° 时，最近的那条路取决于
|
||
0.1° 级的读数差：调用方按自己读到的 `q=+162.0` 算出目标 `−18.0°`，
|
||
而棘轮晚一点读到 `+162.1`，`−180.1°` 折一圈就成了 `+179.9°` —— **方向翻了**。
|
||
于是棘轮朝螺纹反方向走，接着贴限位、让余量、重试，**净位移 0** ⇒ ABORT，
|
||
而现场什么都没坏（正是"最不该失败的一步失败"）。
|
||
|
||
棘轮的转向是**工艺意图**（S08 顺时针拧入、S09 逆时针拧出），不能由数值巧合
|
||
决定：`ratchet_delta_rad(target, q_now, direction=±1)`，四个原子步都显式给方向。
|
||
8. **拧出后不竖直退出孔口，横向动作会把规拽出来。**（2026-09-24，k1 装完、k2 报废）
|
||
k1 的 S08/S09 走完（规"拧出"了），S10 直接斜着换到 k2 —— 而规**还插在 k1 的
|
||
孔里**（仿真里没有螺纹几何，"拧出"只是让规在孔里转；真机也只退回几个毫米）。
|
||
横move把规从孔里**拽**出来：抓取锁被拽断、规掉在台面上。
|
||
|
||
最坑的是**它在这里不报错**：两分钟后表现为"k2 的 S07 下探 35mm 判不到接触"，
|
||
因为工具上已经没有规了。几何差得极小 —— 规尖比工件面低 30mm、斜线只抬了 29mm。
|
||
|
||
现在的口径（`FRAMEWORK_RULES.md` R2.11）：S09 成功出口之前先**纯 Z** 提到
|
||
**跟踪高**（悬停 + 50mm，与 S04 的 `pre_lift` 同一个口径），再做任何横向动作；
|
||
`test_gauge_exit_lift.py` 钉住"顺序 + 高度 + 已高就不动"，并且提出失败只告警
|
||
（那一刻孔里已没有要保护的规，硬失败会把成功的装配变成失败）。
|
||
|
||
---
|
||
|
||
## 9. 相关文档
|
||
|
||
- `config/site.yaml` → `gauge_assembly:` 段：**每个参数的注释与取值建议**
|
||
- [`FRAMEWORK_RULES.md`](FRAMEWORK_RULES.md)：改代码前先读（本文件第 8 节已进反例表）
|
||
- [`GRIPPER.md`](GRIPPER.md)、[`HANDEYE.md`](HANDEYE.md)、[`TACTILE.md`](TACTILE.md)、[`HOLE_DETECT.md`](HOLE_DETECT.md)
|
||
- [`HANDBOOK.md`](HANDBOOK.md)：新手从这读起
|