真机落地前的仿真补齐：夹持/腕力/真值位姿，让仿真能验证 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。

scripts/build_sim.sh

@@ -77,6 +77,29 @@ if [[ "${NO_SKIP}" -eq 0 ]]; then
   ARGS+=(--packages-skip "${SKIP[@]}")
 fi
 
+# --- 先做语法自检（colcon **不**编译 .py）------------------------------------
+# colcon 对 Python 包只做拷贝：语法错了它照样"构建成功"，而报错会出现在
+# 很久以后的 `ros2 run` / `pytest` 收集阶段，且长这样：
+#     File "install_sim/.../sim_truth_perception.py", line 441
+# 看上去像"安装产物坏了"，其实根因在源码里（实测过一个中文字符串里混了
+# ASCII 引号）。在这里提前炸，能把"源码错"和"安装产物错"分开。
+echo "检查 Python 源码语法（colcon 不编译 .py，语法错要到运行时才炸）…"
+python3 - <<'PY'
+import pathlib
+import sys
+
+bad = []
+for p in sorted(pathlib.Path("src").rglob("*.py")):
+    try:
+        compile(p.read_text(encoding="utf-8"), str(p), "exec")
+    except SyntaxError as e:  # noqa: PERF203
+        bad.append(f"  {p}:{e.lineno}: {e.msg}")
+if bad:
+    print("源码有语法错误，先修再构建：", file=sys.stderr)
+    print("\n".join(bad), file=sys.stderr)
+    raise SystemExit(1)
+PY
+
 colcon --log-base log_sim "${ARGS[@]}" 2>&1 \
   | grep -vE 'anaconda-auth|conda-content-trust|Unknown distribution option|warnings\.warn'
 

scripts/check_sim_scene.py

@@ -329,6 +329,134 @@ def check_d435_visibility(scene, root) -> tuple[list[str], str]:
     return probs, info
 
 
