refactor(layout): src/ 收成顶层包 arm/,入口统一 python -m;提升承担构建的 scratch 脚本

单文件约束取消后,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 段逐字节一致。
This commit is contained in:
cbb
2026-10-15 03:31:20 +08:00
parent b32d44efe0
commit 649bd69810
57 changed files with 843 additions and 453 deletions
+726
View File
@@ -0,0 +1,726 @@
# -*- 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)