
    ni%                        S r SSKrSSKr\R                  R	                  \R                  R                  \R                  " \R                  " 5       5      5      5      r\R                  R	                  \R                  R	                  \5      5      r	\R                  R                  R                  S\	5        SSKrSSKrSSKJr  SSKrSSKJr  SrSrSrS\-  r\R0                  " S	S
5      r " S S\R4                  5      rg)zHThis file implements the gym environment of minitaur alternating legs.

    N)spaces)minitaur_gym_envg       @g              MinitaurPosezswing_angle_1, swing_angle_2, swing_angle_3, swing_angle_4, extension_angle_1, extension_angle_2, extension_angle_3, extension_angle_4c                      ^  \ rS rSrSrSS/SS.r                 SU 4S jjrU 4S jrS	 rS
 r	S r
S rS rS rS rS rSrU =r$ )MinitaurReactiveEnv   ac  The gym environment for the minitaur.

It simulates the locomotion of a minitaur, a quadruped robot. The state space
include the angles, velocities and torques for all the motors and the action
space is the desired motor angle for each motor. The reward function is based
on how far the minitaur walks in 1000 steps and penalizes the energy
expenditure.

human	rgb_array   )zrender.modeszvideo.frames_per_secondc                    > Xl         [        [        U ]  UUUSUU	U
UUUUUUUUUUS9  Sn[        R
                  " S/U-  5      nU* n[        R                  " UU5      U l        SU l	        SU l
        SU l        g)	a  Initialize the minitaur trotting gym environment.

Args:
  urdf_version: [DEFAULT_URDF_VERSION, DERPY_V0_URDF_VERSION] are allowable
    versions. If None, DEFAULT_URDF_VERSION is used. Refer to
    minitaur_gym_env for more details.
  energy_weight: The weight of the energy term in the reward function. Refer
    to minitaur_gym_env for more details.
  control_time_step: The time step between two successive control signals.
  action_repeat: The number of simulation steps that an action is repeated.
  control_latency: The latency between get_observation() and the actual
    observation. See minituar.py for more details.
  pd_latency: The latency used to get motor angles/velocities used to
    compute PD controllers. See minitaur.py for more details.
  on_rack: Whether to place the minitaur on rack. This is only used to debug
    the walking gait. In this mode, the minitaur"s base is hung midair so
    that its walking gait is clearer to visualize.
  motor_kp: The P gain of the motor.
  motor_kd: The D gain of the motor.
  remove_default_joint_damping: Whether to remove the default joint damping.
  render: Whether to render the simulation.
  num_steps_to_log: The max number of control steps in one episode. If the
    number of steps is over num_steps_to_log, the environment will still
    be running, but only first num_steps_to_log will be recorded in logging.
  accurate_motor_model_enabled: Whether to use the accurate motor model from
    system identification. Refer to minitaur_gym_env for more details.
  use_angle_in_observation: Whether to include motor angles in observation.
  hard_reset: Whether hard reset (swipe out everything and reload) the
    simulation. If it is false, the minitaur is set to the default pose
    and moved to the origin.
  env_randomizer: An instance (or a list) of EnvRanzomier(s) that can
    randomize the environment during when env.reset() is called and add
    perturbations when env.step() is called.
  log_path: The path to write out logs. For the details of logging, refer to
    minitaur_logging.proto.
T)urdf_versionenergy_weightaccurate_motor_model_enabledmotor_overheat_protectionmotor_kpmotor_kdremove_default_joint_dampingcontrol_latency
pd_latencyon_rackrender
hard_resetnum_steps_to_logenv_randomizerlog_pathcontrol_time_stepaction_repeat   g            ?   iN)_use_angle_in_observationsuperr	   __init__nparrayr   Boxaction_space	_cam_dist_cam_yaw
_cam_pitch)selfr   r   r   r   r   r   r   r   r   r   r   r   r   use_angle_in_observationr   r   r   
action_dim
action_lowaction_high	__class__s                        k/home/james-whalen/.local/lib/python3.13/site-packages/pybullet_envs/minitaur/envs/minitaur_reactive_env.pyr%   MinitaurReactiveEnv.__init__'   s    l &>"	

&2'46R37"*"*6R)8$.!( &$.*:(6"*+<'4!  6$ J4&:-.J+K

