
    niD                         S r SSKJr  SSKJr  SSKJr  SSKrSSKJr  SSKJ	r	  SSKJ
r
  SS	KJr  SS
KJr  Sr " S S\5      rg)zThe generic safety checking interface.

Defines the generic safety checker class that can detect bad motor states, imu
states, self-collisions, unsafe motor commands, unusual temperature reading,
etc. Safety criterions are provided by the robot class.
    )absolute_import)division)print_functionN)robot_config)
data_types)motor_action_validator)motor_state_validator)	utilitiesi  c                       \ rS rSrSrS\R                  4S jrSS jrS\R                  \
   S	\R                  SS4S
 jrSrg)SafetyChecker   z&The generic safety checking interface.robotc                 $   Xl         / U l        / U l        [        UR                  5       GH  nU R                  R                  [        R                  " UUR                  R                  U   UR                  R                  U   UR                  R                  U   UR                  R                  U   UR                  R                  U   UR                  R                  UR                  R                  U   UR                  R                   U   ["        S9
5        U R                  R                  [$        R&                  " UUR                  R                  U   UR                  R                  U   UR                  R                  U   UR                  R                  U   UR                  R                  U   UR                  R(                  UR                  R*                  U   UR                  R,                  U   ["        S9
5        GM     g)zInitilaizes the class.

TODO(b/131377892): Implement other state checkings including the
IMU/temperature/contact force if enabled.

Args:
  robot: A robot instance such like Minitaur/Laikago/Vision60.
)
motor_idposition_boundposition_gain_boundvelocity_boundvelocity_gain_boundtorque_boundtimestamp_delta_boundaverage_abs_velocity_boundaverage_abs_power_boundstate_buffer_size)
r   r   r   r   r   r   r   delta_position_bound average_abs_delta_position_boundr   N)_robot_motor_state_validators_motor_action_validatorsrange
num_motorsappendr	   MotorStateValidatorsafety_configmotor_position_boundmotor_position_gain_boundmotor_velocity_boundmotor_velocity_gain_boundmotor_torque_boundr    motor_average_abs_velocity_boundmotor_average_abs_power_bound_MOTOR_STATE_BUFFER_SIZEr   MotorActionValidator"state_action_timestamp_delta_boundmotor_delta_position_bound&motor_average_abs_delta_position_bound)selfr   is      m/home/james-whalen/.local/lib/python3.13/site-packages/pybullet_envs/minitaur/robots/safety/safety_checker.py__init__SafetyChecker.__init__   s    K#%D $&D!5##$
""))

3
3"00EEaH"'"5"5((#,"00EEaH"'"5"5((#, ..AA!D$)$7$7$M$M).)<)<//*3&+&9&9,,Q'0 8" ##**
 
5
5"00EEaH"'"5"5((#,"00EEaH"'"5"5((#, ..AA!D$)$7$711#(#6#6))!$-/4/B/B55a09 8% %    returnNc           
      @   [        [        U R                  R                  5      U R                  U R
                  5       H  u  pn[        R                  " U R                  R                  U R                  R                  5       U   U R                  R                  5       U   U R                  R                  5       U   U R                  R                  5       U   U R                  R                  5       U   S9nUR                  U5        UR                  U5        M     g)zValidates the state of the robot.

TODO(b/131377892): Implement other state checkings including the
IMU/temperature/contact force if enabled.

Raises:
  A safety exception if any state checking (motor/imu/etc) fails.
)	timestamppositionvelocityposition_gainvelocity_gaintorqueN)zipr   r   r    r   r   r   
MotorStatelast_state_timeGetMotorAnglesGetMotorVelocitiesGetMotorPositionGainsGetMotorVelocityGainsGetMotorTorqueson_state)r0   r   state_validatoraction_validatormotor_states        r2   check_stateSafetyChecker.check_stateN   s     8;dkk$$%t'C'C%%8'3#3 ))KK//;;--/9;;113H=99;HE99;HE,,.x8k {+,8'r5   actioncontrol_modec                     [         R                  " U R                  X5      n[        U R                  5       H  u  pEUR                  X4   U5        M     g)zValidate the action w.r.t to the motor states.

Args:
  action: The motor commands sent to the robot.
  control_mode: The motor control mode.

Raises:
  A safety exception if action checking fails.
N)r
   convert_to_motor_actionr   	enumerater   	on_action)r0   rL   rM   motor_action_listr   	validators         r2   check_motor_action SafetyChecker.check_motor_actionf   sK     "99V+()F)FG+5|D  Hr5   )r   r   r   )r6   N)__name__
__module____qualname____firstlineno____doc__typingAnyr3   rJ   Sequencefloatr   MotorControlModerT   __static_attributes__ r5   r2   r   r      sP    .3ZZ3j-0Eooe$E !11E 	Er5   r   )rZ   
__future__r   r   r   r[   pybullet_envs.minitaur.robotsr   $pybullet_envs.minitaur.robots.safetyr   r   r	   r
   r+   objectr   ra   r5   r2   <module>rf      s=    '  %  6 ; G F : aEF aEr5   