ros_contorl/scripts/sim_load_gauge.py

#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""把通止规装进夹爪 —— **仿真专用**的"人工上料"。

为什么必须有这一步（不是仿真偷懒，是仿真缺了真机天生就有的东西）
------------------------------------------------------------------
通止规装配整条流程（S00–S11）的前提是：**规已经被夹爪夹住、规轴≈法兰轴**。
这个前提在真机上是**人**完成的：现场把规塞进两指之间、合爪、看一眼轴线
（`stage2_locate_node` 悬停时还会再提醒一次"请确认：左臂夹爪已夹持通止规，
且通止规轴线≈法兰轴线"）。仿真里没有这个人，而 S02 的夹持确认是**预留跳过**
的（`gauge_assembly.grip_check: false`），于是：

    夹爪全程张开（实测 position=99.93mm = 全开）
    → 规一直躺在规座上（实测世界位姿 = 场景里的摆放位姿，一动没动）
    → S07 下压 25mm 什么也碰不到 → 报"没触发接触判据" → 中止

**看着像力阈值/算法问题，实际是"手里没规"。** 这类错在日志里完全不显眼
（自检全绿、定位成功、轨迹正常），所以这一步单独做成脚本、并且在
`run_gauge_assembly.sh --sim` 里自动跑，跑不过就直接停 —— 不允许"带着空夹爪
继续往下跑"。

它做的事（三步，每步都有客观校验）
----------------------------------
1. **摆位**：读 TF 里两指的 TCP（`*_finger_tcp`，= 指腹内侧接触点），取中点
   = 工具轴；沿工具轴向下 55.5mm 得到规尖该在的位置
   （55.5mm = 规的夹持段中点，见 `models/gauge/model.sdf`：夹持段 z 33~78mm），
   姿态让规的 +Z 朝上（与真机"竖直朝下装配"一致）。用 Gazebo 的
   `/world/<world>/set_pose` 服务把规搬到那里。
2. **合爪**：调 `/gripper/close`（**契约后端**，`arm_msgs/srv/GripperExecute`）。
   仿真夹爪全行程 99.93mm、全合拢 27.09mm，而规的夹持段是 φ30mm ——
   所以合拢就是**刚性地压住**（位置控制，没有力控限幅）。这正是触觉能读到
   力的原因：指腹与规之间有了真实的接触力。
3. **校验**（不做假成功）：
     a) 规的位姿在合爪后仍然 ≈ 目标位姿（掉出来会躺在台面上，差得很远）；
     b) 触觉 `/tactile/{left,right}/status` 至少一侧 `contact: true` 或
        `|fnormal|` 明显高于空载噪声。
   两条都不过就报红并给出排查方向（帧名/模型名/世界名/合爪是否真动了）。

用法
----
    bash  scripts/sim_env.sh                      # 先 source 仿真环境（含 DDS 域）
    python3 scripts/sim_load_gauge.py             # 默认 --arm sim --world assembly
    python3 scripts/sim_load_gauge.py --dry-run   # 只算位姿、不碰仿真
    python3 scripts/sim_load_gauge.py --place-only  # 只摆位、不碰夹爪（调参用）

