ros_contorl/scripts/lib/real_geometry_check.py

#!/usr/bin/env python3
"""开工前的**几何自检**：手眼标定矩阵 + 悬停高度会不会把工具捅进工件。

这两件事都属于"配置/文件看着没问题，但一跑就把工件推走"的那一类，而且
**征兆出现在别的环节**（S05 残差恒不收敛、看着像检测坏了）。所以放在开工前
用几何查一遍，比在流程里翻日志便宜得多。

1) 手眼矩阵自洽：``handeye/results/<SN>_eye_in_hand.yaml`` 的 ``T_4x4``
   必须是刚体变换 —— R 正交、det=+1、平移量级合理（几厘米到几十厘米）。
   标定中断/写坏了半个文件时，矩阵会是"差不多但不正交"的，用它算出来的位姿
   会整体偏几毫米到几厘米，而**不会**报任何错。

2) 悬停高度 vs 工具长：判据不在这里重写 —— 直接调用 ``hover_z_guard``
   （``arm_application.gauge_fsm``），那是流程里唯一的判定处。
   2026-09-24 的教训：悬停高按**相机**视距定，而工具比相机长 ⇒ 悬停时规尖
   已经钻进工件 25mm，S05 横向对准把工件拖走 92mm。这里提前喊出来。
"""

from __future__ import annotations

import argparse
import json
import math
import sys
from pathlib import Path
from typing import Any, Dict, List, Optional, Sequence

ROOT = Path(__file__).resolve().parents[2]
SCRIPTS_LIB = ROOT / "scripts" / "lib"

# 平移量级合理区间 [m]：腕部相机 eye-in-hand 的平移是"法兰到相机"的距离。
T_MIN_M = 0.01
T_MAX_M = 0.50
# 正交性容差：R^T R 与 I 的每个元素偏差上限
ORTHO_TOL = 2e-3


