o
    ^j	B                     @   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
 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m Z  ddl!m"Z"m#Z#m$Z$ ddl%m&Z& ddl'm(Z(m)Z) dZ*dZ+dZ,ddgZ-g dZ.e/g dZ0dd Z1dd Z2dd  Z3d!d" Z4e*dfd#d$Z5d,d'd(Z6d)d* Z7e8d+kre7  dS dS )-u  OOD closed-loop libero eval for DinoVolumeQuery2View — adds shift/clean/teleport flags.

Based on eval_libero_2view.py with these additions:
  --shift_dx, --shift_dy  : shift pick+place objects in init_state (deltas, not absolute)
  --clean_scene           : hide distractors + furniture (match OOD train conditions)
  --zero_rotation         : zero out predicted rotation deltas (use upright gripper)
  --teleport              : closed-loop servo to predicted 3D target, then apply gripper
    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	MUJOCO_GLosmesaPYOPENGL_PLATFORM)	benchmarkget_libero_path)OffScreenRenderEnv)get_camera_extrinsic_matrixget_camera_intrinsic_matrixget_camera_transform_matrix#project_points_from_world_to_camera)DinoVolumeQuery2View	PRED_SIZEbuild_bev_world_xyz_table)*recover_3d_from_direct_keypoint_and_height)preprocess_obseef_to_start_kpi  g?      ?	   %   )         )      $@r   ?c                 C   sB   |   }tD ]}|d }||  |7  < ||d   |7  < q|S )N   )copyTASK0_PICK_PLACE_QPOS)statedxdysqpsi r)   2/data/cameron/para/libero/eval_libero_2view_ood.pyshift_init_state.   s   r+   c                 C   s.   |   }tD ]}|d }t|||d < q|S )Nr       )r!   TASK0_DISTRACTOR_QPOSDISTRACTOR_FAR)r#   r&   r'   r(   r)   r)   r*   hide_distractors_in_state7   s
   r/   c                 C   s.  ddl m} | j|}| jj|  }| jj| dd}|dddf  }d}	|	|d  |d d  }
||
|  }t	j
|| }|| | }t	g d}tt	||d	krat	g d
}t	||}|t	j
| }t	||}t	|}t	|}t	|t	| | t	|t	| |  t	||  }|||  }|| }|t	j
|d  }| }t	g d}tt	||d	krt	g d}t	||}|t	j
|d  }t	||}t	j|||gdd}|| }t	|d |d |d |d g}|| jj|< || jj|< |   dS )zKSpherical-cap camera reposition (matches generate_ood_viewpoint.py's grid).r   r   r,   N   r   g:0yE>)r   r         ?gGz?)r    r           g-q=)r2   r2   r1   )r2   r1   r2   )axisr    )scipy.spatial.transformr   modelcamera_name2iddatacam_xposr!   cam_xmatreshapenplinalgnormarrayabsdotcrossradianssincosstackfrom_matrixas_quatcam_poscam_quatforward)simcamera_nameZ	theta_degZphi_degScipyRcam_idZdefault_posr:   rK   ZTABLE_ZZt_hitZlook_atradiusZdefault_diruprightZtrue_upthetaphioffsetnew_posfwdZcam_zZup_hintZcam_xZcam_yRqnew_quatr)   r)   r*   reposition_camera?   sH    "r[   c              	   C   s   dD ]}z| j |}tg d| j j|< W q ty    Y qw |   t }dD ]}z|| j | W q* ty@   Y q*w t	| j j
D ]}| j j| |v rYd| j j| d< qGdS )z8Hide furniture + distractors to match OOD training data.)Zwooden_cabinet_1_mainZflat_stove_1_main)r   r   g      )Zakita_black_bowl_2_mainZcookies_1_mainZ#glazed_rim_porcelain_ramekin_1_mainr2   r,   N)r6   body_name2idr<   r?   body_pos	ExceptionrK   setaddrangengeomgeom_bodyid	geom_rgba)rL   fnamebidZdistractor_bodiesdngidr)   r)   r*   apply_clean_scenee   s(   ri   c           4      C   s  | d d }| d d }| d d }|j \}}}}|j d }|j d }|| }||djdd}|||    }|||    }|| tj}|| tj}|jdd  }|jdd  }t	|} g }!g }"|
 }#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 )Nvolume_logitsr   gripper_logitsrotation_logitsr3   )dimr   r    dtype      r1   xyzr2      r,      )shaper;   argmaxcpunumpyastyper<   int64rN   	from_quatr!   ra   floatmaxr   r?   float64appendr=   r>   clipOSC_POS_SCALE
