+
    [Tj                         R t ^ RIt^ RIHu Ht ^ RIt^ RIt^ RI	t	^ RI
HtHtHtHt RtR]P                   ,          R,          tRR ltR tRR ltR	 tR
 tR tR tRR ltRR ltR t ! R R4      tR# )a&  Render/localization core for the AR view (no servo/hardware deps, so it can be
unit-tested against the sim alone). A Layer = one MuJoCo model + one ArUco board:
it measures the board's world frame by render loop-closure, poses itself from joint
angles, and renders from a localized camera pose.N)detect_boards
solve_posecam_mj_to_cvRX180i   g       @g      @c                 >    W\         ,
          ,          \        ,          # N)CENTER_TICKTICKS_TO_RAD)ticksigns   &&ar_view/render_core.pyticks_to_radr      s    +%&55    c                     \         P                  ! ^4      p\        P                  ! V\         P                  ! V \
        4      P                  4       4       V# )   )npzerosmujocomju_mat2Quatascontiguousarrayfloatravel)Rqs   & r   mat2quatr      s:    
A
2//59??ABHr   c                   \         P                  ! V \        4      p \         P                  ! V\        4      V ,
          pV\         P                  P	                  V4      ,          p\         P
                  ! V\         P                  ! V\        4      4      pV\         P                  P	                  V4      ,          p\         P
                  ! WC4      pV \         P                  ! WEV) .4      3# )zRRotation for a MuJoCo camera at `eye` looking at `target` (MJ cam views along -Z).)r   asarrayr   linalgnormcrosscolumn_stack)eyetargetupfwdrightup2s   &&&   r   
look_at_mjr'      s    
**S%
 C
**VU
#c
)C299>>#CHHS"**R/0E	RYY^^E""E
((5
CcT 2333r   c                    \         P                  ! V 4      p\         P                  ! VP                  4       P	                  R4      RRRRRRRR	R
/4       V P                  RR4      pVP                  V4       V# )zCopy the model with an extra 'extcam' worldbody camera we repose per frame,
written next to the original so its meshes/textures still resolve.	worldbodycameranameextcamposz0 0 1xyaxesz1 0 0 0 1 0fovy45z.xmlz_arview.xml)ETparse
SubElementgetrootfindreplacewrite)
model_pathttmps   &  r   inject_extcamr;   '   sk     	AMM!))+"";/8UGX}fVZ[]


V]
3CGGCLJr   c                    VR,          V P                   V&   \        VR,          \        ,          4      V P                  V&   W0P                  V&   R# )zJPoint the extcam at an OpenCV camera world pose with a given vertical FOV.N)N   Nr>   )r=   r=   )cam_posr   r   cam_quatcam_fovy)modelcam_id	T_camCV_wfovy_degs   &&&&r   set_camrF   2   s>    %e,EMM&%i&7%&?@ENN6%NN6r   c                   \         P                  ! V \         P                  P                  V4      p\	        V P
                  4       F{  pV P                  V,          V8X  g   K  \        P                  ! VP                  V,          4      \        P                  ! VP                  V,          4      P                  ^^4      3u # 	  \        RV R24      h)zJWorld (pos, 3x3 rotation) of the geom carrying the board texture material.zno geom uses material '')r   
mj_name2idmjtObjmjOBJ_MATERIALrangengeom
geom_matidr   array	geom_xpos	geom_xmatreshapeRuntimeError)rB   datamat_namemidgs   &&&  r   board_geom_poserX   9   s    


E6==#?#?
JC5;;A#%88DNN1-.9J0K0S0STUWX0YYY   0
!<
==r   c                   R;rgRpV^\         P                  ! \         P                  ! V4      ^,          4      ,          ,          p	\         P                  ! V	^ V^,          .^ W^,          .. RO.4      p
RpR EF{  pVR,          V,          pWMR,          ,           VR,          R,          ,           p\	        WVR,          R7      w  ppWP
                  V&   \        V4      V P                  V&   WP                  V&   \        P                  ! W4       \        VP                  V,          VP                  V,          4      p\        P                  ! WV4      ;_uu_ 4       pVP                  WR7       VP!                  4       pRRR4       \#        \$        P&                  ! V\$        P(                  4      V.4      pVR,          V9   g   EKR  \+        . VVR,          ,          OV
N5!  w  ppVV,          V3u # 	  VeF   \$        P,                  ! R	VR,           R
2\$        P&                  ! V\$        P.                  4      4       \1        RVR,           R24      h  + '       g   i     L; i)zLoop-closure: look at the board along its normal, detect it, recover its GridBoard
frame in the sim world. Tries both normal directions (the textured face may point either way).i  g      I@Ng)\(?g{Gz?)r#   r*   r+   _measure_debug_z.pngzcould not detect 'z' board in the sim render    r]      )g      ?g      )NNN   )r_   r]   )r_   r^   )r   tanradiansrO   r'   r?   r   r@   rA   r   
mj_forwardr   cam_xposcam_xmatRendererupdate_scenerenderr   cv2cvtColorCOLOR_RGB2GRAYr   imwriteCOLOR_RGB2BGRrS   )rB   rT   rC   
board_specr-   xmatWHr/   fKimgsgnnormalr!   pR_mjrD   rendetsT_B_camerrs   &&&&&&                r   measure_board_worldr}   B   s    LAD	Q

