o
    ]iz7                     @   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
 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dddddddddddddddZddgZe	g dZeddedddeddedddeddeddddZ dZ!d Z"d!d" Z#d#d$ Z$d%d& Z%d'd( Z&d)d* Z'd+d, Z(d-d. Z)dDd1d2Z*d3d4 Z+d5d6 Z,	:	<dEd=d>Z-d?d@ Z.dAdB Z/e0dCkre/  dS dS )Fa  Generate OOD object position dataset for PARA training.

Creates a 16x16 grid of (dx, dy) object position shifts for task 0.
For each grid point, replays the first demo trajectory with shifted objects
using servo teleport, and saves the resulting frames + EEF data in the
same format as parsed_libero for CachedTrajectoryDataset.

Usage:
    python generate_ood_objpos.py --grid_size 16 --out_root /data/libero/ood_objpos_task0
    N)Path)tqdmz/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   	   pick)
qpos_startrole   
distractor      %   place)akita_black_bowl_1akita_black_bowl_2	cookies_1glazed_rim_porcelain_ramekin_1Zplate_1Zwooden_cabinet_1_mainZflat_stove_1_main)      $@r   g?      )qposdof   !   )r   r   r   	agentviewZrobot0_eye_in_handc                 C   s   | t  S )N)STATE_QPOS_OFFSET)r    r$   B/data/cameron/para_normalized_losses/libero/generate_ood_objpos.py_si9   s   r&   c                 C   sF   |   }t D ]\}}|d dkr t|d }t|||d < q|S )Nr   r   r      )copyTASK0_OBJECTSitemsr&   DISTRACTOR_POS)statesnameinfoir$   r$   r%   move_distractors_in_state=   s   r1   c                 C   sz   t  }t D ]\}}|d dkr| j| d}|| qt| jjD ]}| jj| }||v r:d| jj	| d< q&d S )Nr   r   _main        r'   )
setr)   r*   modelbody_name2idaddrangeZngeomZgeom_bodyidZ	geom_rgba)simZdistractor_bodiesr.   r/   bidZgeom_idZbody_idr$   r$   r%   hide_distractors_visualF   s   
r;   c                 C   sB   t  D ]}t| jj|d j|d jd < d| jj|d < qd S )Nr   r'   r3   r   )DISTRACTOR_DOFSvaluesr+   datar   startZqvel)r9   Zdofsr$   r$   r%   freeze_distractorsR   s    r@   c                 C   sV   |   }t D ] }|d dv r(t|d }||  |7  < ||d   |7  < q|S )Nr   )r   r   r   r   )r(   r)   r=   r&   )r,   dxdyr-   r/   r0   r$   r$   r%   shift_pick_placeX   s   rC   c                 C   s8   t D ]}| j|}tg d| jj|< q|   d S )N)r   r   g      )FURNITURE_BODIESr5   r6   nparrayZbody_posforward)r9   r.   r:   r$   r$   r%   hide_furnitureb   s   rH   c                 C   s^   g }t t|D ]!}| ||  | jj  | j }|tj	|d tj
d qt	|S )z,Replay demo states to extract EEF positions.robot0_eef_posdtype)r8   lenset_init_stateenvr9   rG   _get_observationsappendrE   rF   float64)rN   statesZeef_positionstobsr$   r$   r%   extract_demo_eef_positionsi   s   

rU   2   ~jth?c                 C   s   | j j}d }t|D ]O}| j  }tj|d tjd}	||	 }
tj|
}||k r-|} n-t	|
d dd}tj
dtjd}||d d< ||d< | |\}}}}t| |rY nq
|d u rc| j  }|S )	NrI   rJ   g?            ?   r'      )rN   r9   r8   rO   rE   rF   rQ   linalgnormclipzerosfloat32stepr@   )rN   
target_posgripper_cmd	max_servo	thresholdr9   rT   _Zcur_obsZcur_posdeltadistZdelta_clippedactiondoner$   r$   r%   servo_to_positiont   s,   

