o
    5Ti&X                     @   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 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W n eyZ   d
ZY nw 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_cameraTF   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_videopolicy/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_cam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 )NdatasetszLIBERO demo file not found: rc                 S   s   g | ]	}| d r|qS )Zdemo_)
startswith.0kr   r   r   
<listcomp>K   s    z2RealTrajectoryDataset.__init__.<locals>.<listcomp>data   c                        g | ]} d | d d qS )data/z/statesr   r   r,   fr   r   r/   N        c                    r2   )r3   z/actionsr   r   r,   r4   r   r   r/   O   r6   Z
bddl_files)Zbddl_file_nameZcamera_heightsZcamera_widthsZcamera_namesr   Zrobot0_baser           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   envseedresetsimmodelZbody_name2idr   r0   Zxposbase_zsamples	enumeraterangeshaper   printlenr   name)selfZdataset_rootr8   benchmark_nametask_idr;   	max_demosr<   ZbenchtaskdemosrH   Zbase_body_nameZbase_body_idd_idxstatestr   r4   r   __init__5   sH   


("zRealTrajectoryDataset.__init__c                 C   
   t | jS r   rT   rO   rV   r   r   r   __len__i      
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   r1   r        o@interpolationZrobot0_eef_posr   Zrobot0_eef_quat   r   r   )pointsZworld_to_camera_transformZcamera_heightZcamera_widthc                 S      g | ]
}t |d qS xyzScipyR	from_quatas_eulerr-   qr   r   r   r/          z5RealTrajectoryDataset.__getitem__.<locals>.<listcomp>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_camerarN   	target_3dr   
cam_K_normdemo_idxstart_t)2rO   rF   rG   r;   rQ   r:   r   r<   rR   rI   Zset_init_staterL   forwardr   r   copyr   float32astyper   ascontiguousarrayflipudr8   cv2resizeINTER_LINEARr   r   rT   r   clipr   r   r
   r   reshaperE   arrayr   r	   stackroundzerostorch
from_numpypermutetensorviewrN   long))rV   idxr   r   r]   actionsr{   r|   r}   r~   r   rgb_refZcam_pose_refZcam_k_norm_refZworld_to_camera_refZimg_keyr.   r^   Zobsry   eef_poseef_quatgriphwr   pix_rcZv_rawZu_rawr    Z
cam_k_normr   heatmap_targetsxyx_iy_ihmrgb_tmeanstdr   r   r   __getitem__l   s   

 





  z!RealTrajectoryDataset.__getitem__)Nr%   r&   r   r'   r(   r   )__name__
__module____qualname__r_   rc   r   r   r   r   r   r$   4   s    
4r$   rv   r   rw   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              	      sh  || _ || _|| _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 n|d }| sqt|d}|sqt|}|d }| o|d  }i d|dt|d dt|d dt|d dt|d dt|d d t|d! d"t|d# d$tt|d% d&|d'|d(|d)|d*|rHt|dnd d+|rUt|d nd d,|rbt|d- nd d.|rot|d/ nd d0|r|t|d1 nd i}t| j}| j| t|D ]}| j	||f q|d7 }qqtd2t| j d3t| j	 d4 d S )5NzCache not found: ztask_*c                    s(   g | ]}t |jd d  v r|qS )_r1   )rE   rU   split)r-   dtask_idsr   r   r/     s   ( z4CachedTrajectoryDataset.__init__.<locals>.<listcomp>ztask_*_clip.ptr   r1   cpu)map_locationz  Loaded CLIP embeddings for z tasksztask_*_description.txtz  Loaded task descriptions for ztask r   zdemo_*framesz*.pngZwrist_frameszwrist_pix_uv.npyframe_pathsr   zeef_pos.npyr   zeef_quat.npygripperzgripper.npypix_uvz
pix_uv.npycam_extrinsiczcam_extrinsic.npyr   zcam_K_norm.npyworld_to_camzworld_to_cam.npyrN   z
base_z.npyTclip_embeddingtask_description	has_wristwrist_frame_pathsZwrist_pix_uvwrist_extrinsicszwrist_extrinsics.npywrist_cam_K_normzwrist_cam_K_norm.npy	wrist_w2czwrist_w2c.npyzCachedTrajectoryDataset: z demos, z samples)r8   r:   r<   r   existsrA   rC   globr[   rO   Zclip_embeddingsrE   stemr   r   loadrS   rT   Ztask_descriptions	read_textstriprU   getr   r   r   rQ   )rV   
cache_rootrW   r   r8   r:   r<   rY   Z
bench_rootZ	task_dirsZ	clip_pathtidZ	desc_pathZtask_dirrX   Zclip_embZ	task_descZtask_demo_countZdemo_dirZ
frames_dirr   r   Zwrist_frames_dirr   demor   r^   r   r   r   r_      s   

	


