
    HTj                         d Z ddlZddlmc mZ ddlZddlZddl	Z	ddl
mZmZmZmZ d ZddZd Zd Zd Zd	 Zdd
ZddZd Z G d d      Zy)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RX180c                     t        j                  d      }t        j                  |t        j                  | t
              j                                |S )N   )npzerosmujocomju_mat2Quatascontiguousarrayfloatravel)Rqs     2/home/cameronsmith/cad_recovery_mnt/render_core.pymat2quatr      s:    
A
2//59??ABH    c                    t        j                  | t              } t        j                  |t              | z
  }|t         j                  j	                  |      z  }t        j
                  |t        j                  |t                    }|t         j                  j	                  |      z  }t        j
                  ||      }| t        j                  ||| g      fS )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           	          t        j                  |       }t        j                  |j                         j	                  d      dddddd       | j                  dd	      }|j                  |       |S )
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extcamz0 0 1z1 0 0 0 1 045)nameposxyaxesfovyz.xmlz_arview.xml)ETparse
SubElementgetrootfindreplacewrite)
model_pathttmps      r   inject_extcamr5       sc     	AMM!))+"";/#G}VZ[]


V]
3CGGCLJr   c                     |dddf   | j                   |<   t        |ddddf   t        z        | j                  |<   || j                  |<   y)zJPoint the extcam at an OpenCV camera world pose with a given vertical FOV.N   )cam_posr   r   cam_quatcam_fovy)modelcam_id	T_camCV_wfovy_degs       r   set_camr?   +   sT    %bqb!e,EMM&%iBQB&7%&?@ENN6%ENN6r   c                    t        j                  | t         j                  j                  |      }t	        | j
                        D ]j  }| j                  |   |k(  st        j                  |j                  |         t        j                  |j                  |         j                  dd      fc S  t        d| d      )zJWorld (pos, 3x3 rotation) of the geom carrying the board texture material.r7   zno geom uses material '')r   
mj_name2idmjtObjmjOBJ_MATERIALrangengeom
geom_matidr	   array	geom_xpos	geom_xmatreshapeRuntimeError)r;   datamat_namemidgs        r   board_geom_poserQ   2   s    


E6==#?#?
JC5;; ZA#%88DNN1-.9J0K0S0STUWX0YYYZ 0
!<
==r   c                    dx}}d}|dt        j                  t        j                  |      dz        z  z  }	t        j                  |	d|dz  gd|	|dz  gg dg      }
d}dD ]F  }|dddf   |z  }||dz  z   |dddf   d	z  z   }t	        |||ddd
f         \  }}|| j
                  |<   t        |      | j                  |<   || j                  |<   t        j                  | |       t        |j                  |   |j                  |         }t        j                  | ||      5 }|j                  ||       |j!                         }ddd       t#        t%        j&                  |t$        j(                        |g      }|d   |v s*t+        g ||d      |
 \  }}||z  |fc S  |?t%        j,                  d|d    dt%        j&                  |t$        j.                               t1        d|d    d      # 1 sw Y   xY w)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@   r   r   r      N)g      ?g      g)\(?g{Gz?rU   )r   r$   r'   _measure_debug_z.pngzcould not detect 'z' board in the sim render)r	   tanradiansrH   r!   r8   r   r9   r:   r   