rk   c                 C   sT   | dddf }t dt|D ]}|| dkr#||d  dkr#|  S qt|d S )zAFind first timestep where gripper transitions from open to close.Nr[   r   r      )r8   rL   )actionsgripperrS   r$   r$   r%   find_grasp_timestep   s   ro   c                    s.   t dd|d dd } fdd|D S )z'Linear interpolation from start to end.r   r   Nc                    s   g | ]
}|    qS r$   r$   ).0aend_pos	start_posr$   r%   
<listcomp>   s    z)interpolate_waypoints.<locals>.<listcomp>)rE   linspace)rt   rs   Zn_stepsZalphasr$   rr   r%   interpolate_waypoints   s   rw   r'   Q     r[      c           )         s2  | j j}| }}|| }|| }d| j _d| j _|d  }t|}t|||}| | |  t	dD ]}| 
tjdtjd t| q5t|}td|| }| j  }tj|d tjd}||  }|d  |7  < |d  |7  < d g g g g  fd	d
}|| t|||}|D ]}t| |d|
d}|| qtt	|t||}d}|D ][}||  } | d  |7  < | d  |7  < |t|k rtt||df dd  dkr|	dkr| d  |	7  < t| |  |
d}|| | j jst| j dr| j  rd}qt|ttj}!t|t||tj}"t |t||tj}#|# }$|$d  |  < |$d  |  < t!}%g }&t	t|%D ]'}'t"|%|'|'d  tj|"||d }(|&#tj|(d |(d gtjd qPt!t!tjtjdt!|&|!|$|"td|||dS )zLGenerate a shifted trajectory with natural start (interpolate to pre-grasp).r   F   rZ   rJ   rI   r   rX   c                    sl   t j| d t jd}t j| d t jd}| |   t | t d  }| d S )NrI   rJ   Zrobot0_eef_quatZ_image)rE   rF   r`   rP   flipud	AGENT_CAMr(   )rT   eef_poseef_quatZ	agent_imgrc   Zrecorded_eef_posZrecorded_eef_quatZrecorded_frames_agentZrecorded_gripperr$   r%   record   s   


z#generate_trajectory.<locals>.record)rd   r[   rY   rl   _check_successTgv/?)frames_agentr   r   rn   pix_uvcam_extrinsic
cam_K_normworld_to_cambase_zsuccessrA   rB   )$rN   r9   timesteprj   r(   r1   rC   rM   rG   r8   ra   rE   r_   r`   r@   ro   maxrO   rF   rQ   rw   rk   listrL   floatr^   hasattrr   r   r~   astyper
   r	   stackr   rP   ))rN   rR   rm   eef_origrA   rB   	center_dx	center_dyframe_stridez_offsetrd   
image_sizeZpregrasp_leadZinterp_stepsr9   HWZtotal_dxZtotal_dyZstate_0rf   Zt_graspZ
t_pregrasprT   Zhome_posZpregrasp_targetr   Z
interp_wpsZwpZphase2_indicesr   rS   rb   Z	agent_extZ	agent_w2cZagent_KZagent_K_normZeef_arrZpix_uvsr0   Zpix_rcr$   r   r%   generate_trajectory   s   




$
&r   c                 C   s&  t |}|d }|jddd t| d D ]\}}tt||dd t|tj qt	|d | d  t	|d	 | d
  t	|d | d  t	|d | d  t	|d | d  t	|d | d  t	|d | d  t	|d | d  t	|d tj
