o
    id                     @   s  d Z ddlZddlZddlZddlmZ ddlmZ dZ	ee
jd Zefdeded	efd
dZded	ejfddZefdeded	efddZedkrddlZe Zejdedd e Zedej d eejZeded   eded   eded j  eded j  eded j  ed ed!   ed"ed#   ed$ed  d%d&ed   d%d' ed(ed d   dS dS ))z
Load a DROID episode from HuggingFace (cadene/droid_1.0.1 LeRobot format).
Downloads parquet data + video for a given episode, extracts frames,
and returns joint positions, camera extrinsics, and images.
    N)Path)hf_hub_downloadzcadene/droid_1.0.1cacheepisode_index	cache_dirreturnc                 C   s   |j ddd | d }d| d}d|d}ttd| d	| d
dt|d}ttd| d| ddt|d}ttd| d| ddt|d}|||dS )z?Download parquet + videos for one episode. Returns local paths.T)parentsexist_oki  Zepisode_Z06dzchunk-Z03dzdata//z.parquetZdataset)Z	repo_typer   zvideos/z$/observation.images.exterior_1_left/z.mp4z$/observation.images.exterior_2_left/)parquet
ext1_video
ext2_video)mkdirr   REPO_IDstr)r   r   chunkZep_strZ	chunk_strZparquet_pathZ	ext1_pathZ	ext2_path r   6/data/cameron/para/droid_testing/load_droid_episode.pydownload_episode   s4   r   
video_pathc                 C   sP   ddl }|| }g }|jddD ]}|jdd}|| q|  t|S )zFDecode all frames from an mp4 video. Returns (T, H, W, 3) uint8 array.r   N)videoZrgb24)format)avopendecodeZ
to_ndarrayappendclosenpstack)r   r   	containerframesframeimgr   r   r   decode_video_frames8   s   

r#   c                 C   sH  ddl }t| |}||d }t|d j}|d jtj}t|d j}tj|d j	d tjd}tj|d	 j	d tjd}	tj|d
 j	d tjd}
t
|d }t
|d }t|}|jd |kstJ d|jd  d| |jd |ksJ d|jd  d| |||||||	|
d|jv r|d j	d |d
S d|d
S )a  
    Load a full DROID episode: images, joint positions, camera extrinsics.

    Returns dict with:
        - ext1_images: (T, H, W, 3) uint8
        - ext2_images: (T, H, W, 3) uint8
        - joint_positions: (T, 7) float
        - gripper_positions: (T,) float
        - cartesian_positions: (T, 6) float
        - ext1_extrinsics: (6,) float [x,y,z,roll,pitch,yaw] camera-to-robot-base
        - ext2_extrinsics: (6,) float
        - wrist_extrinsics: (6,) float
    r   Nr   z observation.state.joint_positionz"observation.state.gripper_positionz$observation.state.cartesian_positionz!camera_extrinsics.exterior_1_left)dtypez!camera_extrinsics.exterior_2_leftzcamera_extrinsics.wrist_leftr   r   zext1 frames z != parquet rows zext2 frames language_instruction )
ext1_imagesext2_imagesjoint_positionsgripper_positionscartesian_positionsext1_extrinsicsext2_extrinsicswrist_extrinsicsr%   
num_frames)Zpandasr   Zread_parquetr   r   valuesastypefloat32arrayZilocr#   lenshapecolumns)r   r   pdpathsdfr)   r*   r+   r,   r-   r.   r'   r(   Tr   r   r   load_episodeE   s8   
((	r;   __main__z	--episode)typedefaultzLoading episode z...z  Instruction: r%   z
  Frames: r/   z  ext1 images: r'   z  ext2 images: r(   z  joint_positions: r)   z  ext1_extrinsics: r,   z  ext2_extrinsics: r-   z  Joint pos range: [z.3fz, ]z  First joint pos: )!__doc__osjsonnumpyr   pathlibr   Zhuggingface_hubr   r   __file__parentZ	CACHE_DIRintdictr   r   ndarrayr#   r;   __name__argparseArgumentParserparseradd_argument
parse_argsargsprintepisodedatar5   minmaxr   r   r   r   <module>   s8    '6
*