o
    ºÒi€6  ã                   @   s`  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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ZdZdZdZ dZ!g d¢Z"ddgZ#g d¢Z$e	 %g d¢¡Z&e'ddƒe'ddƒe'ddƒdœZ(dd„ Z)dd„ Z*dd„ Z+d d!„ Z,d"d#„ Z-d$d%„ Z.d/d(d)„Z/d*d+„ Z0d,d-„ Z1e2d.kr®e1ƒ  dS dS )0a  Generate OOD viewpoint dataset (viewpoint variation only, fixed object position).

Creates a grid of camera viewpoints on a spherical cap around the default camera.
Multiple demos per viewpoint (with slight random object position jitter for variety).
Clean scene (no distractors/furniture).

Usage:
    python generate_ood_viewpoint.py --n_views 8 --theta_max 25 --demos_per_view 10
é    N)ÚPath)Útqdm)ÚRotationz/data/cameron/LIBEROÚLIBERO_DATA_PATHz/data/libero)Ú	benchmarkÚget_libero_path)ÚOffScreenRenderEnv)Úget_camera_extrinsic_matrixÚget_camera_intrinsic_matrixÚget_camera_transform_matrixÚ#project_points_from_world_to_cameraÚ	agentviewçÍÌÌÌÌÌì?é   é	   é%   )é   é   é   Zwooden_cabinet_1_mainZflat_stove_1_main)Zakita_black_bowl_2_mainZcookies_1_mainZ#glazed_rim_porcelain_ramekin_1_main)ç      $@r   r   é   é   é   é!   c           	      C   sÂ   ||  }|t j |¡d  }| }t  g d¢¡}tt  ||¡ƒdkr)t  g d¢¡}t  ||¡}|t j |¡d  }t  ||¡}t j|||gdd}t 	|¡ 
¡ }t  |d |d |d	 |d
 g¡S )Ngê-™—q=)ç        r   ç      ð?ç®Gáz®ï?)r   r   r   éÿÿÿÿ)Úaxisé   r   r   é   )ÚnpÚlinalgÚnormÚarrayÚabsÚdotÚcrossÚstackÚScipyRZfrom_matrixZas_quat)	Úcam_posÚtargetÚforwardZcam_zZup_hintZcam_xZcam_yÚRÚq© r/   úE/data/cameron/para_normalized_losses/libero/generate_ood_viewpoint.pyÚlook_at_quat-   s   "r1   c                 C   sx  t j | | ¡}| | | }t  g d¢¡}tt  ||¡ƒdkr&t  g d¢¡}t  ||¡}|t j |¡ }t  ||¡}t  |¡}	t  d|	|¡}
t  ddt j	 dd|   |¡}g g g g f\}}}}t
|
ƒD ]I\}}t
|ƒD ]@\}}t  |¡t  |¡ | t  |¡t  |¡ |  t  |¡|  }|||  }| |¡ | t||ƒ¡ | |¡ | |¡ qlqdt  |¡t  |¡|
|||fS )N)r   r   r   r   )r   r   r   r   r    r   )r!   r"   r#   r$   r%   r&   r'   ÚradiansÚlinspaceÚpiÚ	enumerateÚsinÚcosÚappendr1   )Údefault_posZlook_at_pointÚn_viewsZtheta_max_degÚradiusZdefault_dirÚupÚrightZtrue_upÚ	theta_maxÚthetasÚphisÚ	positionsZquaternionsZtheta_indicesZphi_indicesÚtiÚthetar4   ÚphiÚoffsetÚposr/   r/   r0   Úgenerate_viewpoint_grid<   s4   
 ÿþ

ø	rG   c                 C   sT   | d d …df }t dt|ƒƒD ]}|| dkr#||d  dkr#|  S qt|ƒd S )Né   r   r   r    )ÚrangeÚlen)ÚactionsÚgripperÚtr/   r/   r0   Úfind_grasp_timestepX   s   €rN   c              	   C   s´   t D ]}zt g d¢¡| jj| j |¡< W q ty   Y qw |  ¡  tƒ }t	D ]}z| 
| j |¡¡ W q( ty>   Y q(w t| jjƒD ]}| jj| |v rWd| jj| d< qEd S )N)r   r   g      Àr   r   )ÚFURNITURE_BODIESr!   r$   ÚmodelÚbody_posÚbody_name2idÚ	Exceptionr,   ÚsetÚDISTRACTOR_BODIESÚaddrI   ÚngeomÚgeom_bodyidÚ	geom_rgba)ÚsimÚfnameZdist_bodiesÚdnÚgidr/   r/   r0   Úsetup_clean_scene`   s&   "ÿÿ€þr^   c                 C   s6   t  ¡ D ]\}}t| jj||d …< d| jj|< qd S )Nr   r   )ÚDISTRACTOR_DOFSÚitemsÚDISTRACTOR_POSÚdataÚqposÚqvel)rZ   ÚqpsÚdofr/   r/   r0   Úfreeze_distractorsr   s   þrg   c                 C   sh   |   ¡ }ttfD ]}|t }||  |7  < ||d   |7  < qtD ]}|t }t|||d …< q#|S )Nr   r   )ÚcopyÚ	PICK_QPOSÚ
PLACE_QPOSÚSTATE_QPOS_OFFSETÚDISTRACTOR_QPOS_STARTSra   )ÚstateÚdxÚdyÚsre   Úsir/   r/   r0   Úshift_statex   s   rr   é   çú~j¼t“h?c                 C   s¶   | j j}d }t|ƒD ]I}| j  ¡ }tj|d tjd}|| }	tj |	¡|k r) n+tj	dtj
d}
t |	d dd¡|
d d…< ||
d< |  |
¡\}}}}t|ƒ |rS nq
|pZ| j  ¡ S )	NÚrobot0_eef_pos©Údtypeé   gš™™™™™©?r   r   r   rH   )ÚenvrZ   rI   Ú_get_observationsr!   r$   Úfloat64r"   r#   ÚzerosÚfloat32ÚclipÚsteprg   )ry   r+   Úgripper_cmdÚ	max_servoÚ	thresholdrZ   ÚobsÚ_ÚcurÚdeltaÚactionÚdoner/   r/   r0   Úservo_to„   s"   
ÿr‰   c                    s.   t  dd|d ¡dd … }‡ ‡fdd„|D ƒS )Nr   r   c                    s   g | ]
}ˆ|ˆ ˆ   ‘qS r/   r/   )Ú.0Úa©ÚendÚstartr/   r0   Ú
<listcomp>™   s    z)interpolate_waypoints.<locals>.<listcomp>)r!   r3   )rŽ   r   ÚnÚalphasr/   rŒ   r0   Úinterpolate_waypoints—   s   r’   c            D         s	  t  ¡ } | jdtddd | jdtddd | jdtd	d
d | jdtddd | jdtddd | jdtddd | jdtddd | jdtdd | jdtdd | jdtdd | jdtdd |  ¡ }t ¡ d  ƒ }| 	d!¡}t
j td"ƒ| d!¡¡}t
j td#ƒ|j|j¡}t |d$¡+}td%d&„ |d'  ¡ D ƒƒd! }|d(|› d) d* }|d(|› d+ d* }	W d   ƒ n1 s¿w   Y  td,ƒ t||j|jtgd-}
|
 d!¡ |
 ¡  g }tt|ƒƒD ]}|
 || ¡ |
