
    ֡EjdB              	          d Z ddlZddlZddlZddlmc mZ ddl	m
Z ddlmZ ddg dfdd	g d
fddg dfgZd0dZ ed       ed      gZd Z ed      ZdZd Zd Zd Zd1dZdZdZdZd1dZefdZdZdZdZ d  Z!d! Z"d" Z#d# Z$d$ Z%d% Z&d& Z'e(d'k(  rv e)d( e*e            D ]S  Z+ e'e+      Z,e,j[                  ej]                  e+              e/d)e+ d*e,j`                  d+z  d,d-e,jb                          U  ed.        ed/       yy)2a  Two-servo layout -> URDF (canonical) + GLB (for Blender), from ONE shared LAYOUT list.

This is the seed of the Blender<->connector pipeline: LAYOUT = module poses in servo_0's frame (meters,
servo_0 = identity = base). Each module = servo + holder + yoke (constant, the parts we made). The GLB
has one node per servo (servo_0, servo_1, ...) so Cameron can grab + reorient each in Blender and export
it back; I then read those node transforms straight back into LAYOUT. No connector geometry yet.
    N)Rotation)boxz
st3215.stlz0.35 0.35 0.40 1)Z   r   d      holder_approx_drilled.stlz0.20 0.52 0.85 1)4         r   yoke_exact.stlz0.85 0.60 0.20 1)r      3   r   r   r   r   c                     t        j                  d      }t        j                  d|      j	                         |d dd df<   | |d ddf<   |S )N   xyz   )npeyeRot
from_euler	as_matrix)r   rpyMs      8/data/cameron/repos/smith300_para_stuff/gen_two_servo.py_Tr      sN    
q	Auc*446Abqb"1"fIAbqb!eHH    )g333333ÿr   r   c                 t   |j                  d      rdnd}t        j                  | d      }t        j                  |d|dd       t        j                  t        j                  |d      d|d	d
       t        j                  t        j                  |dd|t        t	        |            z   i      dd|i       y )Nyokez
0.0255 0 00 0 0visualoriginr   r   geometrymeshz0.001 0.001 0.001)filenamescalematerialnamecolorrgba)
startswithET
SubElementstrid)linkfncoloffvs        r   _visr6   #   s    --/,WC
dH%AMM!Xs7;<MM"--:.RRe8fgMM"--:SAZ/HI7U[]`Tabr   )g&1r   r   z
1.5708 0 0c                     t        j                  | d dd df         j                  d      \  }}}| d ddf   }|d   dd|d   dd|d   d|dd|dd|ddS )	Nr   r   r   .6f       r#   )r   from_matrixas_euler)r   rpyts        r   _originrB   0   s    ooaBQBi(11%8GAq!	"1"a%Ad3Zq1c
!AaD:6!C!CPQRSTWQX?YZZr   c                  `   g } t               }t        t              D ]  \  }}| j                  t	        t        j                  |j                  t        |            dddf   j                                      |dkD  sct        j                  j                  t        j                  |            st        j                  t        j                  |      d      }| j                  t	        t        j                  |j                  t        |            dddf   j                                       t        j                  j                  d      rN| j                  t	        t        j                  dd      j                  dddf   j                                      t        |       dz  S )zLowest servo_0-frame Y over the whole assembly (modules + connectors + base). After VIEW_RPY (+90 X)
    world-Z == servo_0-frame Y, so this is the robot's lowest point; -this lifts it onto the floor (Z=0).Nr:   r   r%   forcebase_holder.stl     @@)module_mesh	enumerateLAYOUTappendfloattrimeshtransform_pointsvertices_mmminospathexistsCONNECTOR_FMTformatload)ysmodir   cs        r   _assembly_min_yr\   6   s'    
B
-C&!1
		%00s1vFq!tLPPRSTq5RWW^^M$8$8$;<]11!4FCAIIeG44QZZQHANRRTUV	 "
 
ww~~'(
		%%6fENNqRStTXXZ[\r7Vr   c           
      :   t        j                  dddi      }t        j                  |dddi       t        j                  |dddd	      }t        j                  |d
