
    niZ@                     ^   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SKJr  SSKrSSKrSSKJr  SSKJr  SSKrSS	KJr  S
rSrSr \ \-   r!\!\-   r"\"\-   r#Sr$Sr%Sr&Sr'/ SQr(/ SQr) " S S\RT                  5      r+g)z7This file implements the gym environment of minitaur.

    N)parse_version)spaces)seeding)bullet_client   )minitaur)minitaur_env_randomizer      {Gz?i  i  )r   r   g      ?)      ?r   r   r   c                   z   \ rS rSrSrSS/SS.r\R                  " 5       SSS	S
S
\" S5      S
S\	R                  SSSSSSSSSSS\R                  " 5       4S jrS rS rS rS&S jrS rS rS'S jrS rS rS rS rS rS rS rS  rS! rS" rS# r\ " \!RD                  5      \ " S$5      :  a  \r#\r$\r%\r&S%r'gS%r'g)(MinitaurBulletDuckEnv(   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_array2   )zrender.modeszvideo.frames_per_secondr         ?g{Gzt?        infTFg{Gz?g333333?c                    SU l         X l        SU l        Xl        Xl        Xl        / U l        SU l        UU l        / SQU l	        X0l
        X@l        X`l        XPl        Xpl        Xl        SU l        Xl        Xl        Xl        Xl        Xl        UU l        UU l        UU l        SU l        SU l        SU l        SU l        S	U l        UU l        S
U l        [A        SU R                  -   5        UU l!        U(       d  U(       aK  U =R                   [D        -  sl         U =R                  [D        -  sl        U =R                  [D        -  sl        U R                  (       a(  [F        RH                  " [J        RL                  S9U l'        O[F        RH                  " 5       U l'        U RQ                  5         U RS                  5         U RT                  RW                  5       [X        -   nU RT                  R[                  5       [X        -
  nSn[\        R^                  " U R                   /U-  5      n[`        Rb                  " U* U[\        Rd                  S9U l3        [`        Rb                  " UU[\        Rd                  S9U l4        SU l5        UU l        g)a  Initialize the minitaur gym environment.

Args:
  urdf_root: The path to the urdf data folder.
  action_repeat: The number of simulation steps before actions are applied.
  distance_weight: The weight of the distance term in the reward.
  energy_weight: The weight of the energy term in the reward.
  shake_weight: The weight of the vertical shakiness term in the reward.
  drift_weight: The weight of the sideways drift term in the reward.
  distance_limit: The maximum distance to terminate the episode.
  observation_noise_stdev: The standard deviation of observation noise.
  self_collision_enabled: Whether to enable self collision in the sim.
  motor_velocity_limit: The velocity limit of each motor.
  pd_control_enabled: Whether to use PD controller for each motor.
  leg_model_enabled: Whether to use a leg motor to reparameterize the action
    space.
  accurate_motor_model_enabled: Whether to use the accurate DC motor model.
  motor_kp: proportional gain for the accurate motor model.
  motor_kd: derivative gain for the accurate motor model.
  torque_control_enabled: Whether to use the torque control, if set to
    False, pose control will be used.
  motor_overheat_protection: Whether to shutdown the motor that has exerted
    large torque (OVERHEAT_SHUTDOWN_TORQUE) for an extended amount of time
    (OVERHEAT_SHUTDOWN_TIME). See ApplyAction() in minitaur.py for more
    details.
  hard_reset: Whether to wipe the simulation and load everything when reset
    is called. If set to false, reset just place the minitaur back to start
    position and set its pose to initial configuration.
  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 hanged midair so
    that its walking gait is clearer to visualize.
  render: Whether to render the simulation.
  kd_for_pd_controllers: kd value for the pd controllers of the motors
  env_randomizer: An EnvRandomizer to randomize the physical properties
    during reset().
