Robot_Arm/arm/analysis/sweep_v2.py

# -*- coding: utf-8 -*-
"""全对全动态行程扫描与碰撞验证（权威版 v2，多进程并行）

与旧 travel_sweep.py / real_arm_collision_test.py 的区别（旧脚本漏掉了用户实测到的干涉）：
1. 直接在进程内构建当前模型（arm_model.build()），杜绝 STEP/网格过期；
2. 关节轴取自 Joint.rear/.d（human_layout 唯一事实源），与网页查看器逐位一致；
3. 检查【全部】零件对 —— 包括相邻关节的零件对与电机模组（旧脚本跳过相邻对，
   而 P04xP05 / P07xP08 恰恰在关节贴面处被卡死）；
4. 单关节双向精扫（粗扫 + 二分）得出真实硬限位；
5. 组合姿态扫描：每个关节置于其极限，再扫其余关节，收敛出滑块安全范围；
6. 写 docs/travel.json（查看器滑块范围）+ docs/travel_detail.json（逐对明细）。

网格化预筛（用户要求，2026-09）：
  - 每零件先按 0.5mm 容差网格化取顶点云（凸包顶点集）；
  - 候选对先跑 GJK（Minkowski 差 + 0.8mm 安全外扩）：只有"凸包外扩后仍分离"
    才允许跳过精确布尔 —— 该判据数学上保守（网格内缩 ≤0.5mm < 外扩 0.8mm），
    其余情况（可能干涉/数值退化）一律回落 OCCT 实体会交，结果与纯布尔完全一致；
  - --no-gjk 可关闭预筛（对比校验用）。

并行化（用户要求）：
  - mp.Pool 每个工作进程独立构建一次模型，角度任务批量分发（imap 保序）；
  - OCCT 布尔运算 SetRunParallel(True)；
  - 预剪枝：按关节轴计算每对零件的轴向/径向区间，区间不相交的零件对在该关节的
    任何角度都不可能碰撞，直接跳过（对单关节扫描是精确安全的）。

台架（P01 立柱 / P02 底盘）代表躯干与机身的连接界面，默认【参与】碰撞计算——臂撞立柱
就是臂撞躯干，是真约束。--no-stand 仅用于诊断：排除台架后重扫，给出"不含躯干约束"的
自由空间行程，用来分辨某个限位是臂自身折叠产生的还是躯干挡的（台架位置本身是假设，
改它等于改测试工装，不等于改设计）。
运行：python -m arm.analysis.sweep_v2 [--workers 20] [--step 2] [--combo-step 6]
                           [--no-combos] [--joints J2,J4] [--no-gjk] [--no-stand]
"""
import json
import math
import os
import sys
import time

import numpy as np
try:
    sys.stdout.reconfigure(encoding="utf-8")
except Exception:
    pass

import multiprocessing as mp
from arm.cad import arm_build as AB
import cadquery as cq
from arm import kinematics as HL
from arm.paths import DOCS
from arm.cad.helpers import place, bb
VOL_MIN = 2.0            # mm3，超过即计为干涉
CHAIN = ["root", "J1", "J2", "J3", "J4", "J5", "J6"]
MOUNT_PAIRS = {
    ("P03_肩吊臂_螺旋体35%_墙5底5_竖打", "M1_J1"), ("P04_肩叉_螺旋体30%_墙5底5_竖打", "M1_J1"),
    ("P04_肩叉_螺旋体30%_墙5底5_竖打", "M2_J2"), ("P05_大臂旋转座_螺旋体30%_墙4底5_竖打", "M2_J2"),
    ("P05_大臂旋转座_螺旋体30%_墙4底5_竖打", "M3_J3"), ("P06_大臂骨架_螺旋体25%_墙4底5_竖打", "M3_J3"),
    ("P06_大臂骨架_螺旋体25%_墙4底5_竖打", "M4_J4"), ("P07_小臂骨架_螺旋体30%_墙5底5_竖打", "M4_J4"),
    ("P07_小臂骨架_螺旋体30%_墙5底5_竖打", "M5_J5"), ("P08_腕偏转座_螺旋体25%_墙4底5_竖打", "M5_J5"),
    ("P08_腕偏转座_螺旋体25%_墙4底5_竖打", "M6_J6"), ("P09_工具法兰_螺旋体30%_墙4底5_竖打", "M6_J6"),
}

# 网格化预筛参数：外扩必须 ≥ 网格化偏差，判"分离"才保守可信
MESH_TOL = 0.5           # mm，零件网格化容差
MESH_MARGIN = 0.8        # mm，凸包安全外扩（> MESH_TOL 保证网格内缩不致漏判）
USE_GJK = True           # 进程内全局；工作进程由 initializer 传入
INCLUDE_FIXTURE = True   # True=把 P01 立柱/P02 底盘（代表躯干）算进碰撞；--no-stand 置 False