+def check_table_slot(scene, root) -> tuple[list[str], str]:
+    """台面通槽 vs 工件：既要"规的通端过得去"，又要"工件掉不下去"。
+
+    两个方向都会出事，而且都是**静默**的（2026-09-24 各栽过一次）：
+
+      a) 槽开得不够 **窄**（< 规的通端）：规下压到 12mm 就顶在台面上，
+         S07 一路压到上限都等不到台肩落面 → 报"没触发接触判据"，
+         看起来像传感器/阈值坏了。
+      b) 槽开得**太宽**：工件跨在槽上时承压面太小，一有扰动就翻进槽里、
+         落到地面 —— 于是 `perception.source=truth` 开始撒谎：孔位按名义位姿
+         投到像素上，而孔位正下方已经不是工件（是槽底/地面），
+         深度反投影把 58mm 孔距算成 169mm，S05 连丢 9 拍超时。
+
+    所以这里同时查两件事：孔位必须落在**开口**内，承压宽度必须**够**。
+    """
+    import xml.etree.ElementTree as ET
+
+    from arm_sim_gazebo.sim_scene import rings_local, world_boxes
+    from arm_sim_gazebo.sim_site import resolve_world
+
+    world_path, _ = resolve_world("", root)
+    try:
+        tree = ET.parse(world_path)
+    except (OSError, ET.ParseError) as exc:
+        return [f"读不出 world（{world_path}）：{exc}"], ""
+    table = None
+    for m in tree.getroot().iter("model"):
+        if m.get("name") == "work_table":
+            table = m
+            break
+    if table is None:
+        return [f"{world_path} 里没有 <model name=\"work_table\"> —— 台面被改名了？"], ""
+
+    def _pose_xyz(node) -> tuple[float, float, float]:
+        p = node.find("pose")
+        if p is None or not (p.text or "").strip():
+            return (0.0, 0.0, 0.0)
+        v = [float(x) for x in p.text.split()[:3]]
+        while len(v) < 3:
+            v.append(0.0)
+        return (v[0], v[1], v[2])
+
+    my = _pose_xyz(table)[1]
+    spans: list[tuple[float, float]] = []
+    for link in table.findall("link"):
+        ly = my + _pose_xyz(link)[1]
+        for col in link.findall("collision"):
+            box = col.find("geometry/box/size")
+            if box is None or not (box.text or "").strip():
+                continue
+            cy = ly + _pose_xyz(col)[1]
+            half = float(box.text.split()[1]) / 2.0
+            spans.append((cy - half, cy + half))
+    if not spans:
+        return ["work_table 里读不到任何 box 碰撞体 —— 台面几何解析不出来"], ""
+    spans.sort()
+
+    b = scene.table_bounds
+    # 开口 = 台面范围内的"没被碰撞体盖住"的 y 区间。
+    gaps: list[tuple[float, float]] = []
+    cur = b.y_min
+    for lo, hi in spans:
+        if lo > cur + 1e-9:
+            gaps.append((cur, min(lo, b.y_max)))
+        cur = max(cur, hi)
+    if cur < b.y_max - 1e-9:
+        gaps.append((cur, b.y_max))
+    gaps = [(lo, hi) for lo, hi in gaps if hi - lo > 1e-6]
+
+    part = next((o for o in scene.objects if o.name == scene.operating.part_object), None)
+    if part is None:
+        return [f"场景里没有 {scene.operating.part_object!r}"], ""
+    boxes = world_boxes(scene, part, root)
+    if not boxes:
+        return ["读不出工件的碰撞体"], ""
+    wy0 = min(float(v) for bx in boxes for v in (bx[2], bx[3]))
+    wy1 = max(float(v) for bx in boxes for v in (bx[2], bx[3]))
+
+    # 承压宽度 = 工件在 y 方向上被台面碰撞体盖住的长度（区间求交后累加）。
+    supported = 0.0
+    for lo, hi in spans:
+        supported += max(0.0, min(hi, wy1) - max(lo, wy0))
+
+    rings = rings_local(scene, part, root)
+    hole_ys: list[float] = []
+    for r in rings:
+        c, s = math.cos(float(part.yaw)), math.sin(float(part.yaw))
+        hole_ys.append(float(part.y) + s * float(r.x) + c * float(r.y))
+
+    probs: list[str] = []
+    info = (
+        f"台面沿 y 的支撑区间 {[(round(lo, 4), round(hi, 4)) for lo, hi in spans]}；"
+        f"开口 {[(round(lo, 4), round(hi, 4)) for lo, hi in gaps]}；"
+        f"工件 y∈[{wy0:.4f},{wy1:.4f}] 承压 {supported * 1000:.1f}mm"
+    )
+
+    # (a) 孔位必须落在开口里 —— 否则规的通端顶在台面上，S07 永远等不到台肩落面。
+    for i, hy in enumerate(hole_ys):
+        ok = any(lo < hy < hi for lo, hi in gaps)
+        if not ok:
+            probs.append(
+                f"第 {i + 1} 个孔（世界 y={hy:+.4f}）**不在台面开口内** —— "
+                f"规的通端会顶在台面上。\n"
+                f"      后果：S07 下压 12mm 就到底，一路到上限都等不到台肩落面，"
+                f"报「没触发接触判据」，看起来像传感器/阈值坏了。\n"
+                f"      修法：把开口挪到孔位下方（worlds/assembly.world 的 work_table "
+                f"碰撞体 y 区间），或改工件摆放。\n"
+                f"      当前开口：{[(round(lo, 4), round(hi, 4)) for lo, hi in gaps]}"
+            )
+
+    # (b) 承压宽度必须够 —— 否则工件会翻进槽里掉下去，而真值感知会继续按
+    #     名义位姿算，把 58mm 孔距算成 169mm（见 truth_holes.HoleTruth 的说明）。
+    min_support = 0.020
+    if supported < min_support:
+        probs.append(
+            f"工件跨在台面开口上的**承压宽度只有 {supported * 1000:.1f}mm**"
+            f"（要求 ≥ {min_support * 1000:.0f}mm）。\n"
+            f"      后果：板一有扰动就翻进槽里、落到地面；而 "
+            f"perception.source=truth 仍按 scene.yaml 的**名义位姿**投孔 → "
+            f"孔位下方已经不是工件而是槽底/地面 →\n"
+            f"      深度反投影把 58mm 孔距算成 169mm → S05 每拍都丢、最后超时。"
+            f"现象看起来是「检测坏了」，根因却是**工件掉了**。\n"
+            f"      修法：把开口收窄（改 worlds/assembly.world 里 work_table 碰撞体的 "
+            f"y 尺寸/位姿，开口只要够规的通端过就行，不需要覆盖两个孔所在的整个 y 带）。"
+        )
+    return probs, info
+
+
 def check_gazebo_poses(scene, root) -> tuple[list[str], str]:
     """查 Gazebo 里物体的实际位姿，与 scene.yaml 对照。
 
@@ -441,7 +569,17 @@ def main() -> int:
     if p:
         print("\n".join(f"    ✘ {x}" for x in p))
 
-    print("\n[5] 粗定位可见性（工件是否完整落在手外相机画面内）")
+    print("\n[5] 台面通槽 vs 工件（规过得去 + 工件掉不下去）")
+    p, info = check_table_slot(scene, root)
+    problems += p
+    if info:
+        print(f"    {info}")
+    if p:
+        print("\n".join(f"    ✘ {x}" for x in p))
+    else:
+        print("    ✓ 孔位都在开口内，且承压宽度够")
+
+    print("\n[6] 粗定位可见性（工件是否完整落在手外相机画面内）")
     p, info = check_d435_visibility(scene, root)
     problems += p
     if info:
@@ -452,7 +590,7 @@ def main() -> int:
         print("    ✓ 完整可见")
 
     if not args.no_poses:
-        print("\n[6] Gazebo 实际位姿核对")
+        print("\n[7] Gazebo 实际位姿核对")
         p, info = check_gazebo_poses(scene, root)
         problems += p
         if info:

scripts/lib/export_site_env.py

@@ -82,6 +82,11 @@ _SIDE_WORDS = {
 
 _ARM_SECTION_KEYS = (
     "camera", "gripper", "paths", "servo", "handeye", "go_home", "yolo", "perception",
+    # gauge_assembly：几何类参数是**按台子**的 —— 同一个数在仿真与真机上不同。
+    # 例：workpiece_surface_z_m 仿真 0.412（臂站地面、台面 0.40m），真机 0.012。
+    # 不登记就会**静默失效**：site.yaml 里写了 robots[sim].gauge_assembly，
+    # 合出来还是全局值，S07 的几何自检算错 0.4m 却不报错。
+    "gauge_assembly",
 )
 
 

scripts/run_gauge_assembly.sh

@@ -136,6 +136,23 @@ if [[ "${USE_SIM}" == true ]]; then
     echo "       python3 scripts/sim_load_gauge.py --dry-run  # 只看算出来的位姿" >&2
     exit 1
   fi
+  # 工具长**用实测值**，不用 site.yaml 的标称值（2026-09-24 的教训，完整说明见
+  # sim_load_gauge._write_measured_geometry）：仿真里规会在指间下滑到落座位置，
+  # 而下滑量随装夹变化（实测见过 243 / 254mm）。工具长写小了，二阶段悬停就会把
+  # 规尖停在工件**里面** —— 接着 S05 的横向对准变成拿规尖拨工件（工件被推走、
+  # 残差恒不收敛），日志却报"检查手眼/相机安装"；写大了则 S07 提前判"台肩落面"。
+  # 真机没有这个文件（现场装夹后把实测值写进 site.yaml），所以这条只在仿真里走。
+  GEOM_FILE="${ROOT}/devices/sim/gazebo/config/sim_tool_geometry.yaml"
+  if [[ -f "${GEOM_FILE}" ]]; then
+    TOOL_LEN="$(python3 -c 'import sys,yaml;print(yaml.safe_load(open(sys.argv[1],encoding="utf-8"))["tool_tip_below_flange_m"])' "${GEOM_FILE}")"
+    if [[ -n "${TOOL_LEN}" ]]; then
+      ARGS+=(tool_tip_below_flange_m:="${TOOL_LEN}")
+      echo "=== 工具长用**实测值** ${TOOL_LEN}m（${GEOM_FILE#"${ROOT}/"}）==="
+    fi
+  else
+    echo "⚠ 没有 ${GEOM_FILE#"${ROOT}/"}：工具长退回 site.yaml 的标称值。" >&2
+    echo "  上料脚本每次都会重写它 —— 没有这个文件说明上料没走完（或没落盘）。" >&2
+  fi
 fi
 
 echo "=== launch gauge_assembly（自检不过会自动报缺什么） ==="

scripts/sim_env.sh

@@ -87,7 +87,22 @@ export YOLO_AUTOINSTALL=false
 export ROS2_WS="${ROOT}"
 # 仿真单独一个 DDS domain：避免与正在跑的真机节点互相抢话题/污染 TF。
 # 真机用的是默认 domain，两者天然隔离。
-export ROS_DOMAIN_ID="${ROS_DOMAIN_ID:-77}"
+#
+# ⚠️ 这里**强制** 77，不是 `${ROS_DOMAIN_ID:-77}`（2026-09-24 修的）。
+#    写法上"尊重已有值"看起来更礼貌，实际会把隔离**交给运气**：实测本机 shell 里
+#    就 export 着 ROS_DOMAIN_ID=0（真机默认），于是 `${ROS_DOMAIN_ID:-77}` 当场取到 0，
+#    仿真栈和真机落在**同一个 domain** 上 —— 仿真的运动指令可能被真机控制器订阅到。
+#    这是最坏的一类错误（动真机），而现场只会看到"脚本打印了 77、实际是 0"。
+#    要换 domain 请显式设 SIM_ROS_DOMAIN_ID（名字里带 SIM，不会被真机环境的 export 撞上）。
+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}（真机通常用 0）——" >&2
+  echo "[sim_env]   已**强制**改成 77：隔离不能靠「环境里恰好没设」。要另指定请设 SIM_ROS_DOMAIN_ID。" >&2
+  export ROS_DOMAIN_ID=77
+else
+  export ROS_DOMAIN_ID=77
+fi
 
 # =============================================================================
 # Gazebo 的插件与资源搜索路径 —— 必须在这里设，不能只在 launch 里设

scripts/sim_load_gauge.py

@@ -91,12 +91,78 @@ GAUGE_GRIP_DIA_MM = 30.0
 #:     gz 报得出 Contacts、夹爪也能读到接触，但规就是托不住、一路滑下去。
 GAUGE_GRASP_CENTER_M = 0.043
 
+#: 标定结果的合理性上限 [mm]：|开口 0 时的指间间距| 超过它就判标定无效。
+#: 指节是连杆机构、间距与开口斜率实测 1.000，所以这个量应是个小偏置；
+#: 大得离谱只有一个原因 —— 合爪时手指被台面/工件挡住了（量到的是被挡的值）。
+#: 实测（2026-09-24）：home 位姿下量出 −10.15mm，扫描区间整体偏 10mm，
+#: 17 档全掉，白扫 80 秒。
+CAL_SANITY_MM = 6.0
+
 # 台面高度（scene.yaml 的 table_top_z）：用来判断"够不够空间把规塞进去"。
 FALLBACK_TABLE_Z = 0.40
 # 触觉"夹住了"的判据：contact 位或 |fnormal| 超过这个噪声地板。
 # 空载时仿真触觉的 fnormal 实测 ~1e-5（见 config/tactile.yaml 的基线说明）。
 TACTILE_NOISE_FLOOR = 0.05
 
+#: 合爪后规在指间**允许**的下滑量 [m]。超过就判"这次装夹和上次不一样"。
+#:
+#: 为什么是 3mm：下滑量会直接加在工具长上（site.yaml 的
+#: ``gauge_assembly.tool_tip_below_flange_m``）。而流程现在**读实测值**
+#:（见 :func:`_write_measured_geometry`），所以下滑本身不再是错误 ——
+#: 它只是"这次的工具长和上次不同"，改由实测值带着走。
+#: 超过这个量仍然要**说出来**：说明装夹方式不稳定，值得人看一眼。
+#: 实测（2026-09-24）：同一个仿真台先后测到 254.4mm 与 243mm（差 11mm）。
+DRIFT_SLIP_MAX_M = 0.003
+
+
+def _write_measured_geometry(
+    path: Path,
+    *,
+    below: float,
+    flange_z: float,
+    tip_z: float,
+    world: str,
+    model: str,
+) -> Optional[Path]:
+    """把本次实测的工具几何落盘（供 ``run_gauge_assembly.sh --sim`` 读）。
+
+    **为什么要有这个文件**（2026-09-24 一整轮排查换来的）：
+
+        仿真里规在指间会下滑到落座位置（合爪时规被两指挤住，会一直滑到
+        它的台阶卡在指腹上为止）。下滑量**随装夹变化** —— 实测见过 243mm
+        与 254mm 两种，差 11mm。而 site.yaml 里那个值决定两件事：
+          · 二阶段悬停高度（写小了 → 悬停时规尖已经插进工件 → S05 的横向
+            对准变成拿规尖拨工件，工件被推走、残差恒不收敛）；
+          · S07 的下探预算（写小了 → 台肩还没落面就用完预算）。
+        两个症状都离真因（几何）很远，所以**不许**让它靠"人手抄一个数字"
+        来保持一致：上料脚本测出来就落盘，流程读它。
+
+    写入失败**不抛异常**（返回 ``None``）：上料这件事本身已经成功了，
+    调用方只需要把"退回标称值"这件事喊出来。
+    """
+    try:
+        data = {
+            "tool_tip_below_flange_m": round(float(below), 4),
+            "measured_at": time.strftime("%Y-%m-%dT%H:%M:%S"),
+            "world": world,
+            "model": model,
+            "flange_z_m": round(float(flange_z), 4),
+            "tip_z_m": round(float(tip_z), 4),
+        }
+        header = (
+            "# ⚠ 本文件由 scripts/sim_load_gauge.py 自动生成（每次仿真上料都覆盖）。\n"
+            "#    它是**这次装夹**实测到的工具几何，run_gauge_assembly.sh --sim 会读它\n"
+            "#    并作为启动参数覆盖 site.yaml 的 gauge_assembly.tool_tip_below_flange_m。\n"
+            "#    为什么必须这样：仿真里规会在指间下滑到落座位置，下滑量随装夹变化\n"
+            "#    （实测见过 243mm / 254mm 两种）。写成固定值迟早会算错悬停高度。\n"
+            "#    真机没有这个文件（现场装夹后把实测值写进 site.yaml）。\n"
+        )
+        path.parent.mkdir(parents=True, exist_ok=True)
+        path.write_text(header + yaml.safe_dump(data, allow_unicode=True), encoding="utf-8")
+        return path
+    except Exception:  # noqa: BLE001 - 见 docstring：失败只退回标称值
+        return None
+
 
 def _load_yaml(path: Path) -> dict:
     with path.open(encoding="utf-8") as fh:
@@ -194,6 +260,59 @@ def gauge_target_pose(
     return tip, q_gauge
 
 
+def _reset_scene_objects(scene_cfg, world: str, spin_for) -> None:
+    """把 ``scene.yaml`` 里**所有**物体放回名义位姿（幂等）。
+
+    为什么不止复位通止规（2026-09-24 实测踩到，代价是白跑一轮）：
+        上一轮 S08 带着 ~70N 的轴向力转了一圈，把**工件**推走了 ——
+        实测 |Δp|=76.9mm、yaw 偏 8.3°。而真值感知是**纯几何**的：它按
+        ``scene.yaml`` 的名义位姿算孔位/目标点，物体一挪，算出来的就是假测量。
+        幸好有漂移看门狗（``sim_truth_perception``）当场停下不发点，否则现象会是
+        「S05 两孔间距 169mm」或「S07 一路下压不接触」，看起来像检测坏了。
+
+        但看门狗只会"停下来"，它**不会把工件搬回去** —— 于是下一轮跑不了，
+        报的还是那句「一个视觉目标点都没收到」，看起来像感知节点根本没起来。
+        复位这一下，让"重跑"真的从干净状态开始，不必重启整个仿真。
+
+    顺序：先全部搬回名义位姿，再等物理稳一下。逐条打印结果 ——
+    哪一件没复位成功必须看得见（它会以"真值不发点"的形式在下游爆出来）。
+    """
+    objs = [o for o in (scene_cfg.get("objects") or []) if o.get("pose")]
+    if not objs:
+        print("⚠ scene.yaml 里没有带 pose 的物体，跳过场景复位。")
+        return
+    failed: List[str] = []
+    for obj in objs:
+        name = str(obj.get("name") or "")
+        if not name:
+            continue
+        p = obj.get("pose") or {}
+        try:
+            x = float(p.get("x", 0.0))
+            y = float(p.get("y", 0.0))
+            z = float(p.get("z", 0.0))
+            yaw = float(p.get("yaw", 0.0))
+        except (TypeError, ValueError) as exc:
+            print(f"⚠ {name}: pose 字段不是数字（{exc}），跳过。")
+            failed.append(name)
+            continue
+        quat = (0.0, 0.0, math.sin(yaw / 2.0), math.cos(yaw / 2.0))
+        ok, out = _set_gauge_pose(world, name, (x, y, z), quat)
+        if ok:
+            print(f"=== 复位: {name} → ({x:.3f}, {y:.3f}, {z:.3f}) yaw={math.degrees(yaw):.1f}° ===")
+        else:
+            print(f"⚠ 复位失败: {name}：{out[:120]}")
+            failed.append(name)
+    if failed:
+        print(f"⚠ 以下物体没能复位：{', '.join(failed)}\n"
+              f"  后果：真值感知会（正确地）拒绝发点，S04 收不到目标点、报"
+              f"「一个视觉目标点都没收到」。\n"
+              f"  真修法：重启仿真（bash scripts/run_sim_gz.sh）让它按场景重新 spawn。")
+    # 让物理静一下再往下走：连着瞬移多个物体会让接触求解器抖几下。
+    if objs:
+        spin_for(1.5)
+
+
 def _run(cmd: List[str], timeout: float = 8.0) -> Tuple[int, str]:
     """跑一条外部命令，返回 (退出码, 合并输出)。不抛异常（调用方自己判）。"""
     try:
@@ -208,34 +327,13 @@ def _run(cmd: List[str], timeout: float = 8.0) -> Tuple[int, str]:
 def _set_gauge_pose(world: str, name: str, pos, quat) -> Tuple[bool, str]:
     """用 Gazebo 的世界服务把模型搬到指定位姿。
 
