cbb
649bd69810
单文件约束取消后，Python 侧没必要再靠 16 处 sys.path.insert 维持扁平模块互认。
现在按职责分包：
arm/paths.py 路径唯一来源（含新增 CACHE、TPL）
arm/constants.py 实测接口数值（原 armcore 的 M/C/CLR/FIT/MOTOR_TOL 段）
arm/cad/{helpers,arm_build,part_tools,parts/} 几何与装配
arm/kinematics.py（原 human_layout.py，唯一运动学事实源）
arm/analysis/{sweep_v2,strength_check,torque_balance,finalize_travel,ws_sweep,gen_env_mesh}
arm/dataio/{screw_mesh,draw_sheet} arm/viewer/{build_viewer,shot}
arm/build_model.py（原 arm_model.py）
顺带修掉的既有缺陷（都是实测撞出来的，不是预防性改动）：
- arm_model 第二次 sys.path 跳到 gitignore 的 scratch/ 去 import 构建必需的包络脚本：
ws_sweep/gen_env_mesh 提升进 arm/analysis/，缓存挪到 data/cache/（gitignore，可重生成）。
- finalize_travel / draw_sheet 顶层就是"算完写文件"，一次 importlib 冒烟测试就把
docs/travel.json 重写了一遍；现在被 import 直接抛错。
- strength_check 用 OUT.replace("/out","/web") 靠字符串形状推路径，改成直接用 WEB。
- build_viewer 里 WORKSPACE 使用点漏改导致 NameError —— 已由构建关卡抓到并修掉。
os._exit 的处置与计划里的假设相反：实测光是 `python -c "import arm.cad.helpers"`
就会在解释器退出时段错误（exit 139，OCC 静态析构），全流程跑完也一样。所以入口
脚本尾部保留 flush + os._exit，并把原因写进注释；被 import 时不生效。
关卡（tests/）：
- verify.py 一条命令跑三层：产物 md5、IK 数值 exact 段、截图。
- 截图基线重做并分级：同一构建连跑两轮实测 102 张逐字节可复现（要求 md5 相同），
3 张线束 x-ray 不可复现（mean<=0.0017、无像素差>8）改为与参考像素比容差；
这 3 张不再入库，免得每次跑关卡都脏 git。
- 新增 ?view=sheet / ?sheet=N / ?theme / ?guide=N 四个 URL 参数：工程图与装配向导
占了约一半 CSS 和 700 行 JS，原先没有任何截图覆盖，拆它们等于没有关卡。
- 关卡灵敏度已验：改坏 IK 基线一个数 -> FAIL；换旧版求解器 -> 5 处 exact 差异。
实测：python -m arm.build_model 退出码 0，全部产物 md5 与基线一致，
105 视图截图全过，IK exact 段逐字节一致。
727 lines
34 KiB
Python
727 lines
34 KiB
Python
# -*- 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)
|