a
    ijGA                     @   s   d Z ddlZddlmZmZmZ ddlZddlm  m	Z
 ddlm  mZ ddlm  mZ ddlm  mZ ddlZddlmZ ddlmZ ddlmZ ddlmZ ddlmZ G d	d
 d
ZdS )zUSD exporter.    N)ListOptionalUnion)Image)ImageOps)Sdf)Usd)UsdGeomc                   @   sz  e Zd ZdZd<ejeeeeeeee	e  ee e
d
d	d
Zedd Zedd Zdd Zdd Zd=ejeeef eej dddZd>ejeej dddZdd ZejdddZdd  Zd!d" Zd#d$ Zd%d& Zd?ejeej dd'd(Zd)eg d*d+d,fe	e  eee  eej! ee ee d-d.d/Z"d@e	e  e	e  ee d1d2d3Z#dAed5d6d7Z$d8d9 Z%d:d; Z&dS )BUSDExporterz@MuJoCo to USD exporter for porting scenes to external renderers.  '  mujoco_usdpkg./NT)
modelheightwidthmax_geomoutput_directoryoutput_directory_rootlight_intensitycamera_namesspecialized_materials_fileverbosec                 C   s   |j jj}|j jj}||kr6td| d| d ||krXtd| d| d || _|| _|| _|| _	|| _
|| _|| _|| _|	| _|
| _d| _d| _t | _i | _g | _g | _tj||d| _|   t | _|   |   d	S )
a  Initializes a new USD Exporter.

    Args:
        model: an MjModel instance.
        height: image height in pixels.
        width: image width in pixels.
        max_geom: optional integer specifying the maximum number of geoms that
          can be rendered in the same scene. If None this will be chosen
          automatically based on the estimated maximum number of renderable
          geoms in the model.
        output_directory: name of root directory to store outputted frames
          and assets generated by the USD renderer.
        output_directory_root: path to root directory storing generated frames
          and assets by the USD renderer.
        light_intensity: intensity of the light in the scene.
        camera_names: list of camera names to be used in the scene.
        specialized_materials_file: path to a file containing a list of
          materials to be used in the scene.
        verbose: decides whether to print updates.
    z
                Image width z > framebuffer width z. Either reduce the image
                width or specify a larger offscreen framebuffer in the model XML using the
                clause:
                <visual>
                <global offwidth="my_width"/>
                </visual>z
                Image height z > framebuffer height z. Either reduce the
                image height or specify a larger offscreen framebuffer in the model XML using
                the clause:
                <visual>
                <global offheight="my_height"/>
                </visual>r   )r   ZmaxgeomN)ZvisZglobal_ZoffwidthZ	offheight
ValueErrorlstripr   r   r   r   r   r   r   r   r   r   frame_countupdatesset
geom_names	geom_refs
usd_lightsusd_camerasmujocoZMjvScene_scene_initialize_usd_stage	MjvOption_scene_option_initialize_output_directories_load_textures)selfr   r   r   r   r   r   r   r   r   r   Zbuffer_widthZbuffer_height r*   S/home/ghrups/robot-bench/.venv/lib64/python3.9/site-packages/mujoco/usd/exporter.py__init__'   sF    "


zUSDExporter.__init__c                 C   s   | j   S )z!Returns the USD file as a string.)stageZGetRootLayerZExportToStringr)   r*   r*   r+   usd~   s    zUSDExporter.usdc                 C   s   | j S )zReturns the scene.)r#   r.   r*   r*   r+   scene   s    zUSDExporter.scenec                 C   s`   t j | _t| jtjj | jd | j	d tj
| jtd }| j| dS )z6Initializes a USD stage to represent the mujoco scene.r   g      N@z/WorldN)r   ZStageZCreateInMemoryr-   r	   ZSetStageUpAxisZTokenszZSetStartTimeCodeZSetTimeCodesPerSecondZXformZDefiner   PathZGetPrimZSetDefaultPrim)r)   Zdefault_primr*   r*   r+   r$      s    z!USDExporter._initialize_usd_stagec                 C   s   t j| j| j| _t j| js.t | j t j| jd| _t j| jsZt | j t j| jd| _	t j| j	st | j	 | j
rtd| j  dS )z:Initializes output directories to store frames and assets.framesZassetsz$Writing output frames and assets to N)ospathjoinr   r   Zoutput_directory_pathexistsmakedirsframes_directoryassets_directoryr   printr.   r*   r*   r+   r'      s    z*USDExporter._initialize_output_directories)datacamerascene_optionc              	   C   s   |}t |tr<t| jtjjj|}|dkr<td| d|dk sP|| jj	krjtd| d| jj	 dt
 }||_|dkrtjj|_t| j| n