&z CachedTrajectoryDataset.__init__c                 C   r`   r   ra   rb   r   r   r   rc   S  rd   zCachedTrajectoryDataset.__len__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g dtjd}t| tj f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< || qt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}|| | }|&dd}|rt||d d }tt|d | }t	|tj
tjd }|jd | jks|jd | jkrtj|| j| jftjd}t |!ddd } | | | } ddl'm(}! |d | }"g }#g }$t| jD ]k}t||| j  |d d }|d | tj}%|!|%)dd|"| j| jd }&tj|&d |&d gtjd}'|#|' d}(|(|'d   koD| j|( k n  oY|(|'d   koW| j|( k n  })|$t|) qtj|#dd}#tj|$tjd}$|d | tj}*|d tj}+|d | tj},n+t*|} t*|}#tj| jtjd}$tj+dtjd}*tj+dtjd}+tj+dtj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 | d.tj"|tj,dd/tj"|tj,dd0|d0 d urO|d0 ntd1|d2 tj"|tj-d| t |# t |* t |+ t |, t |$ d3S )4Nr   r1   r   re   r   rf   r   r   r   r   rt   r   c                 S   rj   rk   rm   rq   r   r   r   r/   v  rs   z7CachedTrajectoryDataset.__getitem__.<locals>.<listcomp>)r7   r7   r7   r   c                    s"   g | ]}   t|  qS r   )invrn   ro   	as_rotvecrq   Zref_rotr   r   r/   |  s    r   r   r   r   Fr   )r   r      r   r   r   ry   rz   r{   r|   r}   r~   r   r   r   r   rN   r   rx   r   r   r   trajectory_delta_rotvecr   r   r   i   r   )r   r   	wrist_rgbZwrist_trajectory_2dZwrist_camera_poser   Zwrist_world_to_camerawrist_in_view).rO   r[   rQ   r:   r   r<   r   imreadstrcvtColorCOLOR_BGR2RGBr   r   r   rR   r8   r   r   r   r   r   r   r   r   r   r   rn   ro   rE   r   r   r   r   r   r   IMAGENET_MEANr   IMAGENET_STDr   robosuite.utils.camera_utilsr   r   
zeros_likeeyer   bool)-rV   r   r   r   r   r   r{   r|   r}   r~   r   r   r.   r^   Zbgrry   r   Zref_quatr   r   r   r   r   r   r   r   r   r   r   Zwrist_tZ	wrist_bgrr   Zwrist_rgb_tZ_projZwrist_w2c_startZwrist_traj_2dr   Zeef_3dr   uvmarginZin_viewZwrist_cam_extrinsicr   r   r   r   r   r   V  s  
 
 

  $
&"

	
 z#CachedTrajectoryDataset.__getitem__)r   r   r   __doc__r9   r_   rc   r   r   r   r   r   r      s    
[r   ))r   r=   pathlibr   r   rB   numpyr   r   torch.utils.datar   scipy.spatial.transformr   rn   Zlibero.liberor   r   Zlibero.libero.envsr   r   r   r	   r
   r   Zrobosuite.utils.transform_utilsutilsZtransform_utilsZT_robZ
HAS_LIBEROImportErrorr9   r   r   r   r#   r$   r   r   r   r   r   r   r   r   r   <module>   s8    		 3