# ---------------------------------------------------------------- 网格化 + GJK
def tessellate_verts(shape):
    """零件 → 顶点云 (N,3)。凸包 GJK 的支撑函数直接在顶点云上取极值（等价于其凸包），
    第二步做精确凸包精简（scipy Qhull）：电机 STEP 网格 15 万顶点 → 约千余点。
    顶点均为真实表面上的点，亦用作"顶点侵入"快证的采样点。"""
    try:
        vs, _ts = shape.tessellate(MESH_TOL)
        v = np.asarray([(q.x, q.y, q.z) for q in vs], dtype=np.float64)
        if v.size == 0:
            return None
        v = np.unique(v, axis=0)
        try:
            from scipy.spatial import ConvexHull
            v = v[ConvexHull(v).vertices]
        except Exception:
            pass                                    # 精简失败则用全顶点云（仍正确，仅慢）
        return np.ascontiguousarray(v)
    except Exception:
        return None




def gjk_separated(va, vb, margin=MESH_MARGIN, max_iter=40):
    """凸包 GJK（带 margin）：va/vb 顶点云。返回 True 仅当
    conv(va)⊕Ball(margin) 与 conv(vb) 确证分离 —— 保守，可安全跳过精确布尔。
    可能干涉 / 数值退化 / 迭代超限 一律返回 False，回落 OCCT 精确实体求交。"""
    d = vb.mean(axis=0) - va.mean(axis=0)
    nd = np.linalg.norm(d)
    d = np.array([1.0, 0.0, 0.0]) if nd < 1e-9 else d / nd
    s = []                                  # 单纯形顶点，s[-1] 为最新
    for _ in range(max_iter):
        pa = va[np.argmax(va @ d)]
        pb = vb[np.argmax(vb @ (-d))]
        w = pa - pb + margin * d            # ⊕Ball(margin) 的支撑
        if w @ d < 0.0:                     # Minkowski 差不含原点 → 确证分离
            return True
        s.append(w)
        d, state = _do_simplex(s)
        if state != "go":
            return False                    # 'hit' 可能干涉 / 'stop' 退化 → 精确布尔
        nd = np.linalg.norm(d)
        if nd < 1e-9:
            return False                    # 原点贴着单纯形 → 交给精确布尔
        d = d / nd
    return False


def _do_simplex(s):
    """GJK 单纯形约化（Muratori 形式）。返回 (下一方向 d, state)：
    'go' 继续 / 'hit' 原点可能在内（→ 精确布尔）/ 'stop' 数值退化（→ 精确布尔）。"""
    n = len(s)
    if n == 1:
        return -s[0], "go"
    if n == 2:
        a, b = s[1], s[0]
        ab, ao = b - a, -a
        if ab @ ao > 0.0:
            abv = np.cross(np.cross(ab, ao), ab)
            return (None, "stop") if abv @ abv < 1e-18 else (abv, "go")
        del s[0]
        return -a, "go"
    if n == 3:
        a, b, c = s[2], s[1], s[0]
        ab, ac, ao = b - a, c - a, -a
        abv = np.cross(np.cross(ab, ao), ab)
        acv = np.cross(np.cross(ac, ao), ac)
        if abv @ ao > 0.0:                                   # AB 边外侧
            if ab @ ao > 0.0:
                del s[0]
                return (None, "stop") if abv @ abv < 1e-18 else (abv, "go")
            s[:] = [a]
            return -a, "go"
        if acv @ ao > 0.0:                                   # AC 边外侧
            if ac @ ao > 0.0:
                del s[1]
                return (None, "stop") if acv @ acv < 1e-18 else (acv, "go")
            s[:] = [a]
            return -a, "go"
        nrm = np.cross(ab, ac)
        if nrm @ nrm < 1e-18:
            return None, "stop"
        return (nrm if nrm @ ao > 0.0 else -nrm), "go"       # 三角面上/下方
    # n == 4 四面体：原点与被排除顶点异侧的面即"外侧"，约化到该面
    a3, b3, c3, d3 = s[3], s[2], s[1], s[0]
    for tri, e in (((a3, b3, c3), d3), ((a3, b3, d3), c3),
                   ((a3, c3, d3), b3), ((b3, c3, d3), a3)):
        u, v, w = tri
        nrm = np.cross(v - u, w - u)
        if nrm @ nrm < 1e-18:
            continue
        so = (-u) @ nrm
        sd = (e - u) @ nrm
        if abs(so) < 1e-12:
            return None, "hit"
        if so * sd < 0.0:
            s[:] = [u, v, w]
            return (nrm if so > 0.0 else -nrm), "go"
    return None, "hit"                                       # 原点在四面体内 → 可能干涉


