o
    E
j                     @   s   d 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 ddlmZmZmZmZm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  Minimal window dataset for the volume AR model.

Per sample (at frame t of demo d):
  rgb:                (3, 448, 448) — current frame, ImageNet-normalized
  past_eef_world:     (20, 3)       — world EEF at frames [t-19..t]; clamped at 0 (repeat earliest)
  current_eef_world:  (3,)          — == past_eef_world[-1]
  target_eef_world:   (8, 3)        — world EEF at frames [t+stride..t+8*stride] (clamped at demo end)
  target_grip:        (8,)          — gripper at those frames (-1 / +1)
  target_rot_euler:   (8, 3)        — EEF euler XYZ at those frames
  target_voxel_idx:   (8,)          — flat voxel index of each target world coord
  world_to_camera:    (4, 4)
  valid_mask:         (8,)          — False where future is clamped at demo end (skip loss)
    )PathN)Dataset)Rotation)voxel_centers_worldworld_to_voxel_idx
N_PAST_EEFT_FUTURE
IMAGE_SIZE)g
ףp=
?gv/?gCl?dtype)gZd;O?gy&1?g?c                   @   s<   e Zd ZddeeeddfddZdd Zdd	 Zd
d Z	dS )VolumeWindowDatasetlibero_spatialr      c	                 C   s@  || _ || _|| _|| _t|| d|  }	|	 st|	g | _g | _t	t
|	dD ]U\}
}|dkr<|
|kr< nHt
|d d}|sHq.| j|t|d t|d t|d t|d	 t|d
 tt|d D ]}| jt| jd |f qsq.tdt| j dt| j d| d| d| d d S )NZtask_zdemo_*r   framesz*.pngzeef_pos.npyzeef_quat.npyzgripper.npyzworld_to_cam.npy)frame_pathseef_poseef_quatgripperworld_to_camT   zVolumeWindowDataset: z demos, z samples (past=z	, future=z	, stride=))
image_sizen_pastt_futuresr   existsFileNotFoundErrordemossamples	enumeratesortedglobappendnploadlenrangeprint)self
cache_rootZbenchmark_nametask_idr   r   r   frame_stride	max_demosZ
bench_rootd_idxZdemo_dirr   t r0   +/data/cameron/para/libero/data_volume_ar.py__init__    sB   	zVolumeWindowDataset.__init__c                 C   s
   t | jS )N)r&   r   )r)   r0   r0   r1   __len__B   s   
zVolumeWindowDataset.__len__c                 C   sv   t t|}t |t jtjd }|jd | j	kr)t j
|| j	| j	ft jd}|t t }t|ddd S )Ng     o@r   )interpolation   r   )cv2imreadstrcvtColorCOLOR_BGR2RGBastyper$   float32shaper   resizeINTER_LINEARIMAGENET_MEANIMAGENET_STDtorch
from_numpy	transposefloat)r)   pathbgrrgbr0   r0   r1   _load_frameE   s   zVolumeWindowDataset._load_framec                    s  | j | \}| j| }|d }| j | j}| j fddt D }|d | tj}|d }|d fddt|D }	fdd|	D }
t	j
fd	d|	D t	jd
}|d |
 tj}|d |
 tj}|d d g df }ztjdd |D ddtj}W n ty   t|}Y nw |d |
 tj}tt	| }| |d  }|t	|t	|t	|t	|t	|||t	|d t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.0i)Nr/   r0   r1   
<listcomp>V   s   " z3VolumeWindowDataset.__getitem__.<locals>.<listcomp>r   r   c                    s   g | ]
}|d     qS )r   r0   rK   )r   r/   r0   r1   rO   \       c                    s   g | ]}t | qS r0   )minrK   	last_realr0   r1   rO   ]   s    c                    s   g | ]}| kqS r0   r0   )rL   rawrS   r0   r1   rO   ^   s    r
   r   )r   r5   r   r   c                 S   s   g | ]
}t |d qS )xyz)ScipyRZ	from_quatZas_euler)rL   qr0   r0   r1   rO   g   rQ   r   )axisr   r   r   )rH   past_eef_worldcurrent_eef_worldtarget_eef_worldtarget_griptarget_rot_eulertarget_voxel_idx
valid_maskworld_to_cameraZdemo_idxstart_t)r   r   r   r   r   r'   r;   r$   r<   rB   tensorboolstack
ValueError
zeros_liker   rC   longrI   )r)   idxdZdemor   TfZpast_idxZpast_posZcurrent_posZfuture_idx_rawZ
future_idxr`   Z
target_posZtarget_quatZtarget_quat_xyzwZ
target_eulZ
target_grpr_   rH   r0   )rN   rT   r   r/   r1   __getitem__M   sR   
zVolumeWindowDataset.__getitem__N)
__name__
__module____qualname__r	   r   r   r2   r3   rI   rl   r0   r0   r0   r1   r      s    
"r   )__doc__pathlibr   r6   numpyr$   rB   torch.utils.datar   Zscipy.spatial.transformr   rW   robot_volumer   r   r   r   r	   arrayr<   r@   rA   r   r0   r0   r0   r1   <module>   s    