docs: 更新V2版机器人手臂零件设计说明与渲染截图

src/sweep_v2.py

@@ -0,0 +1,436 @@
+# -*- coding: utf-8 -*-
+"""全对全动态行程扫描与碰撞验证（权威版 v2，多进程并行）
+
+与旧 travel_sweep.py / real_arm_collision_test.py 的区别（旧脚本漏掉了用户实测到的干涉）：
+1. 直接在进程内构建当前模型（arm_model_v2_rounded.build()），杜绝 STEP/网格过期；
+2. 关节轴取自 Joint.rear/.d（human_layout 唯一事实源），与网页查看器逐位一致；
+3. 检查【全部】零件对 —— 包括相邻关节的零件对与电机模组（旧脚本跳过相邻对，
+   而 P04xP05 / P07xP08 恰恰在关节贴面处被卡死）；
+4. 单关节双向精扫（粗扫 + 二分）得出真实硬限位；
+5. 组合姿态扫描：每个关节置于其极限，再扫其余关节，收敛出滑块安全范围；
+6. 写 docs/travel.json（查看器滑块范围）+ docs/travel_detail.json（逐对明细）。
+
+并行化（用户要求）：
+  - mp.Pool 每个工作进程独立构建一次模型，角度任务批量分发（imap 保序）；
+  - OCCT 布尔运算 SetRunParallel(True)；
+  - 预剪枝：按关节轴计算每对零件的轴向/径向区间，区间不相交的零件对在该关节的
+    任何角度都不可能碰撞，直接跳过（对单关节扫描是精确安全的）。
+
+运行：python src/sweep_v2.py [--workers 20] [--step 2] [--combo-step 6]
+                           [--no-combos] [--joints J2,J4]
+"""
+import json
+import math
+import os
+import sys
+import time
+
+sys.path.insert(0, os.path.dirname(os.path.abspath(__file__)))
+try:
+    sys.stdout.reconfigure(encoding="utf-8")
+except Exception:
+    pass
+
+import multiprocessing as mp
+
+import arm_build as AB
+import cadquery as cq
+from armcore import place, bb, DOCS
+
+VOL_MIN = 2.0            # mm3，超过即计为干涉
+CHAIN = ["root", "J1", "J2", "J3", "J4", "J5", "J6"]
+
+
+# ---------------------------------------------------------------- 模型与上下文
+def load_model():
+    import arm_model_v2_rounded 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)))
+    joints = {t: (JS[t].rear, JS[t].d) for t in CHAIN[1:]}
+    return parts, joints
+
+
+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():
+    parts, joints = load_model()
+    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):
+    """返回 (violations, errors)：violations = [(na, nb, vol, 干涉质心)]。"""
+    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 = []
+    for p in parts:
+        t = acc[p["g"]]
+        boxes.append(p["box"] if t is None else xf_c8(p["c8"], t))
+    shapes = {}
+    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
+            span = [k for k in active if lo < k <= hi]
+            if 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
+            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
+            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))
+    return (viol, errs) if want_errors else viol
+
+
+# ---------------------------------------------------------------- 工作进程
+_CTX = None
+_POOL = None
+_WORKERS = 4
+
+
+def _init_worker():
+    global _CTX
+    _CTX = make_ctx()
+
+
+def _task(task):
+    angles = {int(k): v for k, v in task["a"].items()}
+    return test_posture(_CTX, angles)   # imap 保序，无需回传 id
+
+
+def get_pool(workers):
+    global _POOL, _WORKERS
+    _WORKERS = workers
+    if _POOL is None and workers > 1:
+        _POOL = mp.Pool(workers, initializer=_init_worker)
+    return _POOL
+
+
+def eval_angles(angles_list, workers, chunk=None):
+    """批量评估姿态列表 [{idx: ang}]，按输入顺序返回 violations 列表。"""
+    tasks = [dict(id=i, a={str(k): v for k, v in a.items()}) for i, a in enumerate(angles_list)]
+    if workers <= 1:
+        ctx = _ctx_single()
+        return [test_posture(ctx, a) for a in angles_list]
+    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
+    return out
+
+
+def _ctx_single():
+    global _CTX
+    if _CTX is None:
+        _CTX = make_ctx()
+    return _CTX
+
+
+# ---------------------------------------------------------------- 扫描逻辑
+def bisect(workers, k, a_free, a_jam, tol=0.2):
+    while abs(a_jam - a_free) > tol:
+        mid = 0.5 * (a_free + a_jam)
+        if eval_angles([{k: mid}], workers)[0]:
+            a_jam = mid
+        else:
+            a_free = mid
+    return a_free
+
+
+def sweep_single(k, step, workers):
+    jams = {}
+    lo = hi = 0.0
+    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)
+        for s in range(0, len(angles), B):
+            batch = angles[s:s + B]
+            results = eval_angles([{k: a} for a in batch], workers)
+            for a, viol in zip(batch, results):
+                if viol:
+                    jams[a] = viol
+                    stop = bisect(workers, k, sign * (a - step), 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
+    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 = {}
+    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)
+        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)
+            for a, viol in zip(batch, results):
+                if viol:
+                    jams[a] = viol
+                    lo_f, hi_j = sign * (a - step), a
+                    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]:
+                            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
+    workers = max(1, min(20, (os.cpu_count() or 4) - 2))
+    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，轴=human_layout，含相邻对与电机）" % workers)
+    print("=" * 78, 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(%.0f)" % (p, q, c, 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,
+                  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()