# ---------------------------------------------------------------- 模型与上下文
def load_model(include_fixture=True):
    """include_fixture=False 时把 P01 立柱 / P02 底盘（固定测试台架，非本体）排除在
    碰撞配对之外——用来得到真机工作空间而不是台架工作空间。见 human_layout.FIXTURE_PREFIX。"""
    from arm import build_model as AM
    JS = AM.build()
    parts = []
    for p in AB.PARTS:
        if not include_fixture and HL.is_fixture(p["name"]):
            continue
        g = CHAIN.index(AB.GRP.get(p["name"], "root"))
        lo, hi = bb(p["shape"])
        corners = [(x, y, z) for x in (lo[0], hi[0]) for y in (lo[1], hi[1]) for z in (lo[2], hi[2])]
        # 包围球（局部坐标）：球心=包围盒中心，半径=半对角线。零件 ⊆ 包围球 恒成立，
        # 球心随刚体变换、半径不变 —— 旋转下依旧精确保守，是最廉价的确定性预筛。
        ctr = [(lo[k] + hi[k]) / 2.0 for k in range(3)]
        rad = 0.5 * math.dist(lo, hi) + 0.21
        verts = tessellate_verts(p["shape"])
        parts.append(dict(name=p["name"], shape=p["shape"], g=g, c8=corners, box=(lo, hi),
                          ctr=ctr, rad=rad, verts=verts))
    joints = {t: (JS[t].rear, JS[t].d) for t in CHAIN[1:]}
    return parts, joints


def _trsf_verts(verts, t):
    """顶点云按 gp_Trsf 刚体变换（3x4 矩阵）。"""
    m = np.array([[t.Value(r, c) for c in (1, 2, 3, 4)] for r in (1, 2, 3)])
    return verts @ m[:, :3].T + m[:, 3]


def axis_intervals(parts, rear, d):
    """每个零件沿关节轴的 (轴向区间, 径向最小/最大距离)。要求 d 为坐标轴方向。"""
    out = []
    for p in parts:
        lo, hi = p["box"]
        if abs(d[0]) > 0.5:
            ax = (lo[0] - rear[0], hi[0] - rear[0])
            u = (lo[1], hi[1], rear[1]); v = (lo[2], hi[2], rear[2])
        elif abs(d[1]) > 0.5:
            ax = (lo[1] - rear[1], hi[1] - rear[1])
            u = (lo[0], hi[0], rear[0]); v = (lo[2], hi[2], rear[2])
        else:
            ax = (lo[2] - rear[2], hi[2] - rear[2])
            u = (lo[0], hi[0], rear[0]); v = (lo[1], hi[1], rear[1])
        du = max(u[0] - u[2], 0.0, u[2] - u[1])
        dv = max(v[0] - v[2], 0.0, v[2] - v[1])
        rmin = math.hypot(du, dv)
        rmax = max(math.hypot(a - u[2], b - v[2]) for a in (u[0], u[1]) for b in (v[0], v[1]))
        out.append((ax, rmin, rmax))
    return out


def make_ctx(include_fixture=True):
    parts, joints = load_model(include_fixture)
    impossible = {}
    for k in range(1, len(CHAIN)):
        rear, d = joints[CHAIN[k]]
        iv = axis_intervals(parts, rear, d)
        s = set()
        for i in range(len(parts)):
            for j in range(i + 1, len(parts)):
                ga, gb = parts[i]["g"], parts[j]["g"]
                lo, hi = (ga, gb) if ga < gb else (gb, ga)
                if lo == hi or not (lo < k <= hi):
                    continue
                ax_i, r_i = iv[i][0], (iv[i][1], iv[i][2])
                ax_j, r_j = iv[j][0], (iv[j][1], iv[j][2])
                if ax_i[1] < ax_j[0] or ax_j[1] < ax_i[0] or \
                   r_i[1] < r_j[0] or r_j[1] < r_i[0]:
                    s.add((i, j))
        impossible[k] = s
    return dict(parts=parts, joints=joints, impossible=impossible)


# ---------------------------------------------------------------- 姿态测试
def accum_trsf(joints, angles_by_idx):
    """链式累积变换：acc[i] = acc[i-1] ∘ R_i（与查看器嵌套 pivot 完全一致）。"""
    acc = [None] * len(CHAIN)
    for i in range(1, len(CHAIN)):
        ang = angles_by_idx.get(i, 0.0)
        if abs(ang) < 1e-9:
            acc[i] = acc[i - 1]
            continue
        rear, d = joints[CHAIN[i]]
        r = AB.R(ang, d[0], d[1], d[2], rear[0], rear[1], rear[2])
        acc[i] = r if acc[i - 1] is None else acc[i - 1].Multiplied(r)
    return acc


