o
    j+                     @   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Zddl	Z	ddl
mZ ddlmZ ddlmZ ejdeje ejdd ejdd ejd	d
 ddlmZmZ ddlmZ ddlmZmZmZ ddl m!Z!m"Z" ddl#m$Z$ ddl%m&Z&m'Z'm(Z( dZ)dZ*dZ+e)dfddZ,dd Z-e.dkre-  dS dS )u  Closed-loop libero eval for DinoVolumeQuery (query-MLP arch).

Adapts the existing eval.py logic to the query-MLP output format:
  - model(rgb, start_pix=current_eef_pix) returns dict with volume_logits,
    gripper_logits, rotation_logits already per-timestep (no separate predict_at_pixels)
  - Rotation is 1D PCA: decode bin → mu + val·v1 → euler XYZ
  - Volume CE is joint over (Z, H, W); argmax gives (z*, y*, x*) per timestep

Run:
  CUDA_VISIBLE_DEVICES=8 PYTHONPATH=/data/cameron/LIBERO \
  DINO_REPO_DIR=/data/cameron/keygrip/dinov3 \
  python eval_libero_query.py \
    --checkpoint checkpoints/libero_query_libero_spatial_t0_v0/latest.pth \
    --benchmark libero_spatial --task_id 0 --n_episodes 10 --save_video
    N)Path)tqdm)Rotationz/data/cameron/LIBERODINO_REPO_DIRz/data/cameron/keygrip/dinov3DINO_WEIGHTS_PATHzU/data/cameron/keygrip/dinov3/weights/dinov3_vits16plus_pretrain_lvd1689m-4057cbaa.pth)	benchmarkget_libero_path)OffScreenRenderEnv)get_camera_extrinsic_matrixget_camera_intrinsic_matrixget_camera_transform_matrix)DinoVolumeQuery	PRED_SIZE)*recover_3d_from_direct_keypoint_and_height)preprocess_obsget_camera_paramseef_to_start_kpi  g?      ?c           4      C   s  | d d }| d d }| d d }|j \}}}}|j d }|j d }|| }g }g }| }||djdd}|||    }|||    }|| tj}|| tj} |jdd  }!|jdd  }"t	
|}#t|D ]}$t| |$ d | }%t||$ d | }&||$ t|d d ||  | }'ttj|%|&gtjd	t|'||}(|(d
u r|r|d n| }(||( |(| })tj|)}*|*|kr|)|* | })t|)t dd}+||"|$ d | ||   },|	|,|
  }-t	d|-}.|.|#  }/t|/ t dd}0|!|$ t|d d ||  | }1|1dkr%dnd}2tjdtjd	}3|+|3d
d< |0|3dd< |2|3d< ||3 qz||fS )u?  Decode all T predicted timesteps from a query-MLP forward into OSC_POSE actions.

    out_dict contains volume_logits (1, T, Z, H, W), gripper_logits (1, T, n_grip),
    rotation_logits (1, T, n_rot) — all already per-timestep.

    Returns (actions: list of T (7,) numpy, pred_3d_targets: list of T (3,) numpy).
    volume_logitsr   gripper_logitsrotation_logits)dimr      dtypeNg      g      ?xyzg                 )shapecopyreshapeargmaxcpunumpyastypenpint64ScipyR	from_quatrangefloatmaxr   arrayfloat64appendlinalgnormclipOSC_POS_SCALE
from_eulerinv	as_rotvecOSC_ROT_SCALEzerosfloat32)4Zout_dictcamera_posecam_Kcurrent_eef_poscurrent_eef_quatmin_hmax_hmin_gmax_grot_pca_meanrot_pca_axisrot_pca_minrot_pca_max
image_size	max_deltavolgriprotTZZHgZWgZn_gripZn_rotscaleactionspred_3dref_posflatZh_binsyxZpy_gridZpx_gridZgrip_argmaxZ
rot_argmax	R_currenttpx_fullpy_fullheightZp3d	delta_posr2   
delta_normZpca_val
euler_predR_predR_deltadelta_rot_normZgrip_continuousgripper_cmdaction ra   ./data/cameron/para/libero/eval_libero_query.pydecode_query_actions-   s^   


"
"rc   c            5         s  t  } | jdtdd | jdtdd | jdtdd | jd	td
d | jdtdd | jdtdd | jdtdd | jddd | jdtdd |  }ttj	 rXdnd}t
|j}| sktd| tj|ddd}t|d t|d }}t|d t|d }}tj|d tjd}	tj|d  tjd}
t|d! }t|d" }t|d# }t|d$d%}t|d&t}td'|d(  d)| d*|  td+|d,d-|d,d.|d,d-|d,d/	 td0|	 d1|
 d2|d3d-|d3d/	 t|d4d4||tdd5d6| }|j|d7 dd8 td9 t |j  }||j}|j }td:|j d;|  t!j"#t$d<|%|j}t&'|d= t(d>d?  d@ ) D } fdAd?|D }W d    n	1 siw   Y  t*|j+t,|}tdB| dCt,| dD t!j"#t$dE|j-|j.}t/||||j0gdF}|1|j1 |2  tdG |j3rt