r   i,  r   r   r   r   r   r   iTr   z
urdf_root=)connection_moder   )dtypeN)6
_time_step_action_repeat_num_bullet_solver_iterations
_urdf_root_self_collision_enabled_motor_velocity_limit_observation_env_step_counter
_is_render_last_base_position_distance_weight_energy_weight_drift_weight_shake_weight_distance_limit_observation_noise_stdev_action_bound_pd_control_enabled_leg_model_enabled_accurate_motor_model_enabled	_motor_kp	_motor_kd_torque_control_enabled_motor_overheat_protection_on_rack	_cam_dist_cam_yaw_duckId
_cam_pitch_hard_reset_kd_for_pd_controllers_last_frame_timeprint_env_randomizerNUM_SUBSTEPSbcBulletClientpybulletGUI_pybullet_clientseedresetr   GetObservationUpperBoundOBSERVATION_EPSGetObservationLowerBoundnparrayr   Boxfloat32action_spaceobservation_spaceviewer)self	urdf_rootaction_repeatdistance_weightenergy_weightshake_weightdrift_weightdistance_limitobservation_noise_stdevself_collision_enabledmotor_velocity_limitpd_control_enabledleg_model_enabledaccurate_motor_model_enabledmotor_kpmotor_kdtorque_control_enabledmotor_overheat_protection
hard_reseton_rackrenderkd_for_pd_controllersenv_randomizerobservation_highobservation_low
action_dimaction_highs                              d/home/james-whalen/.local/lib/python3.13/site-packages/pybullet_envs/bullet/minitaur_duck_gym_env.py__init__MinitaurBulletDuckEnv.__init__4   s    x DO'),D&O#9 !5DDDO(D+'%%)$;!D1/)E&NN#9D &?D#DMDNDMDLDOD"7DD	,
())D9
oo%o
((L8(
\) oohllKd oo/dIIKJJL>>@?R}}==?/QOJ((D../*<=K

K<BJJOD#ZZ9IQSQ[Q[\DDK!D    c                     Xl         g N)r=   )rP   rf   s     rk   set_env_randomizer(MinitaurBulletDuckEnv.set_env_randomizer   s    )rn   c                     Xl         g rp   )_args)rP   argss     rk   	configureMinitaurBulletDuckEnv.configure   s    Jrn   c                    U R                   (       Ga  U R                  R                  5         U R                  R                  [	        U R
                  5      S9  U R                  R                  U R                  5        U R                  R                  SU R                  -  5      U l
        U R                  R                  SU R                  -  [        [        5      U l        U R                  R                  SSS5        U R                  nU R                   n["        R$                  " U R                  U R                  U R                  U R&                  U R(                  U R*                  UU R,                  U R.                  U R0                  UU R2                  U R4                  S9U l        OHU R"                  R7                  SS9  U R                  R9                  U R                  [        [        5        U R:                  b  U R:                  R=                  U 5        SU l        / S	QU l         / U l!        U R                  RE                  U RF                  U RH                  U RJ                  / S	Q5        U R0                  (       d~  [M        S
5       Ho  nU R*                  (       d  U R                  (       a0  U R"                  RO                  [P        RR                  S-  /S-  5        U R                  RU                  5         Mq     U RW                  5       $ )N)numSolverIterationsz%s/plane.urdfz%s/duck_vhacd.urdfr   i)pybullet_clientrQ   	time_steprY   rZ   r[   r]   r^   r_   r`   ra   rc   re   F)reload_urdfr   d      r   ),r9   rC   resetSimulationsetPhysicsEngineParameterintr   setTimeStepr   loadURDFr   	_groundIdduckStartPosduckStartOrnr7   
setGravityr/   r3   r   Minitaurr    r!   r-   r0   r1   r2   r4   r:   ResetresetBasePositionAndOrientationr=   randomize_envr#   r%   _objectivesresetDebugVisualizerCamerar5   r6   r8   rangeApplyActionmathpistepSimulation_noisy_observation)rP   	acc_motormotor_protect_s       rk   rE   MinitaurBulletDuckEnv.reset   sO   
++-
55!$"D"DE 6 G
''8,,55o6WXdn**334H4??4Z4@,Pdl
&&q!S144i55m((9N9N37??37??@D@\@\>B>X>X<@<T<TFO26..26..@D@\@\CP15?C?Z?Z\dm mme,
;;DLL,<HJ'
((.D(DD44T^^T]]59__iQ''Sz!##t'I'I
--
#
#TWWq[MA$5
6,,.  ""$$rn   Nc                 D    [         R                  " U5      u  U l        nU/$ rp   )r   	np_random)rP   rD   s     rk   rD   MinitaurBulletDuckEnv.seed   s     ",,T2DND6Mrn   c                    U R                   (       a{  [        U5       HQ  u  p#U R                  * [        -
  Us=::  a  U R                  [        -   ::  a  M7  O  [	        SR                  X#5      5      e   U R                  R                  U5      nU$ )Nz{}th action {} out of bounds.)r.   	enumerater,   
ACTION_EPS
ValueErrorformatr   ConvertFromLegModel)rP   actioniaction_components       rk   "_transform_action_to_motor_command8MinitaurBulletDuckEnv._transform_action_to_motor_command   s{    !*6!2
!###j04D 0""Z/0:AA!VW
W "3 }}008fMrn   c                 4   U R                   (       a  [        R                  " 5       U R                  -
  n[        R                  " 5       U l        U R                  U R                  -  U-
  nUS:  a  [        R
                  " U5        U R                  R                  5       nU R                  R                  U R                  U R                  U R                  U5        U R                  U5      n[        U R                  5       H8  nU R                  R                  U5        U R                  R!                  5         M:     U =R"                  S-  sl        U R%                  5       nU R'                  5       n[(        R*                  " U R-                  5       5      Xg0 4$ )a  Step forward the simulation, given the action.

Args:
  action: A list of desired motor angles for eight motors.

Returns:
  observations: The angles, velocities and torques of all motors.
  reward: The reward for the current state-action pair.
  done: Whether the episode has ended.
  info: A dictionary that stores diagnostic information.

Raises:
  ValueError: The action dimension is not the same as the number of motors.
  ValueError: The magnitude of actions is out of bounds.
r   r   )r$   timer;   r   r   sleepr   GetBasePositionrC   r   r5   r6   r8   r   r   r   r   r#   _reward_terminationrI   rJ   r   )rP   r   
time_spenttime_to_sleepbase_posr   rewarddones           rk   stepMinitaurBulletDuckEnv.step   s.      99;!6!66j"iikd))DOO;jHm		

=!..0h
66t~~t}}7;R44V<F4&&'
mm'
**, ( 	a\\^FD88D++-.b@@rn   c           	         US:w  a  [         R                  " / 5      $ U R                  R                  5       nU R                  R                  UU R                  U R                  U R                  SSS9nU R                  R                  S[        [        5      [        -  SSS9nU R                  R                  [        [        UU[        R                  S	9u    pgnn[         R                  " U5      nUS S 2S S 2S S
24   nU$ )Nr   r   r~   )cameraTargetPositiondistanceyawpitchrollupAxisIndex<   g?g      Y@)fovaspectnearValfarVal)widthheight
viewMatrixprojectionMatrixrenderer   )rI   rJ   r   r   rC   !computeViewMatrixFromYawPitchRollr5   r6   r8   computeProjectionMatrixFOVfloatRENDER_WIDTHRENDER_HEIGHTgetCameraImagerA   ER_BULLET_HARDWARE_OPENGL)	rP   modecloser   view_matrixproj_matrixr   pxr   s	            rk   rd   MinitaurBulletDuckEnv.render  s   {XXb\}},,.H''II%MMoo J K ''BBrJOP\J]CPKQKNJO	 C QK 


.
.\6C:E@K8@8Z8Z	 / \Qq
 I!Q(#Irn   c                 f    [         R                  " U R                  [        [        [        -    5      $ )zLGet the minitaur's motor angles.

Returns:
  A numpy array of motor angles.
)rI   rJ   r"   MOTOR_ANGLE_OBSERVATION_INDEX
NUM_MOTORSrP   s    rk   get_minitaur_motor_angles/MinitaurBulletDuckEnv.get_minitaur_motor_angles&  s1     88D%%&CDa&0E1 2 3 3rn   c                 f    [         R                  " U R                  [        [        [        -    5      $ )zTGet the minitaur's motor velocities.

Returns:
  A numpy array of motor velocities.
)rI   rJ   r"    MOTOR_VELOCITY_OBSERVATION_INDEXr   r   s    rk   get_minitaur_motor_velocities3MinitaurBulletDuckEnv.get_minitaur_motor_velocities/  s1     88:;[$<% 	&' 'rn   c                 f    [         R                  " U R                  [        [        [        -    5      $ )zNGet the minitaur's motor torques.

Returns:
  A numpy array of motor torques.
)rI   rJ   r"   MOTOR_TORQUE_OBSERVATION_INDEXr   r   s    rk   get_minitaur_motor_torques0MinitaurBulletDuckEnv.get_minitaur_motor_torques9  s1     8889W$:% 	&' 'rn   c                 P    [         R                  " U R                  [        S 5      $ )zwGet the minitaur's base orientation, represented by a quaternion.

Returns:
  A numpy array of minitaur's orientation.
N)rI   rJ   r"   "BASE_ORIENTATION_OBSERVATION_INDEXr   s    rk   get_minitaur_base_orientation3MinitaurBulletDuckEnv.get_minitaur_base_orientationC  s"     88D%%&H&IJKKrn   c                 ~    U R                   R                  U R                  U R                  5      n[	        U5      S:  $ )Nr   )rC   getContactPointsr7   r   len)rP   pointss     rk   	lost_duckMinitaurBulletDuckEnv.lost_duckK  s0    ""33DLL$..QFv;?rn   c                 P   U R                   R                  5       nU R                  R                  U5      nUSS nU R                   R	                  5       n[
        R                  " [
        R                  " / SQ5      [
        R                  " U5      5      S:  =(       d    US   S:  $ )aJ  Decide whether the minitaur has fallen.

If the up directions between the base and the world is larger (the dot
product is smaller than 0.85) or the base is very low on the ground
(the height is smaller than 0.13 meter), the minitaur is considered fallen.

Returns:
  Boolean value that indicates whether the minitaur has fallen.
   N)r   r   r   g333333?r~   gp=
ף?)r   GetBaseOrientationrC   getMatrixFromQuaternionr   rI   dotasarray)rP   orientationrot_matlocal_upposs        rk   	is_fallenMinitaurBulletDuckEnv.is_fallenO  s     --224K##;;KHGqr{H
--
'
'
)CFF2::i("**X*>?$FW#a&SW-Xrn   c                     U R                   R                  5       n[        R                  " US   S-  US   S-  -   5      nU R	                  5       =(       d%    U R                  5       =(       d    X R                  :  $ )Nr   r~   r   )r   r   r   sqrtr   r   r*   )rP   positionr   s      rk   r   "MinitaurBulletDuckEnv._termination_  s^    }},,.Hyy!a(1+q.89H>>Rt~~/R8>R>R3RRrn   c                 t   U R                   R                  5       nUS   U R                  S   -
  n[        US   U R                  S   -
  5      * n[        US   U R                  S   -
  5      * nXl        [        R                  " [        R
                  " U R                   R                  5       U R                   R                  5       5      5      U R                  -  nU R                  U-  U R                  U-  -
  U R                  U-  -   U R                  U-  -   nU R                  R                  X%X4/5        U$ )Nr   r   r~   )r   r   r%   absrI   r   GetMotorTorquesGetMotorVelocitiesr   r&   r'   r(   r)   r   append)rP   current_base_positionforward_rewarddrift_rewardshake_rewardenergy_rewardr   s          rk   r   MinitaurBulletDuckEnv._rewardd  s(    MM99;*1-0H0H0KKN-a043K3KA3NNOOL-a043K3KA3NNOOL4FF
t}},,.}}//1	346:ooFM ##n4t7J7J]7ZZ  </0262D2D|2STF^LWXMrn   c                     U R                   $ rp   )r   r   s    rk   get_objectives$MinitaurBulletDuckEnv.get_objectivesr  s    rn   c                 X    U R                   R                  5       U l        U R                  $ rp   )r   GetObservationr"   r   s    rk   _get_observation&MinitaurBulletDuckEnv._get_observationu  s#    446Drn   c                 &   U R                  5         [        R                  " U R                  5      nU R                  S:  aP  U[        R
                  R                  U R                  UR                  S9U R                  R                  5       -  -  nU$ )Nr   )scalesize)
r
  rI   rJ   r"   r+   randomnormalshaper   rF   )rP   observations     rk   r   (MinitaurBulletDuckEnv._noisy_observationy  sz    ((4,,-K$$q(
))

!>!>[EVEV

W
--
0
0
234k rn   z0.9.6)*r/   r,   r   rt   r5   r8   r6   r*   r&   r(   r7   r'   r=   r#   r   r9   r$   r:   r%   r;   r.   r1   r0   r3   r!   r   r   r"   r+   r4   r-   rC   r    r)   r   r2   r   rM   r   r   rN   rO   rp   )r   F)(__name__
__module____qualname____firstlineno____doc__metadatapybullet_datagetDataPathr   rI   r   r	   MinitaurEnvRandomizerrl   rq   rv   rE   rD   r   r   rd   r   r   r   r   r   r   r   r   r  r
  r   r   gym__version___render_reset_seed_step__static_attributes__ rn   rk   r   r   (   s     '4QST( ))+5\!!66#'" $,BBD/r"h**%X#AJ43''LY S
 3??#mG&<<GFEE	 =rn   r   ),r  osinspectpathdirnameabspathgetfilecurrentframe
currentdir	parentdirsysinsertr   r   pkg_resourcesr   r  r   	gym.utilsr   numpyrI   rA   pybullet_utilsr   r?    r   r  r	   r>   r   r   r   r   r   r   rG   r   r   r   r   Envr   r$  rn   rk   <module>r6     s    
 WW__RWW__W__W=Q=Q=S-TUV
GGOOBGGOOJ78	   1i     ' 
     .   %
 ! #@:#M  !AJ!N %Cj%P "
#^CGG ^rn   