
    r;Kj0C                        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d(dZ ed       ed      gZd Z ed      ZdZd Zd Zd Zd)dZdZdZdZdZdZd)dZd Zd Z d  Z!d! Z"d" Z#d# Z$d$ Z%d% Z&e'd&k(  r= e(d' e)e            D ]*  Z* e&e*      Z+e+jY                  ej[                  e*             , yy)*u  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.

RECOVERY NOTE (2026-07-06): reconstructed after the phe108 outage. Most is verbatim from session context;
sections marked [RECONSTRUCTED] (gen_glb tail, read_layout, two constants) were rewritten from documented
behavior — diff against the lab original when it's back.
    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      4/home/cameronsmith/cad_recovery_mnt/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   4   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\   :   s.    
B
-C&! W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	W
 
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   I   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; 3}}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32 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   q   s}     E 	Ar /B%(1+4EOBLL&)..0!Q	
 <<##E**r   g      $@zconnector_{}.stlg      @g      Hclosestc           	         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
      j                         }g d|j$                  _        |j                  dt        z         |j                  |d| d| d|         |rt        j                  j                  |      rt        j"                  |      }	t        j                  j                  t        j(                  |	j*                  d   d   t,                    }
t/        |	j*                  j0                        D ]t  }|j3                  d      s|	j*                  |   \  }}|
t        j(                  |t,              z  }|j                  |	j4                  |   j                         ||d|	       v |j7                  |        t9        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   r   r   	base_cuberv   )rM   ScenerH   apply_scale	GLB_SCALErI   rJ   r   add_geometryr   r|   r}   rR   rS   rT   rU   rV   rW   r!   r   arraygraphrL   sortednodes_geometryr,   r$   exportr   )rS   src_glbscener   rZ   r   Trelr[   srcsrc_base_invnodegns                r   gen_glbr      s~   
 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CHHJA#6AHH MM%)+,qj,<*UVTWHX28  >>& 277>>'*ll7#yy}}RXXcii	.B1.Eu%MN399334 	ND{+		$2"RXXa%77""3<<#3#8#8#:dVZ4= # N		N 
LL	'4r   c                    t        j                  |       }g }|j                  j                  D ]  }|j	                  d      s|j                  d      d   }|j                  d      \  }}}	 t        |j                  d      d         }||rt        |      ndf}	t        j                  |j                  |   d   t              }
|
j                         }|d	d
d
fxx   t        z  cc<   |j                  |	|f        |j!                  d        |D cg c]  \  }}|	 }}}t        j"                  j%                  |d         }|D cg c]  }||z  	 c}S # t        t        f$ r Y ?w xY wc c}}w c c}w )a  [RECONSTRUCTED] Read Cameron's Blender GLB back into LAYOUT: resolve each servo_* node's WORLD pose
    through the parent chain, divide translations by GLB_SCALE (-> meters), order by index. Cameron's hacky
    Blender duplication makes nodes like servo_1.001 (poses live on the MESH nodes); sort key = (base_idx,
    sub_idx) with a BARE servo_N sorting LAST (his bare servo_1 was the end servo).servo/.r   r:   i@B r   Nr   c                     | d   S )Nr    )es    r   <lambda>zread_layout.<locals>.<lambda>   s
    qt r   key)rM   rW   r   r   r,   split	partitionint
IndexError
ValueErrorr   r   rL   r   r   rK   sortr|   r}   )rS   r   entriesr   r)   headr   tailbase_idx_keyWr   laybase_invs                 r   read_layoutr      sV   
 LLEG** "w'zz#r"s+a	4::c?1-.H t#d)7HHU[[&q)51FFH	"1"a%Iay!" LL^L$ 
!A1
!C
!yy}}SV$H"%&QHqL&& J' 		 "&s   -EE1E7E.-E.c           
         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   cornersks0s1centernormals                  r   
_box_facesr      s   EAh ga,1AGQ,,2#r"v7 
	gHC*/(3Qy3G3()IJ <8B!$
2$&qtHb1h#7#;
1Q4 $&qtHb1h#7#;
1Q4 < 498<ar!ur!u})<F<F2J$FF2JLL"((651288GU3KRXXV\^cMdef
	gg 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   r   z _closest_faces.<locals>.<lambda>   sP    dW-E-EvayQR|nVX-YZ[-\&\2A2rr6
VAYq\)&+ r   r   c              3   @   K   | ]  \  }\  }}}|d    dk  s|  yw)r         Nr   ).0r   ycycornyns        r   	<genexpr>z!_closest_faces.<locals>.<genexpr>   s%     L&AE2r!ut|aLs   )rM   rN   r   meanmaxry   nextrI   )yfacesr   Tyr   r   hbyar   s    ` `   @r   _closest_facesr      sr     ##RWWF-Cqad-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 981Irwws1a4y1Q4'7888I9 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 981Irwws1a4y1Q4'7888I9 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[   nur5   r?   s         r   ccwz_loft_faces.<locals>.ccw
  s    JJq% FF1IHHQqTAaD[!A$1+.		q!!qTAX!q11HHQN1MaRZZQ!a!eq[AMNOOMs   )D 
Nr   r   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 0UaK	q!QUmaQA.//0 	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>1  s"     Ctq!AaDGdNC   c              3   >   K   | ]  \  }}|d    d   dkD  s|  yw)r;   r   g      ?Nr   r!  s      r   r   z!connector_mesh.<locals>.<genexpr>2  s"     Btq!AaDGcMBr"  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:   )r   )N).__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)  r   r   r   r   r   rP   r  r  r@  __name__ry   rz   rZ   r[   r   rV   r   r   r   <module>rI     s+  
 
   " " 3 ) #57IJ #57JK#57JK	 Y-N+	,c 	[%P	+ 	" 
%P'6&	!	>!H z1c&k" *1	%%a()* r   