cbb/Robot_Arm
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: P01/P02阶梯榫卯面承载+P03侧开式托架/P04加长避让重构，精简零件显示名并同步重建模型数据与查看器，清理历史文档

Browse Source
This commit is contained in:
cbb
2026-09-27 21:08:28 +08:00
parent b11814410a
commit bdcc27d506
179 changed files with 1598 additions and 14836 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/sweep_v2.py
+105 -23
View File
@@ -2,7 +2,7 @@
"""全对全动态行程扫描与碰撞验证（权威版 v2，多进程并行）
与旧 travel_sweep.py / real_arm_collision_test.py 的区别（旧脚本漏掉了用户实测到的干涉）：
1. 直接在进程内构建当前模型（arm_model_v2_rounded.build()），杜绝 STEP/网格过期；
1. 直接在进程内构建当前模型（arm_model.build()），杜绝 STEP/网格过期；
2. 关节轴取自 Joint.rear/.d（human_layout 唯一事实源），与网页查看器逐位一致；
3. 检查【全部】零件对 —— 包括相邻关节的零件对与电机模组（旧脚本跳过相邻对，
而 P04xP05 / P07xP08 恰恰在关节贴面处被卡死）；
@@ -65,9 +65,9 @@ USE_GJK = True # 进程内全局；工作进程由 initializer 传入
# ---------------------------------------------------------------- 网格化 + GJK
def tessellate_verts(shape):
"""零件 → 顶点云 (N,3)；凸包 GJK 的支撑函数直接在顶点云上取极值（等价于其凸包）。
第二步做精确凸包精简（scipy Qhull）：支撑函数结果与全顶点云逐位一致，
但电机 STEP 网格 15 万顶点 → 数百点，GJK 从毫秒级降到微秒级。"""
"""零件 → 顶点云 (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)
@@ -84,6 +84,8 @@ def tessellate_verts(shape):
return None
def gjk_separated(va, vb, margin=MESH_MARGIN, max_iter=40):
"""凸包 GJK（带 margin）：va/vb 顶点云。返回 True 仅当
conv(va)⊕Ball(margin) 与 conv(vb) 确证分离 —— 保守，可安全跳过精确布尔。
@@ -171,8 +173,13 @@ def load_model():
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),
verts=tessellate_verts(p["shape"])))
ctr=ctr, rad=rad, verts=verts))
joints = {t: (JS[t].rear, JS[t].d) for t in CHAIN[1:]}
return parts, joints
@@ -269,15 +276,24 @@ def common_vol_center(a, b):
return -1.0, None
def test_posture(ctx, angles_by_idx, want_errors=False):
"""返回 (violations, errors)：violations = [(na, nb, vol, 干涉质心)]。"""
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 = [], []
@@ -303,6 +319,11 @@ def test_posture(ctx, angles_by_idx, want_errors=False):
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:
@@ -323,12 +344,48 @@ def test_posture(ctx, angles_by_idx, want_errors=False):
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
@@ -346,7 +403,7 @@ def _init_worker(use_gjk=True):
def _task(task):
angles = {int(k): v for k, v in task["a"].items()}
return test_posture(_CTX, angles) # imap 保序，无需回传 id
return task["id"], test_posture(_CTX, angles) # 带id回传，配合 imap_unordered 实时进度
def get_pool(workers):
@@ -357,17 +414,34 @@ def get_pool(workers):
return _POOL
def eval_angles(angles_list, workers, chunk=None):
"""批量评估姿态列表 [{idx: ang}]，按输入顺序返回 violations 列表。"""
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()
return [test_posture(ctx, a) for a in angles_list]
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] * len(tasks)
cs = chunk or max(1, len(tasks) // (workers * 4) or 1)
for idx, viol in enumerate(pool.imap(_task, tasks, chunksize=cs)):
out[idx] = viol
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
@@ -385,7 +459,7 @@ def bisect(workers, k, a_free, a_jam, tol=0.2):
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]:
if eval_angles([{k: mid}], workers, label="%s二分" % CHAIN[k])[0]:
a_jam = mid
else:
a_free = mid
@@ -400,10 +474,11 @@ def sweep_single(k, step, workers):
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 * 2, 8)
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)
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),
@@ -413,7 +488,8 @@ def sweep_single(k, step, workers):
jams[a] = viol
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])),
"; ".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)
# 在负方向算出未测过的另一侧角度、把二分踢进远端慢区的问题
@@ -440,10 +516,11 @@ def sweep_combo(k_hold, e_hold, j_sweep, a_lo, a_hi, step, workers):
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 * 2, 8)
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)
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),
@@ -454,7 +531,8 @@ def sweep_combo(k_hold, e_hold, j_sweep, a_lo, a_hi, step, workers):
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]:
if eval_angles([{k_hold: e_hold, j_sweep: mid}], workers,
label="%s组合二分" % tag)[0]:
hi_j = mid
else:
lo_f = mid
@@ -510,6 +588,8 @@ def main():
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) # 启动工作进程（各自并行建模，父进程不重复建模）
# 零位检查
@@ -526,7 +606,9 @@ def main():
singles[tag] = r
jam_s = "; ".join(
"%s%.1f°:%s" % ("+" if a > 0 else "", a,
",".join("%sx%s@%s(%.0f)" % (p, q, c, v) for p, q, v, c in vs[:2]))
",".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)