mj_forwardr   cam_xposcam_xmatRendererupdate_scenerenderr   cv2cvtColorCOLOR_RGB2GRAYr   imwriteCOLOR_RGB2BGRrL   )r;   rM   r<   
board_specr(   xmatWHr*   fKimgsgnnormalr   pR_mjr=   rendetsT_B_camerrs                         r   measure_board_worldrt   ;   s    LAD	Q

4(1,--.A
1aQ-!QA	:;A
C ,adc!FTM!DAJ$55S#$q!t*54 !f!)$v!%v%& v!6f8MN	__UAq) 	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   ($G??H	c           	          t        j                  | |dt         j                  ddd       t        j                  | |dt         j                  d|d       | S )N      g333333?)r   r   r   r7   rU   )r`   putTextFONT_HERSHEY_SIMPLEX)rk   textcolors      r   labelr}   X   sC    KKT7C$<$<c9aPKKT7C$<$<c5!LJr   c           	          t        j                  | |dft         j                        }|r)t        j                  ||dt        j
                  ddd       |S )z@A dark filler panel (keeps the grid aligned) with a dim caption.r7   rv   g?)n   r   r   rU   )r	   r
   uint8r`   ry   rz   )rh   rg   r{   rn   s       r   blankr   ^   sC    
!QBHH%AAtWc&>&>oWXYHr   c           	          |j                   dd \  }}|j                  | j                        }||t        ||| j                   d      S t        g || \  }}| j                  t        j                  j                  |      z  }	dt        j                  t        j                  |dz  |d   z              z  }
| j                  |	|
||      }t	        t        j                  |d|dd      | j                   d      S )	zUreal|sim 50% blend for a layer if its board is observed, else a 'not observed' panel.NrS   z: not observedg       @)rU   rU   g      ?r   z render)shapeget
board_namer   r}   r   	T_board_wr	   r   invdegreesarctanr_   r`   addWeighted)layerrq   rj   realrh   rg   drr   _r=   r*   sims               r   
blend_cellr   f   s    ::bq>DAq!!"AyAIQekk].9::"Q""JGQ"))--"88Irzz"))AGag$5677D
,,y$1
-CsCa8U[[M:QRRr   c                   .    e Zd ZdZd Zd Zd Zd Zd Zy)Layerz:One render source: a model localized from one ArUco board.c                    || _         || _        |d   | _        || _        t	        |      | _        t        j                  j                  | j
                        | _	        t        j                  | j                        | _        t        j                  | j                  t        j                  j                  d      | _        t        j                   | j                  | j                         t#        | j                  | j                  |      \  }}t%        | j                  | j                  | j                  |||      \  | _        | _        g | _        i | _        y )Nr'   r%   )r}   re   r   	map_jointr5   _tmpr   MjModelfrom_xml_pathr;   MjDatarM   rB   rC   mjOBJ_CAMERAr%   rZ   rQ   rt   r   reprojposing	renderers)selflabel_r2   re   rN   r   r(   rf   s           r   __init__zLayer.__init__u   s    
$$V,"!*-	^^11$))<
MM$**-	''

FMM4N4NPXY$**dii0#DJJ		8D	T&9JJ		4;;
C'G#r   c                 l   |j                         D ]  \  }}| j                  |d         }|t        j                  | j                  t        j
                  j                  |      }|dk\  s\| j                  j                  t        |      t        | j                  j                  |         f        y)zHcalib = {servo_id(str): {joint, sign, ...}}; map to this model's joints.jointNr   )itemsr   r   rB   r;   rC   mjOBJ_JOINTr   appendintjnt_qposadr)r   calibsidcjnjids         r   build_posingzLayer.build_posing   s    kkm 	QFC'
+Bz##DJJ0I0I2NCax""CHc$**2H2H2M.N#OP	Qr   c                 n    | j                   D ]&  \  }}||v s||   | j                  j                  |<   ( y N)r   rM   qpos)r   
servo_radsr   adrs       r   posez
Layer.pose   s7     	6HCj &0o		s#	6r   c                    t        | j                  | j                  ||       t        j                  | j                  | j
                         | j                  j                  ||f      }|2t        j                  | j                  ||      }|| j                  ||f<   |j                  | j
                  | j                         t        j                  |j                         t        j                        S )NrV   )r?   r;   r%   r   rZ   rM   r   r   r]   r^   r`   ra   r_   rd   )r   r=   r*   rh   rg   rs         r   r_   zLayer.render   s    

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

Aq1A%&DNNAq6"	tyy5||AHHJ(9(9::r   c                     | j                   j                         D ]  }|j                           t        j                  j                  | j                        r t        j                  | j                         y y r   )r   valuescloseospathexistsr   remove)r   r   s     r   r   zLayer.close   sP    &&( 	AGGI	77>>$))$IIdii  %r   N)	__name__
__module____qualname____doc__r   r   r   r_   r    r   r   r   r   s   s    D Q6
;!r   r   )rT   ))r      r   ) )r   r   xml.etree.ElementTreeetreeElementTreer+   numpyr	   r`   r   calib_utilsr   r   r   r   r   r!   r5   r?   rQ   rt   r}   r   r   r   r   r   r   <module>r      s]   5 
 " "  
  F F4&>[:
S/! /!r   