
    h}                         S SK Jr  S SKrS SKJr  S SKrS SKJrJ	r	  S SK
Jr   S SKrSrS\S	\4S
 jr " S S\R&                  5      rg! \ a  r\R                  " S5      \eSrCff = f)    )pathN)NDArray)errorspaces)Spacez>MuJoCo is not installed, run `pip install "gymnasium[mujoco]"`i  
model_pathreturnc                 r   U R                  S5      (       d  U R                  S5      (       a  U nO]U R                  S5      (       a  [        R                  " U 5      nO0[        R                  " [        R                  " [
        5      SU 5      n[        R                  " U5      (       d  [        SU S35      eU$ )zLExpands the `model_path` to a full path if it starts with '~' or '.' or '/'../~assetszFile z does not exist)
startswithr   
expanduserjoindirname__file__existsOSError)r   fullpaths     Z/home/james-whalen/.local/lib/python3.13/site-packages/gymnasium/envs/mujoco/mujoco_env.pyexpand_model_pathr      s    S!!Z%:%:3%?%?			s	#	#??:.99T\\(3XzJ;;x  hZ788O    c                     ^  \ rS rSrSrS\\SSSS0 4S\S\S\S-  S\S-  S	\S
\S\S-  S\S-  S\	\\
\-  4   S-  S\S\	\\4   4S jjrS rS\S   4S jrS rS rS rS rS rSSS.S\S-  S\	S-  4U 4S jjjr\S\
4S j5       rS&S jrS\\R6                     4S  jrS!\\R:                     S\\\R6                     \R6                  \\\	\\R6                  4   4   4S" jrS\\R6                     4S# jrS\	\\
4   4S$ jr S%r!U =r"$ )'	MujocoEnv$   z)Superclass for MuJoCo based environments.Ni  r   
frame_skipobservation_spacerender_modewidthheight	camera_idcamera_namedefault_camera_configmax_geomvisual_optionsc                 \   [        U5      U l        XPl        X`l        U R	                  5       u  U l        U l        U R                  R                  R                  5       R                  5       U l
        U R                  R                  R                  5       R                  5       U l        X l        SU R                  ;   a  [        [         R"                  " SU R$                  -  5      5      U R                  S   :X  dD   S[        [         R"                  " SU R$                  -  5      5       SU R                  S    35       eUb  X0l        U R)                  5         X@l        Xl        Xpl        SSKJn  U" U R
                  U R                  U	U R                  U R                  U
UUU5	      U l        g)a  Base abstract class for mujoco based environments.

Args:
    model_path: Path to the MuJoCo Model.
    frame_skip: Number of MuJoCo simulation steps per gym `step()`.
    observation_space: The observation space of the environment.
    render_mode: The `render_mode` used.
    width: The width of the render window.
    height: The height of the render window.
    camera_id: The camera ID used.
    camera_name: The name of the camera used (can not be used in conjunction with `camera_id`).
    default_camera_config: configuration for rendering camera.
    max_geom: max number of rendered geometries.
    visual_options: render flag options.

Raises:
    OSError: when the `model_path` does not exist.
    error.DependencyNotInstalled: When `mujoco` is not installed.

render_fpsg      ?zExpected value: z, Actual value: Nr   )MujocoRenderer)r   r   r    r!   _initialize_simulationmodeldataqposravelcopy	init_qposqvel	init_qvelr   metadataintnprounddtr   _set_action_spacer   r#   r"   &gymnasium.envs.mujoco.mujoco_renderingr)   mujoco_renderer)selfr   r   r   r   r    r!   r"   r#   r$   r%   r&   r)   s                r   __init__MujocoEnv.__init__'   sU   B **5
 $ ; ; =
DI--/446--/446$4==(BHHS477]+,l0KKn!#bhhsTWW}&=">!??OPTP]P]^jPkOlmnK(%6" &&"I-JJII!JJKK
 
r   c                    U R                   R                  R                  5       R                  [        R
                  5      nUR                  u  p#[        R                  " X#[        R
                  S9U l	        U R                  $ )N)lowhighdtype)
r+   actuator_ctrlranger/   astyper5   float32Tr   Boxaction_space)r;   boundsr?   r@   s       r   r8   MujocoEnv._set_action_spacen   sW    ..335<<RZZHHH	"JJ3L   r   r	   )zmujoco.MjModelzmujoco.MjDatac                    [         R                  R                  U R                  5      nU R                  UR
                  R                  l        U R                  UR
                  R                  l	        [         R                  " U5      nX4$ )zF
Initialize MuJoCo simulation data structures `mjModel` and `mjData`.
)mujocoMjModelfrom_xml_pathr   r    visglobal_offwidthr!   	offheightMjData)r;   r+   r,   s      r   r*    MujocoEnv._initialize_simulationt   s^     ,,T]];%)ZZ		"&*kk		#}}U#{r   c                    UR                   U R                  R                  4:X  a%  UR                   U R                  R                  4:X  d   e[        R
                  " U5      U R                  R                  SS& [        R
                  " U5      U R                  R                  SS& U R                  R                  S:X  a  SU R                  R                  SS& [        R                  " U R                  U R                  5        g)zSet the joints position qpos and velocity qvel of the model.

Note: `qpos` and `qvel` is not the full physics state for all mujoco models/environments https://mujoco.readthedocs.io/en/stable/APIreference/APItypes.html#mjtstate
Nr   )shaper+   nqnvr5   r/   r,   r-   r1   naactrK   
mj_forward)r;   r-   r1   s      r   	set_stateMujocoEnv.set_state   s    
 zzdjjmm--$**@P2PPPGGDM		qGGDM		q::==A#DIIMM!$**dii0r   c                     XR                   R                  SS& [        R                  " U R                  U R                   US9  [        R
                  " U R                  U R                   5        g)z"
Step over the MuJoCo simulation.
N)nstep)r,   ctrlrK   mj_stepr+   mj_rnePostConstraintr;   r_   n_framess      r   _step_mujoco_simulation!MujocoEnv._step_mujoco_simulation   sF     !		qtzz499H=
 	##DJJ		:r   c                 L    U R                   R                  U R                  5      $ )zL
Render a frame from the MuJoCo simulation as specified by the render_mode.
)r:   renderr   r;   s    r   rg   MujocoEnv.render   s!     ##**4+;+;<<r   c                 T    U R                   b  U R                   R                  5         gg)z#Close rendering contexts processes.N)r:   closerh   s    r   rk   MujocoEnv.close   s%    +  &&( ,r   c                 L    U R                   R                  U5      R                  $ )z.Return the cartesian position of a body frame.)r,   bodyxpos)r;   	body_names     r   get_body_comMujocoEnv.get_body_com   s    yy~~i(---r   )seedoptionsrs   rt   c                   > [         TU ]  US9  [        R                  " U R                  U R
                  5        U R                  5       nU R                  5       nU R                  S:X  a  U R                  5         X44$ )N)rs   human)
superresetrK   mj_resetDatar+   r,   reset_model_get_reset_infor   rg   )r;   rs   rt   obinfo	__class__s        r   rx   MujocoEnv.reset   se     	4 DJJ		2##%w&KKMxr   c                 \    U R                   R                  R                  U R                  -  $ N)r+   opttimestepr   rh   s    r   r7   MujocoEnv.dt   s    zz~~&&88r   c                     [         R                  " U5      R                  U R                  R                  4:w  aD  [        SU R                  R                  4 S[         R                  " U5      R                   35      eU R                  X5        g)zG
Step the simulation n number of frames and applying a control action.
z$Action dimension mismatch. Expected z, found N)r5   arrayrU   r+   nu
ValueErrorrd   rb   s      r   do_simulationMujocoEnv.do_simulation   sr    
 88D>DJJMM#336

7G6HQSQYQYZ^Q_QeQePfg  	$$T4r   c                     [         R                  " U R                  R                  R                  U R                  R
                  R                  /5      $ )zReturn the position and velocity joint states of the model.

Note: `qpos` and `qvel` does not constitute the full physics state for all `mujoco` environments see https://mujoco.readthedocs.io/en/stable/computation/index.html#the-state.
)r5   concatenater,   r-   flatr1   rh   s    r   state_vectorMujocoEnv.state_vector   s4    
 ~~tyy~~22DIINN4G4GHIIr   actionc                     [         er   NotImplementedError)r;   r   s     r   stepMujocoEnv.step   s
     "!r   c                     [         e)zb
Reset the robot degrees of freedom (qpos and qvel).
Implement this in each environment subclass.
r   rh   s    r   rz   MujocoEnv.reset_model   s
    
 "!r   c                     0 $ )zGFunction that generates the `info` that is returned during a `reset()`. rh   s    r   r{   MujocoEnv._get_reset_info   s    	r   )rG   r"   r#   r,   r   r   r!   r0   r2   r+   r:   r   r   r    )r	   N)#__name__
__module____qualname____firstlineno____doc__DEFAULT_SIZEstrr4   r   dictfloatboolr<   r8   tupler*   r[   rd   rg   rk   rq   rx   propertyr7   r   r   r5   float64r   rD   r   rz   r{   __static_attributes____classcell__)r~   s   @r   r   r   $   s   3 #'!" $"&?C*,E
E
 E
 !4<	E

 4ZE
 E
 E
 :E
 4ZE
  $C$45<E
 E
 S$YE
N!	0	1
1;=)
.  #	 Dj 	 " 9E 9 9	5Jgbjj1 J"bjj)"	wrzz"BJJdDbjj<QQ	R"
"WRZZ0 "c5j!1  r   r   )osr   numpyr5   numpy.typingr   	gymnasiumgymr   r   gymnasium.spacesr   rK   ImportErroreDependencyNotInstalledr   r   r   Envr   r   r   r   <module>r      sv         # " # # | |/  

&
&Hs   A	 	A'A""A'