o
    si2F                     @   s   d Z ddlZddlmZ ddlZddlZddlZddlZddl	m
Z
 ddlmZ ddlmZmZ ddlmZ ddlmZmZmZmZ ddlm  mZ d	Zd
ZdZdd Zdd Z G dd de
Z!ej"g dej#dZ$ej"g dej#dZ%G dd de
Z&dS )u[  LIBERO-native dataset for PARA training.

Two dataset classes:
  RealTrajectoryDataset    — renders on-the-fly via OffScreenRenderEnv (slow, no multiprocessing)
  CachedTrajectoryDataset  — loads pre-rendered frames from disk (fast, supports num_workers>0)

Use prerender_dataset.py to generate the cache before using CachedTrajectoryDataset.
    N)Path)Dataset)Rotation)	benchmarkget_libero_path)OffScreenRenderEnv)get_camera_extrinsic_matrixget_camera_intrinsic_matrixget_camera_transform_matrix#project_points_from_world_to_camera   g            ?c                 C   s   t tttt| S N)maxMIN_GRIPPERminMAX_GRIPPERfloat)Zgripper_value r   !/data/cameron/para/libero/data.pyprocess_gripper_value#   s   r   c                 C   sT   t t j| t jdd}|| }|d dkrd S ||d d  }|d d |d  S )Ndtyper      r      )npappendasarrayfloat64)Zpoint_3dcamera_posecam_kpZp_camZpixr   r   r   project_3d_to_2d'   s   r"   c                   @   s4   e Zd Z							ddd	Zd
d Zdd ZdS )RealTrajectoryDatasetN  libero_spatialr   	agentview
   r   c                    s  || _ t| _|| _|| _t |  }||}	tj	
td||| _tj	| js4td| j t| jd; tdd  d  D }
|d urX|
d tdt| }
 fdd|
D | _ fd	d|
D | _|
| _W d    n1 syw   Y  tj	
td
|	j|	j}t||||gd| _| jd | j  d}| jjjj !|}|dkrt"| jjjj#j$| d nd| _%g | _&t'| jD ]\}}t(|j)d D ]
}| j&*||f qqt+dt,| j dt-| jj.  t+dt,| j& d d S )NZdatasetszLIBERO demo file not found: rc                 S   s   g | ]	}| d r|qS )Zdemo_)
startswith.0kr   r   r   
<listcomp>G   s    z2RealTrajectoryDataset.__init__.<locals>.<listcomp>data   c                        g | ]} d | d d qS )data/z/statesr   r   r*   fr   r   r-   J        c                    r0   )r1   z/actionsr   r   r*   r2   r   r   r-   K   r4   Z
bddl_files)Zbddl_file_nameZcamera_heightsZcamera_widthsZcamera_namesr   Zrobot0_baser   g        zLoaded z demos from zCreated z samples (one per state))/
image_sizeN_WINDOWn_windowcameraframe_strider   Zget_benchmark_dictZget_taskospathjoinr   Zget_task_demonstrationZ	demo_pathisfileFileNotFoundErrorh5pyZFilesortedkeysr   intdemo_statesdemo_actionsZ
demo_namesZproblem_folder	bddl_filer   envZseedresetsimZmodelZbody_name2idr   r.   Zxposbase_zsamples	enumeraterangeshaper   printlenr   name)selfZdataset_rootr5   benchmark_nametask_idr8   	max_demosr9   ZbenchZtaskdemosrE   Zbase_body_nameZbase_body_idZd_idxstatestr   r2   r   __init__1   sH   


