o
    jj/                     @   sd  d Z ddlZddl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
mZ ddlmZ ejdeje ddlmZmZmZmZmZ ejdd ddlmZmZmZmZ ddlZejg d	ej d
Z!ejg dej d
Z"dd Z#G dd deZ$e%dkre$dddZ&e'de(e& e&d Z)e)* D ]\Z+Z,e'de+ de-e,dre.e,j/ne,  qdS dS )u  All-in-memory data loader for the smith300 first_mobile_collection sessions.

Loads ALL 9 robot sessions, runs MuJoCo FK once per frame at init, preloads every
RGB at 448×448 as a single normalized float tensor. Per-sample yield matches the
existing VolumeWindowDataset shape so train_volume_smooth_v2.py runs unchanged
once we point it at this loader.

Per-sample dict:
  rgb:                (3, 448, 448)   ImageNet-normalized
  past_eef_world:     (20, 3)         clamped at episode start (repeat earliest)
  current_eef_world:  (3,)            == past_eef_world[-1]
  target_eef_world:   (8, 3)          future +1..+8 stride frames, clamped at episode end
  target_grip:        (8,)
  target_rot_euler:   (8, 3)          XYZ euler radians
  target_voxel_idx:   (8,)            flat voxel index
  valid_mask:         (8,)            False where future is clamped
  world_to_camera:    (4, 4)          T_camera_arucoBase @ T_W_baseBody_inv_aruco_offset
    N)Path)Dataset)Rotation)voxel_centers_worldworld_to_voxel_idx
N_PAST_EEFT_FUTURE
IMAGE_SIZEz/data/cameron/para/para_mac)DEFAULT_SMITH300_XMLEEF_BODY_NAMEproject_to_pixel_scale_K_to)g
ףp=
?gv/?gCl?dtype)gZd;O?gy&1?g?c                 C   sJ   t | t jtjd }t j|||ft jd}|t t	 }|
dddS )Ng     o@)interpolation   r      )cv2ZcvtColorZCOLOR_BGR2RGBastypenpfloat32resizeZINTER_LINEARIMAGENET_MEANIMAGENET_STD	transpose)bgr
image_sizergb r   1/data/cameron/para/libero/data_smith300_volume.py_resize_normalize-   s   r    c                   @   s2   e Zd ZdeeedefddZdd Zdd Z	d	S )
Smith300VolumeDatasetz8/data/cameron/mac_robot_datasets/first_mobile_collectionr   c           6         sN  || _ || _|| _|| _t|}tdd | D }|s$td| tdt	| d|  t
j|}	t
|	}
t
|	t
jjt}|dk rRtdtd| |	j}g | _g | _g | _g | _t|D ]\ }|d	 }| s|td
|j d qett|}|d \}}tj|d tj d}tj|d tj d}tj|d tj d}|| }t!||f||}tj"dtj d}||d dd d f  |d dd d f< | j#|$tj% |d d |d g}t&dd |D d }|d u rtd
|j d qett|}|d }t|d }tj'|d tj d}|j(d }|j(d }t) } |D ]"}!t*t+|!d |d }"t,t+|!d |"d D ]}#| -|# q>q&i }$t| D ]}#tj.|tj d}%||#d |f |%d t*||< |%|
j/d |< t
0|	|
 |
j1| 2 $tj%}&|
j3| 2 }'|'g d $tj%}(t45|(6d$tj%})|d krt%||#d!f nt%d"}*|d#|#d$d% }+t78t9|+},|,d u rǐqOt:|,|}-t	| j}.|.|$|#< | j#|&|(|)|* d& | j#|- qO|D ]P}!t+|!d }/t*t+|!d |d }0g }1t,|/|0d D ]}#|#|$v r|1#|$|#  qt	|1d'k r q| j# |!;d(d)t	| j tj'|1tj<dd* qtd+|j d,t	 fd-d| jD  d.t= fd/d| jD  d0 qet>?tj@| jdd1| _t>?tj@| jdd1| _t	| j}2t>Atj@d2d | jD dd1| _Bt>Atj@d3d | jD dd1| _Ct>Atj@d4d | jD dd1| _Dt>Atj@d5d | jD dd1| _Et>jAtj@d6d | jD dd1t>jFd| _Gg | _Ht| jD ]\}3}!t	|!d7 }4t,|4d D ]}5| jH#|3|5f qqtd8t	| j d9t	| j d:t	| jH d;| jj( d<| jI | jJ  d= d>d? d S )@Nc                 S   s.   g | ]}|  r|jd s|jdkr|qS )Zdataset_Zextra_singleview_capture)is_dirname
startswith).0dr   r   r   
<listcomp>?   s    