ddi       t        j                  |dddi       t                }t        j                  |dd|dt        d       t        t        t                    D ]y  }t        j                  |ddd| i      }t        |t        d   d   t        d   d          |dk(  r7t        j                  j                  d      rt        |dt        d   d          n!t        |t        d   d   t        d   d          |dkD  r t        |t        j                  |      d       t        j                  |dd| dd	      }t        j                  |d
dd| i       t        j                  |ddd| i       t        j                  |dddd       t        j                  |dddi       t        j                  |ddd d!d"d#       t        j                  |ddd| i      }t        |t        d$   d   t        d$   d          |dkD  st        j                   j#                  t$              t        j                   j#                  t        |dz
           z  t        |   z  }t        j                  |dd%| dd	      }	t        j                  |	d
dd|dz
   i       t        j                  |	ddd| i       t        j                  |	dt'        |             | t        j(                  t        j*                  |      d&       t        j*                  |      j-                  | d'd()       t/        d*|        y+),zProper serial chain: servo_0 -> output_0 -> yoke_0 -> connect_1 -> servo_1 -> output_1 -> yoke_1.
    servo_i is CARRIED BY servo_{i-1}'s yoke, so rotating output_{i-1} swings the whole downstream arm.robotr)   	two_servor1   worldjointworld_to_servo_0fixed)r)   typeparentchildservo_0r"   z0 0 r8   r#   servo_r   r:   rF   z0.30 0.66 0.45 1output_revoluteyoke_z-0.0255 0 0r    axisr   z0 1 0limitz-1.74z1.74z2.94z3.0)loweruppereffortvelocityr;   connect_z  unicodeT)encodingxml_declarationwroteN)r-   Elementr.   r\   VIEW_RPYrangelenrJ   r6   PARTSrR   rS   rT   rU   rV   r   linalginvOUTrB   indentElementTreewriteprint)
rS   r^   wjliftrZ   baseojykCjs
             r   gen_urdfr   E   s    JJw 56EMM%&'!23	ug0BG'T	UBMM"h 12MM"g	23DMM"h$tCj(9( KL3v;}}UFVvaS\,BCT58A;a,6bggnn%67(%(1+6uQx{E!HQK0q5}++A.0BC]]5'gaSM:+VW
b(VvaS\$:;
b'FeA3K#89
b(M'$JK
b&5'"23
b'WvQWej#kl]]5&6U1#;*?@Rq!eAhqk*q5 		c"RYY]]6!a%=%AAF1IMAeW!~w.WXAMM!X%!u'>?MM!Wvs|&<=MM!Xwqz21  2 IIbnnU#T*NN5y$O	'4r   c                    g }t         D ]b  \  }}}| r|t         d   d   k(  r| n|}t        j                  |d      j                         }||j                  _        |j                  |       d t        j                  j                  |      S )zservo+holder+yoke concatenated (mm), face-coloured per part. holder_override swaps the holder file
    (e.g. base_holder.stl for servo_0) so the base pieces show and the plain holder isn't doubled.r:   r   r%   rD   )	r{   rM   rW   copyr!   face_colorsrK   utilconcatenate)holder_overridepartsr2   _fcfms          r   rH   rH   m   sx     E	Ar /B%(1+4EOBLL&)..0!Q	 
 <<##E**r   g      $@zconnector_{}.stlg      @c                    t        j                         }t               }|j                  dt        z         t        t              D ]  \  }}|dk(  rI|j                         }|dddfxx   t        z  cc<   |j                  |j                         dd|       nxt        j                  j                  t        |dz
           t        |   z  }|dddfxx   t        z  cc<   |j                  |j                         d| d| d|dz
   |	       |dkD  st        j                  j                  t        j!                  |            st        j"                  t        j!                  |      d
      }g d|j$                  _        |j                  dt        z         |j                  |d| d| d| t        j(                  d      	        |rt        j"                  |      }	t        j                  j                  t        j*                  |	j,                  d   d   t.                    }
|	j,                  j0                  D ]r  }|j3                  d      s|	j,                  |   \  }}|j                  |	j4                  |   j                         ||d|
t        j*                  |t.              z  	       t |j7                  |        t9        d| dt;        |j,                  j0                        dt         d       y)af  Export a PARENT-CHAIN GLB: servo_0 -> servo_1 -> ... -> servo_N, each with a transform relative to
    its parent. In Blender moving/rotating any servo then carries all its descendants. connector_i and the
    base pieces are parented to the servo they belong to so they ride along. Round-trip is unaffected
    (read_layout resolves world poses either way).gMbP?r   Nr   rg   )	node_name	geom_name	transformr:   rh   )r   r   parent_node_namer   r%   rD   )M      s   r   