("zRealTrajectoryDataset.__init__c                 C   
   t | jS r   rO   rJ   rQ   r   r   r   __len__e      
zRealTrajectoryDataset.__len__c           )      C   s  | j | \}}| j| }| j| }g }g }g }g }	g }
d }d }d }d }| jdkr.| j dnd}t| jD ]@}t||| j  |jd d }| j	
|| }| j	j	j  t||  }|jtjkru|tj}| dkru|d }tt|}|jd | jks|jd | jkrtj|| j| jftjd}|d u r|}tj|d	 tjd
}tj|d tjd
}t|t|t|d df }|| |	| |t|tt  |
|  |jd d \}}t!| j	j	j| j||}t"|#dd|||dd }t$|d t$|d }}|tj%tt|d|d tt|d|d gtjd
 |d u rvt&| j	j	j| jtj}t'| j	j	j| j||tj}| }|d  t|  < |d  t|  < |}t!| j	j	j| j||tj}q5tj|tjd
}tj|tjd
}tj|tjd
}tj|	tjd
}	tj(dd |	D ddtj}tj(|
ddtj}
g } t| jD ]@}|| \}!}"t$tt)t|!d| jd }#t$tt)t|"d| jd }$tj*| j| jftjd
}%d|%|$|#f< | |% qtj(| dd} t+,|-ddd }&t+j.g dt+jd
/ddd}'t+j.g dt+jd
/ddd}(|&|' |( }&|&t+,|  t+,| t+,| t+,| t+,|	 t+,| t+,|
 t+,| t+j.| j0t+jd
t+,|d  t+,| t+,| t+j.|t+j1d
t+j.|t+j1d
dS )NZrobot0_eye_in_handZ_imageZrobot0_eye_in_hand_imager   r/   r        o@ZinterpolationZrobot0_eef_posr   Zrobot0_eef_quatr   r   r   )ZpointsZworld_to_camera_transformZcamera_heightZcamera_widthc                 S      g | ]
}t |d qS ZxyzScipyRZ	from_quatZas_eulerr+   qr   r   r   r-          z5RealTrajectoryDataset.__getitem__.<locals>.<listcomp>Zaxisg
ףp=
?gv/?gCl?gZd;O?gy&1?g?)rgbheatmap_targettrajectory_2dtrajectory_3dtrajectory_grippertrajectory_quattrajectory_eulerrgb_frames_rawworld_to_camerarI   	target_3dr   
cam_K_normdemo_idxstart_t)2rJ   rC   rD   r8   rL   r7   r   r9   rM   rF   Zset_init_staterH   Zforwardr   r   copyr   float32astyper   ZascontiguousarrayZflipudr5   cv2resizeINTER_LINEARr   r   rO   r   clipr   r   r
   r   ZreshaperB   arrayr   r	   stackroundzerostorch
from_numpypermutetensorviewrI   long))rQ   idxrv   rw   rV   Zactionsrm   rn   ro   rp   rr   rgb_refZcam_pose_refZcam_k_norm_refZworld_to_camera_refZimg_keyr,   rW   Zobsrk   eef_poseef_quatZgriphwrs   Zpix_rcZv_rawZu_rawr    Z
cam_k_normrq   heatmap_targetsxyx_iy_ihmrgb_tmeanstdr   r   r   __getitem__h   s   

 





  z!RealTrajectoryDataset.__getitem__)Nr$   r%   r   r&   r'   r   )__name__
__module____qualname__rX   r\   r   r   r   r   r   r#   0   s    
4r#   rh   r   ri   c                   @   s6   e Zd ZdZdddeddfddZd	d
 Zdd ZdS )CachedTrajectoryDatasetu  Dataset that loads pre-rendered LIBERO frames from disk.

    Run libero/prerender_dataset.py first to generate the cache.

    Expected layout under cache_root:
        <benchmark>/task_<id>/demo_<idx>/
            frames/000000.png ...
            eef_pos.npy   eef_quat.npy   gripper.npy   pix_uv.npy
            cam_extrinsic.npy   cam_K_norm.npy   world_to_cam.npy   base_z.npy

    __getitem__ is pure I/O + numpy — safe for num_workers > 0.
    r%   Nr$   r   r   c                    s  || _ || _|| _t|| }| std| t|d}	 d ur. fdd|	D }	g | _g | _	i | _
t|dD ]}
t|
jdd }tj|
dd	| j
|< q>| j
rctd
t| j
 d i | _t|dD ]}t|jdd }|  | j|< qm| jrtdt| j d |	D ]}t|jdd }| j