from_eulerinv	as_rotvecOSC_ROT_SCALEzerosfloat32)4Zout_dictcamera_pose_bevZcam_K_bev_pixel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flatZh_binsyxZpy_gridZpx_gridZgrip_argmaxZ
rot_argmax	R_currentactionspred_3d_listref_postpx_fullpy_fullheightp3d	delta_posr>   
delta_normZpca_val
euler_predR_predR_deltadelta_rot_normZgrip_continuousgripper_cmdactionr)   r)   r*   decode_actionsy   s\   


"
"r      {Gzt?c                 C   s   d}d}d}t |D ]O}| j }	tj|	d tjd}
||
 }tj|}||k r-|	} n-t|t	 dd}tj
dtjd}||dd	< ||d
< | |\}}}}|d7 }|rY nq
|du rc| j }|||fS )u   Closed-loop servo to a 3D target position with given gripper command.

    Returns (obs, n_steps, done) — n_steps is the actual env.step count consumed.
    Nr   Frobot0_eef_posrn   rp   r1   rr   r,   rs   r    )ra   env_get_observationsr<   r?   r}   r=   r>   r   r   r   r   step)r   
target_posr   	max_servo	thresholdobsn_stepsdone_Zcur_obscur_posdeltadistZdelta_clippedr   r)   r)   r*   servo_to_pos   s.   


r   c            A         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tdd | jddd | jddd | jddd | jdtddd | jdtddd | jdtdd |  }ttj	
 rdnd}tj|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j|d. tjd&}tj|d/ tjd&}td0|d1  d2|  td3|jd4d5|jd4d6|j d7|j d8|j 
 t|d9d:}td;|  t|d<d<||td=|d>| }|j|d? dd@ d }t |j   }|!|j"}tdA|j  dB|j#  t$j%&t'dC|(|j"}t)*|dD t+dEdF  dG , D } fdHdF|D }W d    n	1 sw   Y  t-|j.t/|}t$j%&t'dI|j0|j1}t2|||dJdKgdL}|3|j3 |4  |jrt5|j6j7 tdM |8 9tj}|d  |9  < |dN  |9  < |9tj}g g }}t:t;|dOdPD ])} |4  |jrt5|j6j7 ||  8 }!|jdks|jdkr%t<|!|j|j}!t=|!}!|>|!}"t;d
