
    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trackbodyidc                   :    \ rS rSr/ SQSS.rS rS rS rS rS	r	g
)
ReacherEnv   )human	rgb_arraydepth_array
rgbd_tuple2   )render_modes
render_fpsc                     [         R                  R                  " U 40 UD6  [        [        R
                  * [        R
                  S[        R                  S9n[        R                  " U SS4U[        S.UD6  g )N)r	   )lowhighshapedtypezreacher.xml   )observation_spacedefault_camera_config)	r   EzPickle__init__r   npinffloat64r   DEFAULT_CAMERA_CONFIG)selfkwargsr   s      Z/home/james-whalen/.local/lib/python3.13/site-packages/gymnasium/envs/mujoco/reacher_v4.pyr   ReacherEnv.__init__   sf    //RVVG"&&RZZX	
 0"7	
 	
    c           	         U R                  S5      U R                  S5      -
  n[        R                  R                  U5      * n[        R                  " U5      R                  5       * nX4-   nU R                  XR                  5        U R                  S:X  a  U R                  5         U R                  5       nUUSS[        X4S94$ )N	fingertiptargetr
   F)reward_distreward_ctrl)get_body_comr   linalgnormsquaresumdo_simulation
frame_skiprender_moderender_get_obsdict)r   avecr'   r(   rewardobs          r!   stepReacherEnv.step"   s    ,t/@/@/JJyy~~c**yy|''))*1oo.w&KKM]]_ [B
 	
r#   c                    U R                   R                  SSU R                  R                  S9U R                  -   n U R                   R                  SSSS9U l        [        R                  R                  U R
                  5      S:  a  OMP  U R
                  USS & U R                  U R                   R                  SS	U R                  R                  S9-   nS
USS & U R                  X5        U R                  5       $ )Ngg?)r   r   sizegɿg?r   g{Gztg{Gzt?r   )	np_randomuniformmodelnq	init_qposgoalr   r*   r+   	init_qvelnv	set_stater2   )r   qposqvels      r!   reset_modelReacherEnv.reset_model6   s    NN""t#DJJMM"Jnn 	 ..4c.JDIyy~~dii(3.  IIRS	~~ 6 6U !7 !
 
 RS	t"}}r#   c           
         U R                   R                  R                  S S n[        R                  " [        R
                  " U5      [        R                  " U5      U R                   R                  R                  SS  U R                   R                  R                  S S U R                  S5      U R                  S5      -
  /5      $ )Nr   r%   r&   )	datarF   flatr   concatenatecossinrG   r)   )r   thetas     r!   r2   ReacherEnv._get_obsG   s    		##BQ'~~uu		##AB'		##BQ'!!+.1B1B81LL
 	
r#   )rB   N)
__name__
__module____qualname____firstlineno__metadatar   r8   rH   r2   __static_attributes__ r#   r!   r   r      s'    
 H


("

r#   r   )numpyr   	gymnasiumr   gymnasium.envs.mujocor   gymnasium.spacesr   r   r   r   rX   r#   r!   <module>r]      s2      +   '* F
ENN F
r#   