o
    wi2	                     @   s   d Z ddlZddlZddlmZ ddlmZmZmZ e	ej
Zd\ZZdeed d  d	  Zejd
dejdddZejeefeed eedZdZ	 G dd dZ&e& Z'dS )z=Large 8x8 ArUco board for pointcloud alignment configuration.    N)aruco   )BLENDER_STL_DIRSO100_MODEL_DIRBOARD_IMG_DIR)   r   g      #@
   d         )dtype)sizeZmarkerLengthZmarkerSeparation
dictionaryidsgRQ?)Image)   r   z+robot_models/board_imgs/alignment_board.pngg
F%u?)dpic                   @   sJ   e Zd ZdZdZeZeg dZ	eg dZ
eZedefddZdS )AlignmentBoardConfigz2Configuration for large 8x8 ArUco alignment board.ZAlignment_Board)r   r   r   returnc                 C   s   dt  dt dS )z0Get XML snippet to add alignment board to scene.zH
          <asset>
            <mesh name="alignment_aruco_plane" file="z/plane.obj" scale="0.3166666 0.3166666 .001" inertia="shell"/>
            <texture name="alignment_aruco_tex" type="2d" file="a  /alignment_board.png"/>
            <material name="alignment_aruco_mat" texture="alignment_aruco_tex" rgba="1 1 1 1"/>
          </asset>

          <worldbody>
            <body mocap="true" name="alignment_board" pos="0 -0.3 0.001" quat="1 0 0 0">
              <geom name="alignment_board_geom" type="mesh" mesh="alignment_aruco_plane" contype="0" conaffinity="0" material="alignment_aruco_mat"/>
            </body>
          </worldbody>
        )r   r   )cls r   R/data/cameron/para/panda_streaming/hand_eye_calib/../ExoConfigs/alignment_board.pyget_xml_addition%   s
   z%AlignmentBoardConfig.get_xml_additionN)__name__
__module____qualname____doc__namealignment_boardaruco_boardnparrayaruco_offset_posaruco_offset_rotboard_lengthclassmethodstrr   r   r   r   r   r      s    r   )(r   numpyr!   cv2r   exoskeletonr   r   r   getPredefinedDictionaryDICT_6X6_250
aruco_dictn_aruco_rown_aruco_colxarangeint32reshapeZboard_ids_alignment	GridBoardr   r%   PILr   generateImagemarkermarker_pathimwriteopenimgr   Zwidth_pxZ	height_pxZboard_length_inintroundr   saver   ALIGNMENT_BOARD_CONFIGr   r   r   r   <module>   s    