|d }| j|d| }d}t|dD ]{}|dkr||kr np|d }| sqt|d}|sqt|}|t|d t|d t|d t|d t|d t|d t|d tt|d |||d}t| j}| j| t|D ]}| j	||f q$|d7 }qqtdt| j dt| j	 d d S )NzCache not found: ztask_*c                    s(   g | ]}t |jd d  v r|qS )_r/   )rB   rP   split)r+   dtask_idsr   r   r-     s   ( z4CachedTrajectoryDataset.__init__.<locals>.<listcomp>ztask_*_clip.ptr   r/   Zcpu)Zmap_locationz  Loaded CLIP embeddings for z tasksztask_*_description.txtz  Loaded task descriptions for ztask r   zdemo_*Zframesz*.pngzeef_pos.npyzeef_quat.npyzgripper.npyz
pix_uv.npyzcam_extrinsic.npyzcam_K_norm.npyzworld_to_cam.npyz
base_z.npy)frame_pathsr   r   gripperpix_uvcam_extrinsicru   world_to_camrI   Tclip_embeddingtask_descriptionzCachedTrajectoryDataset: z demos, z samples)r5   r7   r9   r   existsr>   r@   globrU   rJ   Zclip_embeddingsrB   stemr   r   loadrN   rO   Ztask_descriptions	read_textstriprP   getr   r   r   rL   )rQ   Z
cache_rootrR   r   r5   r7   r9   rT   Z
bench_rootZ	task_dirsZ	clip_pathtidZ	desc_pathZtask_dirrS   Zclip_embZ	task_descZtask_demo_countZdemo_dirZ
frames_dirr   r   demorv   rW   r   r   r   rX      st   


&z CachedTrajectoryDataset.__init__c                 C   rY   r   rZ   r[   r   r   r   r\   D  r]   zCachedTrajectoryDataset.__len__c              	   C   s  | j | \}}| j| }|d }g }g }g }g }	g }
d }t| jD ]}t||| j  |d }tt|d | }t	|tj
tjd }|jd | jksW|jd | jkrdtj|| j| jftjd}|d u rj|}|
| ||d | tj |	|d | tj |tt|d	 | tt ||d
 | tj q!tj|ddtj}tj|ddtj}tj|tjd}tj|	ddtj}	tjdd |	D ddtj}tj|
ddtj}
g }t| jD ]?}|| \}}tttt|d| jd }tttt|d| jd }tj| j| jftjd}d|||f< || qtj|dd}t|ddd }tj t!tjd"ddd}tj t#tjd"ddd}|| | }i d|dt| dt| dt| dt| dt|	 dt| dt|
 dt|d  dtj |d tjddt|d  dt|d   d!t|d!  d"tj |tj$dd#tj |tj$dd$|d$ d ur|d$ ntd%d&|d& S )'Nr   r/   r   r^   r   r_   r   r   r   r   rg   r   c                 S   r`   ra   rb   rd   r   r   r   r-   g  rf   z7CachedTrajectoryDataset.__getitem__.<locals>.<listcomp>r   r   r   rk   rl   rm   rn   ro   rp   rq   rr   rs   r   rI   rt   rj   r   r   ru   rv   rw   r   i   r   )%rJ   rU   rL   r7   r   r9   r{   ZimreadstrZcvtColorZCOLOR_BGR2RGBrz   r   ry   rM   r5   r|   r}   r   r   r   r~   r   r   r   r   rB   r   r   r   r   r   r   IMAGENET_MEANr   IMAGENET_STDr   )rQ   r   rv   rw   r   r   rm   rn   ro   rp   rr   r   r,   rW   Zbgrrk   rq   r   r   r   r   r   r   r   r   r   r   r   r   r   G  s   
 
 
  	
 z#CachedTrajectoryDataset.__getitem__)r   r   r   __doc__r6   rX   r\   r   r   r   r   r   r      s    
Pr   )'r   r:   pathlibr   r{   r?   Znumpyr   r   Ztorch.utils.datar   Zscipy.spatial.transformr   rc   Zlibero.liberor   r   Zlibero.libero.envsr   Zrobosuite.utils.camera_utilsr   r	   r
   r   Zrobosuite.utils.transform_utilsZutilsZtransform_utilsZT_robr6   r   r   r   r"   r#   r   ry   r   r   r   r   r   r   r   <module>   s.    		 3