tjj|_|p| j}t| j||d|tjjj| j dS )zUpdates the scene.r<   zThe camera 'z' does not exist.zThe camera id z is out of range [-1, z).N)
isinstancestrr"   Z
mj_name2idr   mjtObjZmjOBJ_CAMERAvaluer   ZncamZ	MjvCameraZ
fixedcamidZ	mjtCameraZmjCAMERA_FREEtypeZmjv_defaultFreeCameraZmjCAMERA_FIXEDr&   Zmjv_updateSceneZ	mjtCatBitZ	mjCAT_ALLr#   )r)   r=   r>   r?   Z	camera_idr*   r*   r+   _update_scene   s6    



zUSDExporter._update_scene)r=   r?   c                 C   sx   |  j d7  _ |p| j}| j||d | jdkrH|   |   |   |   |   | j	||d |  jd7  _dS )zUpdates the scene with latest sim data.

    Args:
        data: structure storing current simulation state
        scene_option: we use this to determine which geom groups to activate
       )r?   r   N)
r   r&   rE   r   r$   _load_lights_load_cameras_update_geoms_update_lights_update_cameras)r)   r=   r?   r*   r*   r+   update_scene   s    

zUSDExporter.update_scenec                 C   s   d}g | _ t| jjD ]}| jj| }| jj| }| jj| }|| | }t| jj	|||  
||d}t|}d| d}|tj| j| tj| j| j}	tj|	|}
| j |
 ||7 }q| jrtd| jj d| j  dS )zLoad textures.r      Ztexture_z.pngzCompleted writing z textures to N)texture_filesranger   ZntexZ
tex_heightZ	tex_widthZtex_nchannelimZ	fromarrayZtex_dataZreshaper   flipsaver4   r5   r6   r:   relpathr9   appendr   r;   )r)   Zdata_adrZ
texture_idZtexture_heightZtexture_widthZtexture_nchannelZpixelsZimgZtexture_file_namerelative_pathZimg_pathr*   r*   r+   r(      s:    

zUSDExporter._load_textures)geomc              	      s    |}| jvsJ |jdkr(g }n fdd jj|j D }|jtjjkr|t	j
 j j|| jj|j |j|d}n~|jtjjkrtj||jtg ddd}t	j| j j|||j|d}n2tj||j|jd	}t	j| j j|||j|d} j| | j|< d
S )z Loads a geom into the USD scene.r<   c                    s.   g | ]&}|d kr& j |  jj| fndqS )r<   N)rN   r   Ztex_type).0ir.   r*   r+   
<listcomp>  s   z*USDExporter._load_geom.<locals>.<listcomp>)r-   r   rV   obj_nameZdataidrgbageom_textures)      ?r]   r]   T)name	geom_typesizeZdecouple)mesh_configr-   r   rV   rZ   r[   r\   )r^   r_   r`   N)_get_geom_namer   Zmatidr   Z	mat_texidrD   r"   ZmjtGeomZmjGEOM_MESHobject_moduleZUSDMeshr-   Zgeom_dataidobjidr[   objtyperB   mjOBJ_TENDONshapes_moduleZmesh_config_generatornparrayZ	USDTendonr`   ZUSDPrimitiveMeshaddr   )r)   rV   	geom_namer\   Zusd_geomra   r*   r.   r+   
_load_geom  sb    



zUSDExporter._load_geomc                 C   s   t | jjD ]}| jj| }| |}|| jvr:| | |jtj	j
krz|j}| j| j|j|j||jd dk| jd q| j| j|j|j|jd dk| jd qdS )z8Iterate through all geoms in the scene and makes update.rM   r   )posmatscalevisibleframe)rm   rn   rp   rq   N)rO   r0   ngeomgeomsrb   r   rl   re   r"   rB   rf   r`   r   updaterm   rn   r[   r   )r)   rX   rV   rk   Ztendon_scaler*   r*   r+   rI   T  s(    




zUSDExporter._update_geomsc                 C   s^   t | jjD ]L}| jj| }t|jg dsL| jt	j
| jt|d q| jd  qd S )Nr   r   r   r-   rZ   )rO   r0   nlightlightsrh   allcloserm   r    rT   light_moduleUSDSphereLightr-   rA   r)   rX   Zlightr*   r*   r+   rG   o  s    zUSDExporter._load_lightsc                 C   st   t | jjD ]b}| jj| }t|jg dr0q|t| jks| j| d u rNq| j| j	|j| j
|j| jd qd S )Nru   rm   	intensitycolorrq   )rO   r0   rw   rx   rh   ry   rm   lenr    rt   r   Zdiffuser   r|   r*   r*   r+   rJ   z  s    
zUSDExporter._update_lightsc                 C   s2   | j d ur.| j D ]}| jtj| j|d qd S )Nrv   )r   r!   rT   camera_module	USDCamerar-   )r)   r^   r*   r*   r+   rH     s
    

