o
    !i$                     @   s  d Z ddlZddl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ZdZdZdZdZejg d	g d
g dg dgejdZejg dg dg dgejdZd\ZZejed e e ded e e gded e e ed e e gg dgejdZdd ZG dd de	ZedkredZedee  ddlZe Z e!e"deeD ]Z#ee# Z$qe e  Z%ede%d d!de% d"d# dS dS )$zPanda real-robot dataset for PARA training.

Loads pre-cached 448x448 JPG frames + joint state NPYs, with FK and projection
pre-computed at init time for fast __getitem__.
    N)Dataset)Rotation      g{Gz?i  )gS?gCH?g8$K?gp^ڿ)g̺=r?g,2ۿg gvMiyu?)ghݫɿg*i?g
lPk޿gtUE}?)        r   r         ?dtype)gr@r   g33333@)r   g4@g     X@)r   r   r   )i  i8  r   r   r   r         r   r   r   c                 C   s   |ddddf |  |dddf  }|d dkrdS |d |d  |d  |d  }|d |d  |d  |d	  }t j||gt jd
S )z1Project a 3D world point to 2D pixel coordinates.N   r   r   r
   r   r   r   r   r   )nparrayfloat32)Z	pos_worldZT_cwKZp_camuv r   5/data/cameron/para/panda_streaming/data_panda_para.pyproject_to_pixel-   s   (  r   c                   @   s0   e Zd ZdZdedfddZdd Zdd	 ZdS )
PandaTrajectoryDatasetzFast dataset for PARA training on real Panda data.

    Pre-computes FK, projection, and gripper values at init.
    __getitem__ only reads cached 448x448 JPGs and assembles tensors.
    Nr   c              	   C   s  || _ tj|d| _|| _t| _|| _tj	| js$t
d| j d|d u r/tj|d}t|}t|}W d    n1 sCw   Y  |d | _tddd d	d
lm} tj|j}t|}	t|tjjd}
t }| jD ]}t|d |d d D ]}|| qqti | _t|D ]}tj||dd}tj|sqt |!t j"}|d t# |	j$d t#< t%|dkr|d nd}|	j$j&t#d kr|t' |	j$t#< |t' |	j$t#d < t(||	 |	j)|
 * !t j+}|	j,|
 * }|g d !t j+}t-.|/d!t j+}t0|!t j"t1t2}|d ur7t j3t 4|d	 d	|d t 4|d d	|d gt j+d}nt j5dt j+d}t +d| d }|||||d| j|< qtdt%| j ddd g | _6t7| jD ]2\}}|d |d }}|| d }td | d }t|| d D ]}| j68||| f qqit9* !t j+| _:| j:d	  t;  < | j:d  t<  < t j3g dt j+d| _=t j3g dt j+d| _>tdt%| j dt%| j6 d| dd d S ) NZ
cached_448zCached images not found at z!. Run the pre-cache script first.zepisodes.jsonepisodesz"Pre-computing FK for all frames...T)flushr   )PANDA_HANDEYE_4X2_CONFIGhandstartendr   06dz.npyr   r   r   )r   r   r   r   xyzr   g       @)eef_poseef_quat	eef_eulerpixel_2dgripperz  Pre-computed z frames)g
ףp=
?gv/?gCl?)gZd;O?gy&1?g?zPandaDataset: z episodes, z samples, stride=)?data_dirospathjoin	cache_dir
image_sizeN_WINDOWn_windowframe_strideisdirFileNotFoundErroropenjsonloadr   print ExoConfigs.panda_exo_handeye_4x2r   mujocoMjModelfrom_xml_stringxmlMjData
mj_name2idmjtObj
mjOBJ_BODYsetrangeadd
frame_datasortedexistsr   astypefloat64N_ARM_JOINTSqposlensizeGRIPPER_POS_MAX
mj_forwardxposcopyr   xquatScipyR	from_quatas_eulerr   T_CAM_WORLD	CAM_K_448r   clipzerossamples	enumerateappendCAM_K
cam_k_normIMG_WIMG_Hmeanstd)selfr(   Zepisodes_jsonr-   r0   fep_datar   mj_modelmj_datahand_idZall_frame_indicesepidxnpy_pathjsgwr#   	quat_wxyzr$   r%   pixr&   Zgripep_idxZep_startZep_endZep_lenZ
window_lentr   r   r   __init__>   s   





zPandaTrajectoryDataset.__init__c                 C   s
   t | jS )N)rJ   rX   )ra   r   r   r   __len__   s   
zPandaTrajectoryDataset.__len__c                 C   s  | j | \}}g }g }g }g }g }g }	d }
t| jD ]_}||| j  }|d}tj| j| d}t	|tj
}t|tjtjd }|
d u rL|}
|	| | j| }||d  ||d  ||d  ||d  ||d  qt|}t|}tj|tjd	}t|}t|}t|	}	tj| j| j| jftjd	}t| jD ]1}tttt||d
f d
| jd }tttt||df d
| jd }d||||f< qt|
d| jd d d d f  | jd d d d f  }ttj}t| t| t| t| t| t| t| t|	 t| tj dtjd	t|d  ttj!"ttj t| j# tj |tj$d	tj |tj$d	dS )Nr!   z.jpgg     o@r#   r$   r%   r&   r'   r   r   r   r   )r   r   r   r   )rgbZheatmap_targettrajectory_2dtrajectory_3dtrajectory_grippertrajectory_quattrajectory_eulerrgb_frames_rawworld_to_cameraZbase_zZ	target_3dZcamera_poseZ
cam_K_normZdemo_idxstart_t)%rX   rA   r/   r0   r)   r*   r+   r,   cv2imreadIMREAD_COLORcvtColorCOLOR_BGR2RGBrF   r   r   rZ   rC   stackr   rW   r-   intrV   roundfloat	transposer_   r`   rT   torch
from_numpytensorlinalginvr\   long)ra   rh   rn   Zstart_framert   ru   rv   rw   rx   ry   Zrgb_refkZ	frame_idxtsZimg_pathZbgrrs   fdZheatmap_targetsro   Zx_iZy_iZrgb_trz   r   r   r   __getitem__   sn   






((4z"PandaTrajectoryDataset.__getitem__)__name__
__module____qualname____doc__
IMAGE_SIZErp   rq   r   r   r   r   r   r   7   s
    `r   __main__z8/data/cameron/panda_data/data_20260420_115853_632_frameszDataset size:    z20 samples in z.2fzs = z.1fz samples/sec)&r   r)   r4   globr|   numpyr   r   torch.utils.datar   scipy.spatial.transformr   rQ   r8   r.   rH   rL   r   r   rG   rT   r[   r]   r^   rU   r   r   r   dsr6   rJ   timet0rA   mini_elapsedr   r   r   r   <module>   sh      
 2
"