
    niB                         S SK Jr  S SKJr  SSKrSSKrSSKJrJ	r	J
r
JrJrJrJr   " S S\5      r " S S	\5      r " S
 S\5      r " S S\5      r " S S\5      r " S S\5      r " S S\5      r " S S\5      rg)   )SinglePlayerStadiumScene)MJCFBaseBulletEnv    N)HopperWalker2DHalfCheetahAntHumanoidHumanoidFlagrunHumanoidFlagrunHarderc                   d    \ rS rSrSS jrS rS rS rS rSr	Sr
S	r\" S
/5      rSrS rS rSrg)WalkerBaseBulletEnv   c                 j    SU l         SU l        SU l        SU l        [        R
                  " XU5        g )Nr   g     @@)camera_xwalk_target_xwalk_target_ystateIdr   __init__selfrobotrenders      [/home/james-whalen/.local/lib/python3.13/site-packages/pybullet_envs/gym_locomotion_envs.pyr   WalkerBaseBulletEnv.__init__
   s0    DMDDDLtF3    c                 <    [        USSSS9U l        U R                  $ )Ng#@gL7A`p?   )gravitytimestep
frame_skip)r   stadium_scene)r   bullet_clients     r   create_single_player_scene.WalkerBaseBulletEnv.create_single_player_scene   s)    1-:=;E=>@D r   c                 <   U R                   S:  a%  U R                  R                  U R                   5        [        R                  " U 5      nU R                  R                  [        R                  S5        U R                  R                  U R                  U R                  R                  5      u  U l        U l        U l        U l        [!        U R"                   Vs/ s HO  nU R                  U   R$                  U R                  U   R&                     U R                  U   R(                  4PMQ     sn5      U l        U R                  R                  [        R                  S5        U R                   S:  a  U R                  R-                  5       U l         U$ s  snf )Nr   r   )r   _prestoreStater   resetconfigureDebugVisualizerpybulletCOV_ENABLE_RENDERINGr   
addToScener#   ground_plane_mjcfpartsjdictordered_joints
robot_bodysetfoot_ground_object_namesbodies	bodyIndexbodyPartIndex
ground_ids	saveState)r   rfs      r   r*   WalkerBaseBulletEnv.reset   s8   
gg4<<(%AGG$$X%B%BAFCG::CXCX##55D7@DJ
D/BFB_B_aB_Q !JJqM00A1H1HI JJqM779B_a bDOGG$$X%B%BAFqWW&&(dl Has   AFc                      U R                   S:  $ )Nr   )_aliver   s    r   _isDoneWalkerBaseBulletEnv._isDone.   s    ;;?r   c                     U R                   R                  5         U R                   R                  R                  5       nUR	                  XU5        U R                   R                  U5        g)zFUsed by multiplayer stadium to move sideways, to another running lane.N)	cpp_robotquery_position	root_partposemove_xyzset_pose)r   init_xinit_yinit_zrG   s        r   
move_robotWalkerBaseBulletEnv.move_robot1   sO    NN!!#>>##((*DMM 	NND!r   g       gg      floorc                    U R                   R                  (       d5  U R                  R                  U5        U R                   R	                  5         U R                  R                  5       n[        U R                  R                  US   U R                  R                  -   U R                  R                  S   5      5      U l
        U R                  5       n[        R                  " U5      R                  5       (       d  [        SU5        SnU R                   nU R                  R#                  5       U l        [        U R                   U-
  5      nSn[%        U R                  R&                  5       Hm  u  px[)        S UR+                  5        5       5      n	U R,                  U	-  (       a  SU R                  R.                  U'   MT  SU R                  R.                  U'   Mo     U R0                  [        [        R2                  " XR                  R4                  -  5      R7                  5       5      -  n
XR8                  [        [        R:                  " U5      R7                  5       5      -  -  n
[        U R<                  U R                  R>                  -  5      nSnU(       ax  [        S5        [        U R                  5        [        S	5        [        U5        [        S
5        [        U
5        [        S5        [        U5        [        S5        [        U5        U R                  XZX/U l         U(       aI  [        S5        [        U R@                  5        [        S5        [        [C        U R@                  5      5        U RE                  X!U5        U =RF                  [C        U R@                  5      -  sl#        U[C        U R@                  5      [I        U5      0 4$ )Nr   r   z~INF~Tg        c              3   4   #    U  H  oS    US   4v   M     g7f)   r   N ).0xs     r   	<genexpr>+WalkerBaseBulletEnv.step.<locals>.<genexpr>X   s     ?.>1qt.>s         ?zalive=progresselectricity_costjoints_at_limit_costfeet_collision_costzrewards=zsum rewards)%scenemultiplayerr   apply_actionglobal_step
calc_statefloatalive_bonus	initial_zbody_rpyr?   rA   npisfiniteallprint	potentialcalc_potential	enumeratefeetr4   contact_listr9   feet_contactrZ   absjoint_speedsmeanstall_torque_costsquarer[   joints_at_limitrewardssumHUDrewardbool)r   astatedonepotential_oldrY   r\   ir<   contact_idsrZ   r[   	debugmodes                r   stepWalkerBaseBulletEnv.step@   s   ::!!
jja 
jjJJ!!#E

!Htzz+++JJ"	$%DK <<>D;;u!!##GUdNNMZZ..0DNT^^m34H

 ?ann.>??k
//K
' &)

"%(

" ,,uRVVA

@W@W<W5X5]5] 6 0 ..ryy|7H7H7J1KKK !:!:TZZ=W=W!WXIHoDKKJHo"# !!"  	X1EDL 	JDLLMCHHUtKK3t||$$K#dll#T$Z33r   c                     U R                   R                  u  pnXl        U R                  R	                  U R                  USXS5        g )Ngffffff?rX   )r   body_real_xyzr   cameramove_and_look_at)r   rU   yzs       r   camera_adjust!WalkerBaseBulletEnv.camera_adjust   s8    jj&&GA!MKK  CsCr   )r?   r   r9   r1   r2   r0   rj   rv   r3   r#   r   r   r   NF)__name__
__module____qualname____firstlineno__r   r%   r*   rA   rM   rZ   rs   foot_collision_costr4   r5   r[   r   r   __static_attributes__rS   r   r   r   r      sM    4&"  '^>4@Dr   r   c                       \ rS rSrSS jrSrg)HopperBulletEnv   c                 b    [        5       U l        [        R                  X R                  U5        g N)r   r   r   r   r   r   s     r   r   HopperBulletEnv.__init__   s    DJ  zz6:r   r   Nr   r   r   r   r   r   r   rS   r   r   r   r          ;r   r   c                       \ rS rSrSS jrSrg)Walker2DBulletEnv   c                 b    [        5       U l        [        R                  X R                  U5        g r   )r   r   r   r   r   s     r   r   Walker2DBulletEnv.__init__   s    DJ  zz6:r   r   Nr   r   rS   r   r   r   r      r   r   r   c                   $    \ rS rSrSS jrS rSrg)HalfCheetahBulletEnv   c                 b    [        5       U l        [        R                  X R                  U5        g r   )r   r   r   r   r   s     r   r   HalfCheetahBulletEnv.__init__   s    DJ  zz6:r   c                     gNFrS   r@   s    r   rA   HalfCheetahBulletEnv._isDone   s    r   r   Nr   )r   r   r   r   r   rA   r   rS   r   r   r   r      s    ;r   r   c                       \ rS rSrSS jrSrg)AntBulletEnv   c                 b    [        5       U l        [        R                  X R                  U5        g r   )r	   r   r   r   r   s     r   r   AntBulletEnv.__init__   s    DJ  zz6:r   r   Nr   r   rS   r   r   r   r      r   r   r   c                       \ rS rSrSS jrSrg)HumanoidBulletEnv   Nc                     Uc  [        5       U l        OXl        [        R                  X R                  U5        S[        R                  -  U l        S[        R
                  -  U l        g )Ng      @)r
   r   r   r   rZ   rs   r   s      r   r   HumanoidBulletEnv.__init__   sQ    }:djj  zz6: #6#G#GGD!$7$I$IIDr   )rZ   r   rs   r   r   rS   r   r   r   r      s    Jr   r   c                   (    \ rS rSrSrSS jrS rSrg)HumanoidFlagrunBulletEnv   Tc                 b    [        5       U l        [        R                  X R                  U5        g r   )r   r   r   r   r   s     r   r   !HumanoidFlagrunBulletEnv.__init__   s      "DJtZZ8r   c                 >    [         R                  X5      nSUl        U$ r   r   r%    zero_at_running_strip_start_liner   r$   ss      r   r%   3HumanoidFlagrunBulletEnv.create_single_player_scene       44TIA).A&Hr   r   Nr   )r   r   r   r   
random_yawr   r%   r   rS   r   r   r   r      s    *9r   r   c                   (    \ rS rSrSrSS jrS rSrg)HumanoidFlagrunHarderBulletEnv   Tc                     [        5       U l        U =R                  S-  sl        [        R	                  X R                  U5        g )Nr   )r   r   rZ   r   r   r   s     r   r   'HumanoidFlagrunHarderBulletEnv.__init__   s1    &(DJQtZZ8r   c                 >    [         R                  X5      nSUl        U$ r   r   r   s      r   r%   9HumanoidFlagrunHarderBulletEnv.create_single_player_scene   r   r   r   Nr   )r   r   r   r   random_leanr   r%   r   rS   r   r   r   r      s    +9
r   r   )scene_stadiumr   	env_basesr   numpyrf   r,   robot_locomotorsr   r   r   r	   r
   r   r   r   r   r   r   r   r   r   r   rS   r   r   <module>r      s    3 (   q q q|D+ |D~;) ;;+ ;. ;& ;	J+ 	J
0 
%6 r   