def _matmul3(a: Sequence[Sequence[float]], b: Sequence[Sequence[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 check_transform(T: Any) -> List[str]:
    """检查 4x4 齐次矩阵是不是一个像样的刚体变换。返回问题列表（空=没问题）。"""
    problems: List[str] = []
    try:
        rows = [list(map(float, r)) for r in T]
    except (TypeError, ValueError) as exc:
        return [f"T_4x4 里有非数字：{exc}"]
    if len(rows) != 4 or any(len(r) != 4 for r in rows):
        return [f"T_4x4 必须是 4x4，实际 {len(rows)}x{len(rows[0]) if rows else 0}"]
    if any(abs(rows[3][i] - v) > 1e-9 for i, v in enumerate((0.0, 0.0, 0.0, 1.0))):
        problems.append(f"最后一行应该是 [0,0,0,1]，实际 {rows[3]}")

    R = [r[:3] for r in rows[:3]]
    # R^T R 应该等于 I
    Rt = [[R[j][i] for j in range(3)] for i in range(3)]
    RtR = _matmul3(Rt, R)
    worst = max(
        abs(RtR[i][j] - (1.0 if i == j else 0.0)) for i in range(3) for j in range(3)
    )
    if worst > ORTHO_TOL:
        problems.append(
            f"旋转部分不正交（|RᵀR−I| 最大 {worst:.2e} > {ORTHO_TOL:.0e}）"
            "—— 标定文件写坏了或标定没收敛；这种矩阵不会报错，只会让位姿整体偏"
        )
    det = (
        R[0][0] * (R[1][1] * R[2][2] - R[1][2] * R[2][1])
        - R[0][1] * (R[1][0] * R[2][2] - R[1][2] * R[2][0])
        + R[0][2] * (R[1][0] * R[2][1] - R[1][1] * R[2][0])
    )
    if abs(det - 1.0) > 1e-3:
        problems.append(f"det(R)={det:.4f}，应该是 +1（−1 表示镜像/坐标系搞反了）")

    t = [rows[i][3] for i in range(3)]
    norm = math.sqrt(sum(v * v for v in t))
    if not (T_MIN_M <= norm <= T_MAX_M):
        problems.append(
            f"平移量级 {norm * 1000:.1f}mm 不在合理区间 "
            f"[{T_MIN_M * 1000:.0f}, {T_MAX_M * 1000:.0f}]mm —— "
            "要么单位不是米，要么标定结果跑飞了"
        )
    return problems


def load_T(path: Path) -> Any:
    import yaml

    doc = yaml.safe_load(path.read_text(encoding="utf-8")) or {}
    if "T_4x4" not in doc:
        raise KeyError(f"{path} 里没有 T_4x4（键：{sorted(doc)}）")
    return doc["T_4x4"]


def geometry_report(site_path: Path, sn: str) -> Dict[str, Any]:
    """跑一遍两项几何检查，返回结构化结论。"""
    sys.path.insert(0, str(SCRIPTS_LIB))
    import export_site_env as ex

    sys.path.insert(0, str(ROOT / "install" / "arm_application" / "lib" / "python3.11" / "site-packages"))
    from arm_application.gauge_config import from_site
    from arm_application.gauge_fsm import hover_z_guard

    data = ex.load_site(site_path)
    merged, _robot = ex.apply_active_arm(data, sn)
    cfg = from_site(merged)

    out: Dict[str, Any] = {"handeye": {}, "hover": {}}

    he_file = Path(str((merged.get("paths") or {}).get("handeye_file") or ""))
    out["handeye"]["file"] = str(he_file)
    if not he_file.is_file():
        out["handeye"]["problems"] = [f"手眼文件不存在：{he_file}"]
    else:
        try:
            T = load_T(he_file)
            out["handeye"]["translation_m"] = [T[i][3] for i in range(3)]
            out["handeye"]["problems"] = check_transform(T)
        except Exception as exc:  # noqa: BLE001
            out["handeye"]["problems"] = [f"读手眼文件失败：{exc}"]

    servo = merged.get("servo") or {}
    stage2 = merged.get("stage2") or {}
    hover = servo.get("hole_hover_z_m")
    hover = float(hover) if hover is not None else float(stage2.get("hover_z_m") or 0.30)
    gap = float(stage2.get("final_gap_mm") or 1.5) / 1000.0
    out["hover"] = {
        "hole_hover_z_m": hover,
        "tool_tip_below_flange_m": cfg.tool_tip_below_flange_m,
        "workpiece_surface_z_m": cfg.workpiece_surface_z_m,
        "final_gap_m": gap,
        "suggested_z_m": hover,
        "problems": [],
        "uncalibrated_surface": cfg.workpiece_surface_z_m <= 0.0,
    }
    if out["hover"]["uncalibrated_surface"]:
        # site.yaml 用 0.0 表示"还不知道"（见 gauge_config 的默认值注释）。这时
        # hover_z_guard 拿不到真正的工件面，算出来的结论是**假的** —— 与其报一个
        # 假的 WARN，不如直接说"这一项现在判不了，得先标定"。
        out["hover"]["problems"] = [
            "工件上表面 z 没标定（site.yaml 写 0.0 = 未知）—— 悬停高度**没有**可靠的"
            "几何依据。判据要把「工具长」和「工件面」比，缺一个就算不出来；"
            "而 S05 的横向对准正好是在悬停高度上做的：悬停时工具尖端低于工件面就会把"
            "工件推走（2026-09-24 实测推走 92mm，之后残差恒 13mm 永不收敛，"
            "看着像检测坏了）。"
            "修法：跑一次二阶段定位（bash scripts/run_stage2_locate.sh --arm 89），"
            "它会把实测的工件面 z 打出来，把这个值填回 "
            "robots[<sn>].gauge_assembly.workpiece_surface_z_m，再重启流程。"
        ]
    else:
        _fixed, why = hover_z_guard(
            hover, cfg.workpiece_surface_z_m, cfg.tool_tip_below_flange_m, gap
        )
        out["hover"]["suggested_z_m"] = _fixed
        out["hover"]["problems"] = [why] if why else []
    return out


def main(argv: Optional[Sequence[str]] = None) -> int:
    ap = argparse.ArgumentParser(description="开工前几何自检（手眼 + 悬停高度）")
    ap.add_argument("--sn", default="")
    ap.add_argument("--site", default=str(ROOT / "config" / "site.yaml"))
    ap.add_argument("--json", action="store_true")
    args = ap.parse_args(argv)

    rep = geometry_report(Path(args.site), args.sn)
    if args.json:
        print(json.dumps(rep, ensure_ascii=False, indent=2))
    problems: List[str] = []
    he = rep["handeye"]
    print(f"  手眼：{he.get('file')}")
    if he.get("translation_m"):
        t = he["translation_m"]
        print(f"    平移 [{t[0]:+.4f}, {t[1]:+.4f}, {t[2]:+.4f}] m")
    for p in he.get("problems") or []:
        print(f"    [FAIL] {p}")
        problems.append(p)
    if not (he.get("problems") or []):
        print("    [OK  ] 矩阵自洽（正交、det=+1、量级合理）")

    hv = rep["hover"]
    print(
        f"  悬停几何：法兰 {hv['hole_hover_z_m']:.4f} − 工具长 "
        f"{hv['tool_tip_below_flange_m'] * 1000:.1f}mm → 尖端 "
        f"{hv['hole_hover_z_m'] - hv['tool_tip_below_flange_m']:.4f}，"
        f"工件面 {hv['workpiece_surface_z_m']:.4f}"
        + ("（未标定）" if hv.get("uncalibrated_surface") else "")
    )
    for p in hv.get("problems") or []:
        print(f"    [FAIL] {p}")
        problems.append(p)
    if not (hv.get("problems") or []):
        print(
            f"    [OK  ] 悬停时工具尖端在工件面上方（几何修正值 "
            f"{hv['suggested_z_m']:.4f}）"
        )
    return 1 if problems else 0


if __name__ == "__main__":
    raise SystemExit(main())