109 lines
4.6 KiB
Python
109 lines
4.6 KiB
Python
|
# -*- 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 prefix in ['P01_C', 'P03', 'P04', 'P05', 'P06', 'P07', 'P08']:
|
||
|
|
for f in os.listdir('out/parts_step'):
|
||
|
|
if f.startswith(prefix):
|
||
|
|
print(f"Loading {prefix} ({f})...", flush=True)
|
||
|
|
shape = cq.importers.importStep(os.path.join('out/parts_step', f)).val()
|
||
|
|
clfs[prefix] = BRepClass3d_SolidClassifier(shape.wrapped)
|
||
|
|
break
|
||
|
|
return clfs
|
||
|
|
|
||
|
|
def main():
|
||
|
|
clfs = load_solids()
|
||
|
|
print("=" * 80)
|
||
|
|
print("【线束三维物理干涉检测】全通道与 22x22mm 接口零碰撞验证")
|
||
|
|
print("=" * 80)
|
||
|
|
|
||
|
|
with open('src/viewer_tpl.html', '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)
|
||
|
|
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]})")
|
||
|
|
else:
|
||
|
|
print(f" ✓ PASS: {name} ({len(sampled)} 采样点零干涉)")
|
||
|
|
|
||
|
|
# Test dynamic bridges at zero pose
|
||
|
|
bridge_specs = [
|
||
|
|
('Bridge 1 (J1 轴向扭转段)', [[-15.0, 128.0, 610.0], [0.0, 128.0, 610.0], [15.0, 128.0, 610.0]], ['P03', 'P04']),
|
||
|
|
('Bridge 2 (J2 俯仰跨越段)', [[28.0, 115.0, 480.0], [26.5, 115.0, 467.5], [25.0, 115.0, 455.0]], ['P04', 'P05']),
|
||
|
|
('Bridge 3 (J3 旋转过渡段)', [[25.0, 90.0, 390.0], [25.0, 90.0, 381.0], [25.0, 90.0, 372.0]], ['P05', 'P06']),
|
||
|
|
('Bridge 4 (J4 俯仰跨越段)', [[25.0, 115.0, 294.0], [22.0, 140.0, 294.0], [19.5, 160.0, 296.5]], ['P06', 'P07']),
|
||
|
|
('Bridge 5 (J5 俯仰跨越段)', [[25.0, 115.0, 190.0], [25.0, 115.0, 187.5], [25.0, 115.0, 185.0]], ['P07', 'P08']),
|
||
|
|
]
|
||
|
|
|
||
|
|
for name, pts, parts 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 in parts:
|
||
|
|
clf = clfs.get(part)
|
||
|
|
if not clf: continue
|
||
|
|
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)} 处实体穿透!)")
|
||
|
|
else:
|
||
|
|
print(f" ✓ PASS: {name} ({len(sampled)} 采样点零干涉)")
|
||
|
|
|
||
|
|
print("=" * 80)
|
||
|
|
print(f"【最终结论】共评估 {checked_count} 个空间采样点，固体穿透总数: {total_hits}")
|
||
|
|
if total_hits == 0:
|
||
|
|
print("🎉 恭喜！线束系统已实现 100% 结构腔体内规范走线与零碰撞干涉！")
|
||
|
|
print("=" * 80)
|
||
|
|
|
||
|
|
if __name__ == '__main__':
|
||
|
|
main()
|