D ]}#|?tj@dQtjd&\}"}#}#}#q.|jAdksM|jBdkrgtC|j6j7dJ|jA|jB |?tj@dQtjd&\}"}#}#}#tD|j6j7dJ||9tj}$|$8 }%|%d  |  < |%dN  |  < tE|j6j7dJ9tj}&tFtjG|%tj|dRtjG|&tj|dRd<||tt||	}|%8 9tj}|d  |9  < |dN  |9  < |&9tj}d}'d}(d})|)|jHk r|'stjI|"dS tjd&}*tjI|"dT tjd&}+|"dU },|"dV }-tJ|,||}.tJ|-||}/tK|j7dJ||}0tL|*|0||}1|1M dNkr#|1Nd}1tE|j7dK9tj}2tD|j7dK||9tj}3|38 }4|4d  t|  < |4dN  t|  < tjG|4tj|dRNd}5tjG|2tj|dRNd}6tO  ||.|/|1||5|6}7W d    n	1 sw   Y  tP|7|||*|+||||||	|
||dW\}8}9tQ|8D ]c\}:};|jrd|;dXdY< |jr|9|: 9tj}<t|;dY }=tR||<|=dZd[\}"}>}?|)|>7 })|?stS|j6d\r|j6T rd}(d}' nn|?|;\}"}#}'}#|)dN7 })|'rd}( n
|)|jHkr nq|)|jHkr
n	|)|jHk r|'r|Ut|( |U|) qttV|}@td]|@d^d_tW| d`| dattV|db |jXr|j|j|j||||@|j|j|jdc
}7tY|jXjZj[dddd t\|jXde t]j^|7 dfdg W d    d S 1 s~w   Y  d S d S )hNz--checkpointT)typerequiredz--benchmarklibero_spatial)r   defaultz	--task_idr   z--n_episodes   z--max_stepsiX  z--seedz
--shift_dxr2   z
--shift_dyz--clean_scene
store_true)r   z--zero_rotationz
--teleportz--cam_thetazBEV camera polar angle (deg))r   r   helpz	--cam_phizBEV camera azimuth (deg)z
--out_json cudarv   F)map_locationweights_only
min_height
max_heightmin_gripmax_gripr   rn   r   r   r   
n_rot_binsn_window   r   
bev_K_normbev_extrinsiczLoaded ckpt: epoch=epochz, n_window=z  shift_dx=z+.3fz
 shift_dy=z  clean=z  zero_rot=z  teleport=fusion_modesumz  fusion_mode:     1d_pca)r   n_height_binsn_gripper_binsr   r   	pred_sizerotation_moder   model_state_dict)strictzTask: [z] datasetsrc                 S   s   g | ]	}| d r|qS )demo_)
startswith.0kr)   r)   r*   
<listcomp>  s    zmain.<locals>.<listcomp>r8   c                    s    g | ]} d | d d qS )zdata/z/statesr   r)   r   fr)   r*   r     s     
bddl_files	agentviewrobot0_eye_in_hand)bddl_file_namecamera_heightscamera_widthscamera_namesu   ✓ Clean scene appliedr    Episodes)descrr   )ro   devicer   robot0_eef_quatagentview_imagerobot0_eye_in_hand_image)r   r,   rs   r   )r   _check_successz
Success Rate: z.1%z  (/z)  avg_steps=z.1f)

checkpointshift_dxshift_dy
n_episodes	successesstep_countssuccess_rateclean_scenezero_rotationteleport)parentsexist_okwr0   )indent)_argparseArgumentParseradd_argumentstrintr{   
parse_argstorchr   r   is_availableloadr   r<   asarrayr}   get
LIBERO_IMGr   printr   r   r   r   r   r   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minr   lenproblem_folder	bddl_filer   seedresetri   r   rL   r!   rx   r   ra   r+   r/   set_init_stater   r   	cam_thetacam_phir[   r   r   r   tensor	max_stepsr?   r   r   r   rm   	unsqueezeno_gradr   	enumerater   hasattrr   r~   meanr   Zout_jsonr   parentmkdiropenjsondump)Apargsr   ckptr   r   r   r   r   r   r   r   r   r   r   r   r   r   r6   bev_xyz_tablebenchtask	demo_path	demo_keysinit_statesr   bddlr   K_bev_pixelr   r   r   ep_idx
init_stater   r   	cur_bev_Kcur_bev_K_normcur_bev_extr   successstep_idxr   r   rgb_bev_obsrgb_wrist_obsimg_bev	img_wristworld_to_cam_bev	start_pixwrist_ext_np
wrist_K_npwrist_K_norm	wrist_K_twrist_ext_toutwindow_actionsr   r   r   targetr   n_servoep_donesrr)   r   r*   main   s2  4
 

"( (

 22$rZ  __main__)r   r   )9__doc__r  sysr  r5  rw   r<   r  cv2r  pathlibr   r   r5   r   rN   r  insertdirname__file__environ
setdefaultlibero.liberor   r  r   libero.libero.envsr   robosuite.utils.camera_utilsr   r   r   r   model_dino_volume_query_2viewr   r   r   utilsr   r  r   r   r  r   r   r"   r-   r?   r.   r+   r/   r[   ri   r   r   rZ  __name__r)   r)   r)   r*   <module>   sN     	&

= ,
