
    (Ej              
          d Z ddlZddlZddlmZ ddlZdZ	dZ
d Zedk(  r e ej                  e	            e_        de_         ed	 eej                              D ]  Z ej&                  e      Zej+                  d
e d       ej                  e   dddf   dz  Z ede d ej0                  ed	      j3                          dej4                  dz  dd         ej6                  d       yy)aw  ALIGNED variant (separate 'branch'): snap the layout so connectors go orthogonal instead of slightly
diagonal. Orientations -> nearest 90deg; position offsets between consecutive servos with |component| <
POS_THRESH_MM -> 0 (kept: the big intentional offsets). Rounded connectors. Writes two_servo_aligned.urdf
+ connector_aligned_*.stl. Does NOT touch the default arm files.    N)Rotationztwo_servo_from_blender.glbg      .@c                 P   | d   j                         g}| d   d ddf   j                         }t        dt        |             D ]  }| |   j                         }t        j                  |d dd df         j                  dd      }t        j                  dt        j                  |dz        dz  d      j                         |d dd df<   | |   d ddf   |z
  dz  }d	|t        j                  |      t        k  <   ||dz  z   }||d ddf<   |j                  |        |S )
Nr         xyzT)degreesg     V@g     @@g        )copyrangelenRfrom_matrixas_euler
from_eulernpround	as_matrixabsPOS_THRESH_MMappend)laysnappedpreviMrpyoff_mms          6/data/cameron/repos/smith300_para_stuff/gen_aligned.pysnap_layoutr      s(   1v{{}oGq6"1"a%=D1c#hFKKMmmAbqb"1"fI&//t/DLLt(<t(CTR\\^"1"bqb&	a&!Q-$&&014rvvf~-.fvo%"1"a%q   N    __main__zconnector_aligned_{}.stlr   connector_aligned_z.stlr   i  servo_z: snapped pos=z  connector vol=z.1fcm3ztwo_servo_aligned.urdf)__doc__numpyr   trimeshscipy.spatial.transformr   r   gen_two_servogGLBr   r   __name__read_layoutLAYOUTCONNECTOR_FMTr
   r   r   connector_meshcexportpprintr   tolistvolumegen_urdf r   r   <module>r8      s  D   1 " z=1==-.AH0AO1c!((m$AQ	%aS-.HHQKA%qc1(<(<(>'??OPQPXPXY]P]^aObbefg	 %
 AJJ'( r   