-    这里用 ``ign service`` 而不是另起一个 ROS 桥：仿真的场景加载本来就是
-    用命令行 `ros_gz_sim create` 做的（见 arm_sim_gazebo/sim_scene.py），
-    口径一致、少一个需要维护的话题/服务桥。
-
-    注意 ``--reqtype`` 必须写 ``ignition.msgs.Pose``（本环境实测：写
-    ``gz.msgs.Pose`` 会报 "Unable to create request"，一个字母的差别、
-    报错信息完全没提原因）。
+    实现**只有一份**：``arm_sim_gazebo.sim_pose_service.set_model_pose``
+    （那个 `--reqtype` 必须写 ``ignition.msgs.Pose`` 的坑也写在那边）。
+    真值感知的启动复位走的是同一个函数。
     """
-    req = (
-        f'name: "{name}", '
-        f"position: {{x: {pos[0]:.6f}, y: {pos[1]:.6f}, z: {pos[2]:.6f}}}, "
-        "orientation: {"
-        f"x: {quat[0]:.8f}, y: {quat[1]:.8f}, z: {quat[2]:.8f}, w: {quat[3]:.8f}"
-        "}"
-    )
-    code, out = _run(
-        [
-            "ign", "service",
-            "-s", f"/world/{world}/set_pose",
-            "--reqtype", "ignition.msgs.Pose",
-            "--reptype", "ignition.msgs.Boolean",
-            "--timeout", "3000",
-            "--req", req,
-        ],
-        timeout=10.0,
-    )
-    ok = code == 0 and "data: true" in out
-    return ok, out.strip()
+    from arm_sim_gazebo.sim_pose_service import set_model_pose
+
+    return set_model_pose(world, name, pos, quat)
 
 
 def _gauge_pose_now(name: str) -> Optional[Tuple[float, float, float]]:
@@ -977,6 +1075,64 @@ def _ros_side(args) -> int:  # noqa: C901 - 线性流程，拆开反而更难读
         print("=== --place-only：不动夹爪，只按当前姿态摆位 ===")
     else:
         print("=== 开爪（保证两指之间能让规进去） ===")
+
+    # ---- 0. 归位：先把规放回规座（幂等，避免上一次的残留污染这一次）--------
+    # 为什么必须做：本脚本是要**反复重跑**的（调试时一分钟一次）。上一次跑完规
+    # 可能还卡在指间/落在台面槽里，而后面两步都会被"指间有东西"毁掉：
+    #   · 标定指间间距：规卡在指间时量到的是**规的外径**（实测 28.29mm 而不是
+    #     20mm），于是 gap_cal 凭空多出 8mm，整个扫描区间偏移 → 一处都夹不到；
+    #   · 摆位：规已经在指间时再瞬移一次，落点是在旧位姿上叠的。
+    # 先把它放回 scene.yaml 写的位置再开始，行为就与"刚启动仿真"完全一致。
+    if not args.place_only:
+        # 先解开可能残留的抓取锁：上锁之后规是被**固定关节**钉在手指上的，
+        # 这时瞬移回规座会被关节立刻拉回来（实测：瞬移瞬间 z 变 0.36 又弹回），
+        # 而手指合到底也只能到"规的外径"（实测 28.29mm 而不是 20mm）——
+        # 于是标定读到的正是规径，gap_cal 凭空多 8mm。
+        try:
+            from arm_sim_gazebo.sim_grasp_lock import load_grasp_lock, publish_lock_event
+
+            _spec = load_grasp_lock(root=ROOT)
+            if _spec.enabled and publish_lock_event(_spec, False):
+                print("=== 归位前先解锁（清掉上一次可能残留的抓取锁） ===")
+                spin_for(0.5)
+        except Exception:  # noqa: BLE001 - 锁是可选补救措施，读不到就跳过
+            pass
+
+        nom = None
+        for obj in (scene_cfg.get("objects") or []):
+            if str(obj.get("name")) == args.model:
+                p = obj.get("pose") or {}
+                try:
+                    nom = (
+                        float(p.get("x", 0.0)),
+                        float(p.get("y", 0.0)),
+                        float(p.get("z", 0.0)),
+                        float(p.get("yaw", 0.0)),
+                    )
+                except (TypeError, ValueError):
+                    nom = None
+                break
+        if nom is None:
+            print(f"⚠ scene.yaml 里没有 {args.model} 的名义位姿，跳过「归位」这一步。")
+        else:
+            yaw = nom[3]
+            quat = (0.0, 0.0, math.sin(yaw / 2.0), math.cos(yaw / 2.0))
+            ok, out = _set_gauge_pose(args.world, args.model, nom[:3], quat)
+            if ok:
+                print(f"=== 归位：把 {args.model} 放回规座 "
+                      f"({nom[0]:.3f}, {nom[1]:.3f}, {nom[2]:.3f}) ===")
+                wait_tf(tcp_frames["left"], timeout_s=1.0)  # 顺便让物理先稳一下
+                spin_for(1.5)
+            else:
+                print(f"⚠ 归位失败（{args.model} 可能还在指间）：{out[:120]}")
+                print("  后果：下面的标定会量到规的外径，扫描区间整体偏移。")
+        # 顺带把**其它**场景物体也复位（最要紧的是工件）。
+        # 为什么：上一轮 S08 若带着大轴向力转了一圈，工件会被推走，而真值感知
+        # 按名义位姿算孔位 —— 看门狗会（正确地）停发点，于是下一轮报
+        # "一个视觉目标点都没收到"，看起来像感知节点没起来。
+        # 详见 _reset_scene_objects 的说明。
+        _reset_scene_objects(scene_cfg, args.world, spin_for)
+
     if not args.place_only:
         if not gripper_call(cli_open, "open"):
             print("✗ /gripper/open 调用超时")
@@ -1015,6 +1171,24 @@ def _ros_side(args) -> int:  # noqa: C901 - 线性流程，拆开反而更难读
         dz = pts["left"][2] - pts["right"][2]
         return (dx * dx + dy * dy + dz * dz) ** 0.5 * 1000.0
 
+    # ---- 1.5 摆到"竖直上料位姿"（工具轴朝下 + 离台面够高） ---------------
+    # ⚠️ 这一步必须在**标定之前**（见下面对 gap_cal 的合理性检查）：
+    #    标定要在"手指前方是空的"前提下量，而零位/ home 位姿又低又斜，
+    #    合爪时手指很容易顶在台面或工件上 —— 那时量到的间距是**被挡住的值**，
+    #    于是 gap_cal 凭空差出十几毫米，整个扫描区间都跟着错，一处都夹不到。
+    #    实测（2026-09-24）：home 位姿下标定出"开口 20mm 时间距 9.85mm"，
+    #    扫了 17 档全掉，白跑 80 秒。
+    # 不摆的话：仿真零位工具轴是**水平**的，水平夹一根钢规会被重力抽出去
+    #（实测合爪后漂移 857mm，直接掉回规座）；home 位姿又斜又低，放不下。
+    # 这里用 IK 把两指 TCP 中点送到台面上方的空旷处、工具轴朝下。
+    if not args.place_only and not args.no_pose:
+        if not _go_vertical_load_pose(
+            node, buf, spin_for, wait_tf, tcp_frames, args,
+        ):
+            print("✗ 摆竖直上料位姿失败。可以用 --no-pose 跳过（自己先把臂摆好）。")
+            rclpy.shutdown()
+            return 2
+
     if not args.place_only:
         if not gripper_call(cli_move, "move", position=args.cal_aperture_mm):
             print("✗ /gripper/move 调用超时（标定用）")
@@ -1029,18 +1203,19 @@ def _ros_side(args) -> int:  # noqa: C901 - 线性流程，拆开反而更难读
             print(f"  开口 {args.cal_aperture_mm:.2f}mm → 指间间距 {gap_at_aperture:.2f}mm")
             print(f"  ⇒ 开口 0（全闭合）时间距 {gap_cal_mm:.2f}mm，"
                   f"间距 = 开口 + {gap_cal_mm:.2f}mm")
-
-    # ---- 1.5 摆到"竖直上料位姿"（工具轴朝下 + 离台面够高） ---------------
-    # 不摆的话：仿真零位工具轴是**水平**的，水平夹一根钢规会被重力抽出去
-    #（实测合爪后漂移 857mm，直接掉回规座）；home 位姿又斜又低，放不下。
-    # 这里用 IK 把两指 TCP 中点送到台面上方的空旷处、工具轴朝下。
-    if not args.place_only and not args.no_pose:
-        if not _go_vertical_load_pose(
-            node, buf, spin_for, wait_tf, tcp_frames, args,
-        ):
-            print("✗ 摆竖直上料位姿失败。可以用 --no-pose 跳过（自己先把臂摆好）。")
-            rclpy.shutdown()
-            return 2
+            # 合理性检查：指节是连杆机构，间距与开口严格线性、斜率实测 = 1.000，
+            # 所以 gap_cal（开口=0 时的间距）应该是个**小量**（几毫米量级）。
+            # 它大得离谱时唯一的原因是"手指被挡住了"：量到的是被挡的位置。
+            # 这个检查为什么值得写：不查的话后面会白扫十几档、最后只说
+            # "都没夹住"，还列一堆可能原因 —— 而真因是标定这一步就错了。
+            if abs(gap_cal_mm) > CAL_SANITY_MM:
+                print(f"✗ 标定结果不可信：开口 0 时间距 {gap_cal_mm:.2f}mm"
+                      f"（正常应在 ±{CAL_SANITY_MM:.0f}mm 内）。")
+                print("  最可能：合爪时手指顶在台面/工件上（手指前方不是空的）。")
+                print("  修法：先把臂摆到空旷处再上料 ——")
+                print("        bash scripts/go_home.sh   或用 --no-pose 自行摆位。")
+                rclpy.shutdown()
+                return 2
 
     # ---- 1.8 摆位（纯瞬移）与"人手托着合爪" -------------------------------
     #
@@ -1264,8 +1439,14 @@ def _ros_side(args) -> int:  # noqa: C901 - 线性流程，拆开反而更难读
               "跳过位姿校验，只看触觉。")
     else:
         drift = math.dist(now_pos, tip_pos)
-        print(f"=== 校验：规还在不在手里 ===\n  位姿漂移 {drift * 1000:.1f}mm"
-              f"（合爪后被顶开/顶飞会远大于 20mm）")
+        print(f"=== 校验：规还在不在手里 ===")
+        print(f"  位姿漂移 {drift * 1000:.1f}mm。两种漂移**性质不同**，别混：")
+        print("    · > 20mm  → 合爪时被顶飞 / 模型名或世界名不对（上料失败）")
+        print("    · 3~20mm  → 规在指间**下滑**到了它的落座位置。这不是上料失败，"
+              "而是")
+        print("                 site.yaml 的 tool_tip_below_flange_m 与实际装夹不符 ——")
+        print("                 下滑量会**直接加在工具长上**，写小了二阶段悬停就会把")
+        print("                 规尖停在工件里面（S05 横向对准于是拿规尖拨工件）")
     tac_ok = False
     for side in ("left", "right"):
         item = tac.get(side)
@@ -1283,6 +1464,18 @@ def _ros_side(args) -> int:  # noqa: C901 - 线性流程，拆开反而更难读
     if drift is not None and drift > 0.02:
         problems.append(f"规没在位姿上（漂移 {drift * 1000:.1f}mm）"
                         "：可能合爪时被顶飞、或模型名/世界名不对")
+    elif drift is not None and drift > DRIFT_SLIP_MAX_M:
+        # 3~20mm：规落到了指间的落座位置。**不是**上料失败 —— 它是"这次的
+        # 工具长和 site.yaml 里那个值不一样"，而工具长稍后会实测并落盘，
+        # 流程读的是实测值（见 _write_measured_geometry）。所以这里只"说出来"：
+        # 下滑量偏大说明装夹不稳定，值得人看一眼。
+        print(f"  ⚠ 规在指间下滑了 {drift * 1000:.1f}mm 才落座 —— 这次的工具长与"
+              f"site.yaml 的标称值差了这么多（不是上料失败）：")
+        print("      下滑量会直接加在工具长上。稍后的「实测工具几何」会把它测出来并"
+              "落盘成")
+        print("      sim_tool_geometry.yaml，流程（--sim）会读它覆盖标称值 —— 所以"
+              "能继续跑。")
+        print("      但下滑量偏大说明装夹不稳，值得确认一下规是不是卡在指腹上了。")
     if not tac_ok:
         problems.append("两指触觉都没读到接触：可能没合上（看上面开口）、或规不在两指之间")
     if problems:
@@ -1296,6 +1489,112 @@ def _ros_side(args) -> int:  # noqa: C901 - 线性流程，拆开反而更难读
 
     print("✓ 通止规已装进夹爪（位姿贴合 + 触觉有接触）。可以跑装配了：")
     print("    bash scripts/run_gauge_assembly.sh --sim --holes 1")
+
+    # ---- 4. 上"抓取锁"（仿真专用）------------------------------------------
+    # 为什么由**这里**上锁，而不是等夹爪适配器自己判：
+    #     适配器的 is_gripped 判据是"指令到全闭合、实际被工件顶住"（真机 DH 没有
+    #     夹持状态字，只能这么推）。而本脚本是**按直径精确合到某个开口**的
+    #     （例如 24.9mm 卡住 φ30 的规），指令与实际一致 —— 于是适配器**判不出**
+    #     "夹住了"，锁也就不会触发。
+    #     本脚本是"装夹"这件事的执行者，它自己知道刚刚校验过（位姿贴合 + 触觉
+    #     有接触），所以由它上锁最准，也最好解释。
+    # 解锁不需要在这里做：夹爪适配器在收到 open / 开口变大的 move 时会解锁
+    #（见 sim_gripper_adapter._release_grasp_lock）。
+    try:
+        from arm_sim_gazebo.sim_grasp_lock import load_grasp_lock, publish_lock_event
+
+        _spec = load_grasp_lock(root=ROOT)
+        if _spec.enabled and _spec.object == args.model:
+            if publish_lock_event(_spec, True):
+                print(f"✓ 抓取锁已上：{_spec.object}.{_spec.object_link} 固定到 "
+                      f"{_spec.robot_model}.{_spec.robot_link}")
+                print("    （仿真专用补丁：Gazebo 的刚性接触给不出真实指垫的静摩擦，"
+                      "不上锁时规会在连续轨迹里打滑 —— 真机 46N 握力 + µ≈0.8 本来就不掉）")
+            else:
+                print("⚠ 抓取锁没上成（详见日志）——机械臂连续运动时规可能打滑。")
+        elif _spec.enabled:
+            print(f"⚠ 抓取锁配的对象是 {_spec.object}，与 --model {args.model} 不一致，跳过上锁。")
+    except Exception as exc:  # noqa: BLE001 - 锁是补救措施，读不到配置不该拦住上料
+        print(f"⚠ 读抓取锁配置失败，跳过上锁：{exc}")
+
+    # ---- 7. 实测工具几何（给 site.yaml 的 tool_tip_below_flange_m 用）----
+    # 为什么必须**实测**而不是推算：这个值决定 S07 的悬停/下探预算（见
+    # gauge_fsm.descend_needed_m），差 10mm 就会让"台肩落面"永远到不了。
+    # 推算要凑三层偏移（TCP 在法兰下方多少 + 规尖在 TCP 下方多少），而
+    # 中间任何一层的定义（比如 tcp 帧是零偏移还是带偏置）都可能与假设不同。
+    # 实测只有一次减法：法兰 z − 规尖 z。真机同法（夹住规后直接读 TF）。
+    got_e2 = wait_tf(args.ee_frame, timeout_s=3.0)
+    tip_now = _gauge_pose_now(name)
+    if got_e2 is not None and tip_now is not None:
+        p_ee = got_e2[0]
+        below = float(p_ee[2]) - float(tip_now[2])
+        print("\n=== 实测工具几何（抄进 config/site.yaml 的 gauge_assembly）===")
+        print(f"  法兰 z={p_ee[2]:.4f}  规尖 z={tip_now[2]:.4f}"
+              f"  → **tool_tip_below_flange_m: {below:.3f}**")
+        if below <= 0.0:
+            print("  ⚠ 规尖不在法兰下方 —— 多半是规掉了或者没夹在指间，"
+                  "别把这个值抄进配置。")
+        else:
+            # ⚠️ 这里以前算的是「当前法兰 z − 工具长」，也就是**上料时臂停在哪**，
+            # 与悬停高度毫无关系 —— 实测它印出"工件面 z=0.412 → 尖端 z=0.5201"，
+            # 看着很安全，而真正决定"会不会把规捅进工件"的是 site.yaml 里
+            # servo.hole_hover_z_m（当时 0.632 ⇒ 尖端 0.387 = 面**下** 25mm）。
+            # 现在按配置的悬停高度算，并且复用 gauge_fsm 的判据（一份实现）。
+            try:
+                from arm_application.gauge_fsm import hover_z_guard
+
+                site = _load_yaml(ROOT / "config" / "site.yaml")
+                rob = next(
+                    (r for r in (site.get("robot") or {}).get("robots") or []
+                     if str(r.get("sn")) == str(getattr(args, "arm", "sim"))),
+                    {},
+                )
+                ga = dict(site.get("gauge_assembly") or {})
+                ga.update(rob.get("gauge_assembly") or {})
+                servo = dict(site.get("servo") or {})
+                servo.update(rob.get("servo") or {})
+                surface = float(ga.get("workpiece_surface_z_m") or 0.0)
+                hover = float(servo.get("hole_hover_z_m") or 0.0)
+                # 目标间隙与二阶段"最后轻降"用的是**同一个**配置（hole_detect.stage2
+                # .final_gap_mm）：都是"规尖悬在工件面上方多少"。两处各读一份迟早
+                # 会打架，而症状是"上料脚本说安全、stage2 说危险"。
+                st2 = dict((site.get("hole_detect") or {}).get("stage2") or {})
+                gap = float(st2.get("final_gap_mm") or 2.0) / 1000.0
+                fixed, why = hover_z_guard(hover, surface, below, gap)
+                print(f"  用它核二阶段悬停高：servo.hole_hover_z_m={hover:.3f}、"
+                      f"工件上表面 z={surface:.3f}")
+                if why:
+                    print("  ⚠ **悬停时规尖会捅进工件**（S05 的横向对准会拖着工件走）：")
+                    print(f"      {why}")
+                else:
+                    print(f"      → 悬停时规尖离工件面 "
+                          f"{(hover - below - surface) * 1000:+.1f}mm（安全）")
+            except Exception as exc:  # noqa: BLE001 - 这一项只是提示，别拦住上料
+                print(f"  （悬停高度自检跳过：{exc}）")
+
+            # ---- 把实测值落盘，交给流程当启动参数 --------------------------
+            # 为什么不能只"打印出来让人抄进 site.yaml"（2026-09-24 实测的教训）：
+            #   仿真里规会在指间下滑到落座位置，而**下滑量随装夹变化** ——
+            #   同一个仿真台先后测到过 0.243 和 0.254（差 11mm）。写死哪个值，
+            #   下一次都可能算错悬停高度，而错误现象是"S05 残差恒不收敛"
+            #   （看起来像手眼坏了）。所以：**测量 → 落盘 → 流程读它**。
+            # 真机走不到这里（真机由人装夹、同法读 TF，但由现场把值写进 site.yaml）。
+            geom_path = _write_measured_geometry(
+                ROOT / "devices" / "sim" / "gazebo" / "config" / "sim_tool_geometry.yaml",
+                below=below,
+                flange_z=float(p_ee[2]),
+                tip_z=float(tip_now[2]),
+                world=args.world,
+                model=name,
+            )
+            if geom_path is None:
+                print("  ⚠ 实测值没能落盘 —— 流程会退回 site.yaml 的标称值，"
+                      "悬停/下探预算可能差几毫米到十几毫米。")
+            else:
+                print(f"  已落盘：{geom_path.relative_to(ROOT)}"
+                      f"（run_gauge_assembly.sh --sim 会把它当启动参数覆盖 site.yaml）")
+    else:
+        print("\n（读不到法兰/规尖的位姿，跳过工具几何实测）")
     rclpy.shutdown()
     return 0