# -*- 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)