feat: 后置高刚性防倾翻测试台架与J1电机镂空散热防拉脱托架整合 (P01/P02/P03重构)
This commit is contained in:
+188
-11
@@ -10,6 +10,13 @@
|
||||
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)；
|
||||
@@ -17,7 +24,7 @@
|
||||
任何角度都不可能碰撞，直接跳过（对单关节扫描是精确安全的）。
|
||||
|
||||
运行：python src/sweep_v2.py [--workers 20] [--step 2] [--combo-step 6]
|
||||
[--no-combos] [--joints J2,J4]
|
||||
[--no-combos] [--joints J2,J4] [--no-gjk]
|
||||
"""
|
||||
import json
|
||||
import math
|
||||
@@ -25,6 +32,8 @@ import os
|
||||
import sys
|
||||
import time
|
||||
|
||||
import numpy as np
|
||||
|
||||
sys.path.insert(0, os.path.dirname(os.path.abspath(__file__)))
|
||||
try:
|
||||
sys.stdout.reconfigure(encoding="utf-8")
|
||||
@@ -40,21 +49,132 @@ from armcore import place, bb, DOCS
|
||||
VOL_MIN = 2.0 # mm3，超过即计为干涉
|
||||
CHAIN = ["root", "J1", "J2", "J3", "J4", "J5", "J6"]
|
||||
|
||||
# 网格化预筛参数：外扩必须 ≥ 网格化偏差，判"分离"才保守可信
|
||||
MESH_TOL = 0.5 # mm，零件网格化容差
|
||||
MESH_MARGIN = 0.8 # mm，凸包安全外扩（> MESH_TOL 保证网格内缩不致漏判）
|
||||
USE_GJK = True # 进程内全局；工作进程由 initializer 传入
|
||||
|
||||
|
||||
# ---------------------------------------------------------------- 网格化 + GJK
|
||||
def tessellate_verts(shape):
|
||||
"""零件 → 顶点云 (N,3)；凸包 GJK 的支撑函数直接在顶点云上取极值（等价于其凸包）。
|
||||
第二步做精确凸包精简（scipy Qhull）：支撑函数结果与全顶点云逐位一致，
|
||||
但电机 STEP 网格 15 万顶点 → 数百点，GJK 从毫秒级降到微秒级。"""
|
||||
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():
|
||||
import arm_model_v2_rounded as AM
|
||||
import arm_model as AM
|
||||
JS = AM.build()
|
||||
parts = []
|
||||
for p in AB.PARTS:
|
||||
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])]
|
||||
parts.append(dict(name=p["name"], shape=p["shape"], g=g, c8=corners, box=(lo, hi)))
|
||||
parts.append(dict(name=p["name"], shape=p["shape"], g=g, c8=corners, box=(lo, hi),
|
||||
verts=tessellate_verts(p["shape"])))
|
||||
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 = []
|
||||
@@ -151,6 +271,7 @@ def test_posture(ctx, angles_by_idx, want_errors=False):
|
||||
t = acc[p["g"]]
|
||||
boxes.append(p["box"] if t is None else xf_c8(p["c8"], t))
|
||||
shapes = {}
|
||||
wverts = {} # 顶点云世界坐标缓存（本姿态内复用）
|
||||
viol, errs = [], []
|
||||
n = len(parts)
|
||||
for i in range(n):
|
||||
@@ -161,13 +282,24 @@ def test_posture(ctx, angles_by_idx, want_errors=False):
|
||||
if lo == hi:
|
||||
continue
|
||||
span = [k for k in active if lo < k <= hi]
|
||||
if not span:
|
||||
# 关键：无活动关节（零位）时必须测试全部零件对 —— 相对位姿不变不代表不干涉！
|
||||
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
|
||||
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]
|
||||
@@ -193,8 +325,9 @@ _POOL = None
|
||||
_WORKERS = 4
|
||||
|
||||
|
||||
def _init_worker():
|
||||
global _CTX
|
||||
def _init_worker(use_gjk=True):
|
||||
global _CTX, USE_GJK
|
||||
USE_GJK = bool(use_gjk)
|
||||
_CTX = make_ctx()
|
||||
|
||||
|
||||
@@ -207,7 +340,7 @@ def get_pool(workers):
|
||||
global _POOL, _WORKERS
|
||||
_WORKERS = workers
|
||||
if _POOL is None and workers > 1:
|
||||
_POOL = mp.Pool(workers, initializer=_init_worker)
|
||||
_POOL = mp.Pool(workers, initializer=_init_worker, initargs=(USE_GJK,))
|
||||
return _POOL
|
||||
|
||||
|
||||
@@ -234,8 +367,11 @@ def _ctx_single():
|
||||
|
||||
# ---------------------------------------------------------------- 扫描逻辑
|
||||
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)[0]:
|
||||
a_jam = mid
|
||||
else:
|
||||
@@ -244,8 +380,10 @@ def bisect(workers, k, a_free, a_jam, tol=0.2):
|
||||
|
||||
|
||||
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
|
||||
@@ -253,10 +391,20 @@ def sweep_single(k, step, workers):
|
||||
for s in range(0, len(angles), B):
|
||||
batch = angles[s:s + B]
|
||||
results = eval_angles([{k: a} for a in batch], workers)
|
||||
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
|
||||
stop = bisect(workers, k, sign * (a - step), a)
|
||||
print(" %s %s向在 %.1f° 首次干涉：%s"
|
||||
% (tag, "正" if sign > 0 else "反", a,
|
||||
"; ".join("%sx%s(%.0fmm3)" % (p, q, 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:
|
||||
@@ -267,12 +415,15 @@ def sweep_single(k, step, workers):
|
||||
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
|
||||
@@ -280,10 +431,14 @@ def sweep_combo(k_hold, e_hold, j_sweep, a_lo, a_hi, step, workers):
|
||||
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)
|
||||
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 = sign * (a - step), a
|
||||
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)[0]:
|
||||
@@ -306,7 +461,28 @@ def main():
|
||||
step, combo_step = 2.0, 6.0
|
||||
do_combos = "--no-combos" not in sys.argv
|
||||
want_joints = None
|
||||
workers = max(1, min(20, (os.cpu_count() or 4) - 2))
|
||||
if "--no-gjk" in sys.argv:
|
||||
global USE_GJK
|
||||
USE_GJK = 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])
|
||||
@@ -318,7 +494,8 @@ def main():
|
||||
want_joints = set(sys.argv[i + 1].split(","))
|
||||
|
||||
print("=" * 78)
|
||||
print("全对全动态行程扫描 v2 并行版（workers=%d，轴=human_layout，含相邻对与电机）" % workers)
|
||||
print("全对全动态行程扫描 v2 并行版（workers=%d / 可用内存 %.0fGB / 轴=human_layout / 含相邻对与电机 / GJK预筛=%s）"
|
||||
% (workers, avail_gb, "开" if USE_GJK else "关"))
|
||||
print("=" * 78, flush=True)
|
||||
if workers > 1:
|
||||
get_pool(workers) # 启动工作进程（各自并行建模，父进程不重复建模）
|
||||
|
||||
Reference in New Issue
Block a user