jj  ¡  | !t"j#|
j $¡ d. t"j%d/¡ qät" #|¡}|
 &¡  t't( }|d! |  }|d! |d0   }td1|d2›d3|d2›d4ƒ t||j|jtgd-}| d!¡ | ¡  d5|j_)|jj}t*|ƒ |j }}|j+ ,t¡}|j-j.|  /¡ }|j-j0|  1dd¡}|d d …d6f  }t2|d6  |d6 d7  }|||  }td8|› d9|› ƒ t3|||j4|j5ƒ\}}}}}}t|ƒ} | |j6 }!td:|j4› d;|j4› d<| › d=|j6› d>|!› d?ƒ td@t" 7|¡ 8d0¡› ƒ tdAt" 7|¡ 8d0¡› ƒ tdB|j9› d3|j:› dC|j;› d3|j<› dD	ƒ t=|j>ƒd  dE }"|"j?dFdFdG t"j@|"dH t" 7|¡t" 7|¡|j4|j5|j6|||||j9|j:|j;|j<dI t"jA BdJ¡}#tC|	ƒ}$tDd!|$dK ƒ}%d!}&d!}'tEt| ƒdLdMD ]q}(||( |j+jF|< ||( |j+jG|< t|j6ƒD ]W})|"dN|&›  }*|*dO  H¡ rl|&d07 }&qU|# I|j9|j:¡}+|# I|j;|j<¡},||+ }-||, }.d!|j_JdP|j_KtL|d! |-|.ƒ}/| |/¡ |  ¡  tdQƒD ]}0| Mt"jNdRt"jOd/¡ tP|ƒ q¡|j $¡ }1t"j#|1d. t"j%d/}2||%  /¡ }3|3d!  |-7  < |3d0  |.7  < dS‰ g g g g f\‰‰‰‰‡ ‡‡‡‡fdTdU„}4|4|1ƒ tQ|2|3dƒD ]}5tR||5dSddV}1|4|1ƒ qùtSt|%t|ƒ|jTƒƒ}6dP}7|6D ]a}||  /¡ }8|8d!  |-7  < |8d0  |.7  < |t|	ƒk rDtt" U|	|dKf dWd0¡ƒ‰ ˆ d!krX|jVd!krX|8d6  |jV7  < tR||8ˆ ddV}1|4|1ƒ |jjKsvtW|jdXƒrx|j X¡ rxdF}7q|*j?dFdFdG |*dY }9|9j?dFdZ tYˆƒD ]\}:};tZ [t|9|:d[›d\ ƒtZ \|;tZj]¡¡ qt" ^ˆ¡}<t_|t||ƒ}=t`|t||ƒ}>|> /¡ }?|?d!  |  < |?d0  |  < ta|tƒ}@g }Att|<ƒƒD ]'}Btb|<|B|Bd0 …  ct"j%¡|=||ƒd! }C|A !t"j#|Cd0 |Cd! gt"jOd/¡ qÜt" d|*dO |<¡ t" d|*d] t" ^ˆ¡¡ t" d|*d^ t"j#ˆt"jOd/¡ t" d|*d_ t" ^|A¡¡ t" d|*d` |@ ct"jO¡¡ t" d|*da |? ct"jO¡¡ t" d|*db |= ct"jO¡¡ t" d|*dc t" Odd¡¡ t" d|*de t"jNt|<ƒdRft"jOd/¡ t"j@|*df |(|)||( ||( |+|,dg |7rŠ|'d07 }'|&d07 }&|&dh d!kr¬tdi|&› dj|!› dk|(› dl|7› dm|'› dj|&› d4ƒ qUq=| &¡  tdn|'› dj|&› doƒ tdp|"› ƒ d S )qNz	--n_viewsé   z(Grid size (n_views x n_views viewpoints))ÚtypeÚdefaultÚhelpz--theta_maxrs   zMax polar angle in degreesz--demos_per_viewé
   zDemos per viewpointz--dx_mingš™™™™™Ù¿zMin object dx offsetz--dx_maxg{®Gáz„¿zMax object dx offsetz--dy_ming333333Ó¿zMin object dy offsetz--dy_maxg333333Ó?zMax object dy offsetz--image_sizeiÀ  )r”   r•   z--frame_strider   z
