真机落地前的仿真补齐：夹持/腕力/真值位姿，让仿真能验证 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。
This commit is contained in:
arm_dev
+112
-4
@@ -160,6 +160,7 @@ ros2 topic pub --once /gauge/reload_ok std_msgs/msg/Bool "{data: true}"
|
||||
| S07 接触判据 | 阈值**未在真机上标定**（仿真里做过通路验证）。⚠️ 见下面「S07 为什么在仿真里过不去」 |
|
||||
| S08/S09 力矩到位判据 | 同上，属现场标定项 |
|
||||
| 真机全流程 | **未跑过**（本仓库当前无真机）。干跑 / 仿真 / 单测已过，见 §7 |
|
||||
| 仿真全流程 | ✅ **已跑通**（S00→S11、两个孔、DONE，连着两次；基线见 §7.1） |
|
||||
|
||||
### S07 为什么在仿真里过不去（2026-09-24 查到根因，别再往阈值上调）
|
||||
|
||||
@@ -171,6 +172,11 @@ ros2 topic pub --once /gauge/reload_ok std_msgs/msg/Bool "{data: true}"
|
||||
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`（工具几何自检） |
|
||||
@@ -197,17 +203,61 @@ ros2 topic pub --once /gauge/reload_ok std_msgs/msg/Bool "{data: true}"
|
||||
3. 仍用指垫触觉，但阈值要按 **µ·N 反推**（≈23N 量级），且必须把夹持段
|
||||
摩擦系数与握力一起标进 `site.yaml`。**最不推荐**：换了规/换了指垫就得重标。
|
||||
|
||||
**下一步（不要再对着触觉阈值试）**：实现第 1 条 —— S07 判据改读腕部 FT 的
|
||||
轴向分量，触觉只作兜底。
|
||||
**已落地（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|---
|
||||
|
||||
## 7. 验收命令（都是真跑过的）
|
||||
|
||||
```bash
|
||||
# 1) 纯逻辑层（不需要机器人）
|
||||
PYTHONPATH=src/arm_application python3 -m pytest src/arm_application/test -q
|
||||
# 仿真环境：349 passed, 14 skipped（缺 onnx 的用例跳过，不伪装成失败）
|
||||
# 仿真环境：454 passed, 14 skipped（缺 onnx 的用例跳过，不伪装成失败）
|
||||
|
||||
# 2) 干跑全流程：S00→S01→…→S11→DONE，脚本退出码 0，结论 ok=true
|
||||
bash ~/ros2/scripts/run_gauge_assembly.sh --arm 89 --sim --dry-run
|
||||
@@ -217,6 +267,27 @@ bash ~/ros2/scripts/run_gauge_assembly.sh --arm 89 --sim --dry-run
|
||||
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）。
|
||||
|
||||
@@ -251,6 +322,43 @@ bash ~/ros2/scripts/run_gauge_assembly.sh --arm 89 --sim
|
||||
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` 钉住"顺序 + 高度 + 已高就不动"，并且提出失败只告警
|
||||
（那一刻孔里已没有要保护的规，硬失败会把成功的装配变成失败）。
|
||||
|
||||
---
|
||||
|
||||
|
||||
Reference in New Issue
Block a user