
    յ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ddiZd	Zd
Zd ZddZd Zedk(  r ej&                  e	      e_        de_         ed eej(                              D ]H  Z ee      Zej5                  de d        ede dej8                  dz  ddej:                          J  ej<                  d       yy)a$  'Curvy' connector variant: instead of a uniform rounded prism, loft the rounded cross-section through
N scaled sections so it stays FULL at both ends (bonds to holder + yoke) but WAISTS IN toward the middle
(smooth concave slope). Separate files (two_servo_curvy.urdf) so it can be discarded.    N)boxztwo_servo_from_blender.glbg      @g333333?   g333333?      ?   c                    t        j                  |       } | j                  \  }}}| j                  dd      }g }t	        |dz
        D ]F  }||z  |dz   |z  }}t	        |      D ])  }	|	dz   |z  }
|||	z   ||
z   ||
z   g||	z   ||
z   ||	z   ggz  }+ H d|dz
  dffD ]}  \  }}t        |      }t        j                  || |   j                  d      g      }||z  }t	        |      D ]1  }	|	dz   |z  }
|j                  |s|||	z   ||
z   gn
|||
z   ||	z   g       3  t        j                  |t        j                  |      d      }|j                          |S )	Nr      )r   FTr   F)process)npasarrayshapereshaperangelenvstackmeanappendtrimeshTrimesharrayfix_normals)ringsNrP_VFkb0b1jjnring_idxflipcenbasems                   4/data/cameron/repos/smith300_para_stuff/gen_curvy.py_stitchr)      sp   JJuE{{HB1b!A
A26]QQ!BqAa%1B2627BG,rAvrBwQ.GHHA  
 &Q~6$!f"))Qh(<(<Q(?$@Aa(UV,4qAa%1BHHtc4!8TBY/#tbyRVYZRZA[\  7
 	288A;6APQ    c           
      &   t        dt        || dz
  |dz
              }g }| |z
  ||z
  df| |z
   ||z
  t        j                  dz  f| |z
   ||z
   t        j                  f| |z
  ||z
   dt        j                  z  dz  ffD ]z  \  }}}t	        |      D ]f  }|t        j                  dz  |z  |dz
  z  z   }	|j                  ||t        j                  |	      z  z   ||t        j                  |	      z  z   f       h | t        j                  |      S )zWRounded-rectangle outline with a FIXED point count (4*nc) so lofted rings always align.gMbP?g           r   r	   )	maxminr   pir   r   cossinr   )
abrncptscxcya0r   angs
             r(   _rrectr;   #   s   D#aT1t8,-A
CAq1uc*q1uXq1ubeeai,Ha%AE(BEE2QUa!eHa"%%iRSm4TV
BrA	Q"q&11CJJQ_,b1rvvc{?.BCD V
 ::c?r*   c                    t        j                  dd      }t        j                  dd      }t        j                  t        j                  | dz
           t        j                  t        j                  |          }}|j
                  |j
                  d d df   t        j                  k     }t        j                  |j                  d      |j                  d            t        j                  |j                   }}t        j                  ||||      \  }}	t        j                  ||	   d   |      }
|
j                  d      }|
d   |
d   z
  |
d   |
d   z
  }}t        j                  j!                  |      dz  t        j                  j!                  |      dz  }}|t        j                  j!                  |      z  |t        j                  j!                  |      z  }}t        j                  ||   d   g|      d   }||z
  t        j                  j!                  ||z
        z  }t        j                  ||      |z  }t#        |j                               t#        ||z        z
  }t#        |j                               t#        ||z        z
  }|||z
  d	z   t$        z  z   }t        j                  ||   d   |      }||z  j                         ||z  j                         z
  dz  d
z   }||z  j                         ||z  j                         z
  dz  d
z   }t&        j)                  | t*              }g }t-        t.              D ]  }|t.        dz
  z  }d|t        j0                  t        j2                  |z        z  z
  }|d|z
  z  ||z  z   |z  } |d|z
  z  ||z  z   |z  }!t5        | |!t        t6        | dz
  |!dz
              }"|j9                  |||z  |z  z   t        j:                  |"d d df   |      z   t        j:                  |"d d df   |      z           t=        |      }#|#j?                  t        j                  t        j                  jA                  t        j                  |                       |#S )Nzyoke_exact.stlmesh)forcezholder_approx_drilled.stlr	   r   r   r,   g      @g      ?g      ?r   )!r   loadg_mmLAYOUTvertices
YOKE_ARM_X
_box_facesr.   r-   bounds_closest_facestransform_pointsr   r   linalgnormfloatEXTRUDE_FRACWAIST_BY_CONNgetWAISTr   Nr1   r/   r;   Rr   outerr)   apply_transforminv)$iyokeholderTyTharmyfaceshfacesr2   r3   hwhce1e2h1h2e1ue2uycbdirprojnearfardepthaendya1ya2wr   r   tpinchaabbp2conns$                                       r(   curvy_connectorru   /   s   <<(7D\\5VDFUU188AE?#QUU188A;%7B
--ad+all:
;C\\#''!*cggaj91<<;WFFFFB3DAq		!	!&)A,	3B"''!*RURU]BqEBqEMBYY^^B!#RYY^^B%7!%;BBIINN2&&RYY^^B-?(?C		!	!6!9Q<."	5a	8BGryy~~b2g..D##C,t3DuR$Y//D


eBI.
.CC$J$44E##F1IaL"5D3J 0 0 22a
7#
=C3J 0 0 22a
7#
=C!U#AE1XQKa"&&+++AElS1W$-AElS1W$-BC28R#X67R$,**RXXbAh-DDrxxPRSTVWSWPXZ]G^^_  5>Dryy}}QXXa[9:;Kr*   __main__zconnector_curvy_{}.stlr	   connector_curvy_z.stlz: vol=i  z.1fzcm3 wt=ztwo_servo_curvy.urdf)   )__doc__numpyr   r   shapely.geometryr   sboxgen_two_servor@   GLBrQ   rO   rM   rL   rP   r)   r;   ru   __name__read_layoutrB   CONNECTOR_FMTr   r   rU   cexportprintvolumeis_watertightgen_urdf r*   r(   <module>r      s   Y   ( "E
R$	D zq}}S!AH.AO1c!((m$A+;A3d)C D 6!((4-)<GAOOCTUV % AJJ%& r*   