|j4j5dddH g }g }t6t7|dIdJD ]o}|2  |8|| }t7dKD ]}|9tj:dLtj;d\}}}}qt<|j=|j0|\}} }!d}"d}#d}$|j3rg nd }%|$|j>k r|"stj?|dM tjd}&tj?|dN tjd}'||j0 dO }(t@|(||})tA|&|||}*|*B dPkrA|*Cd}*tD  ||)|*dQ}+W d    n	1 sWw   Y  tE|+| |!|&|'|||||	|
|||dR\},}|,D ]b}-|%d ur||j0 dO Ftj;dS  tG H  tIjJ ||ftIjKdT  dS FtjL}.tIM|.dU|$ dVtIjNdWdXdYtIjO |%P|. |9|-\}}}"}|$dP7 }$|"rd}# n
|$|j>kr nqr|$|j>k r|"r|P|# |P|$ |%r0t
|j4dZ|d[d\|#rd]nd^ d_|$ d` }/|%d jQd dY \}0}1tIRt|/tIjSda db|1|0f}2|%D ]}3|2TtIU|3tIjV q|2W  qttX|}4tdctY| dd| de|4dfdg tdhttX|didj|j> dk d S )lNz--checkpointT)typerequiredz--benchmarklibero_spatial)rd   defaultz	--task_idr   z--camera	agentviewz--n_episodes
   z--max_stepsiX  z--seedz--save_video
store_true)r`   z--video_dirz%/data/cameron/para/libero/eval_videoscudar$   zCheckpoint not found: F)map_locationweights_only
min_height
max_heightmin_gripmax_griprC   r   rD   rE   rF   
n_rot_binsn_window   rG   zLoaded ckpt: epoch=epochz, n_window=z, n_rot_bins=z
  height [z.3fz, z	]  grip []z  rot PCA: mu=z  axis=z	  range=[z.2f    1d_pca)rs   n_height_binsn_gripper_binsrr   rG   	pred_sizeuse_eefrotation_modemodel_state_dict)strictzModel loaded.zTask: [z] datasetsrc                 S   s   g | ]	}| d r|qS )demo_)
startswith.0kra   ra   rb   
<listcomp>   s    zmain.<locals>.<listcomp>datac                    s    g | ]} d | d d qS )zdata/z/statesr   ra   r   fra   rb   r      s     zRunning z / z episodes...
bddl_files)bddl_file_namecamera_heightscamera_widthscamera_nameszEnvironment ready.)parentsexist_okEpisodes)desc   r   robot0_eef_posrobot0_eef_quat_imager   )	start_pix)rG   g     o@)interpolationzstep )ri      g?)   r   r      ep03d_OKFAIL_stepsz.mp4mp4v   z
Final: /z = z.1%z success ratez  avg steps: z.1fz  (max ))ZargparseArgumentParseradd_argumentstrint
parse_argstorchdevicerk   is_availabler   
checkpointexistsFileNotFoundErrorloadr,   r'   asarrayr/   get
LIBERO_IMGprintr   r   toevalload_state_dictbmget_benchmark_dictr   get_tasktask_idnameospathjoinr   get_task_demonstrationh5pyFilesortedkeysmin
n_episodeslenproblem_folder	bddl_filer	   cameraseedreset
save_video	video_dirmkdirr   r+   set_init_statestepr9   r:   r   sim	max_stepsr.   r   r   r   	unsqueezeno_gradrc   r&   flipudr!   cv2resizeINTER_LINEARuint8putTextFONT_HERSHEY_SIMPLEXLINE_AAr0   r    VideoWriterVideoWriter_fourccwritecvtColorCOLOR_RGB2BGRreleasemeansum)5pargsr   	ckpt_pathckptr?   r@   rA   rB   rC   rD   rE   rF   rr   rs   rG   modelbenchtask	task_name	demo_path	demo_keysinit_statesr   r   env	successesstep_countsep_idxobsr   world_to_camerar;   r<   donesuccessstep_idxframesr=   r>   rgb_obs
img_tensorr   outwindow_actionsr`   visout_pathhwwriterfrsrra   r   rb   mainz   s   
*&
"



"

$
."(r	  __main__)/__doc__r   sysr   jsonr%   r'   r   r   r   pathlibr   r   scipy.spatial.transformr   r)   r   insertdirname__file__environ
setdefaultlibero.liberor   r   r   libero.libero.envsr	   robosuite.utils.camera_utilsr
   r   r   model_dino_volume_queryr   r   utilsr   r   r   r   r   r   r4   r8   rc   r	  __name__ra   ra   ra   rb   <module>   s:     
M 
