o vi!@sdZddlZddlZddlZddlZddlZddlZddlm Z ddl m Z ddl m Z ddl mZmZmZmZmZmZddlmZmZmZmZmZmZmZddlmZejd ejd Gd d d e Z e!d krhe dJiddddddddddddddddddddddddd d!d"d!d#dd$d%Z"ejd&d'd(id)dZ#e"e#Z$e%d*e$d+j&e$d+j'fe%d,e$d-j&e$d-j'fe%d.e$d-e%d/e$d0j&e$d0j'fe%d1e$d0ej(d2dd3e)d4D]aZ*e+de,e"d5Z-e"e-Z$e$d+.d5d6d dZ/e)e/j&dD]=Z0e/e0Z1d7e-d8e$d9dd:d;e$d<d=e$d>d?e$d@dAe$dBdCdDe0dEZ2e 3e14e2e%dFe2qqe%dGe%dHe%dIdSdS)Kz RoboCasa simulation benchmark dataloader. Run this command to print a few samples from the RoboCasa dataset: python -m cosmos_policy.datasets.robocasa_dataset N)Image)Dataset)tqdm) build_rollout_step_index_mappingcalculate_epoch_structurecompute_monte_carlo_returnsdetermine_sample_type"load_or_compute_dataset_statistics-load_or_compute_post_normalization_statistics)calculate_dataset_statisticsdecode_jpeg_bytes_datasetdecode_single_jpeg_frameget_hdf5_filespreprocess_image rescale_datarescale_episode_data)duplicate_array) precision linewidthc'@seZdZ                     d-d ed ed ed ed ededededededededededededededef&ddZddZd d!Z d"d#Z d.d%d&Z $d.d'd(Z d)d*Z d+d,Zd$S)/RoboCasaDatasetFT?Gz?data_dir chunk_sizefinal_image_sizet5_text_embeddings_path use_image_auguse_stronger_image_auguse_wrist_imagesuse_third_person_images use_proprionum_duplicates_per_imagerollout_data_dirdemonstration_sampling_probsuccess_rollout_sampling_prob p_world_modeltreat_success_rollouts_as_demosreturn_value_function_returnsgammalazy_load_demos!skip_computing_dataset_statisticsc- Cs||_||_||_||_||_||_||_||_| |_| |_ | |_ | |_ | |_ ||_ ||_||_||_||_||_||_||_|j sI|j sIJdt|}tjdddkr^|dd}g}|j rxtj|j ssJd|j dt|j }i|_i|_i|_d |_d |_ d |_!d |_"t#|_$d |_%|jd krt&|D],}t'(|d }t)|d *}t+|d dd}t&|D]}|d|d}d|v}|d|j,d} |j$-| |rt.|d}!nt.|d}!|jrt/|!d|jd}"nd}"|jrt0||| |!||"dur |"1ndd|j|j<nd|vr"|ddd}#nd|vr.t2|d}#nt3dd|vr@|ddd}$nd|vrLt2|d}$nt3dd|vr^|ddd}%nd|vrjt2|d}%nt3d|d|dddd|j%f4t5j6}&|d|d dd4t5j6}'t0|#|$|%|'|&| |!|"dur|"1ndd!|j|j<|jd7_|j |!7_ qWdn 1swYq|7||_|d"krt8|d#}t9:||_;Wdn 1swY|s|jr tjtj<|jd$s t=d%t>|j|jt?d&|_@|js|jrI|sI|js?|jr2tA|j|j@d'|_|jr?tA|j|j@d(|_tB|j|jt?d&|_Ct.|d krt&|d)d*D]}t'(|d }d+|vrmd,}t.|d+}!nd-|vr{d.}t.|d-}!nt3d/||j,d} |j$-| tD|j,d0d,}(t0|| |!|(|d1|j|j!<|jrtD|j,d0}(|(rdnd2})t/|!|)|jd}"|"1|j|j!d3<|j!d7_!|j"|!7_"Wdn 1swYqV|jr|jED]\}*}+|+d0d,sq|jr+|+d3}"|"dur|"1}"t0|+d4d|+d5|+d6|+d7|"d.d8|j|j<n{|F|+},|,d7d,rnt5jGd9d:|,d;Dd d<4t5jH}#t5jGd=d:|,d>Dd d<4t5jH}$t5jGd?d:|,d@Dd d<4t5jH}%n |,d;}#|,d>}$|,d@}%|,d'}&|,d(}'|+d3}"|"dur|"1}"t0|#|$|%|'|&|+d5|+d6|"d!|j|j<|j$-|+d5|jd7_|j |+d67_ q|7|ItJdAt.|j$|KdS)Ba Initialize RoboCasa dataset for training. Args: data_dir (str): Path to directory containing RoboCasa dataset HDF5 files chunk_size (int): Action chunk size final_image_size (int): Target size for resized images (square), defaults to 224 t5_text_embeddings_path (str): Path to precomputed T5 text embeddings dictionary (key: instruction, val: embedding) num_images_per_sample (int): Number of images to return per sample normalize_images (bool): Whether to normalize the images and return as torch.float32 normalize_actions (bool): Whether to normalize the actions normalize_proprio (bool): Whether to normalize the proprioceptive state use_image_aug (bool): Whether to apply image augmentations use_stronger_image_aug (bool): Whether to apply stronger image augmentations use_wrist_images (bool): If True, loads wrist-mounted camera images use_third_person_images (bool): If True, loads third-person images use_proprio (bool): If True, adds proprio to image observations num_duplicates_per_image (int): Number of times to duplicate each image (so that each type of image fills 1 latent frame when encoded with the tokenizer) rollout_data_dir (str): Path to directory containing rollout data (if provided, will load rollout data in addition to base dataset) demonstration_sampling_prob (float): Probability of sampling from demonstration data instead of rollout data success_rollout_sampling_prob (float): Probability of sampling from success rollout data instead of failure rollout data p_world_model (float): Probability of sampling a world model sample instead of a value function sample treat_success_rollouts_as_demos (bool): If True, copy successful rollout episodes into demonstration dataset (self.data) return_value_function_returns (bool): If True, returns value function returns for rollout episodes gamma (float): Discount factor for value function returns lazy_load_demos (bool): If True, only load demo metadata at initialization and load full data on-demand during __getitem__ z=Must use at least one of wrist images or third-person images! DEBUGGINGFalsetrueNzError: Rollout data directory 'z' does not exist.rrdatacSst|ddS)N_r3)intsplitxr<M/data/cameron/vidgen/cosmos-policy/cosmos_policy/datasets/robocasa_dataset.pysz*RoboCasaDataset.__init__..)keydata//obsrobot0_agentview_left_rgb_jpegtask_descriptionrobot0_agentview_left_rgbg?)terminal_rewardr-) file_pathdemo_keycommand num_stepsis_jpegreturnsz\Neither 'robot0_agentview_left_rgb' nor 'robot0_agentview_left_rgb_jpeg' found in HDF5 file.robot0_agentview_right_rgbrobot0_agentview_right_rgb_jpegz^Neither 'robot0_agentview_right_rgb' nor 'robot0_agentview_right_rgb_jpeg' found in HDF5 file.robot0_eye_in_hand_rgbrobot0_eye_in_hand_rgb_jpegzVNeither 'robot0_eye_in_hand_rgb' nor 'robot0_eye_in_hand_rgb_jpeg' found in HDF5 file./actions /robot_states)left_primary_imagesright_primary_images wrist_imagesproprioactionsrHrIrKrrbzdataset_statistics.jsonzDataset statistics file for this dataset does not yet exist. Please rerun with RoboCasaDataset(lazy_load_demos=False) once so that the dataset statistics are computed and saved. Then you can rerun with RoboCasaDataset(lazy_load_demos=True).)rr6!calculate_dataset_statistics_funcrVrUzLoading rollout metadata)descprimary_imagesFprimary_images_jpegTz)No primary images found in rollout file: success)rFrHrIr\rJgrKrFrHrIrJ)rFrGrHrIrJrKis_from_rolloutcSg|]}t|qSr<r .0br<r<r= z,RoboCasaDataset.__init__..rRaxiscSr^r<r_r`r<r<r=rcrdrScSr^r<r_r`r<r<r=rcrdrTz!Number of unique commands found: )Lrrrr normalize_imagesnormalize_actionsnormalize_proprior!r"r#r$r%r&r'r(r)r*r+r,r-r.rosenvirongetlowerpathexistsr6demo_episode_metadatarollout_episode_metadata num_episodesrIrollout_num_episodesrollout_num_stepssetunique_commands action_dimrh5pyFilelistkeyssortedattrsaddlenrdictcopyr KeyErrorastypenpfloat32_build_step_index_mappingopenpickleloadt5_text_embeddingsjoin ValueErrorr r dataset_statsrr dataset_stats_post_normboolitems_load_rollout_episode_datastackuint8!_build_rollout_step_index_mappingprint_calculate_epoch_structure)-selfrrrr rgrhrir!r"r#r$r%r&r'r(r)r*r+r,r-r.r/ hdf5_filesrollout_hdf5_filesfilefdemo_keys_listsorted_demo_keysrG obs_grouprJrHrIrKrRrSrTrVrUr\rEep_idxep_meta episode_datar<r<r=__init__9s4             " a  "     '       zRoboCasaDataset.__init__cCst|dsd|_t|dsd|_t|ds|j|j|_|j}t||j|j|j|jd}|d|_|d|_ |d|_ |d |_ d S) zVCalculate epoch layout with proper scaling: demos, success rollouts, failure rollouts._rollout_success_total_stepsr_rollout_failure_total_steps_rollout_total_steps)rIrollout_success_total_stepsrollout_failure_total_stepsr(r)adjusted_demo_countadjusted_success_rollout_countadjusted_failure_rollout_count epoch_lengthN) hasattrrrrrIrr(r)rrrr)rdemo_base_countresultr<r<r=rs$      z*RoboCasaDataset._calculate_epoch_structurecCsi|_d|_|jr/|jD]\}}|d}t|D]}||f|j|j<|jd7_qqdS|jD]\}}|d}t|D]}||f|j|j<|jd7_q@q4dS)zYBuild a mapping from global step index to (episode index, relative index within episode).rrIr3N)_step_to_episode_map _total_stepsr.rprranger6)r episode_idxepisode_metadatarIirr<r<r=rs"  z)RoboCasaDataset._build_step_index_mappingcCsBti|j}|d|_|d|_|d|_|d|_|d|_dS)zYBuild mapping for rollout dataset with separate tracking for successful/failure episodes.$_rollout_success_step_to_episode_map$_rollout_failure_step_to_episode_maprrrN)rrqrrrrr)rrr<r<r=rs     z1RoboCasaDataset._build_rollout_step_index_mappingNc Cs|ddr|||||S|d}|d}t|dZ}|d|d}|du} |d rs| rCt|d } t|d } t|d } nttt|} i} i} i} | D] }t|d || |<t|d || |<t|d || |<qQnD| r|d dd} |ddd} |ddd} n)tt|} i} i} i} | D]}|d || |<|d|| |<|d|| |<q|dur|dur|d|d||d|jf t j }n|d|dddd|jf t j }|dur| stt|} i}| D]}|d|d| t j ||<qn|d|ddd t j }|j r-t d|i|jd}|jr\t|trS|D]}||dd}t d|i|jd||<q9n t d|i|jd}t| | | |||d|d|d d}|WdS1s{wYdS)a Load demo episode data from HDF5 file using metadata. Optimized to only load required frames and action chunks. Args: episode_metadata (dict): Episode metadata containing file_path, demo_key, etc. frame_indices (set or None): Set of frame indices to load. If None, loads all frames. action_start_idx (int or None): Start index for action chunk. If None, loads all actions. action_end_idx (int or None): End index for action chunk (exclusive). If None, loads all actions. Returns: dict: Episode data dictionary with loaded arrays (only requested frames/actions) r]FrFrGr5r@rANrJrBrMrOrDrLrNrPrQrVr3rUrHrI)rRrSrTrUrVrHrIrJ)rlrrxryr r|rzr rwrrrrhrrri isinstancerreshapeflatten)rr frame_indicesaction_start_idxaction_end_idxrFrGrrload_allrRrSrTframe_indices_listidxrVrU proprio_arrayrr<r<r=_load_demo_episode_datas        "* "     &z'RoboCasaDataset._load_demo_episode_datac Cs|d}t|dC}|du}|drY|r/|ddd}|ddd} |ddd} nntt|} i}i} i} | D]} |d| || <|d| | | <|d| | | <q=nD|rt|ddd}|d dd} |d dd} n)tt|} i}i} i} | D]} |d| || <|d | | | <|d | | | <q|dur|dur|d ||d|jftj} n|d ddd|jftj} |dur|stt|} i}| D]} |d | tj|| <qn |d ddtj}|jrt d | i|j d } |j r0t |t r'|D]} || d d}t d |i|j d || <q n t d |i|j d }t || | || |d|d|d|dd }|WdS1sRwYdS)a Load rollout episode data from HDF5 file using metadata. Optimized to only load required frames and action chunks. Args: episode_metadata (dict): Episode metadata containing file_path, success, etc. frame_indices (set or None): Set of frame indices to load. If None, loads all frames. action_start_idx (int or None): Start index for action chunk. If None, loads all actions. action_end_idx (int or None): End index for action chunk (exclusive). If None, loads all actions. Returns: dict: Episode data dictionary with loaded arrays (only requested frames/actions) rFr5NrJr[Zsecondary_images_jpegwrist_images_jpegrZZsecondary_imagesrTrVrUr3rrHrIr\) rRrSrTrUrVrHrIr\rJ)rxryr|rzrwrrrrhrrrirrrr)rrrrrrFrrrRrSrTrrrVrUrrr<r<r=rWs  $"      &z*RoboCasaDataset._load_rollout_episode_datacCs|jS)z3Returns the total number of samples in the dataset.)r)rr<r<r=__len__szRoboCasaDataset.__len__c; Cs t||j|j}|dkrdnd}|dkrdnd}|dkr^||j}|j|\}}d}|jrV|j|}t||j|dd} || h} t||j|d} |j || || d} n|j |} d} nz|dkr||j}||j } |j | \}}|j |}t||j|dd} || h} t||j|d} |j|| || d} n<||j|j}||j} |j| \}}|j |}t||j|dd} || h} t||j|d} |j|| || d} d }d }|dkr|jrt|jkrd }d }n d }d }nd }d }||j}| dd}||kr |}i}i}i}||h}|D]}t| d tra| d d rGt| d |||<t| d |||<t| d|||<q| d |||<| d |||<| d|||<q|dkr| d d rt| d |||<t| d |||<t| d|||<q| d |||<| d |||<| d|||<qg}d}tjt||dd}|||d7}|jrt| dtr| d|}n| d|}||}t|}t||jd}|||}|d7}|jr||} t| |jd} || |}!|d7}|j r>||}"t|"|jd}"||"|}#|d7}||}$t|$|jd}$||$|}%|d7}t||}t||jd}|||}&|d7}|jrt| dtrj| d|}'n| d|}'t||}t||jd}|||}(|d7}|jr||})t|)|jd})||)|}*|d7}|j r||}+t|+|jd}+||+|},|d7}||}-t|-|jd}-||-|}.|d7}|jrt||}/t|/|jd}/||/|}0|d7}tj!|dd}1t"|1#st$|1#rt%d|t&|1|j'|j(|j)|j*d}1t+"|1#s+t+$|1#r2t%d|| d|}2|dkrA|jpD|dk}3|2|jkrd|3rX| dd|j}4nJ| d|||j}4n>|3rp| ddd}5n| d|d}5|j|2}6|3rt,| dd|6df}7n t,| dd|6df}7tj!|5|7gdd}4|jr|}8|dkr|jr|j|d|8}9n|dur|d|8}9n | d|8}9nt-d}9id|1d| dd|4dt+.|j/| ddt+j0dt+j1dddd t+2d|j'|j'd!|j't+0d"d|jr|nt| d|d#|jr|'nt| d|d$|d%|d&|d'|r7dndd(|r?dndd)| d*|&|jrM|0nd|jrT|nd|jr[|!nd|j rb|#nd|j ri|%nd|jrp|(nd|jrw|*nd|j r~|,nd|j r|.nd|9d+ }:|:S),aM Fetches images and action chunk sample by index. Returns action chunk rather than just single-step action. If the action chunk retrieval would go out of bounds, the last action is repeated however many times needed to fill up the chunk. Args: idx: Integer index to retrieve sample Returns: dict: Data sample: { video=images, actions=action chunk, t5_text_embeddings=text embedding, t5_text_mask=text embedding mask, fps=frames per second, padding_mask=padding mask, num_frames=number of frames per sequence, image_size=image size, proprio=proprio state, __key__=unique sample identifier, } demor3rsuccess_rolloutNrI)rrrrFTrRrJrSrTrerU)total_num_copiesz4Invalid image data detected (NaN or Inf) for sample )rrgr!stronger_image_augz3Invalid processed image tensor detected for sample rVrKz-100videorHr t5_text_maski)dtypefps padding_mask image_sizerfuture_proprio__key__rollout_data_maskrollout_data_success_maskworld_model_sample_maskvalue_function_sample_maskglobal_rollout_idxaction_latent_idx) value_latent_idxcurrent_proprio_latent_idxcurrent_wrist_image_latent_idxcurrent_image_latent_idxcurrent_image2_latent_idxfuture_proprio_latent_idxfuture_wrist_image_latent_idxfuture_image_latent_idxfuture_image2_latent_idxvalue_function_return)3rrrrIrr.rpminrrr6rrrqrrrr,randomr*rrrlr r expand_dims zeros_likeappendr%rr&r#r$ concatenateisnananyisinfrrrrgr!r"torchtilefloatsqueezeronesint64zeros);rr sample_typerrglobal_step_idxrrelative_step_idxrZfuture_frame_idx_tempZframe_indices_neededrrr success_idx failure_idxis_world_model_sampleis_value_function_samplefuture_frame_idxmax_possible_idxZ decompressed_left_primary_imagesZ!decompressed_right_primary_imagesdecompressed_wrist_images frames_needed frame_idx image_listcurrent_sequence_idxfirst_input_imagerUimage blank_imager wrist_imagerZcurrent_left_imagerZcurrent_right_imagerrrrfuture_wrist_imagerZfuture_left_imagerZfuture_right_imager value_imagerimagesremaining_actionsZactions_already_sliced action_chunkavailable_actionsnum_padding_neededpaddingreturn_timestepr sample_dictr<r<r= __getitem__s                                           %zRoboCasaDataset.__getitem__)rrrFTTTTTTTrrrrrFTrFF)NNN)__name__ __module__ __qualname__strr8rrrrrrrrrrr<r<r<r=r8s      x s j r__main__rz9users/user/data/robocasa/robocasa_regen_v2_1199succDemos/r zJusers/user/data/robocasa/robocasa_regen_v2_1199succDemos/t5_embeddings.pklr r!Tr#r$r%rirhr&rr"r'zEusers/user/data/robocasa/robocasa_regen_rollout_data_v2_1291episodes/r(rr)r,r-rrcCs d|S)Nz{0:0.3f})formatr:r<r<r=r>Os r>) formatterz Images shape, dtype: rzActions shape, dtype: rVz Actions: z!T5 text embeddings shape, dtype: rzT5 text embeddings: z./temp)exist_ok2r3z"./temp/LAZYLOAD_global_step_index_z__task=rHz __isRollout=rz__globalRolloutIdx=rz __isSuccess=rz__value=rz.4fz __frameIdx=z.pngzSaved image at path: zQ ================================================================================z1Data samples loading test completed successfully!zQ================================================================================ r<)5__doc__rjrrrxnumpyrrPILrtorch.utils.datarr%cosmos_policy.datasets.dataset_commonrrrrr r $cosmos_policy.datasets.dataset_utilsr r r rrrrcosmos_policy.utils.utilsrset_printoptionsinfrrdatasetrsamplershapermakedirsrr7randintrZglobal_step_indexpermuterrZimg_np image_path fromarraysaver<r<r<r=s    $        N