def xf_c8(c8, t):
    from OCP.gp import gp_Pnt
    gpt = [gp_Pnt(*c).Transformed(t) for c in c8]
    lo = [min(p.X() for p in gpt), min(p.Y() for p in gpt), min(p.Z() for p in gpt)]
    hi = [max(p.X() for p in gpt), max(p.Y() for p in gpt), max(p.Z() for p in gpt)]
    return (lo, hi)


def common_vol_center(a, b):
    """交集体积 + 干涉质心；布尔失败返回 (-1, None)。"""
    try:
        from OCP.BRepAlgoAPI import BRepAlgoAPI_Common
        c = BRepAlgoAPI_Common(a.wrapped, b.wrapped)
        c.SetRunParallel(True)
        c.Build()
        if not c.IsDone() or c.Shape().IsNull():
            return 0.0, None
        sh = cq.Shape(c.Shape())
        v = AB.vol_of(sh)
        if v <= VOL_MIN:
            return v, None
        (lo, hi) = bb(sh)
        return v, tuple(round((lo[i] + hi[i]) / 2.0, 1) for i in range(3))
    except Exception:
        return -1.0, None


def test_posture(ctx, angles_by_idx, want_errors=False, first_only=True):
    """返回 (violations, errors)：violations = [(na, nb, vol, 干涉质心)]。
    first_only：证实第一对干涉即返回（粗扫/二分只需要"卡没卡"的二元答案，卡死姿态
    通常 12+ 对干涉、每对布尔 0.5~12s，全算完要 40s+；命中即返回降到 1~2s）。
    零位诊断（want_errors=True）保持全对扫描以给出完整清单。"""
    parts, joints, impossible = ctx["parts"], ctx["joints"], ctx["impossible"]
    active = [k for k, a in angles_by_idx.items() if abs(a) > 1e-9]
    acc = accum_trsf(joints, angles_by_idx)
    boxes = []
    sph = []
    for p in parts:
        t = acc[p["g"]]
        boxes.append(p["box"] if t is None else xf_c8(p["c8"], t))
        if t is None:
            c = p["ctr"]
        else:                                   # 球心随组变换，半径旋转不变
            c = _trsf_verts(np.array([p["ctr"]], dtype=np.float64), t)[0]
        sph.append((c[0], c[1], c[2], p["rad"]))
    shapes = {}
    wverts = {}                             # 顶点云世界坐标缓存（本姿态内复用）
    viol, errs = [], []
    n = len(parts)
    for i in range(n):
        gi = parts[i]["g"]
        for j in range(i + 1, n):
            gj = parts[j]["g"]
            lo, hi = (gi, gj) if gi < gj else (gj, gi)
            if lo == hi:
                continue
            na, nb = parts[i]["name"], parts[j]["name"]
            if na.startswith("M") and nb.startswith("M"):
                continue
            if (na, nb) in MOUNT_PAIRS or (nb, na) in MOUNT_PAIRS:
                continue
            span = [k for k in active if lo < k <= hi]
            # 关键：无活动关节（零位）时必须测试全部零件对 —— 相对位姿不变不代表不干涉！
            if active and not span:
                continue
            if len(span) == 1 and (i, j) in impossible[span[0]]:
                continue
            bi, bj = boxes[i], boxes[j]
            if any(bi[1][k] < bj[0][k] - 0.1 or bj[1][k] < bi[0][k] - 0.1 for k in range(3)):
                continue
            sa, sb = sph[i], sph[j]          # 包围球预筛：零件 ⊆ 球，球分离 ⇒ 零件必分离
            dxx = sa[0] - sb[0]; dyy = sa[1] - sb[1]; dzz = sa[2] - sb[2]
            rr = sa[3] + sb[3] + 0.1
            if dxx * dxx + dyy * dyy + dzz * dzz > rr * rr:
                continue
            if USE_GJK:                     # 网格化预筛：确证分离则免精确布尔
                for key, gk in ((i, gi), (j, gj)):
                    if key not in wverts:
                        t = acc[gk]
                        lv = parts[key]["verts"]
                        wverts[key] = None if lv is None else (
                            lv if t is None else _trsf_verts(lv, t))
                vi, vj = wverts[i], wverts[j]
                if vi is not None and vj is not None and gjk_separated(vi, vj):
                    continue
            si = shapes.get(i)
            if si is None:
                t = acc[gi]
                si = parts[i]["shape"] if t is None else place(parts[i]["shape"], t)
                shapes[i] = si
            sj = shapes.get(j)
            if sj is None:
                t = acc[gj]
                sj = parts[j]["shape"] if t is None else place(parts[j]["shape"], t)
                shapes[j] = sj
            # 顶点侵入快证：一方的网格顶点（真实表面上的点）落入对方实体内部即实锤干涉，
            # 免掉 4~12s 的 NURBS 布尔。深入重叠时前几十个采样点即命中早退；分类器按
            # 零件缓存（每姿态构建一次）。判"自由"无证明力 —— 无命中则照旧精确布尔。
            jam = False
            try:
                from OCP.BRepClass3d import BRepClass3d_SolidClassifier
                from OCP.gp import gp_Pnt
                from OCP.TopAbs import TopAbs_State
                for pi_, pj_ in ((i, j), (j, i)):
                    pts = wverts.get(pi_)
                    if pts is None or not len(pts):
                        continue
                    ckey = (pj_, id(acc[parts[pj_]["g"]]))
                    if ckey not in classifiers:
                        classifiers[ckey] = BRepClass3d_SolidClassifier(shapes[pj_].wrapped)
                    cls = classifiers[ckey]
                    stepk = max(1, len(pts) // 240)
                    for k in range(0, len(pts), stepk):
                        p = pts[k]
                        # Perform(pnt, tol=1.0)：距边界 <1mm 的点归为 ON（配合贴面不算），
                        # 只有深入 >1mm 的顶点才实锤干涉 —— 与 VOL_MIN 语义对齐
                        cls.Perform(gp_Pnt(p[0], p[1], p[2]), 1.0)
                        if cls.State() == TopAbs_State.TopAbs_IN:
                            jam = True
                            break
                    if jam:
                        break
            except Exception:
                jam = False
            if jam:
                viol.append((parts[i]["name"], parts[j]["name"], -1.0, None))
                if first_only and not want_errors:
                    return viol
                continue
            v, center = common_vol_center(si, sj)
            if v < 0:
                errs.append((parts[i]["name"], parts[j]["name"]))
                continue
            if v > VOL_MIN:
                viol.append((parts[i]["name"], parts[j]["name"], round(v, 1), center))
                if first_only and not want_errors:
                    return viol
    return (viol, errs) if want_errors else viol


# ---------------------------------------------------------------- 工作进程
_CTX = None
_POOL = None
_WORKERS = 4


def _init_worker(use_gjk=True, include_fixture=True):
    global _CTX, USE_GJK
    USE_GJK = bool(use_gjk)
    _CTX = make_ctx(include_fixture)


def _task(task):
    angles = {int(k): v for k, v in task["a"].items()}
    return task["id"], test_posture(_CTX, angles)   # 带id回传，配合 imap_unordered 实时进度


def get_pool(workers):
    global _POOL, _WORKERS
    _WORKERS = workers
    if _POOL is None and workers > 1:
        _POOL = mp.Pool(workers, initializer=_init_worker, initargs=(USE_GJK, INCLUDE_FIXTURE))
    return _POOL


def eval_angles(angles_list, workers, chunk=None, label="姿态"):
    """批量评估姿态列表 [{idx: ang}]，按输入顺序返回 violations 列表。
    进度展示：imap_unordered 按完成序实时回报，≥4 个姿态的批次每 2s 滚动打印一次
    真实完成数（短批次与二分不打扰输出）。"""
    tasks = [dict(id=i, a={str(k): v for k, v in a.items()}) for i, a in enumerate(angles_list)]
    n = len(tasks)
    if workers <= 1:
        ctx = _ctx_single()
        out = []
        t0 = time.time()
        for i, a in enumerate(angles_list):
            out.append(test_posture(ctx, a))
            if n >= 4 and ((i + 1) % max(1, n // 10) == 0 or i + 1 == n):
                print("      %s %d/%d [%.0fs]" % (label, i + 1, n, time.time() - t0),
                      end="\r" if i + 1 < n else "\n", flush=True)
        return out
    pool = get_pool(workers)
    out = [None] * n
    cs = chunk or max(1, n // (workers * 4) or 1)
    t0, t_last, done_n = time.time(), time.time(), 0
    for id_, viol in pool.imap_unordered(_task, tasks, chunksize=cs):
        out[id_] = viol
        done_n += 1
        now = time.time()
        if n >= 4 and (now - t_last >= 2.0 or done_n == n):
            print("      %s %d/%d [%.0fs]" % (label, done_n, n, now - t0),
                  end="\r" if done_n < n else "\n", flush=True)
            t_last = now
    return out


def _ctx_single():
    global _CTX
    if _CTX is None:
        _CTX = make_ctx(INCLUDE_FIXTURE)
    return _CTX


# ---------------------------------------------------------------- 扫描逻辑
def bisect(workers, k, a_free, a_jam, tol=0.2):
    n = 0
    while abs(a_jam - a_free) > tol:
        n += 1
        mid = 0.5 * (a_free + a_jam)
        print("      %s 二分#%d 区间 [%.2f, %.2f]°" % (CHAIN[k], n, a_free, a_jam), flush=True)
        if eval_angles([{k: mid}], workers, label="%s二分" % CHAIN[k])[0]:
            a_jam = mid
        else:
            a_free = mid
    return a_free


def sweep_single(k, step, workers):
    t_start = time.time()
    jams = {}
    lo = hi = 0.0
    tag = CHAIN[k]
    for sign in (+1, -1):
        angles = [sign * step * i for i in range(1, int(180.0 / step) + 1)]
        stop, a_free = None, 0.0
        B = max(workers, 8)          # 小批次：首个卡死角即停，少算深度卡死姿态
        for s in range(0, len(angles), B):
            batch = angles[s:s + B]
            results = eval_angles([{k: a} for a in batch], workers,
                                  label="%s%s向粗扫" % (tag, "正" if sign > 0 else "反"))
            done = min(s + B, len(angles))
            print("    %s %s向 %d/%d 角度（%2.0f%%）[已用 %.0fs]"
                  % (tag, "正" if sign > 0 else "反", done, len(angles),
                     100.0 * done / len(angles), time.time() - t_start), flush=True)
            for a, viol in zip(batch, results):
                if viol:
                    jams[a] = viol
                    print("    %s %s向在 %.1f° 首次干涉：%s"
                          % (tag, "正" if sign > 0 else "反", a,
                             "; ".join("%sx%s(%s)" % (p, q, "顶点侵入" if v < 0 else "%.0fmm3" % v)
                                       for p, q, v, _c in viol[:3])),
                          flush=True)
                    # 二分区间 = [本方向最后自由角, 首个卡死角] —— 修复原 sign*(a-step)
                    # 在负方向算出未测过的另一侧角度、把二分踢进远端慢区的问题
                    stop = bisect(workers, k, a_free, a)
                    break
                a_free = a
            if stop is not None:
                break
        if stop is None:
            stop = a_free
        if sign > 0:
            hi = stop
        else:
            lo = stop
    print("  => %s 硬限位 [%7.2f, %7.2f]，耗时 %.0fs" % (tag, lo, hi, time.time() - t_start),
          flush=True)
    return dict(lo=lo, hi=hi, jams=jams)


def sweep_combo(k_hold, e_hold, j_sweep, a_lo, a_hi, step, workers):
    jams = {}
    res = {}
    tag = "%s@%.0f° 扫%s" % (CHAIN[k_hold], e_hold, CHAIN[j_sweep])
    for sign, lim in ((+1, a_hi), (-1, a_lo)):
        angles = [sign * step * i for i in range(1, int(abs(lim) / step) + 1)] if abs(lim) > 1e-6 else []
        stop, a_free = None, 0.0
        B = max(workers, 8)          # 小批次：首个卡死角即停，少算深度卡死姿态
        for s in range(0, len(angles), B):
            batch = angles[s:s + B]
            results = eval_angles([{k_hold: e_hold, j_sweep: a} for a in batch], workers,
                                  label=tag)
            done = min(s + B, len(angles))
            print("      %s %s向 %d/%d（%2.0f%%）"
                  % (tag, "正" if sign > 0 else "反", done, len(angles),
                     100.0 * done / max(len(angles), 1)), flush=True)
            for a, viol in zip(batch, results):
                if viol:
                    jams[a] = viol
                    lo_f, hi_j = a_free, a           # 同 sweep_single：用本方向最后自由角
                    while abs(hi_j - lo_f) > 0.5:
                        mid = 0.5 * (lo_f + hi_j)
                        if eval_angles([{k_hold: e_hold, j_sweep: mid}], workers,
                                       label="%s组合二分" % tag)[0]:
                            hi_j = mid
                        else:
                            lo_f = mid
                    stop = lo_f
                    break
                a_free = a
            if stop is not None:
                break
        if stop is None:
            stop = a_free
        res[sign] = stop
    return res[+1], res[-1], jams


def main():
    t0 = time.time()
    step, combo_step = 2.0, 6.0
    do_combos = "--no-combos" not in sys.argv
    want_joints = None
    if "--no-gjk" in sys.argv:
        global USE_GJK
        USE_GJK = False
    if "--no-stand" in sys.argv:
        global INCLUDE_FIXTURE
        INCLUDE_FIXTURE = False
    # 内存感知的默认进程数：每个工作进程预留 ~2.5GB 峰值（建模+布尔瞬时开销）
    avail_gb = 16.0
    try:
        import ctypes

        class _MS(ctypes.Structure):
            _fields_ = [("dwLength", ctypes.c_uint32), ("dwMemoryLoad", ctypes.c_uint32),
                        ("ullTotalPhys", ctypes.c_uint64), ("ullAvailPhys", ctypes.c_uint64),
                        ("ullTotalPageFile", ctypes.c_uint64), ("ullAvailPageFile", ctypes.c_uint64),
                        ("ullTotalVirtual", ctypes.c_uint64), ("ullAvailVirtual", ctypes.c_uint64),
                        ("ullAvailExtendedVirtual", ctypes.c_uint64)]
        _ms = _MS()
        _ms.dwLength = ctypes.sizeof(_MS)
        ctypes.windll.kernel32.GlobalMemoryStatusEx(ctypes.byref(_ms))
        avail_gb = _ms.ullAvailPhys / 2**30
    except Exception:
        pass
    mem_cap = max(2, int(avail_gb / 2.5))
    workers = max(1, min(12, (os.cpu_count() or 4) - 2, mem_cap))
    for i, a in enumerate(sys.argv):
        if a == "--step":
            step = float(sys.argv[i + 1])
        if a == "--combo-step":
            combo_step = float(sys.argv[i + 1])
        if a == "--workers":
            workers = int(sys.argv[i + 1])
        if a == "--joints":
            want_joints = set(sys.argv[i + 1].split(","))

    print("=" * 78)
    print("全对全动态行程扫描 v2 并行版（workers=%d / 可用内存 %.0fGB / 轴=human_layout / 含相邻对与电机 / GJK预筛=%s）"
          % (workers, avail_gb, "开" if USE_GJK else "关"))
    print("=" * 78, flush=True)
    print("启动 %d 个工作进程并行建模（每进程独立构建全模型+凸包网格，约 20~60s）..." % workers,
          flush=True)
    if workers > 1:
        get_pool(workers)            # 启动工作进程（各自并行建模，父进程不重复建模）
    # 零位检查
    v0 = eval_angles([{}], workers)[0]
    print("[零位检查] %s" % ("0 对干涉 ✓" if not v0 else "干涉: %s" % v0), flush=True)

    singles = {}
    print("\n[单关节硬限位] 粗扫 %.1f° + 二分 0.2°" % step, flush=True)
    for k in range(1, len(CHAIN)):
        tag = CHAIN[k]
        if want_joints and tag not in want_joints:
            continue
        r = sweep_single(k, step, workers)
        singles[tag] = r
        jam_s = "; ".join(
            "%s%.1f°:%s" % ("+" if a > 0 else "", a,
                            ",".join("%sx%s@%s(%s)" % (p, q, c,
                                                       "侵入" if v < 0 else "%.0f" % v)
                                     for p, q, v, c in vs[:2]))
            for a, vs in sorted(r["jams"].items())[:4])
        print("  %-3s  lo=%7.2f  hi=%7.2f   %s  [%.0fs]" % (tag, r["lo"], r["hi"],
              ("卡死: " + jam_s) if jam_s else "整周无干涉 → 360° 连续", time.time() - t0), flush=True)

    if want_joints:
        json.dump(dict(generated=time.strftime("%Y-%m-%d %H:%M:%S"),
                       singles={t: dict(lo=round(singles[t]["lo"], 2),
                                        hi=round(singles[t]["hi"], 2),
                                        jams={str(a): v for a, v in singles[t]["jams"].items()})
                                for t in singles}),
                  open(DOCS + "/travel_probe.json", "w", encoding="utf-8"),
                  ensure_ascii=False, indent=1)
        print("\n[诊断模式] 明细已写出 docs/travel_probe.json  [%.0fs]" % (time.time() - t0))
        return

    limits = {t: [singles[t]["lo"], singles[t]["hi"]] for t in CHAIN[1:]}

    combos_log = []
    if do_combos:
        print("\n[组合姿态收敛] 保持一个关节于极限，扫其余关节（步长 %.1f°）" % combo_step, flush=True)
        for pass_i in range(2):
            changed = False
            for k in range(1, len(CHAIN)):
                tag_k = CHAIN[k]
                for e in (limits[tag_k][0], limits[tag_k][1]):
                    if abs(e) < 5.0:
                        continue
                    for j in range(1, len(CHAIN)):
                        if j == k:
                            continue
                        tag_j = CHAIN[j]
                        lo_j, hi_j = limits[tag_j]
                        if abs(lo_j) < 5.0 and abs(hi_j) < 5.0:
                            continue
                        pos, neg, jams = sweep_combo(k, e, j, lo_j, hi_j, combo_step, workers)
                        for sign, free, lim in ((+1, pos, hi_j), (-1, neg, lo_j)):
                            new = free - 1.0
                            if sign > 0 and new < limits[tag_j][1] - 0.01:
                                combos_log.append(dict(pass_=pass_i, hold=tag_k, hold_at=e,
                                                       swept=tag_j, direction="hi",
                                                       old=limits[tag_j][1], new=max(new, 0.0),
                                                       jams={str(a): v for a, v in jams.items()}))
                                limits[tag_j][1] = max(new, 0.0)
                                changed = True
                            if sign < 0 and new > limits[tag_j][0] + 0.01:
                                combos_log.append(dict(pass_=pass_i, hold=tag_k, hold_at=e,
                                                       swept=tag_j, direction="lo",
                                                       old=limits[tag_j][0], new=min(new, 0.0),
                                                       jams={str(a): v for a, v in jams.items()}))
                                limits[tag_j][0] = min(new, 0.0)
                                changed = True
                print("  hold %s done [%.0fs]" % (tag_k, time.time() - t0), flush=True)
            print("  pass %d: %s" % (pass_i + 1,
                  "收紧 " + str({t: [round(limits[t][0], 1), round(limits[t][1], 1)] for t in CHAIN[1:]}
                                if changed else "无变化")), flush=True)
            if not changed:
                break

    # 滑块范围 = 单关节验证硬限位（任意单关节推满保证零干涉）。
    # 组合折叠约束（某关节保持极限时扫其它关节发现的卡死）只写入明细/报告，
    # 不压缩滑块 —— 否则手臂会被静态限位锁死（真实机器人由控制器做避碰）。
    final = {}
    print("\n[最终滑块范围]（单关节硬限位，含 1° 安全余量）")
    for t in CHAIN[1:]:
        lo, hi = singles[t]["lo"], singles[t]["hi"]
        if lo <= -179.9 and hi >= 179.9:
            final[t] = dict(axis=t, lo=-180.0, hi=180.0, continuous=True, zero=0.0,
                            note="整周无干涉，可 360° 连续旋转（全对全动态扫描验证）")
        else:
            lo_out = float(math.ceil(lo + 1.0)) if lo < -0.5 else 0.0
            hi_out = float(math.floor(hi - 1.0)) if hi > 0.5 else 0.0
            final[t] = dict(axis=t, lo=lo_out, hi=hi_out, continuous=False, zero=0.0,
                            note="正向 +%d° / 反向 %d°，总行程 %d°（全对全动态扫描验证，含相邻件与电机）"
                                 % (hi_out, lo_out, hi_out - lo_out))
        print("  %-3s  %7.1f .. %7.1f   %s" % (t, final[t]["lo"], final[t]["hi"],
              "360°连续" if final[t]["continuous"] else ""))

    os.makedirs(DOCS, exist_ok=True)
    json.dump([final[t] for t in CHAIN[1:]],
              open(DOCS + "/travel.json", "w", encoding="utf-8"), ensure_ascii=False, indent=1)
    detail = dict(generated=time.strftime("%Y-%m-%d %H:%M:%S"),
                  vol_min=VOL_MIN, step=step, combo_step=combo_step, workers=workers,
                  fixture_included=INCLUDE_FIXTURE,
                  singles={t: dict(lo=round(singles[t]["lo"], 2), hi=round(singles[t]["hi"], 2),
                                   jams={str(a): v for a, v in singles[t]["jams"].items()})
                           for t in singles},
                  combos=combos_log,
                  final=[final[t] for t in CHAIN[1:]])
    json.dump(detail, open(DOCS + "/travel_detail.json", "w", encoding="utf-8"),
              ensure_ascii=False, indent=1)
    print("\n已写出 docs/travel.json（查看器滑块）+ docs/travel_detail.json（明细）  [%.0fs]"
          % (time.time() - t0))
    if _POOL is not None:
        _POOL.terminate()


if __name__ == "__main__":
    main()

    # OCC/cadquery 的静态析构在解释器正常退出时必定段错误（实测：光是
    # `python -c "import arm.cad.helpers"` 就返回 139，与本项目代码无关）。所以入口
    # 脚本跑完后显式刷缓冲再 os._exit —— 否则全流程已经成功、exit code 却是 139,
    # 会把上游脚本的 set -e 判成失败。只在 __main__ 出口生效, 被 import 时不触发。
    sys.stdout.flush()
    os._exit(0)