--z_offsetg¸…ëQ¸Ž¿z
--out_rootz/data/libero/ood_viewpoint_v2Úlibero_spatialr   ÚdatasetsÚ
bddl_filesÚrc                 S   s   g | ]	}|  d ¡r|‘qS )Údemo_)Ú
startswith)rŠ   Úkr/   r/   r0   r   ±   s    zmain.<locals>.<listcomp>rb   zdata/z/statesr/   z/actionszExtracting EEF trajectory...)Úbddl_file_nameÚcamera_heightsÚcamera_widthsÚcamera_namesru   rv   r   zCenter offset: (z+.3fz, ú)i † r    g:Œ0âŽyE>zDefault cam: z, look-at: Ú
Úxz = z viewpoints x z	 demos = z	 episodeszTheta: zPhi: zObject position range: dx=[z], dy=[ú]Útask_0T)ÚparentsÚexist_okzviewpoint_meta.npz)Ú
thetas_degÚphis_degr:   r>   Údemos_per_viewÚvp_positionsÚvp_quatsÚ	center_dxÚ	center_dyÚdx_minÚdx_maxÚdy_minÚdy_maxé*   rH   Z
Viewpoints)Údescrœ   zeef_pos.npyFé   rx   g      ð¿c                    s`   ˆ  tj| d tjd¡ ˆ  tj| d tjd¡ ˆ  ˆ ¡ ˆ  t | t› d ¡ ¡ ¡ d S )Nru   rv   Úrobot0_eef_quatÚ_image)r8   r!   r$   r}   ÚflipudÚ	AGENT_CAMrh   )Úo©r€   Úrec_eefZ
rec_framesÚrec_gripÚrec_quatr/   r0   Úrecord  s   
"zmain.<locals>.record)r   r   Ú_check_successÚframes)r©   Ú06dz.pngzeef_quat.npyzgripper.npyz
pix_uv.npyzcam_extrinsic.npyzcam_K_norm.npyzworld_to_cam.npyz
base_z.npygÉv¾Ÿ/í?zactions.npyzmeta.npz)ÚviÚdiÚ	theta_idxÚphi_idxrn   ro   é2   z  [ú/z] vi=z	 success=z	 (total: z
Done. z succeeded.z	Saved to )eÚargparseÚArgumentParserÚadd_argumentÚintÚfloatÚstrÚ
parse_argsÚbm_libÚget_benchmark_dictÚget_taskÚosÚpathÚjoinr   Úget_task_demonstrationÚproblem_folderÚ	bddl_fileÚh5pyÚFileÚsortedÚkeysÚprintr   Ú
image_sizer»   ÚseedÚresetrI   rJ   Úset_init_statery   rZ   r,   r8   r!   r$   rz   r{   Úcloseri   rk   Úhorizonr^   rP   Úcamera_name2idrb   Úcam_xposrh   Úcam_xmatÚreshapeÚTABLE_ZrG   r:   r>   r¬   ÚdegreesÚroundr±   r²   r³   r´   r   Úout_rootÚmkdirÚsavezÚrandomÚRandomStaterN   Úmaxr   r*   Úcam_quatÚexistsÚuniformÚtimesteprˆ   rr   r   r|   r}   rg   r’   r‰   ÚlistÚframe_strider~   Úz_offsetÚhasattrrÂ   r5   Úcv2ÚimwriteÚcvtColorÚCOLOR_RGB2BGRr(   r   r
   r	   r   ÚastypeÚsave)DÚparserÚargsÚbenchÚtaskÚ	demo_pathrÚ   ÚfÚdkÚstatesrK   Úenv_tmpÚeef_origrM   Úbowl_sir¯   r°   ry   rZ   ÚHÚWÚcam_idr9   rè   r,   Út_hitÚlook_atr­   r®   r?   r@   rÇ   rÈ   Ún_viewpointsÚn_totalÚtask_dirÚrngÚt_graspÚ
t_pregraspÚdemo_idxÚ	successesrÅ   rÆ   Údemo_dirÚ	dx_offsetÚ	dy_offsetÚtotal_dxÚtotal_dyÚstate_0r„   rƒ   Úhome_posÚpregrasp_targetrÁ   ÚwpÚphase2Úsuccessr+   Z
frames_dirÚfiÚframeÚeef_arrÚw2cÚKÚK_normÚextÚpix_uvsÚeiÚpix_rcr/   r½   r0   Úmainœ   sP  
ýÿ"
ÿ
ÿ
.*
ù	

$€*

ÿÿ&$þ.€¡ar.  Ú__main__)rs   rt   )3Ú__doc__rË   rÕ   ÚsysÚpathlibr   rû   rÛ   Únumpyr!   r   Zscipy.spatial.transformr   r)   rÖ   ÚinsertÚenvironÚ
setdefaultÚlibero.liberor   rÒ   r   Úlibero.libero.envsr   Úrobosuite.utils.camera_utilsr	   r
   r   r   r»   rê   rk   ri   rj   rl   rO   rU   r$   ra   Úslicer_   r1   rG   rN   r^   rg   rr   r‰   r’   r.  Ú__name__r/   r/   r/   r0   Ú<module>   sL    	
 A
ÿ