zUSDExporter._load_camerasc                 C   s   t t| jD ]}| j| }| j| }| j|||d t| jjd | jjd }|j	}|j
}t||}	td}
|	|
dddf< ||
dddf< | |
dddf< |j|j|
| jd qdS )ztUpdates cameras.

    Args:
      data: An MjData instance.
      scene_option: An optional MjvOption instance.
    )r>   r?   r   rF   rM   N   Zcam_posZcam_matrq   )rO   r   r!   r   rE   r"   Zmjv_averageCamerar0   r>   forwarduprh   crosseyert   rm   r   )r)   r=   r?   rX   r>   Zcamera_nameZ
avg_camerar   r   rightrotationr*   r*   r+   rK     s*    



zUSDExporter._update_camerasr]   )333333?r   r   Zlight_1sphere)rm   r~   radiusr   rZ   
light_typec                 C   s^   |dkr4t j| j||d}|jt|||dd n&|dkrZt j| j|d}|j||d dS )	aQ  Adds a user defined, fixed light.

    Args:
        pos: position of the light in 3D space.
        intensity: intensity of the light.
        radius: radius of the light to be used by renderer.
        color: color of the light.
        obj_name: name associated with the light.
        light_type: type of light (sphere or dome).
    r   )r-   rZ   r   r   r}   Zdomerv   )r~   r   N)rz   r{   r-   rt   rh   ri   ZUSDDomeLight)r)   rm   r~   r   r   rZ   r   Z	new_lightr*   r*   r+   	add_light  s    zUSDExporter.add_lightcamera_1)rm   rotation_xyzrZ   c                 C   sX   t j| j|d}td}td}t||d t|| |jt	||dd dS )zAdds a user defined, fixed camera.

    Args:
        pos: position of the camera in 3D space.
        rotation_xyz: euler rotation of the camera.
        obj_name: name associated with the camera.
    rv   	      Zxyzr   r   N)
r   r   r-   rh   Zzerosr"   Zmju_euler2QuatZmju_quat2Matrt   ri   )r)   rm   r   rZ   Z
new_camerar   Zquatr*   r*   r+   
add_camera  s    

zUSDExporter.add_camerar/   )filetypec                 C   s   |dv sJ | j | j | j D ]\}}|d|jd  q$| j | j d| j	 dd| j d|   | j
rtd| j d|  dS )	zSaves the scene to a USD file.)r/   usdaZusdcFrF   /zframes/frame_.zCompleted writing frame_N)r-   ZSetEndTimeCoder   r   itemsZupdate_visibilityZlast_visible_frameZExportr   r   r   r;   )r)   r   _Zgeom_refr*   r*   r+   
save_scene  s    zUSDExporter.save_scenec                 C   sf   t | j|j|j}|sd}|d|j 7 }|jt jjkrD|d7 }n|jt jjkrb|d|j 7 }|S )z'Adding id as part of name for USD file.NoneZ_idZ_geomZ_tendon_segid)	r"   Z
mj_id2namer   re   rd   rB   Z
mjOBJ_GEOMrf   Zsegid)r)   rV   rk   r*   r*   r+   rb     s    
zUSDExporter._get_geom_namec                 C   s6   t | jjD ]$}| jj| }| |}t|| qd S )N)rO   r0   rr   rs   rb   r;   )r)   rX   rV   rk   r*   r*   r+   _print_scene_geom_info  s    
z"USDExporter._print_scene_geom_info)	r   r   r   r   r   r   NNT)r<   N)N)N)r   )r/   )'__name__
__module____qualname____doc__r"   ZMjModelintrA   r   r   boolr,   propertyr/   r0   r$   r'   ZMjDatar   r%   rE   rL   r(   ZMjvGeomrl   rI   rG   rJ   rH   rK   rh   ri   floatZndarrayr   r   r   rb   r   r*   r*   r*   r+   r
   $   s            
W

  
- %?	 )# r
   )r   r4   typingr   r   r   r"   Zmujoco.usd.camerar/   r>   r   Zmujoco.usd.lightsrx   rz   Zmujoco.usd.objectsobjectsrc   Zmujoco.usd.shapesZshapesrg   numpyrh   ZPILr   rP   r   Zpxrr   r   r	   r
   r*   r*   r*   r+   <module>   s   