
    ni1                        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rS SKJr  S SKrS SKJr  SrS	r " S
 S\R>                  5      r g)    N)spaces)seeding   )racecar)bullet_client)parse_versioni  i  c                       \ rS rSrSS/SS.r\R                  " 5       SSSS4S jrS	 rS
 r	SS jr
S rS rSS jrS rS r\" \R$                  5      \" S5      :  a  \r\r\
r\rSrgSrg)RacecarGymEnv   human	rgb_array2   )zrender.modeszvideo.frames_per_secondTFc                    [        S5        SU l        Xl        X l        X0l        / U l        SU l        SU l        XPl        X@l	        U R                  (       a(  [        R                  " [        R                  S9U l        O[        R                  " 5       U l        U R                  5         Sn[         R"                  " U5      S-  nU(       a  [$        R&                  " S5      U l        OWSnS	U l        [         R,                  " U R*                  /U-  5      n	[$        R.                  " U	* U	[         R0                  S
9U l        [$        R.                  " U* U[         R0                  S
9U l        S U l        g )Ninitg{Gz?r   )connection_mode     	   r   )dtype)print	_timeStep	_urdfRoot_actionRepeat_isEnableSelfCollision_observation_ballUniqueId_envStepCounter_renders_isDiscretebcBulletClientpybulletGUI_pseednponesr   Discreteaction_space_action_boundarrayBoxfloat32observation_spaceviewer)
selfurdfRootactionRepeatisEnableSelfCollision
isDiscreterendersobservationDimobservation_high
action_dimaction_highs
             \/home/james-whalen/.local/lib/python3.13/site-packages/pybullet_envs/bullet/racecarGymEnv.py__init__RacecarGymEnv.__init__   s    
&MDNN%"7DDDM!}}=dg!dgIIKN ww~.5 //!,djdHHd001J>?k **k\;bjjQd#ZZ)9(9;KSUS]S]^DDK    c                    U R                   R                  5         U R                   R                  U R                  5        U R                   R	                  [
        R                  R                  U R                  S5      5      nSS[        R                  " 5       -  -   nS[        R                  " 5       -  nU[        R                  " U5      -  nU[        R                  " U5      -  nSnU R                   R                  [
        R                  R                  U R                  S5      XEU/5      U l        U R                   R                  SSS5        [         R"                  " U R                   U R                  U R                  S	9U l        SU l        [)        S
5       H  nU R                   R+                  5         M     U R-                  5       U l        [0        R2                  " U R.                  5      $ )Nzstadium.sdf   g       @gӇE!@r   zsphere2.urdfr   i)urdfRootPathtimeStepd   )r%   resetSimulationsetTimeStepr   loadSDFospathjoinr   randommathsincosloadURDFr   
setGravityr   Racecar_racecarr   rangestepSimulationgetExtendedObservationr   r'   r,   )r1   stadiumobjectsdistangballxballyballzis           r;   resetRacecarGymEnv.reset@   sW   GGGG'WW__RWW\\$..-%PQN rFMMO##D
fmmo
-C488C= E488C= EE))"'',,t~~~*V+0*?ADGGq!S!OODGG$..SWSaSabDMD3Z
gg 335D88D%%&&r>   c                     SU l         g )Nr   )r%   r1   s    r;   __del__RacecarGymEnv.__del__]   s	    DGr>   Nc                 D    [         R                  " U5      u  U l        nU/$ N)r   	np_random)r1   r&   s     r;   r&   RacecarGymEnv.seed`   s     ",,T2DND6Mr>   c                    / U l         U R                  R                  U R                  R                  5      u  pU R                  R                  U R
                  5      u  p4U R                  R                  X5      u  pVU R                  R                  XVX45      u  pxU R                   R                  US   US   /5        U R                   $ )Nr   r   )	r   r%   getBasePositionAndOrientationrQ   racecarUniqueIdr   invertTransformmultiplyTransformsextend)	r1   carposcarornballposballorn	invCarPos	invCarOrnballPosInCarballOrnInCars	            r;   rT   $RacecarGymEnv.getExtendedObservationd   s    DWW::4==;X;XYNFww<<T=O=OPG77226BI!%!;!;IRY!cLl1o|A?@r>   c                    U R                   (       a1  U R                  R                  U R                  R                  5      u  p#U R
                  (       a  / SQn/ SQnXA   nXQ   nXg/nOUnU R                  R                  U5        [        U R                  5       H  n	U R                  R                  5         U R                   (       a   [        R                  " U R                  5        U R                  5       U l        U R                  5       (       a    OU =R                   S-  sl        M     U R#                  5       n
U R                  5       n[$        R&                  " U R                  5      X0 4$ )N)	r   r   r   r   r   r   r   r   r   )	333333r   333333?rv   r   rw   rv   r   rw   r   )r   r%   rg   rQ   rh   r    applyActionrR   r   rS   timesleepr   rT   r   _terminationr   _rewardr'   r,   )r1   actionbasePosornfwd	steeringsforwardsteer
realactionr[   rewarddones               r;   stepRacecarGymEnv.stepn   s   WW::4==;X;XYlg 	*c<ige#jjMMj)4%%&
gg	

4>>"557d					
a ' \\^FD 88D%%&b88r>   c           	      0   US:w  a  [         R                  " / 5      $ U R                  R                  U R                  R
                  5      u  p4U R                  R                  UU R                  U R                  U R                  SSS9nU R                  R                  S[        [        5      [        -  SSS9nU R                  R                  [        [        UU[        R                   S	9u    px  n[         R                  " U5      n	U	S S 2S S 2S S
24   n	U	$ )Nr   r   r   )cameraTargetPositiondistanceyawpitchrollupAxisIndex<   g?g      Y@)fovaspectnearValfarVal)widthheight
viewMatrixprojectionMatrixrenderer   )r'   r,   r%   rg   rQ   rh   !computeViewMatrixFromYawPitchRoll	_cam_dist_cam_yaw
_cam_pitchcomputeProjectionMatrixFOVfloatRENDER_WIDTHRENDER_HEIGHTgetCameraImager#   ER_BULLET_HARDWARE_OPENGL)
r1   modeclosebase_posr   view_matrixproj_matrix_pxr   s
             r;   renderRacecarGymEnv.render   s   {XXb\GG99$--:W:WXMH'';;QYEI^^@DBF//ABHI < KK ''44<A,<OR_<_=@<A 5 CK ww--L5B9D?J7?7Y7Y	 . [Qq!
 I!Q(#Ir>   c                      U R                   S:  $ )Nr   )r   r_   s    r;   r{   RacecarGymEnv._termination   s    $&&r>   c                     U R                   R                  U R                  R                  U R                  S5      n[        U5      nSnUS:  a	  US   S   * nU$ )Ni'  ir      )r%   getClosestPointsrQ   rh   r   len)r1   closestPointsnumPtr   s       r;   r|   RacecarGymEnv._reward   s\    GG,,T]]-J-JDL^L^-24M EF	a ##fMr>   z0.9.6)r   r+   r   r   r    r   r   r%   rQ   r   r   r   r*   rd   r/   r0   rc   )r   F)__name__
__module____qualname____firstlineno__metadatapybullet_datagetDataPathr<   r\   r`   r&   rT   r   r   r{   r|   r   gym__version___render_reset_seed_step__static_attributes__ r>   r;   r
   r
      s    &4QST( &113%)$L':9<.' 3??#mG&<<GFEE	 =r>   r
   )!rG   inspectrH   dirnameabspathgetfilecurrentframe
currentdir	parentdirsysinsertrK   r   ry   r   	gym.utilsr   numpyr'   r#    r   rJ   pybullet_utilsr   r!   r   pkg_resourcesr   r   r   Envr
   r   r>   r;   <module>r      s    WW__RWW__W__W=Q=Q=S-TUV
GGOOBGGOOJ78	   1i    
        .  '`CGG `r>   