真机落地前的仿真补齐：夹持/腕力/真值位姿，让仿真能验证 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
@@ -198,6 +198,88 @@ Fortress 恒发 `fx=fy=277, cx=160, cy=120`（它的默认 60° 视场 + 半分
|
||||
物体的落座微动是正常的。卡到 `1e-6` 只会得到永远为真的告警，人就会把检查关掉。
|
||||
默认 5mm 起步（见 `sim_scene.pose_mismatches`）。
|
||||
|
||||
### R2.8 工具长这类"装夹完才知道"的几何：仿真必须**实测并落盘**，不许写死标称值
|
||||
|
||||
悬停高度 = 工件表面 + **工具长** + 间隙。工具长写小几毫米，二阶段悬停时
|
||||
规尖就停在工件**里面** —— 后面 S05 的每条残差、每个告警都是假象
|
||||
（见 §8 的"残差恒定 11mm"那一条）。
|
||||
|
||||
仿真里这个值**装夹完才会定**：合爪后规会在指间下滑到落座位置，下滑量随装夹
|
||||
变化（实测见过 243 / 254mm，差 11mm）。所以口径固定为三级，不许砍：
|
||||
|
||||
1. 上料脚本（`scripts/sim_load_gauge.py`）合爪后**实测**规尖相对法兰的位置；
|
||||
2. 落盘 `devices/sim/gazebo/config/sim_tool_geometry.yaml`（含时间戳/世界名/模型名）；
|
||||
3. `run_gauge_assembly.sh --sim` 读它，作为**启动参数**
|
||||
`tool_tip_below_flange_m:=` 覆盖 `site.yaml` 的标称值。
|
||||
|
||||
优先级与其余参数一致（R1.1：命令行 > `site.yaml` > 包内默认），真机不传这个
|
||||
参数、走 `site.yaml` 的人工实测值。`site.yaml` 里那个数**只是兜底**，别拿它
|
||||
当"这次的真实工具长"。下游拿到工具长后一律过 `gauge_fsm.hover_z_guard()`
|
||||
（会检查悬停高度是否把规尖留在工件里，修不了就明确报错，不静默改）。
|
||||
|
||||
### R2.9 "目标角"和"增量"是两个口径，混用必出错 —— 一律用 `ratchet_delta_rad` 折算
|
||||
|
||||
关节角是**周期量**：`+338.9°` 和 `−21.1°` 是同一个位置，但 `338.9 − 158.9`
|
||||
和 `−21.1 − 158.9` 差了整整一圈（一个"走 180°"、一个"倒着走 180°"）。
|
||||
|
||||
棘轮（S08 拧入 / S09 拧出）里两种口径长得很像 —— 上游给的是**目标角**，
|
||||
下游要的是**增量**。混用不会报错，只会算出"行程不够/足够"的反结论，然后
|
||||
把"让余量"退到限位外面，最后报一句与真因无关的"偏差过大"（§8 那一条）。
|
||||
|
||||
规矩：
|
||||
- 任何"目标 − 当前"都必须过 `gauge_fsm.ratchet_delta_rad(target, q_now)`，
|
||||
它把差折进 `(−180°, +180°]`，同一个目标 + 同一个位置只有一个增量；
|
||||
- **转向要显式给**：`ratchet_delta_rad(target, q_now, direction)`，`+1` 正转、
|
||||
`-1` 反转。步长贴着 180° 时"谁更短"取决于 0.1° 级的读数差 —— 调用方按
|
||||
自己读到的 `q=+162.0` 算出目标 `−18.0`，棘轮晚 0.1° 读到 `+162.1`，
|
||||
`−180.1°` 折一圈就成了 `+179.9°`，**方向翻了**，于是棘轮朝螺纹反方向走、
|
||||
贴限位、让余量、重试，净位移 0 ⇒ ABORT，而现场什么都没坏（§8 那一条）。
|
||||
棘轮的转向是**工艺意图**（S08 顺时针拧入、S09 逆时针拧出），不能交给
|
||||
数值巧合决定；
|
||||
- 传参名必须写清口径（`target_rad` / `delta_rad`），不许叫 `angle`；
|
||||
- 有单测钉住（`test_gauge_fsm.py`：越限目标的折算、让余量后落在限位内、
|
||||
`+338.9°` 与 `−21.1°` 增量相同、180° 刀口不许翻向、给了方向一定落回目标）。
|
||||
|
||||
### R2.10 棘轮的步长受**关节行程**约束：`2×step + 2×余量 ≤ 行程`，且累计是整数圈
|
||||
|
||||
棘轮一个来回（慢转 + 回转）要 `2×step` 的行程，两头还要留余量 —— 贴死限位时
|
||||
"让余量"**腾不出任何空间**（腾一步还是 `2×step`）。所以配 `180°` 的步长给
|
||||
Rizon4/Rizon10 的关节 7（±175° ⇒ 行程 350°）是**必然失败**的：`2×180 = 360 > 350`。
|
||||
|
||||
实测症状（2026-09-24）：第 1、2 轮正常，第 3 轮开始"贴限位 → 让余量 → 还是贴
|
||||
限位"，真因（配置）藏在最后一行，白烧 3 轮节拍。更隐蔽的是**每轮回转都比指令
|
||||
少走 4~5°**（速度/加减速限制），这点亏欠只能靠余量吃 —— 余量给 0 时关节会在
|
||||
7~8 轮后累计偏出限位、再也转不回来。
|
||||
|
||||
规矩：
|
||||
- 步长 ≤ `(行程 − 2×余量) / 2`（Rizon4/10 关节7 ⇒ ≤167°，出厂取 **120°**）；
|
||||
- `screw_max_cycles × screw_step_deg` 必须是 **360° 的整数倍**，否则规停在
|
||||
螺纹中间、S09 也就少拧出几分之一圈；
|
||||
- 判据写成纯函数 `gauge_fsm.ratchet_travel_ok(step, limits, margin)`，
|
||||
**S08 开头**就拿 `/robot_description` 里的限位算一次，不通过直接 ABORT
|
||||
并打印"该改哪个键、改成多少"；
|
||||
- 出厂值有回归锚点：`test_gauge_config.py` 拿 `flexiv_description` 里**每台
|
||||
支持臂**的真实限位去算，以后加一台行程更小的臂会自动报红。
|
||||
|
||||
### R2.11 工具还在孔里时**只许竖直退**，横向动作必须等它出来
|
||||
|
||||
刚拧出的规**仍然插在孔里**（真机只是沿螺纹退回几个毫米；仿真里**没有螺纹几何**，
|
||||
"拧出"只让规在孔里转，深度一点没变）。这时任何带横向分量的动作都会把规**拽**在
|
||||
孔壁上 —— 实测抓取锁被拽断、规掉在台面上。
|
||||
|
||||
**它在这里不报错**，两分钟后表现为"下一孔 S07 下探 35mm 判不到接触"（工具上已经
|
||||
没规了），日志指不到真因（R5.1 的静默失效）。实测那一次的几何差得极小：规尖比
|
||||
工件面低 30mm，而换孔的斜线只往上抬了 29mm —— 差 1mm 就是"拽出孔"和"干净退出"的
|
||||
分界。所以必须是**先后有序的两个动作**，不能靠斜线一起走。
|
||||
|
||||
规矩：
|
||||
- 拧出（S09）的**成功出口之前**先把工具提到**跟踪高**（`_track_z()` =
|
||||
孔悬停高 + 50mm，流程里"已离开工件"的既有定义），之后再走 XY；
|
||||
- 提出是**纯 Z**（`hold_xy=True`），横向分量一律非法（`test_gauge_exit_lift.py`
|
||||
钉住结构顺序 + 目标高度 + "已经高了就不动"）；
|
||||
- 提出失败只告警不失败：那一刻孔里已经没有要保护的规，硬失败会把一次成功的
|
||||
装配变成失败；后面的换孔/复位仍会走到安全位。
|
||||
|
||||
---
|
||||
|
||||
## 3. 触觉与力
|
||||
@@ -362,6 +444,32 @@ conda 激活不会往 `LD_LIBRARY_PATH` 加东西，而 gz 的插件搜索路径
|
||||
- `--verbose` 要把**原始物理量**（指尖坐标）打出来，判错时能回看；
|
||||
- 结论不对时，先怀疑判据，再怀疑被测对象 —— 上面两条都是先怀疑了代码。
|
||||
|
||||
### R4.13 仿真与真机的隔离**不许**依赖"环境里恰好没设某变量"
|
||||
|
||||
`scripts/sim_env.sh` 里原本写着：
|
||||
|
||||
```bash
|
||||
export ROS_DOMAIN_ID="${ROS_DOMAIN_ID:-77}" # ← 错
|
||||
```
|
||||
|
||||
看起来是"尊重调用者已有设置"，实际把隔离交给了运气。实测（2026-09-24）：
|
||||
本机 shell 里就 export 着 `ROS_DOMAIN_ID=0`（**真机默认就是这个**），于是
|
||||
`:-77` 当场取到 0 —— 仿真栈和真机落在**同一个 DDS domain** 上，
|
||||
仿真的运动指令可能被真机控制器订阅到。而现场只会看到"脚本打印了 77、
|
||||
`echo $ROS_DOMAIN_ID` 是 0"这种自相矛盾的输出。
|
||||
|
||||
**规则**：这类"安全隔离项"必须**强制**设成自己的值，要覆盖就用**专用名字**
|
||||
（`SIM_ROS_DOMAIN_ID`）显式覆盖，并且把"我改掉了你的值"喊出来。同类的还有
|
||||
`ROS_LOCALHOST_ONLY`、`CONDA_PREFIX`、代理变量。
|
||||
|
||||
```bash
|
||||
if [[ -n "${SIM_ROS_DOMAIN_ID:-}" ]]; then export ROS_DOMAIN_ID="${SIM_ROS_DOMAIN_ID}"
|
||||
elif [[ "${ROS_DOMAIN_ID:-77}" != "77" ]]; then
|
||||
echo "[sim_env] ⚠ 已有 ROS_DOMAIN_ID=${ROS_DOMAIN_ID}，已强制改为 77 …" >&2
|
||||
export ROS_DOMAIN_ID=77
|
||||
else export ROS_DOMAIN_ID=77; fi
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 5. 代码与测试
|
||||
@@ -422,6 +530,20 @@ diff src/arm_application/arm_application/<文件> \
|
||||
不一致时的现象是"我明明改了，行为却和没改一样"，而且**没有任何提示** ——
|
||||
最容易被误判成"我的改动没效果"。
|
||||
|
||||
**实测代价（2026-09-24）**：修好 verticalize 的静默跳过后忘了重编 `install_sim/`，
|
||||
又白跑一轮 —— 两轮的日志**一字不差**，看起来像"这个改法根本无效"，于是开始怀疑
|
||||
改错了地方。判别只要一条命令：
|
||||
|
||||
```bash
|
||||
grep -c "<你刚写进去的注释关键字>" \
|
||||
install_sim/<包>/lib/python3*/site-packages/<包>/<文件> # 0 = 旧拷贝
|
||||
```
|
||||
|
||||
**同日第二次撞上同一条（所以它值得写两遍）**：给 `sim_truth_perception` 加启动复位后
|
||||
跑测试，`pytest` 直接报 `ImportError: cannot import name 'RESET_GIVE_UP'` ——
|
||||
源码里明明有。因为测试 import 的是 `install_sim/` 里那份**旧拷贝**。
|
||||
所以"改了源码 → 先 `bash scripts/build_sim.sh` → 再跑测试/仿真"，顺序不能反。
|
||||
|
||||
### R5.8 同一份数学/解析实现只许有一份，并加**守卫测试**
|
||||
|
||||
四元数换算曾经在 `pipeline` / `stage2` / `stage3` / `visual_servo` 四个 launch 里
|
||||
@@ -663,8 +785,21 @@ launch 会一直挂着、终端不回提示符。要显式挂 `OnProcessExit →
|
||||
| 每次 pytest 十几条红，没人看测试结果了 | 环境缺 `onnx`，用例直接失败而不是 `skip` | R5.10 |
|
||||
| `site.yaml` 里改了参数，行为一点没变 | 那个键**没接线**（配置表里有、代码里从没读过） | R1.2 |
|
||||
| 改了源码，仿真里行为一点没变，也没有任何提示 | 仿真 `install_sim/` 是**拷贝**（不能 `--symlink-install`），没重编就跑 | R5.7 |
|
||||
| `pytest` 报 `ImportError: cannot import name ...`，而源码里明明有 | 同一件事的另一副面孔：测试导入的是 `install_sim/` 里的旧拷贝 | R5.7 |
|
||||
| S05 带着 6° 倾角去压孔 → 撞件/E2，日志只说"规划停滞" | `verticalize` 的纯旋转 hop 用**平移**做进度判据（dist≈0 ⇒ planned<0 ⇒ 整段跳过） | R5.1、R5.3 |
|
||||
| S07 判到接触、也"卸载成功"，S08 一转却掉件(E1) | 卸载回退量与 `_goto` 容差同量级 ⇒ 浮点回减后 `dist<tol` ⇒ **一步没走**报"已放干净" | R5.1、R5.3 |
|
||||
| S08 第 2 轮就报"通止规掉了"，爪里其实好好的 | 掉件判据吃 `is_gripped` 的**推断值瞬时翻转**，而 S08 会切指令 ⇒ 假 E1 | R5.1、R3.1 |
|
||||
| 下一轮 S04 报"一个视觉目标点都没收到"，像感知没起来 | 上一轮把**工件**推走了；真值看门狗正确地停发点，但没人把工件搬回去 | R5.3、坑 14 |
|
||||
| **开局**就报"工件已经不在 scene.yaml 说的地方了（偏 2.6mm）"，一轮都跑不起来 | 物体是"在名义位姿上方 spawn 再落下"的，落地会偏 2~3mm，正好压在 2mm 容差上；看门狗分不出"落地"和"被撞走" | R2.7、坑 14 |
|
||||
| S09 第一步就 ABORT："期望 +338.9°、实测 +26.0°，偏差过大"，现场什么都没坏 | 棘轮把"关节的**目标角（绝对值）**"当**增量**用：差 180° 被算成 338.9° ⇒ "让余量"那一步算到限位外（−180° < −175°）⇒ 回旋校验必然不过 | R5.1、R5.8 |
|
||||
| S05 迭代用满、残差**恒定** ~11mm，日志让你去查手眼/相机安装 | 悬停时工具尖端已经插进工件：闭环追的是"被自己推着走的工件" ⇒ 残差恒定（新增 `gauge_fsm.drag_verdict` 看门狗专门报这条） | R5.1、R4.12 |
|
||||
| 悬停高度按 `site.yaml` 的工具长配好了，S05 还是把工件推走 | 仿真里规合爪后会**在指间下滑**到落座位置 ⇒ **实际**工具长比标称长十几毫米，且下滑量随装夹变化；写成固定值必然算错悬停高度 | R2.7、R4.12 |
|
||||
| 装配失败了，脚本/CI 却报成功 | `ros2 launch` 的退出码恒为 0，脚本看了 `$?` | R6.9 |
|
||||
| 流程明明打完了，终端一直不回到提示符 | launch 里的静态 TF 发布者是常驻的，主节点退出不会带走它 | R6.9 |
|
||||
| S08 前两轮好好的，第 3 轮起"贴限位 → 让余量 → 还是贴限位"然后 ABORT | 棘轮步长 180° 配关节7 行程 350°：一个来回要 360° > 行程，必然有一头贴死（"让余量"也腾不出空间）；真因在配置里 | R2.10 |
|
||||
| S08 朝**反**方向转，然后贴限位、让余量、重试，净位移 0 ⇒ ABORT | 步长正好 180° 落在"最短路径"的刀口上：0.1° 级的读数差就能把 `−180.1°` 折成 `+179.9°`，棘轮倒着走 | R2.9 |
|
||||
| 回转那一步每轮都比指令少走 4~5°，攒到 7~8 轮后转不回来 | 速度/加减速限制让"快回转"实际走不满整步；余量必须 ≥ 这点亏欠，否则关节均位逐轮外漂 | R2.10 |
|
||||
| 第一孔装得好好的，第二孔的 S07 下探 35mm 判不到接触 | 第一孔拧出后规**还插在孔里**，换孔的斜线把它从孔里**拽**出来 → 锁被拽断、规掉在台面（"工具上已经没规了"）；提出与横move必须**先后有序** | R2.11 |
|
||||
|
||||
---
|
||||
|
||||
|
||||
+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