退出码：0=装好了（校验通过）；2=没装好（原因打印在最后）。
"""

from __future__ import annotations

import argparse
import math
import subprocess
import sys
import time
from pathlib import Path
from typing import Dict, List, Optional, Tuple

import yaml

ROOT = Path(__file__).resolve().parents[1]

# 规的夹持段中点（相对规尖）[m]。来源：models/gauge/model.sdf
#   GO 通端 0~30mm、台肩 30~33mm、夹持段 33~78mm、止端 78~86mm
# 夹持段中点 = (33+78)/2 = 55.5mm。指腹应夹在这个高度上。
#: 规（devices/sim/gazebo/models/gauge/model.sdf）**夹持段**的外径。
#: 夹爪必须夹在这里（φ30），不是细的通端 —— 通端只有 φ6，夹爪合到那里
#: 会直接从规身上滑过去。改 gauge/model.sdf 的 grip 段半径时要一起改这里，
#: 否则 sim_load_gauge.py 会按错的直径去合爪（表现：合爪开口到 0、规掉下去）。
GAUGE_GRIP_DIA_MM = 30.0

#: 规尖 → **指腹接触面中心** 的距离 [m]。
#:
#: 为什么是 0.043 而不是"夹持段中心到规尖"的 0.0555：
#:     gauge/model.sdf 里夹持段中心在规尖上方 55.5mm（0.033~0.078 的中值），
#:     但夹爪**指腹的接触面**并不在 tcp_frame 原点上。URDF 里两条固定关节写着：
#:         sim_*_inner_finger_pad_joint  xyz="0 0.010 0.0235"
#:         sim_*_finger_tcp_joint        xyz="0 0.0135 0.0360"
#:     同一父链节、同一个方向，所以指腹中心比 tcp_frame **高**
#:     0.0360 − 0.0235 = 12.5mm。
#:     要让"夹持段中心"落在"指腹中心"上，规尖就得比 tcp_frame 中点
#:     高 12.5mm 才行 → 0.0555 − 0.0125 = 0.043。
#:
#: 实测验证（夹爪合到 23.9mm = 指间间距 24mm，规径 30mm → 6mm 过盈）：
#:     把规沿 Z 扫一遍，只有"规尖 = 两指中点 − 43mm"这一带能夹住：
#:         Δ 0mm  → 松手就掉 125mm（落台面）
#:         Δ +10mm → 夹住（漂移 0.2mm）
#:         Δ +15mm → 夹住
#:         Δ +20mm → 掉 9mm（已经在夹持段上端、开始收成 φ16 台肩）
#:     所以偏移取 +12.5mm。用错的值（比如 0.0555）的现象非常迷惑：
#:     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:
        return yaml.safe_load(fh) or {}


def _quat_to_R(x: float, y: float, z: float, w: float) -> List[List[float]]:
    """四元数 → 3×3 旋转矩阵（行主序）。自己算，不引入 scipy/tf_transformations。"""
    n = math.sqrt(x * x + y * y + z * z + w * w)
    if n == 0.0:
        raise ValueError("四元数全零")
    x, y, z, w = x / n, y / n, z / n, w / n
    return [
        [1 - 2 * (y * y + z * z), 2 * (x * y - z * w), 2 * (x * z + y * w)],
        [2 * (x * y + z * w), 1 - 2 * (x * x + z * z), 2 * (y * z - x * w)],
        [2 * (x * z - y * w), 2 * (y * z + x * w), 1 - 2 * (x * x + y * y)],
    ]


def _R_to_quat(R: List[List[float]]) -> Tuple[float, float, float, float]:
    """3×3 旋转矩阵 → 四元数 (x, y, z, w)。Shepperd 分支，保证数值稳定。"""
    tr = R[0][0] + R[1][1] + R[2][2]
    if tr > 0.0:
        s = math.sqrt(tr + 1.0) * 2.0
        w = 0.25 * s
        x = (R[2][1] - R[1][2]) / s
        y = (R[0][2] - R[2][0]) / s
        z = (R[1][0] - R[0][1]) / s
    elif R[0][0] > R[1][1] and R[0][0] > R[2][2]:
        s = math.sqrt(1.0 + R[0][0] - R[1][1] - R[2][2]) * 2.0
        w = (R[2][1] - R[1][2]) / s
        x = 0.25 * s
        y = (R[0][1] + R[1][0]) / s
        z = (R[0][2] + R[2][0]) / s
    elif R[1][1] > R[2][2]:
        s = math.sqrt(1.0 + R[1][1] - R[0][0] - R[2][2]) * 2.0
        w = (R[0][2] - R[2][0]) / s
        x = (R[0][1] + R[1][0]) / s
        y = 0.25 * s
        z = (R[1][2] + R[2][1]) / s
    else:
        s = math.sqrt(1.0 + R[2][2] - R[0][0] - R[1][1]) * 2.0
        w = (R[1][0] - R[0][1]) / s
        x = (R[0][2] + R[2][0]) / s
        y = (R[1][2] + R[2][1]) / s
        z = 0.25 * s
    return (x, y, z, w)


def _mat_mul(A: List[List[float]], B: List[List[float]]) -> List[List[float]]:
    return [[sum(A[i][k] * B[k][j] for k in range(3)) for j in range(3)] for i in range(3)]


def _mat_vec(A: List[List[float]], v: Tuple[float, float, float]) -> Tuple[float, float, float]:
    return tuple(sum(A[i][k] * v[k] for k in range(3)) for i in range(3))  # type: ignore[return-value]


#: 绕 X 轴转 180°：把"指尖系（+Z 指向工具外侧=向下）"翻成"规系（+Z 朝上）"。
#: 规的局部原点在**规尖**、+Z 朝上（见 model.sdf），而指尖系的 +Z 朝下
#: （实测：world←finger_tcp 的 Z 列 ≈ (0, 0, -1)），所以两者差一个 Rx(π)。
RX_PI = [[1.0, 0.0, 0.0], [0.0, -1.0, 0.0], [0.0, 0.0, -1.0]]


def gauge_target_pose(
    tips: Dict[str, Tuple[Tuple[float, float, float], Tuple[float, float, float, float]]],
    grasp_center_m: Optional[float] = None,
) -> Tuple[Tuple[float, float, float], Tuple[float, float, float, float]]:
    """两指 TCP 的世界位姿 → 规在世界系里的目标位姿（原点=规尖）。

    ``grasp_center_m=None`` 时用模块常量 ``GAUGE_GRASP_CENTER_M``
    （**调用时**读，不是定义时 —— 否则命令行覆盖会静默无效）。

    纯函数（离线可测）：输入左右两个 ``(位置, 四元数 xyzw)``，输出规的
    ``(位置, 四元数 xyzw)``。规则：
      * 工具轴位姿 = 两指 TCP 的**中点**、姿态取左指（两指只差一个绕轴自转）；
      * 规尖 = 中点沿工具轴**向外**（指尖系 +Z，实测朝下）移 ``grasp_center_m``；
      * 规姿态 = 指尖姿态再绕 X 转 180°，让规的 +Z 朝上（规尖朝下）。
    """
    missing = [k for k in ("left", "right") if k not in tips]
    if missing:
        raise ValueError(f"缺少 TCP 位姿：{missing}（TF 里没有 *_finger_tcp？）")
    if grasp_center_m is None:
        grasp_center_m = GAUGE_GRASP_CENTER_M
    (pl, ql) = tips["left"]
    (pr, _qr) = tips["right"]
    mid = ((pl[0] + pr[0]) / 2.0, (pl[1] + pr[1]) / 2.0, (pl[2] + pr[2]) / 2.0)
    R_tip = _quat_to_R(*ql)
    tool_axis = (R_tip[0][2], R_tip[1][2], R_tip[2][2])  # 指尖系 +Z 在世界里的方向
    tip = (
        mid[0] + tool_axis[0] * grasp_center_m,
        mid[1] + tool_axis[1] * grasp_center_m,
        mid[2] + tool_axis[2] * grasp_center_m,
    )
    q_gauge = _R_to_quat(_mat_mul(R_tip, RX_PI))
    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:
        p = subprocess.run(cmd, capture_output=True, text=True, timeout=timeout, check=False)
        return p.returncode, (p.stdout or "") + (p.stderr or "")
    except FileNotFoundError:
        return 127, f"找不到可执行文件：{cmd[0]}"
    except subprocess.TimeoutExpired:
        return 124, f"命令超时（{timeout}s）：{' '.join(cmd)}"


def _set_gauge_pose(world: str, name: str, pos, quat) -> Tuple[bool, str]:
    """用 Gazebo 的世界服务把模型搬到指定位姿。

    实现**只有一份**：``arm_sim_gazebo.sim_pose_service.set_model_pose``
    （那个 `--reqtype` 必须写 ``ignition.msgs.Pose`` 的坑也写在那边）。
    真值感知的启动复位走的是同一个函数。
    """
    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]]:
    """读回模型当前的世界位置（校验用）。读不到返回 None。"""
    code, out = _run(["ign", "model", "-m", name, "--pose"], timeout=10.0)
    if code != 0:
        return None
    seen_header = False
    for raw in out.splitlines():
        line = raw.strip()
        if "Pose [ XYZ" in line:
            seen_header = True
            continue
        if not seen_header or not (line.startswith("[") and line.endswith("]")):
            continue
        parts = line[1:-1].split()   # 形如 "[0.4926 -0.0039 0.4183]"（XYZ 那行）
        if len(parts) != 3:
            continue
        try:
            return (float(parts[0]), float(parts[1]), float(parts[2]))
        except ValueError:
            continue
    return None


# ---------------------------------------------------------------------------
# 竖直上料位姿：用 PyKDL 对同一份 URDF 做 IK
# ---------------------------------------------------------------------------
# 为什么需要它（实测，2026-09-24）：
#   * 仿真刚起来时关节在**零位**，工具轴是**水平**的（实测 TCP 的 Z 轴 = +X）。
#     水平夹住一根钢规，重力会把它从两指之间抽出去（实测刚合爪就掉回规座，
#     漂移 857mm）——不是夹爪坏了，是几何不对。
#   * 仿真 home 位姿（/set_named_pose home）工具轴斜 21°、指尖离台面只有 2mm，
#     规根本放不下。
#   所以上料前必须自己找一个"工具轴朝下 + 指尖离台面足够高"的位姿。真机上
#   这一步是人做的（把臂摆到方便的位置再装夹），仿真里用 IK 算出来。
#
# 用 PyKDL（环境里现成，不需要 kdl_parser_py / urdf_parser_py）：自己从 URDF
# 里沿着 base→flange 这条链建 KDL.Chain（只认 fixed/revolute 两种关节），
# 再用 ChainIkSolverPos_LMA 解。**不改任何应用代码、不依赖 MoveIt。**
LOAD_MID_Z_M = 0.58       # 想让两指 TCP 中点停在多高 [m]（规尖再低 55.5mm）
LOAD_MID_XY = (0.48, 0.0)  # 想让工具轴落在台面的哪个 XY [m]（台面上方空旷处）


def _parse_chain_joints(urdf_xml: str) -> Tuple[Dict[str, dict], Dict[str, str]]:
    """把 URDF 里的关节读成 ``{name: {...}}`` 与 ``{child_link: joint_name}``。

    每个关节里带上 ``limit``（可动关节的上下限 [rad]），IK 的解要用它筛一遍 ——
    解出的角度超限时控制器会直接拒绝（症状是"轨迹被拒/不动"），不如在这里挡掉。
    """
    import xml.etree.ElementTree as ET

    root = ET.fromstring(urdf_xml)
    joints: Dict[str, dict] = {}
    parent_of_child: Dict[str, str] = {}
    for j in root.findall("joint"):
        name = j.get("name")
        jtype = j.get("type")
        if not name or jtype not in ("fixed", "revolute", "continuous"):
            continue
        org = j.find("origin")
        axis = j.find("axis")
        lim = j.find("limit")
        joints[name] = {
            "type": jtype,
            "parent": (j.find("parent").get("link") if j.find("parent") is not None else None),
            "child": (j.find("child").get("link") if j.find("child") is not None else None),
            "xyz": [float(v) for v in ((org.get("xyz") if org is not None else None) or "0 0 0").split()],
            "rpy": [float(v) for v in ((org.get("rpy") if org is not None else None) or "0 0 0").split()],
            "axis": [float(v) for v in ((axis.get("xyz") if axis is not None else None) or "0 0 1").split()],
            "limit": (
                (float(lim.get("lower", "-3.15")), float(lim.get("upper", "3.15")))
                if lim is not None else None
            ),
        }
        parent_of_child[joints[name]["child"]] = name
    return joints, parent_of_child


def _build_kdl_chain(urdf_xml: str, root_link: str, tip_link: str):
    """按 URDF 从 ``root_link`` 走到 ``tip_link``，建成 ``(KDL.Chain, [关节名…])``。

    返回的关节名顺序 = 链上**可动**关节的顺序（从根到末）。控制器给的关节顺序
    可能不同，所以 IK 结果按名字映射回 ``{关节名: 角度}``，不靠位置对齐。
    """
    import PyKDL as kdl

    joints, parent_of_child = _parse_chain_joints(urdf_xml)
    # 从 tip 往 root 回溯，收集关节
    chain_joints: List[str] = []
    link = tip_link
    guard = 0
    while link != root_link:
        guard += 1
        if guard > 200:
            raise RuntimeError(f"URDF 里 {tip_link} 回溯不到 {root_link}（成环了？）")
        jname = parent_of_child.get(link)
        if jname is None:
            raise RuntimeError(f"URDF 里找不到 {link} 的父关节（根是不是 {root_link}？）")
        chain_joints.append(jname)
        link = joints[jname]["parent"]
    chain_joints.reverse()

    chain = kdl.Chain()
    movable: List[str] = []
    for jname in chain_joints:
        j = joints[jname]
        rpy = j["rpy"]
        R_org = kdl.Rotation.RPY(rpy[0], rpy[1], rpy[2])
        xyz = kdl.Vector(j["xyz"][0], j["xyz"][1], j["xyz"][2])
        if j["type"] == "fixed":
            chain.addSegment(kdl.Segment(jname, kdl.Joint(), kdl.Frame(R_org, xyz)))
        else:
            # URDF: pose(q) = Trans(xyz)·R(rpy)·Rot(a, q)        （a 在关节系里）
            # 但 PyKDL 的 Segment 只能表示"绕**过关节原点**的轴转 q"，转完再乘 f_tip。
            # 所以要把上面的式子拆成**三段**：
            #     Trans(xyz)  →  Rot(R(rpy)·a, q)  →  R(rpy)
            # 依据恒等式 R·Rot(a,q) == Rot(R·a, q)·R（取 R = R(rpy)）：
            #     Trans(xyz)·R(rpy)·Rot(a,q) == Trans(xyz)·Rot(R(rpy)·a,q)·R(rpy)  ✓
            #
            # ⚠ 这段踩过两次坑（2026-09-24），都是"q=0 看不出来、一动就错"：
            #   ① 轴不乘 R(rpy)、且把 xyz 塞进 f_tip：零位 0.007mm 通过，动起来差 263mm；
            #   ② 把 xyz 当成 joint 的 origin：零位就差 1260mm，动起来差 971mm。
            #   本文件下方 `--check-ik` 就是为这个坑准备的：**必须在非零位验**。
            chain.addSegment(kdl.Segment(jname + "_t", kdl.Joint(), kdl.Frame(kdl.Vector(*j["xyz"]))))
            axis = R_org * kdl.Vector(j["axis"][0], j["axis"][1], j["axis"][2])
            chain.addSegment(
                kdl.Segment(
                    jname,
                    kdl.Joint(jname, kdl.Vector(0.0, 0.0, 0.0), axis, kdl.Joint.RotAxis),
                    kdl.Frame(R_org),
                )
            )
            movable.append(jname)
    limits = {nm: joints[nm]["limit"] for nm in movable}
    return chain, movable, limits


def _solve_load_joints(
    chain, movable: List[str], seed: Dict[str, float],
    target_flange_pos, target_R,
) -> Optional[Dict[str, float]]:
    """IK：让 **flange** 落在 ``target_flange_pos`` 且姿态为 ``target_R``。

    调用方已经在 base 系里把"两指中点目标"换算成 flange 目标了（偏移量是
    从 TF 实测的，见 ``_go_vertical_load_pose``）。这里只负责解 + 验收。
    """
    import PyKDL as kdl

    p_flange = kdl.Vector(*target_flange_pos)
    F_target = kdl.Frame(target_R, p_flange)

    q_seed = kdl.JntArray(len(movable))
    for i, nm in enumerate(movable):
        q_seed[i] = float(seed.get(nm, 0.0))
    q_out = kdl.JntArray(len(movable))
    solver = kdl.ChainIkSolverPos_LMA(chain, 1e-6, 500, 1e-9)
    ret = solver.CartToJnt(q_seed, F_target, q_out)
    if ret < 0:
        return None
    # 校验：解出来的 FK 必须真的落在目标附近（LMA 会返回"最接近"，不保证命中）
    fk = kdl.ChainFkSolverPos_recursive(chain)
    F_now = kdl.Frame()
    fk.JntToCart(q_out, F_now)
    if (F_now.p - F_target.p).Norm() > 0.002:
        return None
    if (F_now.M.UnitZ() - F_target.M.UnitZ()).Norm() > 0.02:
        return None
    return {nm: float(q_out[i]) for i, nm in enumerate(movable)}


def _solve_best(
    chain, movable, limits, seed_list, target_pos, target_R, ref_joints,
) -> Optional[Tuple[Dict[str, float], str, float]]:
    """多初值求 IK，返回 ``(解, 用的初值, 代价)``；找不到返回 None。

    为什么要多初值：``ChainIkSolverPos_LMA`` 是**局部**求解器，从零位出发经常停在
    一个解不出来的局部极值上（实测：同一个目标，零位/垂肘初值全失败，换个"侧伸"
    初值就解出来了）。所以这里给一批初值（当前位姿 + 确定性随机扰动），挑**离当前
    位姿最近**的可行解 —— 近 = 动作小 = 安全、也不容易擦到台面。

    ``ref_joints`` 是"离谁近"的参照（一般用当前关节角）。
    """
    import math as _math
    import random

    best: Optional[Dict[str, float]] = None
    best_tag = ""
    best_cost = float("inf")
    for tag, seed in seed_list:
        sol = _solve_load_joints(chain, movable, seed, target_pos, target_R)
        if sol is None:
            continue
        bad = [
            nm for nm in movable
            if limits.get(nm) and not (limits[nm][0] <= sol[nm] <= limits[nm][1])
        ]
        if bad:
            continue
        cost = sum(abs(sol[nm] - ref_joints.get(nm, 0.0)) for nm in movable)
        if cost < best_cost:
            best, best_tag, best_cost = sol, tag, cost
    if best is None:
        return None
    return best, best_tag, best_cost


def _make_seeds(movable, limits, current: Dict[str, float], count: int = 24) -> List[Tuple[str, Dict[str, float]]]:
    """一批 IK 初值：当前位姿 + 全局撒点 + 当前位姿附近的扰动。

    为什么要三种：
      * 当前位姿 —— 能解出来时解最"近"（动作最小）；
      * 全局撒点 —— 目标离当前位姿很远时（比如从"工具轴水平"变到"朝下"），
        只围着一个点扰动是找不到的（区域太窄，实测就是这个问题）；
      * 附近扰动 —— 覆盖同一片构型里的不同肘部/腕部解。
    随机数用固定种子：同一份代码给出同一批初值，失败可复现、好排查。
    """
    import random

    rng = random.Random(20260924)
    out: List[Tuple[str, Dict[str, float]]] = [("当前位姿", dict(current))]
    out.append(("零位", {nm: 0.0 for nm in movable}))

    def _clamp(nm: str, v: float) -> float:
        lo, hi = limits.get(nm) or (-2.5, 2.5)
        return min(max(v, lo + 1e-3), hi - 1e-3)

    for i in range(6):  # 全局
        out.append((f"全局{i + 1}", {nm: _clamp(nm, rng.uniform(*(limits.get(nm) or (-2.5, 2.5)))) for nm in movable}))
    for i in range(count):  # 附近
        s: Dict[str, float] = {}
        for nm in movable:
            lo, hi = limits.get(nm) or (-2.5, 2.5)
            s[nm] = _clamp(nm, current.get(nm, 0.0) + rng.uniform(-(hi - lo) * 0.25, (hi - lo) * 0.25))
        out.append((f"附近{i + 1}", s))
    return out


# ---------------------------------------------------------------------------
# ROS 侧：TF + 夹爪 + 触觉
# ---------------------------------------------------------------------------
def _go_vertical_load_pose(node, buf, spin_for, wait_tf, tcp_frames, args) -> bool:
    """把机械臂摆到"工具轴朝下、两指 TCP 中点停在台面上方空旷处"的位姿。

    为什么不用 /home：仿真 home 的工具轴斜 21°、指尖离台面只有 2mm（实测），
    86mm 长的规放不下。真机上是人把臂摆到合适位置再装夹，仿真里没有这个人。

    做法：从 ``/robot_description`` 拿 URDF → 沿 base→flange 建 KDL 链 →
    对"flange 位姿"做 IK（目标由两指 TCP 中点的目标位姿反推，偏移量从
    当前 TF 实测得到，所以不假设夹爪多长）→ 走 JTC 轨迹 → 等静止 → 复测。
    """
    from std_msgs.msg import String
    from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile, ReliabilityPolicy

    import PyKDL as kdl

    from rclpy.action import ActionClient

    # (1) URDF：robot_state_publisher 用 transient_local 发，订阅端必须同 QoS，
    #     否则"订阅了但永远收不到"（典型的静默失效）。
    urdf_box: Dict[str, str] = {}
    qos = QoSProfile(
        depth=1,
        durability=DurabilityPolicy.TRANSIENT_LOCAL,
        reliability=ReliabilityPolicy.RELIABLE,
        history=HistoryPolicy.KEEP_LAST,
    )
    node.create_subscription(String, "/robot_description", lambda m: urdf_box.__setitem__("x", m.data), qos)
    t0 = time.monotonic()
    while "x" not in urdf_box and time.monotonic() - t0 < 10.0:
        spin_for(0.1)
    if "x" not in urdf_box:
        print("✗ 收不到 /robot_description（robot_state_publisher 没起来？）")
        return False
    urdf_xml = urdf_box["x"]

    # (2) 当前的 TCP 中点在 flange 系里的偏移（实测，不假设）
    base_frame, ee_frame = args.base_frame, args.ee_frame
    got_b = wait_tf(base_frame, timeout_s=5.0)
    got_e = wait_tf(ee_frame, timeout_s=5.0)
    if got_b is None or got_e is None:
        print(f"✗ TF 里找不到 {base_frame} 或 {ee_frame}（--base-frame/--ee-frame 可改）")
        return False
    p_base, q_base = got_b
    p_ee, q_ee = got_e
    tips_px = {}
    for side in ("left", "right"):
        g = wait_tf(tcp_frames[side], timeout_s=5.0)
        if g is None:
            print(f"✗ TF 里找不到 {tcp_frames[side]}")
            return False
        tips_px[side] = g[0]
    p_mid = (
        (tips_px["left"][0] + tips_px["right"][0]) / 2.0,
        (tips_px["left"][1] + tips_px["right"][1]) / 2.0,
        (tips_px["left"][2] + tips_px["right"][2]) / 2.0,
    )
    # 世界 → base 系
    R_base = _quat_to_R(*q_base)
    d_world = (p_mid[0] - p_base[0], p_mid[1] - p_base[1], p_mid[2] - p_base[2])
    R_ee_world = _quat_to_R(*q_ee)
    # 中点在 flange 系里的位置 = R_ee^T · (p_mid - p_ee)
    dm = (p_mid[0] - p_ee[0], p_mid[1] - p_ee[1], p_mid[2] - p_ee[2])
    mid_in_flange = _mat_vec([[R_ee_world[j][i] for j in range(3)] for i in range(3)], dm)
    print(f"  两指中点当前 ({p_mid[0]:.4f}, {p_mid[1]:.4f}, {p_mid[2]:.4f})，"
          f"在 flange 系偏移 ({mid_in_flange[0]:+.4f}, {mid_in_flange[1]:+.4f}, {mid_in_flange[2]:+.4f})")
    _ = d_world  # 仅诊断用（世界系位移），下面直接换到 base 系算目标

    # (3) 目标姿态：工具轴朝下（flange 的 +Z → 世界 -Z）。yaw 在下面 IK 时一起扫
    #     （--load-yaw 是首选值，扫不出来时会自动换别的 yaw）。这里不预先算矩阵：
    #     每个 yaw 都要一套，放在 _down_R_base 里按需算，避免"算了一个、用了另一个"。
    print("  目标姿态：工具轴朝下（世界 -Z）")

    # (4) IK：先解"高位竖直"（approach），再以它为参照解"低位竖直"（final）。
    #     为什么要两段：从零位（工具轴水平、臂抬得高）一步插值到低处竖直，关节
    #     空间的直线**可能从台面里穿过去**（Gazebo 是物理的，会被顶住、看着像
    #     "轨迹失败/不动"）。先到台面上方的高位竖直，再竖直下降，路径就干净了。
    #     两段**必须同一个 yaw**：yaw 不同，下降就成了斜着平移，不是"竖直下行"。
    chain, movable, limits = _build_kdl_chain(urdf_xml, base_frame, ee_frame)
    cur: Dict[str, float] = {nm: 0.0 for nm in movable}
    try:
        from sensor_msgs.msg import JointState

        js_box: Dict[str, object] = {}
        node.create_subscription(JointState, "/joint_states", lambda m: js_box.__setitem__("m", m), 10)
        t0 = time.monotonic()
        while "m" not in js_box and time.monotonic() - t0 < 10.0:
            spin_for(0.1)
        msg = js_box.get("m")
        if msg is not None:
            for nm, pos in zip(getattr(msg, "name", []), getattr(msg, "position", [])):
                if nm in cur:
                    cur[nm] = float(pos)
    except Exception as exc:  # noqa: BLE001
        print(f"⚠ 读 /joint_states 做初值失败（用零位初值继续）：{exc}")

    R_wb_T = [[R_base[j][i] for j in range(3)] for i in range(3)]  # 世界→base 的旋转
    seeds = _make_seeds(movable, limits, cur)

    def _down_R_base(yaw: float):
        """base 系里"工具轴朝下 + 偏航 yaw"的旋转矩阵。"""
        R_down_world = _mat_mul(
            [[math.cos(yaw), -math.sin(yaw), 0.0],
             [math.sin(yaw), math.cos(yaw), 0.0],
             [0.0, 0.0, 1.0]],
            RX_PI,
        )
        return _mat_mul(R_wb_T, R_down_world)

    def _mid_to_flange(mid_xyz, R_down_base):
        """两个 TCP 中点（世界系）→ 需要的 flange 位置（base 系）。"""
        dw = (mid_xyz[0] - p_base[0], mid_xyz[1] - p_base[1], mid_xyz[2] - p_base[2])
        mb = _mat_vec(R_wb_T, dw)
        ob = _mat_vec(R_down_base, mid_in_flange)
        return (mb[0] - ob[0], mb[1] - ob[1], mb[2] - ob[2])

    # yaw 从 --load-yaw 起试，再扫一圈。为什么要扫：工具轴竖直时手腕接近奇异，
    # 有些 yaw 会让 joint7 顶到限位/解不出来，换个 yaw 就解出来了。
    yaws = [args.load_yaw] + [
        math.radians(d) for d in (0, 30, -30, 60, -60, 90, -90, 120, -120, 150, -150, 180)
    ]

    def _solve_at(mid_xyz, ref) -> Optional[Tuple[Dict[str, float], str, float, float]]:
        """在 ``mid_xyz`` 这个点上扫 yaw + 多初值，返回 (解, 初值, 代价, yaw) 里最好的。"""
        best = None
        for yaw in yaws:
            Rdb = _down_R_base(yaw)
            r = _solve_best(chain, movable, limits, seeds,
                            _mid_to_flange(mid_xyz, Rdb),
                            kdl.Rotation(*[Rdb[i][j] for i in range(3) for j in range(3)]), ref)
            if r is not None and (best is None or r[2] < best[2]):
                best = (r[0], r[1], r[2], yaw)
        return best

    # 先在"用户指定的点"（默认值也是这个）上解；解不出来再在附近**换个落点**试。
    # 为什么允许换落点：规是脚本自己摆进指间的（不是从规座上抓的），所以落点只要
    # 在台面上方、够空旷就行 —— 换一个 IK 解得出、且动作最小的点，比死磕一个点强。
    user_mid = (args.load_x, args.load_y, args.load_z)
    grid = [user_mid] + [
        (x, y, z)
        for z in (args.load_z, args.load_z + 0.08, args.load_z - 0.06)
        for x in (args.load_x - 0.08, args.load_x + 0.06, args.load_x - 0.16)
        for y in (args.load_y,)
    ]
    lo = None
    for mid in grid:
        if mid[2] - GAUGE_GRASP_CENTER_M < FALLBACK_TABLE_Z + 0.05:
            continue  # 规尖会贴到台面，跳过
        r = _solve_at(mid, cur)
        if r is None:
            continue
        lo = (r[0], r[1], r[2], r[3], mid)
        if mid == user_mid:
            break  # 指定点能解就用它，不再换落点（可预期）
    if lo is None:
        print("✗ IK 没解出竖直上料位姿。")
        print(f"  目标：两指中点 ≈ ({args.load_x:.2f}, {args.load_y:.2f}, {args.load_z:.2f})、"
              "工具轴朝下（扫了 13 个 yaw、30 组初值、13 个候选落点）。")
        print("  大概率是这些点都在臂的工作空间外：换 --load-x/y/z（比如 x 小一点、z 高一点）再试。")
        return False
    sol, sol_tag, sol_cost, sol_yaw, sol_mid = lo

    # 高位：从"矮到高"找第一个能解出来的提升量。矮一点更接近目标、关节空间路径更短，
    # 只要能把规提到台面之上就够了（规尖在 TCP 中点下方 55.5mm，见 GAUGE_GRASP_CENTER_M）。
    hi = None
    for dz in (0.06, 0.10, 0.16, 0.22, 0.30):
        if dz > args.load_approach_dz:
            break
        r = _solve_at((sol_mid[0], sol_mid[1], sol_mid[2] + dz), sol)
        if r is not None and abs(r[3] - sol_yaw) < 1e-9:
            hi = (r[0], r[1], r[2], r[3], dz)
            break
    if hi is None:
        print("⚠ 找不到「与低位同 yaw 的高位」：改成一步到位"
              "（关节空间直线路径，注意看 Gazebo 会不会擦台面）。")

    print("=== 摆竖直上料位姿（IK） ===")
    print(f"  落点：两指中点 ({sol_mid[0]:.3f}, {sol_mid[1]:.3f}, {sol_mid[2]:.3f})"
          + ("" if sol_mid == user_mid else "（**不是**你给的点：你那个点 IK 解不出来，换了这个）"))
    if hi is not None:
        print(f"  高位：{'  '.join(f'{nm}={hi[0][nm]:+.3f}' for nm in movable)}"
              f"   （提升 {hi[4] * 1000:.0f}mm，初值 {hi[1]}）")
    print(f"  低位：{'  '.join(f'{nm}={sol[nm]:+.3f}' for nm in movable)}"
          f"   （初值 {sol_tag}，yaw {math.degrees(sol_yaw):+.0f}°，离当前位姿代价 {sol_cost:.3f}rad）")
    if args.dry_run:
        print("  --dry-run：只算不发（要看真动作去掉 --dry-run）")
        return True

    # (5) 走 JTC 轨迹
    #     用 **arm_msgs** 版的 FollowJointTrajectory（跟 run_joint_trajectory.py 完全同一条
    #     路：真机是 Flexiv 驱动直接提供，仿真由 arm_control/trajectory_bridge 桥到
    #     control_msgs 的 JTC）。不要用 control_msgs 版 —— 那会绕过应用层的桥，
    #     在仿真里名字/类型都对不上（同名不同型，静默等不到 server）。
    from arm_msgs.action import FollowJointTrajectory as ArmFJT

    prefix = args.joint_prefix
    jnames = [f"{prefix}joint{i}" for i in range(1, 8)]
    missing = [n for n in jnames if n not in sol]
    if missing:
        print(f"✗ IK 结果里没有这些关节：{missing}（--joint-prefix 对不对？当前 {prefix!r}）")
        return False
    goal = ArmFJT.Goal()
    goal.joint_names = jnames
    goal.velocity_scale = float(args.load_velocity_scale)
    if hi is not None:
        goal.num_points = 2
        goal.positions = [float(hi[0][n]) for n in jnames] + [float(sol[n]) for n in jnames]
        half = max(0.5, float(args.load_move_s) * 0.6)
        goal.time_from_start = [half, half + float(args.load_move_s) * 0.5]
    else:
        goal.num_points = 1
        goal.positions = [float(sol[n]) for n in jnames]
        goal.time_from_start = [float(args.load_move_s)]

    cli = ActionClient(node, ArmFJT, args.action_name)
    if not cli.wait_for_server(timeout_sec=10.0):
        print(f"✗ JTC 动作不在：{args.action_name}（arm_msgs/FollowJointTrajectory）")
        print("  仿真：跑 run_sim_gz.sh 了吗（trajectory_bridge 由它拉起）？")
        return False
    fut = cli.send_goal_async(goal)
    t_send = time.monotonic()
    while not fut.done() and time.monotonic() - t_send < 10.0:
        spin_for(0.05)
    handle = fut.result() if fut.done() else None
    if handle is None or not handle.accepted:
        print("✗ JTC 拒绝了这条轨迹")
        return False
    res_fut = handle.get_result_async()
    t1 = time.monotonic()
    while not res_fut.done() and time.monotonic() - t1 < 60.0:
        spin_for(0.1)
    if not res_fut.done():
        print("✗ 等轨迹结果超时（60s）")
        return False
    res = res_fut.result().result
    if not getattr(res, "success", False):
        print(f"✗ 轨迹执行失败：{getattr(res, 'message', '')!r}")
        return False
    # 等静止（关节到位 ≠ 指尖不再动：还有弹性settle）
    last = None
    still = 0.0
    t2 = time.monotonic()
    while time.monotonic() - t2 < 10.0:
        spin_for(0.2)
        g = wait_tf(tcp_frames["left"], timeout_s=0.5)
        if g is None:
            continue
        if last is not None and math.dist(g[0], last) < 0.0005:
            still += 0.2
            if still >= 0.8:
                break
        else:
            still = 0.0
        last = g[0]

    # (6) 复测（闭环验收：不验就是"指令发了就当成功"）
    g = wait_tf(tcp_frames["left"], timeout_s=5.0)
    if g is None:
        return False
    R_after = _quat_to_R(*g[1])
    ax = (R_after[0][2], R_after[1][2], R_after[2][2])
    print(f"  复测：工具轴 Z 分量 {ax[2]:+.3f}（要求 ≤ -0.99）、"
          f"TCP 高度 z={g[0][2]:.3f}（台面 {0.40:.2f}）")
    if ax[2] > -0.99:
        print("  ⚠ 复测没到竖直：可能是关节限位挡住了，或 IK 的种子太远。")
        return False
    return True


def _check_ik(node, buf, spin_for, wait_tf, args) -> int:
    """自检：``/joint_states`` + URDF→KDL 正解 vs TF 里 base→flange 的真实位姿。

    误差应当 <1mm / <0.5°。对不上就是 URDF→KDL 的转换有问题（轴表达系、链、关节
    顺序），**别接着上料** —— IK 会自洽地错，最后表现为"臂动了但规没进指间"。
    """
    import PyKDL as kdl

    from std_msgs.msg import String
    from rclpy.qos import DurabilityPolicy, HistoryPolicy, QoSProfile, ReliabilityPolicy
    from sensor_msgs.msg import JointState

    urdf_box: Dict[str, str] = {}
    qos = QoSProfile(
        depth=1,
        durability=DurabilityPolicy.TRANSIENT_LOCAL,
        reliability=ReliabilityPolicy.RELIABLE,
        history=HistoryPolicy.KEEP_LAST,
    )
    node.create_subscription(String, "/robot_description", lambda m: urdf_box.__setitem__("x", m.data), qos)
    js_box: Dict[str, object] = {}
    node.create_subscription(JointState, "/joint_states", lambda m: js_box.__setitem__("m", m), 10)
    t0 = time.monotonic()
    while ("x" not in urdf_box or "m" not in js_box) and time.monotonic() - t0 < 15.0:
        spin_for(0.1)
    if "x" not in urdf_box or "m" not in js_box:
        print("✗ 15s 内没拿到 /robot_description 或 /joint_states")
        return 2
    js = js_box["m"]
    cur = {str(n): float(p) for n, p in zip(getattr(js, "name", []), getattr(js, "position", []))}

    chain, movable, _limits = _build_kdl_chain(urdf_box["x"], args.base_frame, args.ee_frame)
    print(f"=== --check-ik：链上可动关节 {movable} ===")
    q = kdl.JntArray(len(movable))
    for i, nm in enumerate(movable):
        q[i] = cur.get(nm, 0.0)
    fk = kdl.ChainFkSolverPos_recursive(chain)
    F = kdl.Frame()
    fk.JntToCart(q, F)

    got_b = wait_tf(args.base_frame)
    got_e = wait_tf(args.ee_frame)
    if got_b is None or got_e is None:
        print(f"✗ TF 里找不到 {args.base_frame} 或 {args.ee_frame}")
        return 2
    R_b = _quat_to_R(*got_b[1])
    R_e = _quat_to_R(*got_e[1])

    def _to_base(p):
        d = (p[0] - got_b[0][0], p[1] - got_b[0][1], p[2] - got_b[0][2])
        return _mat_vec([[R_b[j][i] for j in range(3)] for i in range(3)], d)

    p_tf = _to_base(got_e[0])
    # TF 的 flange 姿态 → base 系
    R_tf = _mat_mul([[R_b[j][i] for j in range(3)] for i in range(3)], R_e)
    p_fk = (F.p[0], F.p[1], F.p[2])
    dp = math.dist(p_tf, p_fk)
    # 姿态差：用 Z 轴夹角（工具轴是 IK 唯一关心的方向）+ 全姿态差
    z_fk = (F.M.UnitZ()[0], F.M.UnitZ()[1], F.M.UnitZ()[2])
    z_tf = (R_tf[0][2], R_tf[1][2], R_tf[2][2])
    ang = math.degrees(math.acos(max(-1.0, min(1.0, sum(a * b for a, b in zip(z_fk, z_tf))))))
    print(f"  位置：正解 ({p_fk[0]:+.5f}, {p_fk[1]:+.5f}, {p_fk[2]:+.5f}) base 系")
    print(f"        TF   ({p_tf[0]:+.5f}, {p_tf[1]:+.5f}, {p_tf[2]:+.5f}) base 系")
    print(f"  误差：位置 {dp * 1000:.3f}mm、工具轴夹角 {ang:.3f}°")
    _ = node, buf
    if dp > 0.001 or ang > 0.5:
        print("✗ 对不上：URDF→KDL 的转换有问题（先别上料，IK 会自洽地错）。")
        return 1
    print("✓ 正解与 TF 一致，IK 可信。")
    return 0


def _ros_side(args) -> int:  # noqa: C901 - 线性流程，拆开反而更难读
    import rclpy
    from rclpy.node import Node
    from rclpy.time import Time
    from tf2_ros import Buffer, TransformListener

    from arm_msgs.msg import GripperStatus
    from arm_msgs.srv import GripperExecute
    from arm_msgs.msg import TactileStatus

    tactile_cfg = _load_yaml(ROOT / "devices" / "sim" / "gazebo" / "config" / "tactile.yaml")
    scene_cfg = _load_yaml(ROOT / "devices" / "sim" / "gazebo" / "config" / "scene.yaml")
    sides = tactile_cfg.get("sides") or []
    frames = {str(s.get("side")): str(s.get("frame")) for s in sides if s.get("side")}
    tcp_frames = {str(s.get("side")): str(s.get("tcp_frame") or "") for s in sides if s.get("side")}
    if set(frames) < {"left", "right"}:
        print(f"✗ tactile.yaml 里没有左右指的 frame（读到 {frames}）")
        return 2
    for side in ("left", "right"):
        if not tcp_frames.get(side):
            # 不猜：靠 frame 加后缀推出来的名字是错的（实测 mount / tcp 是并列的
            # 两条链，不是父子命名）。明确提示去补配置，而不是静默用错名字。
            print(f"✗ tactile.yaml 的 sides[{side}] 缺 tcp_frame（指腹夹持点坐标系）。")
            print("  它是上料脚本算工具轴用的：两指 TCP 中点 = 工具轴，规尖在这条轴上。")
            print("  在仿真里查：ros2 run tf2_ros tf2_echo world <指节帧>_tcp 不对时，")
            print("  用 ros2 run tf2_tools view_frames 或 tf2_echo 逐个试 *_finger_tcp。")
            rclpy.shutdown()
            return 2

    rclpy.init()
    node = Node("sim_load_gauge")
    # 仿真的时钟来自 /clock；TF 的时间戳也是仿真时间，必须用 sim time 才能对上
    node.set_parameters([rclpy.parameter.Parameter("use_sim_time", value=True)])
    buf = Buffer(cache_time=rclpy.duration.Duration(seconds=15.0))
    TransformListener(buf, node)

    tac: Dict[str, Tuple[float, object]] = {}
    sub = node.create_subscription(
        TactileStatus, "/tactile/left/status", lambda m: tac.__setitem__("left", (time.monotonic(), m)), 10
    )
    sub_r = node.create_subscription(
        TactileStatus, "/tactile/right/status", lambda m: tac.__setitem__("right", (time.monotonic(), m)), 10
    )
    status: Dict[str, GripperStatus] = {}
    node.create_subscription(
        GripperStatus, "/gripper/status", lambda m: status.__setitem__("now", m), 10
    )

    def spin_for(sec: float) -> None:
        t_end = time.monotonic() + sec
        while time.monotonic() < t_end:
            rclpy.spin_once(node, timeout_sec=0.05)

    def wait_tf(frame: str, timeout_s: float = 15.0):
        """等 world←frame 可用，返回 (位置, 四元数) 或 None。"""
        t0 = time.monotonic()
        while time.monotonic() - t0 < timeout_s:
            spin_for(0.1)
            try:
                tr = buf.lookup_transform("world", frame, Time())
            except Exception:  # noqa: BLE001 - TF 还没起来
                continue
            t = tr.transform.translation
            q = tr.transform.rotation
            return (t.x, t.y, t.z), (q.x, q.y, q.z, q.w)
        return None

    # ---- -1. 自检：URDF→KDL 的正解必须和 TF 对得上（--check-ik） ----------
    # 为什么要有这个开关：IK 用的是"自己从 URDF 建的 KDL 链"，而臂的真实姿态在
    # TF 里。这两者一旦不一致（轴的表达系、父子链、关节顺序写错），IK 会**自洽地
    # 错**（正解逆解都错得很一致），最后表现为"轨迹动了但规装不上"。所以先验一条：
    # 用 /joint_states 当前角度做正解，跟 TF 的 world←base、world←flange 比。
    if args.check_ik:
        return _check_ik(node, buf, spin_for, wait_tf, args)

    # ---- 0. 回 home（可选，默认不做） ------------------------------------
    # ⚠ 实测（2026-09-24）：**不要默认回 home**。仿真里 home 是机械臂的出厂
    #   预备位姿，工具轴是斜的（实测 Z 分量 -0.930），而"把规垂进指间"要求
    #   工具轴竖直。默认调 /home 会把本来竖直的姿态弄斜，规尖算到台面以下。
    #   需要时用 --home 显式调（真机上则由人把臂摆好）。
    from std_srvs.srv import Trigger

    if args.home and not args.place_only:
        home = node.create_client(Trigger, "/home")
        if not home.wait_for_service(timeout_sec=10.0):
            print("⚠ /home 不在（sim_home_service 没起来？）—— 不动臂，直接按当前姿态上料。")
        else:
            print("=== --home：先回 home（注意：home 姿态工具轴可能是斜的） ===")
            fut = home.call_async(Trigger.Request())
            t0 = time.monotonic()
            while not fut.done() and time.monotonic() - t0 < 60.0:
                spin_for(0.1)
            if not fut.done():
                print("✗ /home 调用超时（60s）")
                rclpy.shutdown()
                return 2
            resp = fut.result()
            print(f"  /home 返回 success={getattr(resp, 'success', None)} "
                  f"msg={getattr(resp, 'message', '')!r}")
            # 等指尖真的停下（回 home 是关节运动，立刻量 TCP 会量到中途）
            t0 = time.monotonic()
            last = None
            still = 0.0
            while time.monotonic() - t0 < 30.0:
                spin_for(0.2)
                got = wait_tf(tcp_frames["left"], timeout_s=0.5)
                if got is None:
                    continue
                p = got[0]
                if last is not None and math.dist(p, last) < 0.0005:
                    still += 0.2
                    if still >= 0.8:
                        break
                else:
                    still = 0.0
                last = p
            print("  指尖已静止")

    # ---- 1. 先张开 ------------------------------------------------------
    # 顺序很重要：**先张开再量 TCP**。指节是连杆机构，闭合时指腹会向内/向下
    # 走一大截（实测 TCP 从 z=0.474 掉到 z=0.458），拿"闭合姿态"算出来的规尖
    # 位置会低 15mm —— 规一放就插进台面。
    st0 = None
    cli_open = node.create_client(GripperExecute, "/gripper/open")
    cli_move = node.create_client(GripperExecute, "/gripper/move")
    for cli, nm in ((cli_open, "/gripper/open"), (cli_move, "/gripper/move")):
        if not cli.wait_for_service(timeout_sec=5.0):
            print(f"✗ {nm} 不在（契约后端 arm_msgs/srv/GripperExecute）")
            print("  夹爪服务没起来？看 run_sim_gz.sh 的 gn01 适配器与 /gripper/status。")
            rclpy.shutdown()
            return 2

    def gripper_call(cli, command: str, position: Optional[float] = None) -> bool:
        req = GripperExecute.Request()
        req.command.command = command
        req.command.speed = 50.0
        req.command.force = 30.0
        req.command.wait_complete = False   # 自己等，才能顺便打印中间状态
        req.command.timeout = 5.0
        if position is not None:
            # move 的 position 是**开口**（mm，与 /gripper/status.position 同口径）
            req.command.position = float(position)
        fut = cli.call_async(req)
        t0 = time.monotonic()
        while not fut.done() and time.monotonic() - t0 < 5.0:
            spin_for(0.05)
        return fut.done()

    def wait_gripper_still(timeout_s: float = 6.0) -> None:
        t0 = time.monotonic()
        while time.monotonic() - t0 < timeout_s:
            spin_for(0.1)
            st = status.get("now")
            if st is not None and not st.is_moving:
                return

    if args.place_only:
        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 调用超时")
            rclpy.shutdown()
            return 2
        wait_gripper_still()
        st0 = status.get("now")
        if st0 is not None:
            print(f"  开爪后开口 {st0.position:.2f}mm")

    # ---- 1.1 标定"夹爪开口 ↔ 两指指腹间距" ----------------------------------
    # 为什么必须标定：夹爪适配器的"开口"是**相对全闭合位置**的行程量
    #（真机 GR01 行程 20mm、仿真夹爪 99.9mm，两套口径完全不同），而规是按
    # **物理直径**做的。要判断"开口合到多少才正好卡住 φ30 的规"，就得先把
    # 两指指腹之间的**真实间距**量出来 —— 指腹间距才是和规径同一个量纲的东西。
    #
    # 怎么量：指腹夹持点在 TF 里就是每个指节的 tcp_frame（tactile.yaml 指定），
    # 两点的距离就是指间间距。指节是连杆机构，间距与开口行程严格线性
    #（实测斜率 = 1.000），所以量一个点就能定出"开口=0 时的间距"。
    #
    # 不标定的后果（实测踩过）：按适配器日志里的数字写死开口，换了夹爪型号
    # 或改了 URDF 就静默错位 —— 要么夹不到（规掉），要么把规捏飞。
    gap_cal_mm: Optional[float] = None   # 开口 = 0 时的指间间距 [mm]
    gap_at_aperture = None

    def tcp_gap_mm() -> Optional[float]:
        """按当前 TF 算两指指腹间距 [mm]；TF 不全返回 None。"""
        pts = {}
        for side in ("left", "right"):
            got = wait_tf(tcp_frames[side], timeout_s=3.0)
            if got is None:
                return None
            pts[side] = got[0]
        dx = pts["left"][0] - pts["right"][0]
        dy = pts["left"][1] - pts["right"][1]
        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 调用超时（标定用）")
            rclpy.shutdown()
            return 2
        wait_gripper_still()
        wait_tf(tcp_frames["left"], timeout_s=3.0)
        gap_at_aperture = tcp_gap_mm()
        if gap_at_aperture is not None:
            gap_cal_mm = gap_at_aperture - args.cal_aperture_mm
            print("=== 标定指间间距 ===")
            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.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 摆位（纯瞬移）与"人手托着合爪" -------------------------------
    #
    # ⚠️ 这一段是整个上料脚本里最反直觉的地方，实测踩了很久，先说清因果：
    #
    # ① 为什么不能"先摆位、再合爪"：
    #    把规瞬移到张开的指间之后，它**悬空**。下面什么都没有，于是立刻自由
    #    落体。物理步进是 1ms 级，而一次 ROS 服务往返 + 夹爪关节走完行程要
    #    好几百毫秒 —— 等"合爪"生效时，规早掉出台面了。
    #    实测现象：合爪开口读到 0.00mm（指间是空的）、触觉无接触、规的位姿
    #    漂移 ≈ 规尖到目标点的距离（约 125mm，正好落在台面上）。
    #
    # ② 为什么也不能"先合爪、再把规塞进去"：
    #    规一出现就和指腹重叠，求解器当步建起接触力，把规**弹出去**（实测
    #    0.05s 内就跑掉 116mm）。过盈越大弹得越凶。
    #
    # ③ 所以：**像人手一样托着它**。
    #    合爪的同时，每个控制周期都按当前指腹位姿把规瞬移回去（就相当于人的
    #    手一直扶在规上），规永远不会掉。等指腹真正夹住了（松手后规自己不掉），
    #    再停止托着。
    #
    # ④ 夹爪该合到多少：**扫**出来，不猜。
    #    指腹的**实际接触面**在网格里，未必就在 tcp_frame 原点上（外偏/内偏
    #    都可能），拿"指间间距 = 开口"去反推闭合量，会差出好几毫米 —— 差了就
    #    表现为"明明有接触（gz 报了 Contacts）却夹不住，规自己滑下去"。
    #    所以这里从松到紧一步一步试：能夹住就停。松到紧的顺序很重要，夹住即可，
    #    不要一上来就压到最紧（过盈太大会把规顶飞）。
    def read_tcp_tips() -> Optional[Dict[str, Tuple[Tuple[float, float, float],
                                                   Tuple[float, float, float, float]]]]:
        out: Dict[str, Tuple[Tuple[float, float, float], Tuple[float, float, float, float]]] = {}
        for side in ("left", "right"):
            got = wait_tf(tcp_frames[side], timeout_s=2.0)
            if got is None:
                return None
            out[side] = got
        return out

    def gauge_drift_mm(target_pos) -> Optional[float]:
        now = _gauge_pose_now(args.model)
        if now is None:
            return None
        return math.dist(now, target_pos) * 1000.0

    def hold_gauge_while(seconds: float, max_iter: int = 200) -> Optional[Tuple]:
        """像人手一样托着规：按当前指腹位姿反复把规瞬移回两指之间。

        返回最后一次用的 (target_pos, target_quat)。
        """
        t0 = time.monotonic()
        last = None
        n = 0
        while time.monotonic() - t0 < seconds and n < max_iter:
            tips_now = read_tcp_tips()
            if tips_now is None:
                return last
            pos_q = gauge_target_pose(tips_now, args.grasp_center_m)
            _set_gauge_pose(args.world, args.model, pos_q[0], pos_q[1])
            last = pos_q
            n += 1
            spin_for(args.hold_period_s)
        return last

    held_ap: Optional[float] = None
    final_target: Optional[Tuple[Tuple[float, float, float],
                                 Tuple[float, float, float, float]]] = None
    if not args.place_only:
        if gap_cal_mm is None:
            print("✗ 没能标定指间间距（TF 里量不到两指 tcp_frame），无法确定扫描范围。")
            print(f"  检查 tactile.yaml 的 sides[*].tcp_frame：{tcp_frames}")
            print("  或加 --place-only 只摆位、不碰夹爪。")
            rclpy.shutdown()
            return 2

        # 扫描区间：指间间距从（规径 + 富余）收到（规径 − 富余），按开口口径表示。
        ap_loose = (args.gauge_dia_mm + args.scan_margin_mm) - gap_cal_mm
        ap_tight = (args.gauge_dia_mm - args.scan_margin_mm) - gap_cal_mm
        ap_loose = min(ap_loose, args.cal_aperture_mm * 5)
        ap_tight = max(0.0, ap_tight)
        if ap_loose <= ap_tight:
            print(f"✗ 扫描区间不合法（松 {ap_loose:.2f} ≤ 紧 {ap_tight:.2f}）。")
            rclpy.shutdown()
            return 2

        print("=== 合爪：像人手一样托着规，从松到紧扫出能夹住的开口 ===")
        print(f"  规径 {args.gauge_dia_mm:.1f}mm；扫描开口 {ap_loose:.2f} → "
              f"{ap_tight:.2f}mm，步长 {args.scan_step_mm:.2f}mm；"
              f"每档先托着合、再松手看掉不掉")

        ap = ap_loose
        while ap >= ap_tight - 1e-9:
            if not gripper_call(cli_move, "move", position=ap):
                print("✗ /gripper/move 调用超时")
                rclpy.shutdown()
                return 2
            wait_gripper_still()
            spin_for(0.2)
            # 托着它，让指腹贴着规合进去
            last = hold_gauge_while(args.hold_s)
            if last is None:
                print("✗ 托着规的时候读不到指腹 TF。")
                rclpy.shutdown()
                return 2
            # 松手：停止瞬移，看规掉不掉
            spin_for(args.release_s)
            drift = gauge_drift_mm(last[0])
            st = status.get("now")
            gap_now = tcp_gap_mm()
            tag = "?" if drift is None else f"{drift:.1f}mm"
            print(f"  开口 {ap:5.2f}mm（指间间距 "
                  f"{'?' if gap_now is None else f'{gap_now:.2f}'}mm）→ "
                  f"松手后漂移 {tag}"
                  f"{'' if st is None else f'，夹爪读 {st.position:.2f}mm'}")
            if drift is not None and drift < args.held_tol_mm:
                held_ap = ap
                final_target = last
                print(f"  ✓ 夹住了（漂移 {drift:.1f}mm < {args.held_tol_mm:.1f}mm）")
                break
            ap -= args.scan_step_mm

        if held_ap is None:
            print("✗ 从松到紧扫了一遍都没夹住。")
            print("  可能原因：① 指腹实际接触面和 tcp_frame 差得很远（看上面每档的"
                  "指间间距，规径应落在扫到的某个间距附近）；")
            print("            ② 规的夹持段没落在指腹高度上（--grasp-center-m 要改）;")
            print("            ③ 摩擦/接触刚度太低（gauge/model.sdf 的 grip 段 mu/kp）。")
            rclpy.shutdown()
            return 2

        # 夹住之后稍微再压紧一点，让法向力（也就是摩擦力）够用。
        if args.squeeze_mm > 0.0:
            want_ap = max(ap_tight, held_ap - args.squeeze_mm)
            print(f"=== 压紧 {args.squeeze_mm:.2f}mm（开口 {held_ap:.2f} → "
                  f"{want_ap:.2f}mm），期间继续托着 ===")
            # 一边压一边托：压的过程中指腹会横向走，规不托着就会被搓下去
            if not gripper_call(cli_move, "move", position=want_ap):
                print("✗ /gripper/move 调用超时")
                rclpy.shutdown()
                return 2
            deadline = time.monotonic() + args.hold_s + 1.0
            while time.monotonic() < deadline:
                tips_now = read_tcp_tips()
                if tips_now is None:
                    break
                pos_q = gauge_target_pose(tips_now, args.grasp_center_m)
                _set_gauge_pose(args.world, args.model, pos_q[0], pos_q[1])
                final_target = pos_q
                spin_for(args.hold_period_s)
            held_ap = want_ap

    # ---- 2. 量两指 TCP（**在最终夹持开口下**量） --------------------------
    # 指节是连杆机构：开口变化时指腹不仅横向移动，纵向也会走一截
    #（实测 TCP 从张开到闭合掉 15mm）。所以必须在"将来真正夹住规"的那个
    # 开口下量 TCP，否则算出来的规尖位置会整体偏低，规一放就插进台面。
    tips = {}
    for side in ("left", "right"):
        frame = tcp_frames[side]
        got = wait_tf(frame)
        if got is None:
            print(f"✗ TF 里等不到 world←{frame}（15s）。仿真栈起来了吗？")
            print("   bash scripts/run_sim_gz.sh  之后再跑本脚本。")
            rclpy.shutdown()
            return 2
        tips[side] = got
    tip_pos, tip_quat = gauge_target_pose(tips, args.grasp_center_m)

    R_tip = _quat_to_R(*tips["left"][1])
    axis = (R_tip[0][2], R_tip[1][2], R_tip[2][2])
    print("=== 指尖与工具轴（世界系） ===")
    print(f"  左 TCP = ({tips['left'][0][0]:.4f}, {tips['left'][0][1]:.4f}, {tips['left'][0][2]:.4f})")
    print(f"  右 TCP = ({tips['right'][0][0]:.4f}, {tips['right'][0][1]:.4f}, {tips['right'][0][2]:.4f})")
    print(f"  工具轴（指向规尖）= ({axis[0]:+.3f}, {axis[1]:+.3f}, {axis[2]:+.3f})"
          f"  （Z 分量 ≈ -1 才对：规尖朝下）")
    if abs(axis[2]) < 0.99:
        # 不是硬失败：规是**跟着工具轴**装的，工具轴斜着，规就斜着 ——
        # 流程到 S04 会自动把工具轴竖直（那时规跟着转正）。真正不能接受的
        # 是"规尖算到台面以下"（下面那个检查），不是"现在斜着"。
        print(f"⚠ 工具轴不竖直（|Z| = {abs(axis[2]):.3f}）：规会斜着装。")
        print("  仿真刚起来时关节在零位（臂竖起、离台面高），那是最省事的上料姿态；")
        print("  跑过流程后停在工件上方（竖直）也可以。斜着装不影响后续，S04 会竖直它。")

    print(f"=== 规尖目标位姿 ===\n  ({tip_pos[0]:.4f}, {tip_pos[1]:.4f}, {tip_pos[2]:.4f})"
          f"  四元数 xyzw=({tip_quat[0]:+.6f}, {tip_quat[1]:+.6f}, "
          f"{tip_quat[2]:+.6f}, {tip_quat[3]:+.6f})")

    table_top = float(scene_cfg.get("table_top_z") or FALLBACK_TABLE_Z)
    clearance = tip_pos[2] - table_top
    print(f"  规尖离台面 {clearance * 1000:.0f}mm（规长 86mm，向上全在指间）")
    if args.dry_run:
        print("（--dry-run：到此为止，没有碰仿真）")
        rclpy.shutdown()
        return 0
    if clearance < 0.01:
        # 判据只看**规尖**：规是从指尖往下垂的 86mm，规尖在台面以上就够了
        #（其余部分在指间、在规尖上方）。这里卡住通常是臂压得太低。
        print("✗ 规尖低于台面（或只高不到 10mm），会把规插进台面里。")
        print("  先把臂抬起来（或让流程回到 home 姿态）再上料。")
        rclpy.shutdown()
        return 2

    # ---- 4. 摆位 --------------------------------------------------------
    name = args.model
    before = _gauge_pose_now(name)
    print(f"=== 摆位：把 {name} 搬到目标位姿 ===")
    if before is not None:
        print(f"  摆位前 = ({before[0]:.4f}, {before[1]:.4f}, {before[2]:.4f})")
    ok, out = _set_gauge_pose(args.world, name, tip_pos, tip_quat)
    if not ok:
        print(f"✗ set_pose 失败：{out}")
        print(f"  排查：世界名对不对（--world，默认 assembly）、模型名对不对（--model，"
              f"默认 gauge）、场景有没有加载。")
        rclpy.shutdown()
        return 2
    spin_for(args.settle_s)

    # ---- 6. 校验 --------------------------------------------------------
    now_pos = _gauge_pose_now(name)
    drift: Optional[float] = None
    if now_pos is None:
        print("=== 校验：规还在不在手里 ===")
        print("  ⚠ 读不到规的位姿（ign model --pose 没给可解析的输出）—— "
              "跳过位姿校验，只看触觉。")
    else:
        drift = math.dist(now_pos, tip_pos)
        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)
        if item is None:
            print(f"  触觉 {side}: 没有数据（/tactile/{side}/status 没发？）")
            continue
        _t, m = item
        fn = abs(float(getattr(m, "fnormal", 0.0)))
        hit = bool(getattr(m, "contact", False)) or fn > TACTILE_NOISE_FLOOR
        tac_ok = tac_ok or hit
        print(f"  触觉 {side}: contact={getattr(m, 'contact', None)} |fnormal|={fn:.4g}"
              f" → {'夹到了' if hit else '没接触'}")

    problems: List[str] = []
    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:
        print("✗ 上料没成功：")
        for p in problems:
            print(f"    - {p}")
        print("  排查顺序：① 上面打印的指节帧/TCP 是不是 sim_* 前缀；"
              "② 场景里 gauge 模型在不在（ign model --list）；③ 合爪开口有没有变小。")
        rclpy.shutdown()
        return 2

    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


def main() -> int:
    ap = argparse.ArgumentParser(
        description="把通止规装进夹爪（仿真专用上料）",
        formatter_class=argparse.RawDescriptionHelpFormatter,
        epilog="真机不需要这个脚本：现场由人把规塞进两指之间并合爪。",
    )
    ap.add_argument("--world", default="assembly", help="Gazebo 世界名（默认 assembly）")
    ap.add_argument("--model", default="gauge", help="通止规模型名（默认 gauge）")
    ap.add_argument("--grasp-center-m", type=float, default=GAUGE_GRASP_CENTER_M,
                    help=f"规尖到夹持段中点的距离 [m]（默认 {GAUGE_GRASP_CENTER_M}）")
    ap.add_argument("--settle-s", type=float, default=2.5, help="摆位后等物理稳定的秒数")
    ap.add_argument("--gauge-dia-mm", type=float, default=GAUGE_GRIP_DIA_MM,
                    help=f"规**夹持段**直径 [mm]（默认 {GAUGE_GRIP_DIA_MM}，与 gauge/model.sdf 一致）")
    ap.add_argument("--squeeze-mm", type=float, default=2.0,
                    help="扫到「能夹住」之后再额外压紧多少 [mm]（默认 2.0，让摩擦力有余量）")
    ap.add_argument("--scan-margin-mm", type=float, default=10.0,
                    help="扫描区间：指间间距在（规径±该值）之间扫 [m]（默认 10；"
                         "指腹接触面比 tcp_frame 外偏约 6mm，留够余量才能扫到）")
    ap.add_argument("--scan-step-mm", type=float, default=1.0,
                    help="扫描步长 [mm]（默认 1.0；越小越准但越慢）")
    ap.add_argument("--hold-s", type=float, default=0.9,
                    help="每档「托着规」多久 [s]（默认 0.9，够指腹合到位）")
    ap.add_argument("--hold-period-s", type=float, default=0.02,
                    help="托着规时多久瞬移一次 [s]（默认 0.02 = 50Hz）")
    ap.add_argument("--release-s", type=float, default=0.6,
                    help="松手后等多久再判「掉没掉」 [s]（默认 0.6）")
    ap.add_argument("--held-tol-mm", type=float, default=3.0,
                    help="松手后漂移小于该值就算夹住 [mm]（默认 3.0）")
    ap.add_argument("--cal-aperture-mm", type=float, default=20.0,
                    help="标定指间间距时用的开口 [mm]（默认 20，随便取一个中间值即可）")
    ap.add_argument("--home", action="store_true",
                    help="先调 /home（默认不做：仿真 home 的工具轴是斜的，反而更难上料）")
    ap.add_argument("--place-only", action="store_true",
                    help="只摆位，不碰夹爪（合爪后再「补一次位姿」用，见脚本末尾说明）")
    ap.add_argument("--check-ik", action="store_true",
                    help="自检：URDF→KDL 正解 vs TF（装不上规时先跑这个）")
    ap.add_argument("--dry-run", action="store_true", help="只算并打印目标位姿，不碰仿真")
    # ---- 竖直上料位姿（IK）相关 ----
    ap.add_argument("--no-pose", action="store_true",
                    help="不摆竖直上料位姿（自己先把臂摆好；默认会摆）")
    ap.add_argument("--base-frame", default="sim_base_link", help="IK 链的根链节（默认 sim_base_link）")
    ap.add_argument("--ee-frame", default="sim_flange", help="IK 链的末端链节（默认 sim_flange）")
    ap.add_argument("--joint-prefix", default="sim_", help="JTC 关节名前缀（默认 sim_，site.yaml 同源）")
    ap.add_argument("--action-name", default="follow_joint_trajectory",
                    help="JTC 动作名（arm_msgs 版；默认 follow_joint_trajectory，与"
                         " real_trajectory_adapter / Flexiv 驱动一致）")
    ap.add_argument("--load-velocity-scale", type=float, default=0.3,
                    help="走上料位姿的速度比例（默认 0.3，慢一点更安全）")
    ap.add_argument("--load-x", type=float, default=LOAD_MID_XY[0], help="上料位姿：工具轴落在 X [m]")
    ap.add_argument("--load-y", type=float, default=LOAD_MID_XY[1], help="上料位姿：工具轴落在 Y [m]")
    ap.add_argument("--load-z", type=float, default=LOAD_MID_Z_M,
                    help=f"上料位姿：两指 TCP 中点高度 [m]（默认 {LOAD_MID_Z_M}）")
    ap.add_argument("--load-yaw", type=float, default=0.0, help="上料位姿绕世界 Z 的偏航 [rad]")
    ap.add_argument("--load-approach-dz", type=float, default=0.18,
                    help="先到高位竖直再下降：高位比目标高多少 [m]（默认 0.18；0=一步到位）")
    ap.add_argument("--load-move-s", type=float, default=5.0, help="走到上料位姿用多少秒（默认 5）")
    args = ap.parse_args()
    return _ros_side(args)


if __name__ == "__main__":
    sys.exit(main())