""" 너클붐 용접 경로 — 새 스테이션, 깨끗하게 """ from robodk.robolink import Robolink, ITEM_TYPE_ROBOT, ITEM_TYPE_OBJECT, ITEM_TYPE_PROGRAM, ITEM_TYPE_FRAME, ITEM_TYPE_TOOL from robodk.robomath import * import numpy as np from collections import defaultdict RDK = Robolink() # ── 새 스테이션 ────────────────────────────────────────────── station = RDK.AddStation('너클붐_용접작업') RDK.setActiveStation(station) print('새 스테이션: 너클붐_용접작업') import time; time.sleep(1) # ── 너클붐 로드 ────────────────────────────────────────────── KNUCKLE_PATH = r'E:\도진팩토리\3D스캔및티칭시스템\knuckle.obj' knuckle = RDK.AddFile(KNUCKLE_PATH, station) knuckle.setName('너클붐_12500') knuckle.setPose(eye(4)) print(f'너클붐 로드: {knuckle.Name()}') # ── 야스카와 GP8 로봇 로드 ─────────────────────────────────── robot = RDK.AddFile(r'E:\도진팩토리\VAULT\robots\YASKAWA\Yaskawa-Motoman-AR2010.robot', station) print(f'로봇 로드: {robot.Name()}') # OBJ 파싱해서 너클붐 크기 파악 verts = [] with open(KNUCKLE_PATH) as f: for line in f: if line.startswith('v ') and not line.startswith('vn') and not line.startswith('vt'): p = line.split() verts.append([float(p[1]), float(p[2]), float(p[3])]) verts = np.array(verts) cx = verts[:,0].mean() cy = verts[:,1].mean() cz = verts[:,2].mean() xmax = verts[:,0].max() print(f'너클붐 중심: ({cx:.0f},{cy:.0f},{cz:.0f}), 길이: {xmax:.0f}mm') # 로봇을 너클붐 옆(중심 X, Y+1000, Z 최고점)에 배치 robot.setPose(transl(cx, cy - 1200, cz + 400)) print(f'로봇 위치: 너클붐 앞 1200mm') # ── knuckle.obj 파싱 (faces) ───────────────────────────────── faces = [] with open(KNUCKLE_PATH) as f: for line in f: if line.startswith('f '): p = line.split()[1:] idxs = [int(x.split('/')[0])-1 for x in p] if len(idxs) >= 3: faces.append(idxs[:3]) if len(idxs) == 4: faces.append([idxs[0], idxs[2], idxs[3]]) faces = np.array(faces) # 면 법선 face_norms = [] for face in faces: v0,v1,v2 = verts[face[0]], verts[face[1]], verts[face[2]] n = np.cross(v1-v0, v2-v0) nl = np.linalg.norm(n) face_norms.append(n/nl if nl>1e-10 else np.array([0,1,0])) face_norms = np.array(face_norms) # 용접선 엣지 (30도+ 꺾임 = 판과 판 만나는 선) edge_count = defaultdict(int) edge_faces = defaultdict(list) for fi, face in enumerate(faces): for i in range(3): e = tuple(sorted([face[i], face[(i+1)%3]])) edge_count[e] += 1 edge_faces[e].append(fi) THRESH = np.cos(np.radians(30)) weld_edges = [e for e,c in edge_count.items() if c==2 and np.dot(face_norms[edge_faces[e][0]], face_norms[edge_faces[e][1]]) < THRESH] # 순서 있는 체인 adj = defaultdict(list) for a,b in weld_edges: adj[a].append(b) adj[b].append(a) visited = set() chains = [] endpoints = [v for v,nb in adj.items() if len(nb)==1] starts = endpoints if endpoints else list(adj.keys()) for start in starts: if start in visited: continue chain = [start]; visited.add(start); cur = start while True: nbs = [n for n in adj[cur] if n not in visited] if not nbs: break nxt = min(nbs, key=lambda n: np.linalg.norm(verts[n]-verts[cur])) chain.append(nxt); visited.add(nxt); cur = nxt if len(chain) >= 10: chains.append(chain) def chain_len(ch): pts = verts[ch] return np.sum(np.linalg.norm(np.diff(pts,axis=0),axis=1)) chains.sort(key=chain_len, reverse=True) print(f'\n용접선 체인 {len(chains)}개') for i,ch in enumerate(chains[:5]): print(f' 체인{i+1}: {len(ch)}점 {chain_len(ch):.0f}mm') # ── 프레임/툴/프로그램 ─────────────────────────────────────── ref = RDK.AddFrame('기준_너클붐', robot) ref.setPose(eye(4)) tool = robot.AddTool(transl(0,0,150)*roty(pi), '용접토치') prog = RDK.AddProgram('너클붐_용접', robot) prog.setFrame(ref) prog.setTool(tool) prog.setSpeed(1000) # ── 버텍스별 법선 캐시 ────────────────────────────────────── vert_norm = {} for fi, face in enumerate(faces): for vi in face: if vi not in vert_norm: vert_norm[vi] = [] vert_norm[vi].append(face_norms[fi]) def get_normal(vi): ns = vert_norm.get(vi, [np.array([0,1,0])]) avg = np.mean(ns, axis=0) nl = np.linalg.norm(avg) return avg/nl if nl>1e-6 else np.array([0,1,0]) def make_pose(pt, fwd, normal): # 토치 Z = 법선 반대방향 (표면 향해) Z = -normal / (np.linalg.norm(normal)+1e-9) X = fwd - np.dot(fwd,Z)*Z xl = np.linalg.norm(X) X = X/xl if xl>1e-6 else np.array([1,0,0]) Y = np.cross(Z,X) return Mat([[X[0],Y[0],Z[0],pt[0]], [X[1],Y[1],Z[1],pt[1]], [X[2],Y[2],Z[2],pt[2]], [0, 0, 0, 1 ]]) tc = [0] def add_tgt(name, pose): t = RDK.AddTarget(name, ref, robot) t.setPose(pose); t.setAsCartesianTarget() tc[0] += 1 return t MAX_PTS = 50 APPROACH = 80 # 접근 높이 mm APPROACH2 = 40 WELD_SPD = 50 MOVE_SPD = 1000 for si, chain in enumerate(chains[:3]): pts_idx = chain pts = verts[chain] if len(pts) > MAX_PTS: idx = np.linspace(0,len(pts)-1,MAX_PTS,dtype=int) pts_idx = [chain[i] for i in idx] pts = verts[pts_idx] n_s = get_normal(pts_idx[0]) n_e = get_normal(pts_idx[-1]) p_s = pts[0]; p_e = pts[-1] fwd_s = (pts[1]-pts[0]); fwd_s/=(np.linalg.norm(fwd_s)+1e-9) fwd_e = (pts[-1]-pts[-2]); fwd_e/=(np.linalg.norm(fwd_e)+1e-9) # 접근 3단계 prog.setSpeed(MOVE_SPD) t=add_tgt(f'S{si+1}_A1', make_pose(p_s+n_s*APPROACH, fwd_s, n_s)); prog.MoveL(t) t=add_tgt(f'S{si+1}_A2', make_pose(p_s+n_s*APPROACH2, fwd_s, n_s)); prog.MoveL(t) t=add_tgt(f'S{si+1}_WS', make_pose(p_s, fwd_s, n_s)); prog.MoveL(t) prog.RunInstruction('ArcStart', 8) t=add_tgt(f'S{si+1}_W001', make_pose(p_s, fwd_s, n_s)) prog.setSpeed(WELD_SPD); prog.MoveL(t) for pi in range(1, len(pts)): fwd = pts[pi]-pts[pi-1] if pi>0 else pts[1]-pts[0] fwd/=(np.linalg.norm(fwd)+1e-9) n = get_normal(pts_idx[pi]) t=add_tgt(f'S{si+1}_W{pi+1:03d}', make_pose(pts[pi],fwd,n)) prog.MoveL(t) prog.setSpeed(MOVE_SPD) prog.RunInstruction('ArcEnd', 8) # 이탈 3단계 t=add_tgt(f'S{si+1}_WE', make_pose(p_e, fwd_e, n_e)); prog.MoveL(t) t=add_tgt(f'S{si+1}_D2', make_pose(p_e+n_e*APPROACH2, fwd_e, n_e)); prog.MoveL(t) t=add_tgt(f'S{si+1}_D1', make_pose(p_e+n_e*APPROACH, fwd_e, n_e)); prog.MoveL(t) print(f'구간{si+1} 완료: {len(pts)+5}개 명령') print(f'\n✅ 타겟 {tc[0]}개 등록') print('RoboDK에서 [너클붐_용접작업] 스테이션 확인') RDK.Render(True)