
    Hj{                        d Z ddlZddlmZ ddlZ ee      j                         j                  d   dz  dz  dz  Z	 e
e	      ej                  vr"ej                  j                  d e
e	             ddlmZ dd	lmZ ddlZ ej(                  g d
      \  ZZZ ej0                  e       ej2                  e      cZZ ej0                  e       ej2                  e      cZZ ej0                  e       ej2                  e      cZZ ej@                  g ddee gdeegg       ej@                  edegg de degg      z   ej@                  ee dgeedgg dg      z  Z![[[[[[[[[[ddZ"	 	 ddZ#y)u  
weld_ik.py — 블럭: 토치끝→플랜지 변환 + IK (ik_ar2010 Jacobian DLS)
(welding_pipeline.py에서 2026-07-04 분할 — 로직 원문 그대로, 수정 없음)
⚠️ 동결값 연계: tcp_offset·기구학은 ar2010_kinematics.json에서 읽기만 한다.
    N)Path   u   야스카와티칭보정src
jbi_viewer)ik)load_kinematics)g        g     V@g     f)   r   r   )r   r	   r   )r   r   r	   c                    |
t               }||d   }|d   |d   |d   g}t        j                  | t              }t        j                  |t              }t        j                  |t              }|||z  z
  }|j	                         |fS )u  
    토치 끝 위치+방향 → 플랜지 위치+방향 변환.

    입력 좌표계: 로봇 BASE (S관절 원점)
    가정: TCP 회전성분 없음 (R_tcp = I) — TOOL.CND 번역값 기준

    Parameters
    ----------
    torch_pos_mm   : [X, Y, Z] mm (토치 끝, S관절 원점 기준)
    torch_rot      : 3×3 회전행렬 (토치 프레임 = 플랜지 프레임, R_tcp=I)
    tcp_offset_mm  : [X, Y, Z] mm (플랜지→토치 끝, 플랜지 프레임). None → JSON에서 로드
    kine           : ar2010_kinematics.json dict. None → 자동 로드

    Returns
    -------
    (flange_pos_mm, flange_rot) — ik_ar2010.ik()에 바로 전달 가능
    tcp_offset_mmXYZ)dtype)
_load_kinenparrayfloattolist)	torch_pos_mm	torch_rotr   kinetp_torchR_torchp_tcpp_flanges	            S   E:\도진팩토리\3D스캔및티칭시스템\02_TOOLS\welding_auto_path\weld_ik.pytorch_to_flanger   "   s    $ ||!3330hh|51Ghhy51Ghh}E2E 5(H??g%%    c           	      p    |
t               }|t        }t        | |||      \  }}t        ||||ddd      S )u  
    토치 끝 목표 → 플랜지 변환 → IK 풀기.

    Parameters
    ----------
    torch_pos_mm  : [X, Y, Z] mm (토치 끝, S관절 원점 기준)
    torch_rot     : 3×3 회전행렬. None → PLACEHOLDER_ROT 사용
    q0_deg        : IK 초기 관절각 (도). None → [0]*6

    Returns
    -------
    list[float](6) : 수렴한 관절각 (도, SLURBT 순)
    None            : 미수렴
    i  g      ?)q0_degr   max_iter
tol_pos_mmtol_ang_deg)r   PLACEHOLDER_ROTr   	_ik_solve)r   r   r!   r   r   r   R_flanges          r   solve_ik_torchr(   C   sO      ||#	(y-QUVHhXxT!csD Dr   )NN)NNN)$__doc__syspathlibr   numpyr   __file__resolveparents_IK_DIRstrpathinsert	ik_ar2010r   r&   	fk_ar2010r   r   _npradians_r_p_ycossin_cr_sr_cp_sp_cy_syr   r%   r   r(    r   r   <module>rD      s  
    x.
 
 
"
*
*1
-0J
JU
RUa
aw<sxxHHOOAs7|$ % 3
 S[[,-
B3772;S3772;S3772;SCIIw#sd|Qs3K01CIIAc{7SD3<012CIISD|SQK012 
 r"SSSS&B ?C!%Dr   