
    ni                        S r SSKrSSKr\R                  R	                  \R                  R                  \R                  " \R                  " 5       5      5      5      r\R                  R	                  \R                  R	                  \5      5      r	\R                  R                  R                  S\	5        SSKrSSKrSSKrSSKJr  SSKJr  SSKrSSKrSSKJr  SSKJr  SSKrSSKrSSKJr   " S S	\R:                  5      rg)
zJThis file implements the gym environment of example PyBullet simulation.

    N)spaces)seeding)bullet_client)boxstack_pybullet_sim)parse_versionc                       \ rS rSrSrSS/SS.r\SSSSS	S	S
SS\R                  " 5       4S jr	S r
S rSS jrS rSS jrS rS rS r\" \R(                  5      \" S5      :  a  \r\r\r\rSrgSrg)PyBulletSimGymEnv   z5The gym environment to run pybullet simulations.


  human	rgb_array2   )zrender.modeszvideo.frames_per_secondTF      gq?c                 F   Xl         Xl        Xl        / U l        Xl        Xl        SU l        X l        X@l        X0l	        X`l
        Xpl        SU l        SU l        SU l        SU l        SU l        SR#                  Xg5      n[%        SU R                  -   5        U R                  (       a)  [&        R(                  " [*        R,                  US	9U l        O[&        R(                  " 5       U l        US
:X  a  U R.                  R1                  U R.                  R2                  SS9  U R.                  R1                  U R.                  R4                  SS9  U R.                  R1                  U R.                  R6                  SS9  U R.                  R1                  U R.                  R8                  SS9  U R.                  R;                  U5        U R=                  5         U R?                  5         U R@                  RC                  5       nU R@                  RE                  5       nU R@                  RG                  5       nSU l$        [J        RL                  " U RH                  /U-  5      n[N        RP                  " U* U5      U l)        [N        RP                  " X5      U l*        SU l+        XPl        g)zVInitialize the gym environment.

Args:
  urdf_root: The path to the urdf data folder.
r   g333333?r   iT        z--width={} --height={}z
urdf_root=)connection_modeoptionsF)flagenabler   N),_pybullet_sim_factory
_time_step
_urdf_root_observation_action_repeat_num_bullet_solver_iterations_env_step_counter
_is_render_debug_visualization_render_sleep_render_width_render_height	_cam_dist_cam_yaw
_cam_pitch_hard_reset_last_frame_timeformatprintbcBulletClientpybulletGUI_pybullet_clientconfigureDebugVisualizerCOV_ENABLE_GUICOV_ENABLE_RGB_BUFFER_PREVIEWCOV_ENABLE_DEPTH_BUFFER_PREVIEW$COV_ENABLE_SEGMENTATION_MARK_PREVIEWsetAdditionalSearchPathseedreset_example_simGetObservationUpperBoundGetObservationLowerBoundGetActionDimension_action_boundnparrayr   Boxaction_spaceobservation_spaceviewer)selfpybullet_sim_factoryrenderrender_sleepdebug_visualization
hard_resetrender_widthrender_heightaction_repeat	time_stepnum_bullet_solver_iterations	urdf_rootoptionstringobservation_highobservation_low
action_dimaction_highs                    g/home/james-whalen/.local/lib/python3.13/site-packages/pybullet_envs/prediction/pybullet_sim_gym_env.py__init__PyBulletSimGymEnv.__init__"   s6   " "6OOD')E&DO 3%%'DNDMDODD+22<OL	,
() oohllAMOd !oo/du$
44$:O:O:^:^<= 5 ?
44$$BB1 5 N
44$$DDQ 5 P
44$$IIRS 5 U 	11)<IIKJJL))BBD((AACO""557JD((D../*<=K

K<=D#ZZJDDK!    c                     Xl         g N)_args)rA   argss     rR   	configurePyBulletSimGymEnv.configurei   s    JrU   c                    U R                   (       a  U R                  R                  5         U R                  R                  [	        U R
                  5      S9  U R                  R                  U R                  5        U R                  R                  U R                  U R                  U R                  S9U l        OU R                  R                  SS9  SU l        U R                  5       $ )N)numSolverIterations)pybullet_clientrL   rJ   F)reload_urdfr   )r%   r-   resetSimulationsetPhysicsEngineParameterintr   setTimeStepr   r   	CreateSimr   r6   Resetr   _get_observationrA   s    rR   r5   PyBulletSimGymEnv.resetl   s    
++-
55!$"D"DE 6 G
''844>>//OOOO ? %d
 %0D
   ""rU   Nc                 D    [         R                  " U5      u  U l        nU/$ rW   )r   	np_random)rA   r4   s     rR   r4   PyBulletSimGymEnv.seed   s     ",,T2DND6MrU   c                 f   U R                   (       at  [        R                  " 5       U R                  -
  n[        R                  " 5       U l        U R                  U R                  -  U-
  nUS:  a  [        R
                  " U5        [        U R                  5       H8  nU R                  R                  U5        U R                  R                  5         M:     U =R                  S-  sl        U R                  5       nU R                  5       n[        R                  " U R!                  5       5      XV0 4$ )a  Step forward the simulation, given the action.

Args:
  action: the predicted state

Returns:
  observations: The actual state.
  reward: The reward for how well the prediction matches the actual state.
  done: Whether the episode has ended.
  info: A dictionary that stores diagnostic information.

Raises:
  ValueError: The action dimension is not the same as the number of motors.
  ValueError: The magnitude of actions is out of bounds.
r   r   )r   timer&   r   r   sleepranger6   ApplyActionr-   stepSimulationr   _reward_terminationr;   r<   rf   )rA   action
time_spenttime_to_sleep_rewarddones          rR   stepPyBulletSimGymEnv.step   s       99;!6!66j"iikd))DOO;jHm		

=! 4&&'
##F+
**, ( 	a\\^FD88D))+,fB>>rU   c           	         US:w  a  [         R                  " / 5      $ / SQnU R                  R                  UU R                  U R
                  U R                  SSS9nU R                  R                  S[        U R                  5      U R                  -  SSS	9n/ S
QnU R                  R                  U R                  U R                  UU[        R                  S9u    pg  n[         R                  " U[         R                  S9n[         R                  " XR                  U R                  S45      nUS S 2S S 2S S24   nU$ )Nr   )r   r   r   r      )cameraTargetPositiondistanceyawpitchrollupAxisIndex<   g{Gz?g      Y@)fovaspectnearValfarVal)      ?r   r   r   r   r   r   r   r   r   g     g      r   r   g   zr   )widthheight
viewMatrixprojectionMatrixrenderer)dtype      )r;   r<   r-   !computeViewMatrixFromYawPitchRollr"   r#   r$   computeProjectionMatrixFOVfloatr    getCameraImager!   r+   ER_BULLET_HARDWARE_OPENGLuint8reshape)	rA   modeclosebase_posview_matrixproj_matrixrw   pxr   s	            rR   rC   PyBulletSimGymEnv.render   s6   {XXb\H''II%MMoo J K ''BBuT//043E3EEt\a C cKK ,,;;  ""$33 < 5Qq! 288,I

9':':D<N<NPQ&RSI!Q(#IrU   c                 :    U R                   R                  5       nU$ rW   )r6   Termination)rA   	terminates     rR   rs   PyBulletSimGymEnv._termination   s    !!--/IrU   c                 
    SnU$ )Nr    )rA   rx   s     rR   rr   PyBulletSimGymEnv._reward   s    FMrU   c                 X    U R                   R                  5       U l        U R                  $ rW   )r6   GetObservationr   rg   s    rR   rf   "PyBulletSimGymEnv._get_observation   s%    ))88:DrU   z0.9.6)r:   r   rX   r"   r$   r#   r   r   r6   r%   r   r&   r   r   r-   r   r!   r   r    r   r   r>   rj   r?   r@   rW   )r   F)__name__
__module____qualname____firstlineno____doc__metadatar   pybullet_datagetDataPathrS   rZ   r5   r4   rz   rC   rs   rr   rf   r   gym__version___render_reset_seed_step__static_attributes__r   rU   rR   r	   r	      s      '4QST( %:!#' ",.&224E"N#,$?L8 3??#mG&<<GFEE	 =rU   r	   )r   osinspectpathdirnameabspathgetfilecurrentframe
currentdir	parentdirsysinsertmathrm   r   r   	gym.utilsr   numpyr;   r+   pybullet_utilsr   r)   pybullet_envs.predictionr   r   pkg_resourcesr   Envr	   r   rU   rR   <module>r      s    WW__RWW__W__W=Q=Q=S-TUV
GGOOBGGOOJ78	   1i     
     . : 	  '} }rU   