z2Smith300VolumeDataset.__init__.<locals>.<listcomp>zNo sessions under zSmith300VolumeDataset: loading z sessions from r   zbody z not in z	meta.jsonz  skip z: no meta.jsonZimage_size_whKr   T_camera_arucoBaseZT_W_baseBody_inv_aruco_offset      Zrgb_overlayzepisodes.jsonc                 s   s    | ]	}|  r|V  qd S N)exists)r%   pr   r   r   	<genexpr>l   s    z1Smith300VolumeDataset.__init__.<locals>.<genexpr>z: no episodes.jsonepisodesz
joints.npzZq_motorsr   endstart)r   r   r+   r   xyz      g        Zrgb_Z06dz.jpg)eef_poseef_quat	eef_eulergrippersession_idxr   idZep_)r:   Zep_idglobal_frames  : c                    s   g | ]
}|d   kr|qS r:   r   )r%   esess_idxr   r   r'          z eps, c                 3   s     | ]}|d   krdV  qdS )r:   r   Nr   r%   fdrA   r   r   r/      s    z frames loaded)axisc                 S      g | ]}|d  qS )r6   r   rD   r   r   r   r'          c                 S   rG   )r7   r   rD   r   r   r   r'      rH   c                 S   rG   )r8   r   rD   r   r   r   r'      rH   c                 S   rG   )r9   r   rD   r   r   r   r'      rH   c                 S   rG   r?   r   rD   r   r   r   r'      rH   r<   zSmith300VolumeDataset ready: z episodes, z	 frames, z samples, rgb tensor z (g    eAz.2fz GB))Kr   n_pastt_futuresr   sortediterdirFileNotFoundErrorprintlenmujocoZMjModelZfrom_xml_pathZMjDataZ
mj_name2idZmjtObjZ
mjOBJ_BODYr   RuntimeErrornqr0   Z
frame_data
rgb_tensorw2c_per_session	enumerater-   r#   jsonloadopenr   arrayfloat64r   eyeappendr   r   nextasarrayshapesetminintrangeaddzerosZqposZ
mj_forwardxposcopyZxquatScipyRZ	from_quatZas_eulerr   Zimreadstrr    getint64sumtorch
from_numpystacktensorr6   r7   r8   r9   longsessionsampleselement_sizenumel)6selfroot_dirr   rI   rJ   frame_strideZ
mujoco_xmlrootZsession_dirsZmj_modelZmj_dataZeef_idZn_qposZsess	meta_pathmetaZIMG_WZIMG_HZK_origr)   ZT_W_baseBodyZT_CAM_WORLDZK_targetMZep_pathsZep_pathZep_dataZsess_episodesZjointsZq_motors_allZn_motorsn_framesZneeded_framesepZep_endfZlocal_to_globalqr6   Z	quat_wxyzr7   Zeef_eulZgripZimg_pathr   Zrgb_tZ
global_idxZep_start_localZep_end_localr<   Nep_idxLtr   rA   r   __init__5   s   
(

&



&
""""(zSmith300VolumeDataset.__init__c                 C   s
   t | jS r,   )rP   rt   )rw   r   r   r   __len__   s   
zSmith300VolumeDataset.__len__c                    sR  | j | \}| j| }|d }t|}| j | j}| j fddt D }|| }|d fddt|D }	fdd|	D }
tjfdd|	D tj	d}||
 }| j
|  }| j| }| j|  }| j| }| j| }| j| }t|}t| j|   }| j| }|||||||||tj|tjdtjtjdd	S )
Nr<   c                    s"   g | ]}t d  d |  qS )r   r   )maxr%   i)r   r   r   r   r'      s   " z5Smith300VolumeDataset.__getitem__.<locals>.<listcomp>r   c                    s   g | ]
}|d     qS )r   r   r   )rK   r   r   r   r'      rC   c                    s   g | ]}t | qS r   )rb   r   	last_realr   r   r'      s    c                    s   g | ]}| kqS r   r   )r%   rawr   r   r   r'      rH   r   )r   Zpast_eef_worldcurrent_eef_worldZtarget_eef_worldZtarget_gripZtarget_rot_eulerZtarget_voxel_idx
valid_maskworld_to_camerar   start_t)rt   r0   rP   rI   rJ   rK   rd   rn   rq   boolrT   r6   r9   r8   r   rc   rs   itemrU   rr   )rw   idxr   r   framesr   TfZ
past_localZpast_globalZfuture_local_rawZfuture_localr   Zfuture_globalr   Zpast_posZcur_posZtgt_posZtgt_gripZtgt_eulZtgt_voxrB   Zw2cr   )r   r   rK   r   r   __getitem__   sF   





z!Smith300VolumeDataset.__getitem__N)
__name__
__module____qualname__r	   r   r   r
   r   r   r   r   r   r   r   r!   4   s    
 r!   __main__i  r   )r   ry   zlen(ds):r=   r>   r`   )0__doc__ossysrW   pathlibr   r   numpyr   rn   torch.utils.datar   Zscipy.spatial.transformr   ri   pathinsertdirname__file__robot_volumer   r   r   r   r	   Zdata_smith300_parar
   r   r   r   rQ   rZ   r   r   r   r    r!   r   dsrO   rP   rK   itemskvhasattrtupler`   r   r   r   r   <module>   s4     I*