connector_r   	base_cuberv   znodes:zscale=x)rM   ScenerH   apply_scale	GLB_SCALErI   rJ   r   add_geometryr   r|   r}   rR   rS   rT   rU   rV   rW   r!   r   r   arraygraphrL   nodes_geometryr,   r$   exportr   sorted)rS   src_glbscener   rZ   r   Trelr[   srcsrc_base_invnodegnames                r   gen_glbr   ~   sz   
 MMOE=DUY&'&!16Abqb!eH	!Htyy{i9`ab))--q1u.:CAJ)#Jtyy{qclPVWXVYl28Q0@C  Qq5RWW^^M$8$8$;<]11!4FCA#6AHH MM%)+,qj,<*UVTWHX28q	  S "  ll7#yy}}RXXcii	.B1.Eu%MNII,,D{+99T?5""3<<#6#;#;#=Y]4=-9BHHQ<N-N # P - 
LL	'46%++*D*D#EPY{Z[G\]r   c                    t        j                  |       }i }|j                  j                  D ]R  }|j                  |   \  }}t	        j
                  |t              j                         }|dddfxx   |z  cc<   |||<   T d }t        d |D        |      }t        j                  j                  |d         }	|D 
cg c]
  }
|	||
   z   c}
S c c}
w )zRead servo_i node transforms from a (scaled) GLB Cameron exported -> LAYOUT in metres (servo_0 frame).
    Divides translation by `scale`; rotation is scale-invariant. Rebases to servo_0 = identity.Nr   c                     | j                  d      \  }}}t        |j                  d      d         |rt        |      fS dfS )N.r   r:   i@B )	partitionintsplit)r   headr   tails       r   _keyzread_layout.<locals>._key   sA    s+aDJJsOA&'dTFFFFr   c              3   D   K   | ]  }|j                  d       s|  yw)rh   N)r,   ).0ks     r   	<genexpr>zread_layout.<locals>.<genexpr>   s     ?u!X(>Aus     keyrg   )rM   rW   r   r   r   r   rL   r   r   r|   r}   )rS   r'   r   posesr   r   r   r   orderbase_invns              r   read_layoutr      s     LLEE**{{4 1HHQ##%	"1"a%Ed	 +
G ?u?TJEyy}}U9-.H)./AHuQx///s    Cg      @closestg      Hc           
         g }t        d      D ]9  }t        d      D cg c]
  }||k7  s	| }}d| |   fd||   ffD ]  \  }}t        d      D cg c]  }g d }	}t        g d      D ]H  \  }
\  }}||	|
   |<   | |d      ||d      f|   |	|
   |d   <   | |d      ||d      f|   |	|
   |d   <   J t        d      D cg c]  }| |   ||   z   d	z   }}|||<   g d
}|||<   |j                  t        j                  |t
              t        j                  |	t
              t        j                  |t
              f       	 < |S c c}w c c}w c c}w )z[The 6 faces of an axis-aligned box -> list of (center[3], corners[4,3], outward_normal[3]).r   g            ?r   r   ))r   r   )r   r:   )r:   r:   )r:   r   r   r:   r;   )        r   r   )ry   rI   rK   r   r   rL   )lohifacesaxaosgnvalr   cornersr   s0s1centernormals                  r   
_box_facesr      sx   EAha,1AGQ,2#r"v7HC*/(3(Qy(G3()IJ8B!$
2$&qtHb1h#7#;
1Q4 $&qtHb1h#7#;
1Q4   K 498<8ar!ur!u})8F<F2J$FF2JLL"((651288GU3KRXXV\^cMdef 8  L -3
 =s   
