
    h                     d    S SK rS SKJr  S SKJr  S SKJr  SS0r " S S\\R                  5      r
g)	    N)utils)	MujocoEnv)Boxdistanceg      @c                       \ rS rSr/ SQSS.r          SS jr\S 5       rS r\S 5       r	\S	 5       r
\S
 5       r\S 5       rS rS rS rSrg)AntEnv   )human	rgb_arraydepth_array
rgbd_tuple   )render_modes
render_fpsc                    [         R                  R                  " U UUUUUUUUU	U
40 UD6  X l        X@l        XPl        X`l        Xpl        Xl        Xl	        X0l
        U
U l        SnU
(       d  US-  nU(       a  US-  n[        [        R                  * [        R                  U4[        R                  S9n[         R                  " U US4U["        S.UD6  g )N      T   )lowhighshapedtype   )observation_spacedefault_camera_config)r   EzPickle__init___ctrl_cost_weight_contact_cost_weight_healthy_reward_terminate_when_unhealthy_healthy_z_range_contact_force_range_reset_noise_scale_use_contact_forces+_exclude_current_positions_from_observationr   npinffloat64r   DEFAULT_CAMERA_CONFIG)selfxml_filectrl_cost_weightuse_contact_forcescontact_cost_weighthealthy_rewardterminate_when_unhealthyhealthy_z_rangecontact_force_rangereset_noise_scale*exclude_current_positions_from_observationkwargs	obs_shaper   s                 V/home/james-whalen/.local/lib/python3.13/site-packages/gymnasium/envs/mujoco/ant_v4.pyr   AntEnv.__init__   s     	$6	
 	
 "2$7!-)A& /$7!"3#5  7 	8 	9NIOIbffYL


 		
 0"7	
 	
    c                 l    [        U R                  =(       d    U R                  5      U R                  -  $ N)float
is_healthyr!   r    )r+   s    r8   r0   AntEnv.healthy_rewardY   s.     $//CT%C%CD""#	
r:   c                 t    U R                   [        R                  " [        R                  " U5      5      -  nU$ r<   )r   r'   sumsquare)r+   actioncontrol_costs      r8   rD   AntEnv.control_cost`   s*    --ryy7H0IIr:   c                 |    U R                   R                  nU R                  u  p#[        R                  " XU5      nU$ r<   )datacfrc_extr#   r'   clip)r+   raw_contact_forces	min_value	max_valuecontact_forcess        r8   rM   AntEnv.contact_forcesd   s7    !YY//#88	!3	Jr:   c                     U R                   [        R                  " [        R                  " U R                  5      5      -  nU$ r<   )r   r'   rA   rB   rM   )r+   contact_costs     r8   rP   AntEnv.contact_costk   s7    00266IId))*4
 
 r:   c                     U R                  5       nU R                  u  p#[        R                  " U5      R	                  5       =(       a    X!S   s=:*  =(       a    U:*  nU$ s  nU$ Nr   )state_vectorr"   r'   isfiniteall)r+   statemin_zmax_zr>   s        r8   r>   AntEnv.is_healthyr   s\    !!#,,[['++-L%82L2Lu2L
 3M
r:   c                 R    U R                   (       a  U R                  (       + nU$ SnU$ )NF)r!   r>   )r+   
terminateds     r8   r\   AntEnv.terminatedy   s+    ,0,J,J(
 QV
r:   c           
      l   U R                  S5      S S R                  5       nU R                  XR                  5        U R                  S5      S S R                  5       nX2-
  U R                  -  nUu  pVUnU R
                  nXx-   n	U R                  U5      =pU R                  nU R                  5       nUU* UUS   US   [        R                  R                  USS9UUUS.	nU R                  (       a  U R                  nX-  n
U* US'   X-
  nU R                  S:X  a  U R                  5         UUUS	U4$ )
Ntorsor   r      )ord)	reward_forwardreward_ctrlreward_survive
x_position
y_positiondistance_from_origin
x_velocity
y_velocityforward_rewardrc   r
   F)get_body_comcopydo_simulation
frame_skipdtr0   rD   r\   _get_obsr'   linalgnormr%   rP   render_moderender)r+   rC   xy_position_beforexy_position_afterxy_velocityrh   ri   rj   r0   rewardscosts	ctrl_costr\   observationinforP   rewards                    r8   stepAntEnv.step~   sG   !..w7;@@B6??3 --g6r:??A(=H!,
#,, 1 --f55__
mmo,%:,+A.+A.$&IINN3D!N$L$$,

 ##,,L!E#/-Dw&KKMFJt;;r:   c                    U R                   R                  R                  R                  5       nU R                   R                  R                  R                  5       nU R
                  (       a  USS  nU R                  (       a<  U R                  R                  R                  5       n[        R                  " XU45      $ [        R                  " X45      $ rS   )
rG   qposflatrl   qvelr&   r%   rM   r'   concatenate)r+   positionvelocitycontact_forces       r8   rp   AntEnv._get_obs   s    99>>&&++-99>>&&++-;;|H## //4499;M>>8}"EFF>>8"677r:   c                    U R                   * nU R                   nU R                  U R                  R                  XU R                  R
                  S9-   nU R                  U R                   U R                  R                  U R                  R                  5      -  -   nU R                  X45        U R                  5       nU$ )N)r   r   size)r$   	init_qpos	np_randomuniformmodelnq	init_qvelstandard_normalnv	set_staterp   )r+   	noise_low
noise_highr   r   r{   s         r8   reset_modelAntEnv.reset_model   s    ,,,	,,
~~ 6 6 !7 !
 
 NN%%(F(Ftzz}}(UUV 	 	t"mmor:   )	r   r#   r   r&   r    r"   r$   r!   r%   N)
zant.xmlg      ?FgMb@?      ?T)g?r   )g      r   g?T)__name__
__module____qualname____firstlineno__metadatar   propertyr0   rD   rM   rP   r>   r\   r~   rp   r   __static_attributes__ r:   r8   r   r      s    
 H   !%"'37?
B 
 
        &<P8r:   r   )numpyr'   	gymnasiumr   gymnasium.envs.mujocor   gymnasium.spacesr   r*   r   r   r   r:   r8   <module>r      s4      +    
uY ur:   