4(1,--.A
1aQ-!QA	:;A
Cdc!TM!DJ$55S$t*54 !f!)$v!%v%& v!6f8MN	__Uq))ST1**,C * S\\#s/A/ABZLQf%%CtJv,>'?CCLGSw&++   oj&8%9>SRURcRc@de
+Jv,>+??XY
ZZ *)s   #II&c           	          \         P                  ! WR\         P                  RR^4       \         P                  ! WR\         P                  RV^4       V # )   g333333?r      )r]   r]   r]   )ri   putTextFONT_HERSHEY_SIMPLEX)rt   textcolors   &&&r   labelr   _   s?    KK7C$<$<c9aPKK7C$<$<c5!LJr   c           	         \         P                  ! W^3\         P                  4      pV'       d*   \        P                  ! W2R\        P
                  RR^4       V# )z@A dark filler panel (keeps the grid aligned) with a dim caption.g?r   )n   r   r   )r   r   uint8ri   r   r   )rq   rp   r   rw   s   &&& r   blankr   e   s?    
!BHH%AAWc&>&>oWXYHr   c           	        VP                   R,          w  rVVP                  V P                  4      pVe   Vf   \        WVV P                   R24      # \        . VOVN5!  w  rV P                  \        P                  P                  V4      ,          p
V P                  WWV4      p\	        \        P                  ! VRVR^ 4      V P                   R24      # )zreal|sim 50% blend for a layer if its board is observed, else a 'not observed' panel.
`dets`/`K` are at the DETECTION resolution (full frame); `real` is the (smaller) display
image and `fovy` the resolution-independent vertical FOV to render it at.:Nr`   Nz: not observedg      ?z render)shapeget
board_namer   r   r   	T_board_wr   r   invrh   ri   addWeighted)layerrz   rs   realr/   rq   rp   dr{   _rD   sims   &&&&&       r   
blend_cellr   m   s     ::b>DA!!"AyAIQekk].9::"Q""JG"))--"88I
,,y
-CsCa8U[[M:QRRr   c                   B   a  ] tR t^{t o RtR tR tR tR tR t	Rt
V tR# )	Layerz:One render source: a model localized from one ArUco board.c                   Wn         W0n        VR ,          V n        WPn        \	        V4      V n        \        P                  P                  V P
                  4      V n	        \        P                  ! V P                  4      V n        \        P                  ! V P                  \        P                  P                  R4      V n        \        P                   ! V P                  V P                  4       \#        V P                  V P                  V4      w  rg\%        V P                  V P                  V P                  W6V4      w  V n        V n        . V n        / V n        R# )r+   r,   N)r   rn   r   	map_jointr;   _tmpr   MjModelfrom_xml_pathrB   MjDatarT   rI   rJ   mjOBJ_CAMERAr,   rc   rX   r}   r   reprojposing	renderers)selflabel_r8   rn   rU   r   r-   ro   s   &&&&&&  r   __init__Layer.__init__}   s    
$$V,"!*-	^^11$))<
MM$**-	''

FMM4N4NPXY$**dii0#DJJ		8D	&9JJ		4;;
'G#r   c                   VP                  4        F  w  r#V P                  VR,          4      pVf   K#  \        P                  ! V P                  \        P
                  P                  V4      pV^ 8  g   Kf  V P                  P                  \        V4      \        V P                  P                  V,          4      \        VP                  R^4      4      34       K  	  R# )zcalib = {servo_id(str): {joint, sign, ...}}; map to this model's joints,
keeping each servo's own sign so a shared id can't cross-contaminate layers.jointNr   )itemsr   r   rI   rB   rJ   mjOBJ_JOINTr   appendintjnt_qposadrr   )r   calibsidcjnjids   &&    r   build_posingLayer.build_posing   s     kkmFC'
+Bz##DJJ0I0I2NCax""CHc$**2H2H2M.NPSTUTYTYZ`bcTdPe#fg $r   c                    V P                    F?  w  r#pVP                  V4      pVf   K  \        WT4      V P                  P                  V&   KA  	  R# )zGticks = {servo_id: raw_tick}; sets each mapped joint from its own sign.N)r   r   r   rT   qpos)r   ticksr   adrru   r9   s   &&    r   pose
Layer.pose   s=    ![[MCc		#A}&21&:		s# )r   c                   \        V P                  V P                  W4       \        P                  ! V P                  V P
                  4       V P                  P                  W434      pVf1   \        P                  ! V P                  W44      pWPP                  W43&   VP                  V P
                  V P                  R7       \        P                  ! VP                  4       \        P                  4      # )NrZ   )rF   rB   r,   r   rc   rT   r   r   rf   rg   ri   rj   rh   rm   )r   rD   r/   rq   rp   rs   &&&&& r   rh   Layer.render   s    

DKK9$**dii0NNv&9

A1A%&NNA6"	tyy5||AHHJ(9(9::r   c                
   V P                   P                  4        F  pVP                  4        K  	  \        P                  P                  V P                  4      '       d#   \        P                  ! V P                  4       R # R # r   )r   valuescloseospathexistsr   remove)r   r   s   & r   r   Layer.close   sN    &&(AGGI )77>>$))$$IIdii  %r   )r   r   r   rn   rT   r,   r   r   rB   r   r   r   N)__name__
__module____qualname____firstlineno____doc__r   r   r   rh   r   __static_attributes____classdictcell__)__classdict__s   @r   r   r   {   s'     D 	h;;! !r   r   )r^   )r\   ))r]      r]   ) )r   r   xml.etree.ElementTreeetreeElementTreer1   numpyr   ri   r   calib_utilsr   r   r   r   r   pir	   r   r   r'   r;   rF   rX   r}   r   r   r   r    r   r   <module>r      sy   5 
 " "  
  F FRUU{V#64&>[:S2! 2!r   