EEEEc           	         t        j                  t        j                  | D cg c]  }|d   	 c}d      g|      d   t	        t        d      fd      }t        d t        |       D              }||fS c c}w )a  Cameron 2026-07-01: keep the ORIGINAL yoke face + the current (dynamic) holder face.
    HOLDER face = the one facing the yoke (outward normal points most toward the yoke).
    YOKE face = the arm END face (-X, the original approved connector face). Returns (yoke_idx, holder_idx).r      c                 t    t        j                  |    d   g      d   z
  d dd df   |    d   z  z  S )Nr   r   r;   )rM   rN   )bThhfacesyctrs    r   <lambda>z _closest_faces.<locals>.<lambda>   sN    dW-E-EvayQR|nVX-YZ[-\&\2A2rr6
VAYq\)&+r   r   c              3   @   K   | ]  \  }\  }}}|d    dk  s|  yw)r         N )r   r   ycycornyns        r   r   z!_closest_faces.<locals>.<genexpr>   s(     L*;&AE2r!ut|a*;s   )rM   rN   r   meanmaxry   nextrI   )yfacesr   Tyr   r   hbyar   s    ` `   @r   _closest_facesr      sv     ##RWWF-CFqadF-CQ%G$H"MaPD	U1X + 
,B	L)F*;L	LBr6M	 .Ds   A=c           
         t        j                  | t              } | j                  d      }t	        | j                  d      j                               }| j                         }t        d      D ]<  }||k7  s	|dd|fxx   |t        j                  |dd|f   ||   z
        z  z  cc<   > t        j                  | |g      S )zNExpand a (roughly planar) face point set outward within its plane by `pad` mm.r   r   N)r   asarrayrL   r   r   varargminr   ry   signvstack)ptspadr[   naxextr   s         r   	_pad_facer      s    
**S%
 CA
cggaj!
"C
((*C1X81Irwws1a4y1Q4'7888I  99c3Z  r   c                 x    t        j                  | t              j                         }|dddfxx   dz  cc<   |S )zNConvert a metres transform to apply to a mm mesh (scale only the translation).Nr   rG   )r   r   rL   r   )r   M2s     r   rP   rP      s6    	!U		 	 	"Brr1uIIIr   c           
      `   t        j                  | t              } | j                  d      }t	        | j                  d      j                               }| j                         }t        d      D ]<  }||k7  s	|dd|fxx   |t        j                  |dd|f   ||   z
        z  z  cc<   > |S )zWExpand a 4-corner rectangle outward within its own plane by `pad` mm (keeps 4 corners).r   r   N)
r   r   rL   r   r   r   r   r   ry   r   )r   r   r[   r   outr   s         r   _pad_cornersr      s    jj%(GQA
gkk!n##%
&C
,,.C1X81Irwws1a4y1Q4'7888I  Jr   c           
      b   d } ||        ||      }} d}||ddd   fD ]y  }t        d      D ]i  }t        j                  ||d      }t        t        j                  t        j
                  j                  | |z
  d                  }|	||d   k  sf||f}k { |d   }t        j                  | |g      }g dg d	g d
g dg}	t        d      D ]   }
|
dz   dz  }|	|
|d|z   g|
d|z   d|
z   ggz  }	" t        j                  |t        j                  |	      d      }|j                          |S )a  SOLID frustum between two 4-corner rectangles (world). Corners are ordered CCW and matched by
    nearest so the sides don't cross; the result keeps a full rectangular cross-section and TWISTS to
    follow any relative rotation (no triangular hull collapse). Watertight.c                 
   t        j                  | t              } | j                  d      }t        j                  | d   | d   z
  | d   | d   z
        }|t         j
                  j                  |      z  }| d   |z
  t         j
                  j                  | d   |z
        z  }t        j                  ||      }| t        j                  | D cg c]$  }t        j                  ||z
  |z  ||z
  |z        & c}         S c c}w )Nr   r:   r;   )	r   r   rL   r   crossr|   normargsortarctan2)qr[   r   ur5   r?   s         r   ccwz_loft_faces.<locals>.ccw   s    JJq% FF1IHHQqTAaD[!A$1+.		q!!qTAX!q11HHQN1M1aRZZQ!a!eq[A1MNOOMs   )D 
Nr   r   )rl   r:   )r   r:   r;   )r   r;   r   )r   r      )r      r   F)process)ry   r   rollrL   sumr|   r  r   rM   Trimeshr   fix_normals)qaqbr  bestcand0r>   canddVFrZ   r   r   s                r   _loft_facesr     s.   P Wc"gBDb2hqA775!!,DbffRYY^^BIA^>?@A|q47{4y	    
aB
		2r(A	Iy)4A1XUaK	q!QUmaQA.//  	288A;6AMMOHr   c                    t        j                  dd      }t        j                  dd      }t        t        | dz
           t        t        |          }}|j                  |j                  dddf   t
        k     }t        |j                  d      |j                  d            t        |j                   }}t        dk(  rt        ||||      \  }}	n6t        d	 t        |      D              }t        d
 t        |      D              }	t        j                  ||	   d   |      }
|
j                  d      }t        j                  ||   d   g|      d   }||z
  t         j"                  j%                  ||z
        z  }t        j                  ||      }t'        ||z  j                         ||z  z
        dz   }t(        dkD  r]|
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'        t        t(        |dz
  |dz
              }t+        | |z   | |z   ||z
  ||z
        j-                  |dd      }t         j.                  j1                  |d      }t!        j2                  d      }||||z  |f\  |dddf<   |dddf<   |dddf<   |dddf<   |j5                  |       nt7        |
|
||z  z         }|j5                  t        t         j"                  j9                  t        |                       |S )u$  PERPENDICULAR BLOCK: take the holder's yoke-facing face and extrude it straight to the yoke as a
    UNIFORM rectangular prism (the holder face cross-section held constant) — a block dropping off the
    holder, not a slanted taper. The holder-sized bottom engulfs + bonds the yoke arm end.r   r%   rD   r   r:   Nr   r   c              3   >   K   | ]  \  }}|d    d   dk  s|  yw)r;   r   r   Nr   r   r   r   s      r   r   z!connector_mesh.<locals>.<genexpr>%  s%     C0tq!AaDGdN0   c              3   >   K   | ]  \  }}|d    d   dkD  s|  yw)r;   r   g      ?Nr   r  s      r   r   z!connector_mesh.<locals>.<genexpr>&  s%     B0tq!AaDGcM0r  g      @r   r;   r      )
join_style
resolutionr   )rM   rW   rP   rJ   rO   
YOKE_ARM_Xr   rQ   r   boundsCONNECT_MODEr   r   rI   rN   r   r   r|   r  rL   CONNECTOR_ROUND_sboxbuffercreationextrude_polygonr   apply_transformr  r}   )rZ   r   holderr   r   armr   r   r   r   hwhcr   bdirarm_wdepthe1e2h1h2e1ue2ur>   polyconnMms                             r   connector_meshr:    s    <<(7D\\5VDFAVAYB
--ad+j8
9C
CGGAJ7V]]9SFFy ffb"51Cy0CCBy0BB		!	!&)A,	3B	B		!	!6!9Q<."	5a	8BGryy~~b2g..D$$S"-E54<$$&d23c9EAA11B#a');a)?B		r**B1C,CS#orCxc:;bS1WrcAgrAvrAv6==aAZ[=\//c:VVAY58#te|R5O22A2q5	2bqb!e9b!QiBQBER 2rD5L01RYY]]6!9567Kr   __main__r:   zwrote connector_z
.stl  vol=i  z.1fzcm3  watertight=ztwo_servo.urdfztwo_servo.glb)r   )N)2__doc__rR   numpyr   rM   xml.etree.ElementTreeetreer   r-   scipy.spatial.transformr   r   shapely.geometryr   r%  r{   r   rJ   r6   r~   rx   rB   r\   r   rH   r   rU   r$  r   r   YOKE_FACE_PADr#  r!  r   r   r   rP   r   r  r:  __name__ry   rz   rZ   r[   r   rV   r   volumeis_watertightr   r   r   <module>rF     sv   
   " " 3 ) #57IJ #57JK#57JK	 Y-N+	,c 	[%P	+ 	""^J & 0$  
&	!	>!H z1c&k"1	%%a() :ahhtmC-@@PQRQ`Q`Pabc # O r   