:{;DDNDMDO    c                    > [        [        [        [        [        [        [        [        [        S9nU R                  [	        U5      5      n[
        [        U ]  USS9  U R                  5       $ )N)swing_angle_1swing_angle_2swing_angle_3swing_angle_4extension_angle_1extension_angle_2extension_angle_3extension_angle_4g      ?)initial_motor_anglesreset_duration)	r   INIT_SWING_POSINIT_EXTENSION_POS_convert_from_leg_modellistr$   r	   reset_get_observation)r-   	init_poser?   r2   s      r3   rE   MinitaurReactiveEnv.resetz   sh     >+9+9+9/A/A/A/ACI  77YH	
t*@T:= + ?  ""r5   c                 4   [         R                  " [        5      n[        [        5       Hj  nU[        U-      S[        US-  5      -  X   -  -
  U[        SU-  5      '   U[        U-      S[        US-  5      -  X   -  -   U[        SU-  S-   5      '   Ml     U$ )Nr      )r&   zeros
NUM_MOTORSrangeNUM_LEGSint)r-   leg_pose
motor_poseis       r3   rC   +MinitaurReactiveEnv._convert_from_leg_model   s    *%J8_'15c!a%j8H8;8VVjQU$,X\$:b3q1u:=MPXP[=[$[jQUQY   r5   c           
          [         R                  " [        [        [        [        [        [        [        [        /5      nU$ N)r&   r'   rA   rB   )r-   tinitial_poses      r3   _signalMinitaurReactiveEnv._signal   s2    88HZ.0B L r5   c                 z    XR                  U R                  R                  5       5      -  nU R                  U5      $ rV   )rY   minitaurGetTimeSinceResetrC   )r-   actions     r3   "_transform_action_to_motor_command6MinitaurReactiveEnv._transform_action_to_motor_command   s1    
ll4==::<==F''//r5   c                     U R                   R                  5       u  pn[        R                  " U5      S:  =(       d    [        R                  " U5      S:  $ )zDecides whether the minitaur is in a fallen state.

If the roll or the pitch of the base is greater than 0.3 radians, the
minitaur is considered fallen.

Returns:
  Boolean value that indicates whether the minitaur has fallen.
g333333?)r\   GetTrueBaseRollPitchYawmathfabs)r-   rollpitch_s       r3   	is_fallenMinitaurReactiveEnv.is_fallen   s@     ]]::<ND99T?S :DIIe$4s$::r5   c                 \   U R                   R                  5       u  pnU R                   R                  5       u  pEnXXE/nU R                  (       a7  UR	                  U R                   R                  5       R                  5       5        [        R                  " U5      U l	        U R                  $ )a)  Get the true observations of this environment.

It includes the roll, the pitch, the roll dot and the pitch dot of the base.
If _use_angle_in_observation is true, eight motor angles are added into the
observation.

Returns:
  The observation list, which is a numpy array of floating-point values.
)
r\   rb   GetTrueBaseRollPitchYawRater#   extendGetMotorAnglestolistr&   r'   _true_observationr-   re   rf   rg   	roll_rate
pitch_rateobservations          r3   _get_true_observation)MinitaurReactiveEnv._get_true_observation   s     ]]::<ND#}}HHJI1	6K%%557>>@AXXk2D!!!r5   c                 \   U R                   R                  5       u  pnU R                   R                  5       u  pEnXXE/nU R                  (       a7  UR	                  U R                   R                  5       R                  5       5        [        R                  " U5      U l	        U R                  $ rV   )
r\   GetBaseRollPitchYawGetBaseRollPitchYawRater#   rl   rm   rn   r&   r'   _observationrp   s          r3   rF   $MinitaurReactiveEnv._get_observation   s    ]]668ND#}}DDFI1	6K%%557>>@A-Dr5   c                 |   S[         R                  -  nS[         R                  -  nS[         R                  -  U R                  -  nS[         R                  -  U R                  -  nS[         R                  -  nXX4/nU R                  (       a  UR	                  U/[
        -  5        [        R                  " U5      $ )zGet the upper bound of the observation.

Returns:
  The upper bound of an observation. See _get_true_observation() for the
  details of each element of an observation.
r   )rc   pi
_time_stepr#   rl   rM   r&   r'   )r-   upper_bound_rollupper_bound_pitchupper_bound_roll_dotupper_bound_pitch_dotupper_bound_motor_angleupper_bounds          r3   _get_observation_upper_bound0MinitaurReactiveEnv._get_observation_upper_bound   s     477{DGGtww;8K$//9$''k-AK %%12Z?@88K  r5   c                 (    U R                  5       * nU$ rV   )r   )r-   lower_bounds     r3   _get_observation_lower_bound0MinitaurReactiveEnv._get_observation_lower_bound   s    4466Kr5   )r*   r,   r+   ry   ro   r#   r)   )Ng{Gzt?g~jtx?   g{Gz?g~jth?Fr!   gQ?TFi  TTFNN)__name__
__module____qualname____firstlineno____doc__metadatar%   rE   rC   rY   r_   rh   rt   rF   r   r   __static_attributes____classcell__)r2   s   @r3   r	   r	      s      '4QTU( !"!&#,0 $,0(,"#Qf#"0

;"$!( r5   r	   )r   osinspectpathdirnameabspathgetfilecurrentframe
currentdir	parentdirsysinsertcollectionsrc   gymr   numpyr&   pybullet_envs.minitaur.envsr   rB   rA   rO   rM   
namedtupler   MinitaurGymEnvr	    r5   r3   <module>r      s    WW__RWW__W__W=Q=Q=S-TUV
GGOOBGGOOJ78	   1i       8 \
%% @*99 @r5   