Robot_Arm/tests/test_harness_collision.py

# -*- coding: utf-8 -*-
"""
Automated 3D Wire Harness Collision Verification Suite:
Validates that every wire segment and flexible bridge in the robot arm
passes strictly through dedicated hollow channels and enlarged 22x22mm pass-through holes
with ZERO solid penetrations (BRepClass3d_SolidClassifier against OCC STEP solids).
"""
import sys, io, os, re
sys.stdout.reconfigure(encoding='utf-8')
import numpy as np
import cadquery as cq
from OCP.BRepClass3d import BRepClass3d_SolidClassifier
from OCP.gp import gp_Pnt
from OCP.TopAbs import TopAbs_IN

def catmull_rom_sample(pts, num_samples_per_seg=25):
    pts = np.array(pts, dtype=float)
    if len(pts) == 2:
        t = np.linspace(0, 1, num_samples_per_seg)
        return [(1-s)*pts[0] + s*pts[1] for s in t]
    sampled = []
    p = np.vstack([pts[0], pts, pts[-1]])
    for i in range(1, len(p) - 2):
        p0, p1, p2, p3 = p[i-1], p[i], p[i+1], p[i+2]
        for t in np.linspace(0, 1, num_samples_per_seg, endpoint=(i == len(p)-3)):
            t2, t3 = t*t, t*t*t
            val = 0.5 * ((2*p1) + (-p0 + p2)*t + (2*p0 - 5*p1 + 4*p2 - p3)*t2 + (-p0 + 3*p1 - 3*p2 + p3)*t3)
            sampled.append(val)
    return sampled

def load_solids():
    clfs = {}
    for f in os.listdir('out/parts_step'):
        if f.endswith('.step'):
            tag = f.split('.')[0]
            print(f"Loading {tag}...", flush=True)
            shape = cq.importers.importStep(os.path.join('out/parts_step', f)).val()
            clfs[tag] = BRepClass3d_SolidClassifier(shape.wrapped)
    return clfs

def main():
    clfs = load_solids()
    print("=" * 80)
    print("【线束三维物理干涉检测】全通道、外侧走线与电机全固态零碰撞验证")
    print("=" * 80)

    # 线束通道的锚点坐标写在查看器源码里（现在是模板，拆分后是 webapp/js/harness.js）。
    # 扫候选文件而不是写死一个路径，前端拆分时这条检查不用跟着改；一个都找不到就直接失败，
    # 绝不能"没匹配到 = 没有通道 = 零干涉"地假通过。
    from arm.paths import REPO
    cands = [REPO + "/webapp/js/harness.js", REPO + "/webapp/js/app.js"]
    found = [c for c in cands if os.path.exists(c)]
    if not found:
        print("找不到线束源码，检查项无法执行:", cands, flush=True)
        sys.exit(2)
    with open(found[0], 'r', encoding='utf-8') as f:
        tpl = f.read()

    tube_matches = re.findall(r'makeHarnTube\(\[\s*([\s\S]*?)\s*\]\s*,\s*[\d\.]+\s*,\s*\w+\s*,\s*\'([^\']+)\'', tpl)
    # 读到 0 个通道会被下面当成"零穿透"而 PASS —— 那是假通过，必须先炸。
    if not tube_matches:
        print("找不到任何线束通道定义，源码结构变了:", cands)
        sys.exit(2)
    total_hits = 0
    checked_count = 0

    for raw_pts, name in tube_matches:
        pt_list = []
        for m in re.finditer(r'\[\s*([-\d\.]+)\s*,\s*([-\d\.]+)\s*,\s*([-\d\.]+)\s*\]', raw_pts):
            pt_list.append([float(m.group(1)), float(m.group(2)), float(m.group(3))])
        sampled = catmull_rom_sample(pt_list, num_samples_per_seg=25)
        hits = []
        for p in sampled:
            pt = gp_Pnt(float(p[0]), float(p[1]), float(p[2]))
            for part, clf in clfs.items():
                clf.Perform(pt, 1e-3)
                if clf.State() == TopAbs_IN:
                    hits.append((part, p))
        checked_count += len(sampled)
        if hits:
            total_hits += len(hits)
            print(f"  ❌ FAIL: {name} (发现 {len(hits)} 处实体穿透! 首击穿部件: {hits[0][0]} 坐标: {hits[0][1]})", flush=True)
        else:
            print(f"  ✓ PASS: {name} ({len(sampled)} 采样点零干涉)", flush=True)

    # Test dynamic bridges at zero pose
    bridge_specs = [
        ('Bridge 1 (J1 Ω-Loop 柔性缓冲段)', [[5.5, 86.1, 610.0], [9.13, 79.4, 611.9], [12.75, 76.78, 613.5], [16.38, 79.85, 614.4], [20.0, 87.0, 615.0]]),
        ('Bridge 2 (J2 U-Loop 柔性缓冲段)', [[-17.5, 102.5, 485.0], [-25.0, 102.5, 475.0], [-30.0, 102.5, 465.0], [-25.0, 102.5, 455.0], [-17.5, 102.5, 445.0]]),
        ('Bridge 3 (J3 螺旋柔性过渡段)', [[-17.5, 102.5, 400.0], [-24.0, 96.0, 390.0], [-27.0, 93.0, 380.0], [-23.0, 96.0, 370.0], [-17.5, 102.5, 360.0]]),
        ('Bridge 4 (J4 肘部U-Loop柔性缓冲段)', [[-17.5, 102.5, 305.0], [-24.0, 102.5, 290.0], [-30.0, 102.5, 280.0], [-24.0, 102.5, 270.0], [-17.5, 102.5, 255.0]]),
        ('Bridge 5 (J5 腕部U-Loop柔性缓冲段)', [[-17.5, 102.5, 205.0], [-22.0, 102.5, 195.0], [-26.0, 102.5, 190.0], [-22.0, 102.5, 182.0], [-17.5, 102.5, 175.0]]),
    ]

    for name, pts in bridge_specs:
        sampled = catmull_rom_sample(pts, num_samples_per_seg=25)
        hits = []
        for p in sampled:
            pt = gp_Pnt(float(p[0]), float(p[1]), float(p[2]))
            for part, clf in clfs.items():
                clf.Perform(pt, 1e-3)
                if clf.State() == TopAbs_IN:
                    hits.append((part, p))
        checked_count += len(sampled)
        if hits:
            total_hits += len(hits)
            print(f"  ❌ FAIL: {name} (发现 {len(hits)} 处实体穿透!)", flush=True)
        else:
            print(f"  ✓ PASS: {name} ({len(sampled)} 采样点零干涉)", flush=True)

    print("=" * 80, flush=True)
    print(f"【最终结论】共评估 {checked_count} 个空间采样点，固体穿透总数: {total_hits}", flush=True)
    if total_hits == 0:
        print("🎉 恭喜！线束系统已实现 100% 结构腔体内规范走线与零碰撞干涉！", flush=True)
    print("=" * 80, flush=True)
    sys.stdout.flush()
    # 同上：OCC 析构会段错误，所以用 os._exit 带走正确退出码
    os._exit(0 if total_hits == 0 else 1)

if __name__ == '__main__':
    main()