
    ni                        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Jr  S SKJr  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rS	r " S
 S\R>                  5      r g)    N)spaces)seeding)bullet_client   )racecar)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)RacecarZEDGymEnv   human	rgb_array2   )zrender.modeszvideo.frames_per_second
   TFc                    [        S5        SU l        Xl        X l        X0l        SU l        SU l        XPl        SU l        SU l	        X@l
        U R                  (       a(  [        R                  " [        R                  S9U l        O[        R                  " 5       U l        U R!                  5         U R#                  5         [%        U R'                  5       5      n[(        R*                  " [(        R,                  " [(        R.                  5      R0                  /U-  5      nU(       a  [2        R4                  " S5      U l        OWS	nS
U l        [(        R*                  " U R8                  /U-  5      n	[2        R:                  " U	* U	[(        R.                  S9U l        [2        R:                  " SSU R                  U R                  S4[(        R<                  S9U l        S U l         g )Ninitg{Gz?r   d   r   )connection_mode	      r   )dtype      )lowhighshaper   )!print	_timeStep	_urdfRoot_actionRepeat_isEnableSelfCollision_ballUniqueId_envStepCounter_renders_width_height_isDiscretebcBulletClientpybulletGUI_pseedresetlengetExtendedObservationnparrayfinfofloat32maxr   Discreteaction_space_action_boundBoxuint8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/racecarZEDGymEnv.py__init__RacecarZEDGymEnv.__init__   sV    
&MDNN%"7DDMDKDL!}}=dg!dgIIKJJL4467N xx"**!5!9!9 :^ KL //!,djdHHd001J>?k **k\;bjjQd#ZZA-0/3||T[[!.L.0hh8D
 DK    c                    U R                   R                  5         U R                   R                  U R                  5        U R                   R	                  [
        R                  R                  U R                  S5      5      nU HM  nU R                   R                  U5      u  p4US   US   US   S-   /nU R                   R                  X%U5        MO     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      XU
/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 R1                  5       U l        [4        R6                  " U R2                  5      $ )Nzstadium.sdfr   r   r   皙?   g       @gӇE!@zsphere2red.urdfi)urdfRootPathtimeStepr   )r,   resetSimulationsetTimeStepr   loadSDFospathjoinr   getBasePositionAndOrientationresetBasePositionAndOrientationrandommathsincosloadURDFr"   
setGravityr   Racecar_racecarr#   rangestepSimulationr0   _observationr1   r2   )r=   stadiumobjectsiposornnewposdistangballxballyballzs              rG   r.   RacecarZEDGymEnv.resetF   s   GGGG'WW__RWW\\$..-%PQN66q9hcAAA-f
gg--a= 
 rFMMO##D
fmmo
-C488C= E488C= EE))"'',,t~~GX*Y+0*?ADGGq!S!OODGG$..SWSaSabDMD3Z
gg 335D88D%%&&rJ   c                     SU l         g )Nr   )r,   r=   s    rG   __del__RacecarZEDGymEnv.__del__c   s	    DGrJ   Nc                 D    [         R                  " U5      u  U l        nU/$ N)r   	np_random)r=   r-   s     rG   r-   RacecarZEDGymEnv.seedf   s     ",,T2DND6MrJ   c                 R   U R                   R                  U R                  R                  5      u  pU R                   R	                  U5      nU R                   R                  U R
                  5      u  pEU R                   R                  X5      u  pgU R                   R                  XgXE5      u  pSn
SnUS   XS   -  -   US   XS   -  -   US   XS   -  -   S-   /nUS   XS   -  -   US   XS   -  -   US   XS   -  -   S-   /nUS   US   US	   /nU R                   R                  XU5      n/ S
QnU R                   R                  U R                  U R                  UUS9nUS   n[        R                  " UU R                  U R                  S45      nUU l        U R                  $ )Ng333333?      ?r   r      r      rM      )g   ?        r{   r{   r{   rw   r{   r{   r{   r{   g     g      r{   r{   g   zr{   )widthheight
viewMatrixprojectionMatrixr   )r,   rV   r_   racecarUniqueIdgetMatrixFromQuaternionr"   invertTransformmultiplyTransformscomputeViewMatrixgetCameraImager%   r&   r1   reshaperb   )r=   carposcarorncarmatballposballorn	invCarPos	invCarOrnballPosInCarballOrnInCardist0dist1eyePos	targetPosupviewMat
projMatriximg_arrrgb
np_img_arrs                       rG   r0   'RacecarZEDGymEnv.getExtendedObservationj   s   WW::4==;X;XYNFWW,,V4Fww<<T=O=OPG77226BI!%!;!;IRY!cLEEq	E1I%%vay5!93D'Dq	E1I%%+F
 	q	E1I%%vay5!93D'Dq	E1I%%+I )VAYq		*Bgg''2>GJ gg$$4;;,0LL076@ % BG !*CC$,,Q!?@J"DrJ   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?r   r   r   r   r   r   r   )r$   r,   rV   r_   r   r'   applyActionr`   r    ra   timesleepr   r0   rb   _terminationr#   _rewardr1   r2   )r=   actionbasePosrf   fwd	steeringsforwardsteer
realactionrd   rewarddones               rG   stepRacecarZEDGymEnv.step   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8rJ   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<   rL   g      Y@)fovaspectnearValfarVal)r|   r}   r~   r   rendererrx   )r1   r2   r,   rV   r_   r   !computeViewMatrixFromYawPitchRoll	_cam_dist_cam_yaw
_cam_pitchcomputeProjectionMatrixFOVfloatRENDER_WIDTHRENDER_HEIGHTr   r*   ER_BULLET_HARDWARE_OPENGL)
r=   modeclosebase_posrf   view_matrixproj_matrix_pxr   s
             rG   renderRacecarZEDGymEnv.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rJ   c                      U R                   S:  $ )Ni  )r#   ro   s    rG   r   RacecarZEDGymEnv._termination   s    $&&rJ   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   rz   )r,   getClosestPointsr_   r   r"   r/   )r=   closestPointsnumPtr   s       rG   r   RacecarZEDGymEnv._reward   s\    GG,,T]]-J-JDL^L^-24M EF	a ##fMrJ   z0.9.6)r    r8   r"   r#   r&   r'   r!   rb   r,   r_   r$   r   r   r%   r7   rt   r;   r<   rs   )r   F)__name__
__module____qualname____firstlineno__metadatapybullet_datagetDataPathrH   r.   rp   r-   r0   r   r   r   r   r   gym__version___render_reset_seed_step__static_attributes__ rJ   rG   r
   r
      s    &4QST( &113%)*X': D9<.' 3??#mG&<<GFEE	 =rJ   r
   )!rS   inspectrT   dirnameabspathgetfilecurrentframe
currentdir	parentdirsysinsertrY   r   r   	gym.utilsr   numpyr1   r   r*   pybullet_utilsr   r(    r   rX   r   pkg_resourcesr   r   r   Envr
   r   rJ   rG   <module>r      s    WW__RWW__W__W=Q=Q=S-TUV
GGOOBGGOOJ78	   1i    
      .    '~sww ~rJ   