t| d dftjd dS )z-Save trajectory data in parsed_libero format.ZframesTparentsexist_okr   Z06dz.pngeef_pos.npyr   zeef_quat.npyr   zgripper.npyrn   z
pix_uv.npyr   zcam_extrinsic.npyr   zcam_K_norm.npyr   zworld_to_cam.npyr   z
base_z.npyr   zactions.npyrZ   rJ   N)r   mkdir	enumeratecv2ZimwritestrZcvtColorZCOLOR_RGB2BGRrE   saver_   rL   r`   )r>   demo_dirZ
frames_dirr0   framer$   r$   r%   	save_demo  s   (,r   c                  C   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tdd | jdtdd | jdtdd | jdtdd | jdtdd |  }t |j	  }|
|j}tjtd||j}tjtd|j|j}t|d5}tdd |d   D }|t|jt|d!  }|d"| d# d$ }	|d"| d% d$ }
W d    n1 sw   Y  td&|j  td'| d(t|	 d) td* t||j|jtgd+}|d |   t!||	}|"  t#t$d, d- }|	d |  }|	d |d!   }td.|d/d(|d/d0 |j%}t&'|j(|j)|}t&'|j*|j+|}td1| d2| d3||  d4 td5|j( d(|j) d6 td7|j* d(|j+ d6 t||j|jtgd+}|d |   d8|j,_-|j,j.}t/| t0| t1|j2|j	 d9|j  }d}d}|j3d:d:d; t&j4|d< ||||d= t5t6|d>d?D ]~\}}t5|D ]t\}}|| | }|d@|  }|dA 7 r|d!7 }qd|j,_8dB|j,_9t:||	|
||||||j;|j<|j=|jdC}t>|| |d!7 }|dD r|d!7 }|dE dkr@tdF| dG||  dH|dIdJ|dIdK|dD  dL| dG| d0 q͐q|"  tdM| dG| dN tdO|  d S )PNz--grid_sizer   )typedefaultz--dx_mingQz--dx_maxgI+?z--dy_mingz--dy_maxg?z--image_sizerz   z--frame_strider'   z
--z_offsetrx   z--max_servory   z--benchmarklibero_spatialz	--task_idr   z	--demo_idz
--out_rootz/data/libero/ood_objpos_task0datasets
bddl_filesrc                 S   s   g | ]	}| d r|qS )demo_)
startswith)rp   kr$   r$   r%   ru   9  s    zmain.<locals>.<listcomp>r>   r   zdata/z/statesr$   z/actionszTask: zDemo: z, z framesz%Extracting original EEF trajectory...)bddl_file_namecamera_heightscamera_widthscamera_namesr   r   zCentering offset: (z+.3f)z
Grid: xz = z trajectoriesz  dx: []z  dy: [i Ztask_Tr   zgrid_meta.npz)dx_valsdy_valsr   r   rA   )descr   r   F)r   r   rd   r   r   
   z  [/z] dx=z.3fz dy=z	 success=z	 (total: z
Done. z trajectories succeeded.z	Saved to )?argparseArgumentParseradd_argumentintr   r   
parse_argsbm_libget_benchmark_dictr   get_taskZtask_idospathjoinr   get_task_demonstrationproblem_folder	bddl_fileh5pyFilesortedkeysminZdemo_idrL   printr.   r   r   r~   seedresetrU   closer&   r)   Z	grid_sizerE   rv   Zdx_minZdx_maxZdy_minZdy_maxrN   horizonr9   rH   r;   r   Zout_rootr   savezr   r   existsr   rj   r   r   r   rd   r   )parserargsbenchtask	demo_pathr   f	demo_keysZdemo_keyrR   rm   env_tmpr   bowl_ir   r   Nr   r   rN   r9   Ztask_dirZ	successestotalr0   rA   jrB   Zdemo_idxr   r>   r$   r$   r%   main"  s   

 

	
.
r   __main__)rV   rW   )r'   rx   ry   rz   r[   r{   )1__doc__r   r   syspathlibr   r   r   numpyrE   r   r   insertenviron
setdefaultlibero.liberor   r   r   libero.libero.envsr   Zrobosuite.utils.camera_utilsr   r	   r
   r   r#   r)   rD   rF   r+   slicer<   r~   Z	WRIST_CAMr&   r1   r;   r@   rC   rH   rU   rk   ro   rw   r   r   r   __name__r$   r$   r$   r%   <module>   s^    
	

	
sm
