
    h                         S SK rS SKJr  S SKJr  S SKJr  SS\R                  " S5      SS	.r	S
 r
 " S S\\R                  5      rg)    N)utils)	MujocoEnv)Box   g      @)        r          @g      4)trackbodyiddistancelookat	elevationc                     [         R                  " SU R                  UR                  5      nU R                  R	                  5       nX#-  SS R                  5       $ )zfCalculate center of mass as weighted average: (model.body_mass.T * data.xipos) / sum(model.body_mass).zb,bj->jr      )npeinsum	body_massxipossumcopy)modeldatanumdenoms       [/home/james-whalen/.local/lib/python3.13/site-packages/gymnasium/envs/mujoco/humanoid_v4.pymass_centerr      sF    
))Iu


;COO!EK1""$$    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
 rS rS rSrg)HumanoidEnv   )human	rgb_arraydepth_array
rgbd_tupleC   )render_modes
render_fpsc           
         [         R                  R                  " U UUUUUUU40 UD6  Xl        X l        X0l        X@l        XPl        X`l        UU l	        U(       a8  [        [        R                  * [        R                  S[        R                  S9n	O7[        [        R                  * [        R                  S[        R                  S9n	[        R                  " U SS4U	[        S.UD6  g )N)ix  )lowhighshapedtype)iz  zhumanoid.xml   )observation_spacedefault_camera_config)r   EzPickle__init___forward_reward_weight_ctrl_cost_weight_healthy_reward_terminate_when_unhealthy_healthy_z_range_reset_noise_scale+_exclude_current_positions_from_observationr   r   inffloat64r   DEFAULT_CAMERA_CONFIG)
selfforward_reward_weightctrl_cost_weighthealthy_rewardterminate_when_unhealthyhealthy_z_rangereset_noise_scale*exclude_current_positions_from_observationkwargsr,   s
             r   r/   HumanoidEnv.__init__"   s     	!$6
	
 
	
 '<#!1-)A& /"3 7 	8 6 #VVG"&&bjj! !$VVG"&&bjj! 		
 0"7	
 	
r   c                 l    [        U R                  =(       d    U R                  5      U R                  -  $ N)float
is_healthyr3   r2   )r:   s    r   r=   HumanoidEnv.healthy_rewardW   s.     $//CT%C%CD""#	
r   c                     U R                   [        R                  " [        R                  " U R                  R
                  5      5      -  nU$ rE   )r1   r   r   squarer   ctrl)r:   actioncontrol_costs      r   rM   HumanoidEnv.control_cost^   s2    --ryy7P0QQr   c                 |    U R                   u  pXR                  R                  S   s=:  =(       a    U:  nU$ s  nU$ Nr   )r4   r   qpos)r:   min_zmax_zrG   s       r   rG   HumanoidEnv.is_healthyb   s?    ,,YY^^A.666
 7
r   c                 R    U R                   (       a  U R                  (       + nU$ SnU$ )NF)r3   rG   )r:   
terminateds     r   rV   HumanoidEnv.terminatedi   s+    .2.L.L$//)
 SX
r   c                    U R                   R                  R                  R                  5       nU R                   R                  R                  R                  5       nU R                   R
                  R                  R                  5       nU R                   R                  R                  R                  5       nU R                   R                  R                  R                  5       nU R                   R                  R                  R                  5       nU R                  (       a  USS  n[        R                  " UUUUUU45      $ rP   )r   rQ   flatr   qvelcinertcvelqfrc_actuatorcfrc_extr6   r   concatenate)r:   positionvelocitycom_inertiacom_velocityactuator_forcesexternal_contact_forcess          r   _get_obsHumanoidEnv._get_obsn   s    99>>&&++-99>>&&++-ii&&++002yy~~**//1))1166;;="&))"4"4"9"9">">"@;;|H~~'	
 		
r   c           
      ,   [        U R                  U R                  5      nU R                  XR                  5        [        U R                  U R                  5      nX2-
  U R
                  -  nUu  pVU R                  U5      nU R                  U-  nU R                  n	X-   n
U R                  5       nX-
  nU R                  nUU* U	US   US   [        R                  R                  USS9UUUS.	nU R                  S:X  a  U R                  5         XUSU4$ )Nr   r   r   )ord)	reward_linvelreward_quadctrlreward_alive
x_position
y_positiondistance_from_origin
x_velocity
y_velocityforward_rewardr   F)r   r   r   do_simulation
frame_skipdtrM   r0   r=   rf   rV   r   linalgnormrender_moderender)r:   rL   xy_position_beforexy_position_afterxy_velocityrp   rq   	ctrl_costrr   r=   rewardsobservationrewardrV   infos                  r   stepHumanoidEnv.step   s   (TYY?6??3'

DII>(=H!,
%%f-	44zA,, 1mmo$__
+ )z*+A.+A.$&IINN3D!N$L$$,

 w&KKMJt;;r   c                 f   U R                   * nU R                   nU R                  U R                  R                  XU R                  R
                  S9-   nU R                  U R                  R                  XU R                  R                  S9-   nU R                  X45        U R                  5       nU$ )N)r'   r(   size)
r5   	init_qpos	np_randomuniformr   nq	init_qvelnv	set_staterf   )r:   	noise_low
noise_highrQ   rZ   r   s         r   reset_modelHumanoidEnv.reset_model   s    ,,,	,,
~~ 6 6 !7 !
 
 ~~ 6 6 !7 !
 
 	t"mmor   )r1   r6   r0   r2   r4   r5   r3   N)g      ?g?g      @T)g      ?r   g{Gz?T)__name__
__module____qualname____firstlineno__metadatar/   propertyr=   rM   rG   rV   rf   r   r   __static_attributes__ r   r   r   r      s    
 H #!%"373
j 
 
    
0!<Fr   r   )numpyr   	gymnasiumr   gymnasium.envs.mujocor   gymnasium.spacesr   arrayr9   r   r.   r   r   r   r   <module>r      sI      +   hh'	 %_)U^^ _r   