o
    2_i\                     @   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   8/data/cameron/567_augmentation_viewpoint_project/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 )N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                    r1   )r2   z/actionsr   r   r+   r3   r   r   r.   K   r5   Z
bddl_files)Zbddl_file_namecamera_heightscamera_widths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   get_benchmark_dictZget_taskospathjoinr   Zget_task_demonstration	demo_pathisfileFileNotFoundErrorh5pyFilesortedkeysr   intdemo_statesdemo_actionsZ
demo_namesZproblem_folder	bddl_filer   envseedresetsimmodelbody_name2idr   r/   xposbase_zsamples	enumeraterangeshaper   printlenr   name)selfZdataset_rootr9   benchmark_nametask_idr<   	max_demosr=   ZbenchtaskdemosrL   Zbase_body_nameZbase_body_idd_idxstatestr   r3   r   __init__1   sH   


("zRealTrajectoryDataset.__init__c                 C   
   t | jS r   rZ   rU   r\   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_hand_imageZrobot0_eye_in_hand_imager   r0   r        o@interpolationZrobot0_eef_posr   Zrobot0_eef_quat   r   r   )pointsworld_to_camera_transformcamera_heightcamera_widthc                 S      g | ]
}t |d qS xyzScipyR	from_quatZ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_camerarT   	target_3dr   
cam_K_normdemo_idxstart_t)2rU   rJ   rK   r<   rW   r;   r   r=   rX   rM   Zset_init_staterP   forwardr   r   copyr   float32astyper   ascontiguousarrayflipudr9   cv2resizeINTER_LINEARr   r   rZ   r   clipr   r   r
   r   reshaperI   arrayr   r	   stackroundzerostorch
from_numpypermutetensorviewrT   long))r\   idxr   r   rc   actionsr   r   r   r   r   rgb_refZcam_pose_refZcam_k_norm_refZworld_to_camera_refZimg_keyr-   rd   obsr   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__h   s   

 





  z!RealTrajectoryDataset.__getitem__)Nr$   r%   r   r&   r'   r   )__name__
__module____qualname__re   ri   r   r   r   r   r   r#   0   s    
4r#   r   r   r   c                   @   s8   e Zd ZdZddded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   Fc	              	      sn  || _ || _|| _|| _t|| }	|	 std|	 t|	d}
 d ur1 fdd|
D }
g | _	g | _
i | _t|	dD ]}t|jdd }tj|dd	| j|< qA| jrftd
t| j d i | _t|	dD ]}t|jdd }|  | j|< qp| 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*|rKt|dnd d+|rXt|d nd d,|ret|d- nd d.|rrt|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 )_r0   )rI   r[   split)r,   dtask_idsr   r   r.   
  s   ( z4CachedTrajectoryDataset.__init__.<locals>.<listcomp>ztask_*_clip.ptr   r0   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.npyrT   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)r9   r;   r=   augmentr   existsrD   rG   globra   rU   Zclip_embeddingsrI   stemr   r   loadrY   rZ   Ztask_descriptions	read_textstripr[   getr   r   r   rW   )r\   
cache_rootr]   r   r9   r;   r=   r_   r   Z
bench_rootZ	task_dirsZ	clip_pathtidZ	desc_pathZtask_dirr^   Zclip_embZ	task_descZtask_demo_countZdemo_dirZ
frames_dirr   r   Zwrist_frames_dirr   demor   rd   r   r   r   re      s   
	


&z CachedTrajectoryDataset.__init__c                 C   rf   r   rg   rh   r   r   r   ri   Q  rj   zCachedTrajectoryDataset.__len__c           5         s:  | j | \}}| j| }|d }g }g }g }g }	g }
d }| jrtjdd}tjdd}| j| j}}tddg|dg||gd|gg}t|| || d  || || d  g|||  || d  || || d  g|||  || d  |||  || d  g| | || d  |||  || d  gg}t	||}t
| jD ]}t||| j  |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	}| jrtj||||ftjd
}|d u r|}|
| ||d | tj |	|d | tj |tt|d | tt ||d | tj qt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}dd l }tj|j!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< ||  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}#|!|" |# }!|.dd}$|$r+t||d d }%tt|d |% }&t|&tjtjd }'|'jd | jks_|'jd | jkrltj|'| j| jftjd	}'t'(|')ddd }(|(|" |# }(ddl/m0}) |d |% }*g }+g },t
| jD ]k}t||| j  |d d }|d | tj}-|)|-1dd|*| j| jd }.tj|.d |.d gtjd}/|+|/ d}0|0|/d   ko| j|0 k n  o|0|/d   ko| j|0 k n  }1|,t|1 qtj|+dd}+tj|,tjd},|d |% tj}2|d tj}3|d |% tj}4n+t'2|!}(t2|}+tj&| jtjd},tj3dtjd}2tj3dtjd}3tj3dtjd}4i 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'(| d0t'j*|t'j4dd1t'j*|t'j4dd2|d2 d ur|d2 nt'&d3|d4 t'j*|$t'j5d|(t'(|+ t'(|2 t'(|3 t'(|4 t'(|, d5S )6Nr   g333333ÿg333333?r   r   r0   r   rl   rm   )Z
borderModer   r   r   r   r}   r   c                 S   rt   ru   rw   rz   r   r   r   r.     r|   z7CachedTrajectoryDataset.__getitem__.<locals>.<listcomp>c                    s"   g | ]}   t|  qS r   )invrx   ry   	as_rotvecrz   ref_rotr   r   r.     s    r   r   r   Fr   )r   r      r   r   r   r   r   r   r   r   r   r   r   r   r   rT   r   r   r   r   r   trajectory_delta_rotvecr   r   r   i   r   )r   r   	wrist_rgbwrist_trajectory_2dwrist_camera_poser   wrist_world_to_camerawrist_in_view)6rU   ra   r   r   randomuniformr9   r   r   getPerspectiveTransformrW   r;   r   r=   imreadstrcvtColorCOLOR_BGR2RGBr   rX   r   r   warpPerspectiveBORDER_REFLECT_101r   r   r   r   r   r   r   r   rQ   REF_ROTATION_QUATrx   ry   rI   r   r   r   r   r   r   IMAGENET_MEANr   IMAGENET_STDr   robosuite.utils.camera_utilsr   r   
zeros_likeeyer   bool)5r\   r   r   r   r   r   r   r   r   r   r   r   Z
h_strengthZ
v_strengthZH_imgZW_imgZsrc_ptsZdst_ptsZpersp_Mr-   rd   bgrr   r   Z_model_moduleref_quatr   r   r   r   r   r   r   r   r   r   r   Zwrist_tZ	wrist_bgrr   Zwrist_rgb_tZ_projZwrist_w2c_startwrist_traj_2dr   Zeef_3dr   uvmarginin_viewZwrist_cam_extrinsicr   r   r   r   r   r   T  sJ  
"
 

 

  $
&"

	
 z#CachedTrajectoryDataset.__getitem__)r   r   r   __doc__r:   re   ri   r   r   r   r   r   r      s    
]r   )'r   r?   pathlibr   r   rE   numpyr   r   torch.utils.datar   scipy.spatial.transformr   rx   libero.liberor   r   Zlibero.libero.envsr   r   r   r	   r
   r   robosuite.utils.transform_utilsutilstransform_utilsT_robr:   r   r   r   r"   r#   r   r   r   r   r   r   r   r   r   <module>   s.    		 3