
    7Hj                        d Z ddlZddlmZ ddlZddlmZmZ ddl	m
Z ddlmZ ddlZddlmZmZmZmZmZmZmZmZ ddlmZmZmZmZmZmZ dd	l m!Z!m"Z"m#Z#m$Z$m%Z% dd
Z&e'dk(  rcddlZ e(ejR                        dk  r" e*d        e*d        ejV                  d       ejR                  d   Z,ejR                  d   Z- e&e,e-       yy)u  
AR2010 용접 자동경로 파이프라인
STL → 심검출 → 토치자세 → IK → JBI

[2026-07-04 블럭화] 로직은 3개 블럭으로 분할됨 (원문 그대로 이동, 수정 없음):
  weld_seam.py : STL→포인트클라우드 + 심검출 + 경로 생성
  weld_ik.py   : 토치끝→플랜지 변환 + IK (PLACEHOLDER_ROT 포함)
  weld_jbi.py  : JBI 생성 (펄스 변환 — 동결 로직)
이 파일은 호환 파사드 — 기존 import 경로(from welding_pipeline import ...)는 전부 그대로 작동한다.

출처:
- 심검출: romi-lab/robotic-welding-demo (demo_all.py)
- IK: ik_ar2010.py (Jacobian DLS, 6DOF 위치+방향)
- JBI: 실제 JBI 파일(000000POINT.JBI) 포맷 직독

──────────────────────────────────────────────────────────
입력 인터페이스 스펙 (1번 ↔ 2번 연결 기준)
──────────────────────────────────────────────────────────
좌표계: 로봇 BASE (S관절 원점 = RoboDK 기준)
  ※ 물리적 바닥 기준이 아님. S관절 판면이 Z=0.
  ※ 물리 바닥 ↔ S관절 높이 차: 505mm (상수, ar2010_kinematics.json)

입력 대상: 토치 끝(torch tip) 위치 + 토치 방향
  - pos_mm  : [X, Y, Z] mm  (S관절 원점 기준)
  - rot_3x3 : 3×3 회전행렬  (토치 프레임 방향 = 플랜지 방향, R_tcp=I 가정)
  - 토치 방향 미확정 시: 고정 플레이스홀더 사용 (weld_ik.PLACEHOLDER_ROT)

내부 변환: 토치 끝 → 플랜지
  p_flange = p_torch - R_torch @ tcp_offset_mm   (R_tcp=I 가정)
  R_flange = R_torch
  (tcp_offset_mm: ar2010_kinematics.json TOOL 0, [-133.246, -10.813, 504.281] mm)

실로봇 주의: CEO 승인 없이 투입 금지.
──────────────────────────────────────────────────────────
    N)Path)splprepsplev)Rotation)KDTree)stl_to_pointcloudfind_feature_valuecluster_groove	thin_linesort_pointsgenerate_trajectoryfind_normalfind_orientation)_IK_DIR	_ik_solve
_load_kinePLACEHOLDER_ROTtorch_to_flangesolve_ik_torch)pose_to_motoman_xyzwpr
JBI_HEADER
JBI_FOOTER_deg_to_pulse_inlinegenerate_jbic           	         t        d|        t        |       }|j                  |      }|j                  t        j
                  j                  |dz  d             t        d       t        d||      }t        ||      }t        j                  |j                        }t        dt        |             t        d	       t        |      \  }}t        |      }	t        |	d
      }
|
dz  }t!        |      }t#        |
|      }t        d       t%               }g }d}t'        |      D ]  \  }\  }}|dz  j)                         }|j+                         }t-        ||||      }||}nt        d|dz    d       |j/                  |||f       |dz  dk(  sot        dj1                  |dz   t        |      |du              t        d|       t3        ||       t        d       |S )uR   
    STL → 심검출 → 토치자세 → IK → JBI 전체 파이프라인
    u1   [1/5] STL 로드 + 포인트 클라우드 변환:      )radiusmax_nn)search_paramu#   [2/5] 심검출 (asymmetry feature)	asymmetryu     검출된 홈 포인트:u&   [3/5] 중심선 추출 + 경로 생성2   )
num_outputg     @@u3   [4/5] IK 계산 (AR2010 — ik_ar2010 Jacobian DLS)N)q0_degkineu     [WARN] 포인트    u   : IK 미수렴 — 건너뜀
   r   u'     포인트 {}/{} IK완료 (수렴: {})u   [5/5] JBI 파일 생성:u   === 파이프라인 완료 ===)printr   voxel_down_sampleestimate_normalso3dgeometryKDTreeSearchParamHybridr	   r
   npasarraypointslenr   r   r   r   r   r   	enumeratetolist	as_matrixr   appendformatr   )stl_pathoutput_jbi_path
voxel_sizepcdfeature
groove_pcd
groove_pts	projected_
sorted_pts
trajectorytrajectory_mmsurface_normalposesr%   poses_with_jointsprev_qipos_mrottorch_pos_mm	torch_rot	joint_degs                          \   E:\도진팩토리\3D스캔및티칭시스템\02_TOOLS\welding_auto_path\welding_pipeline.pyrun_pipelinerN   @   s    

=xH
H
%C




+C\\99aXZ9[   

/0 c:>GW-JJ--.J	
&J8	
23Z(LIqY'J$ZB?J 'M %NZ8E	
?@<DF$U+<E3..0}}	"<6PTU	 F'!u,HIJ  ,Y!?@r6Q;;BBAs5z9D#8: ; , 

$o6"O4	
*+    __main__   uC   사용법: python welding_pipeline.py <STL파일> <출력JBI파일>uA   예시: python welding_pipeline.py workpiece.stl WELDING_AUTO.JBIr&      )g~jth?).__doc__syspathlibr   numpyr.   scipy.interpolater   r   scipy.spatial.transformr   Rscipy.spatialr   open3dr+   	weld_seamr   r	   r
   r   r   r   r   r   weld_ikr   r   r   r   r   r   weld_jbir   r   r   r   r   rN   __name__r1   argvr(   exitr7   jbi_path rO   rM   <module>rd      s   "H    , 1       3t z
388}qSTQRxx{Hxx{H8$ rO   