
    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S	 r\S
:X  a  \" 5         gg)zCAn example to run of the minitaur gym environment with sine gaits.
    N)minitaur_gym_env)minitaur_env_randomizerc                  2   Sn [         R                  " 5       n[        R                  " SS[        R
                  SSSUSS9n[        R                  S-  /S-  n[        U 5       H!  nUR                  U5      u    pEnU(       d  M!    O   UR                  5         g)z?An example that the minitaur stands still using the reset pose.  TF)renderleg_model_enabledmotor_velocity_limitpd_control_enabledaccurate_motor_model_enabledmotor_overheat_protectionenv_randomizer
hard_reset      N)r   MinitaurEnvRandomizerr   MinitaurBulletEnvnpinfmathpirangestepreset)steps
randomizerenvironmentaction_dones         i/home/james-whalen/.local/lib/python3.13/site-packages/pybullet_envs/examples/minitaur_gym_env_example.pyResetPoseExampler!      s    
%'==?* 22$EJHJFJPTMQBL>CE+ GGaK=1&<a$$V,MAqt      c                     [         R                  " SS[        R                  SSSSSS9n S/S-  n[	        S5       H#  nSSUS-  S	:X  a  S
OS-  US:  a  S
OS-  -
  X'   M%     Sn/ n[	        U5       Hc  nSnXV-  nU/nUR                  U5        U R                  U5      u  n	    n
UR                  U	R                  5       5        UR                  U5        Me     U R                  5         g)zAn example of minitaur motor overheat protection is triggered.

The minitaur is leaning forward and the motors are getting obove threshold
torques. The overheat protection will be triggered in ~1 sec.
TF333333?g        r   r   r	   r   r   motor_kpmotor_kdon_rackr   皙?r   r         i  {Gz?N)
r   r   r   r   r   extendr   tolistappendr   )r   r   ir   actions_and_observationsstep_counter	time_steptcurrent_rowobservationr   s              r    MotorOverheatExampler8   %   s     !22$EJHJMQPT<@<@;@B+ 4!8&8aS!a%1*B!4a!eKKFI  %EllI A#Kv&++F3KAq{))+,##K0 # r"   c                     [         R                  " SS[        R                  SSSSSS9n SnSnSn/ n[	        U5       H  nS	nXV-  nU/n[
        R                  " X7-  5      U-  [
        R                  S
-  -   /S-  n	UR                  U	5        U R                  U	5      u  n
    nUR                  U
R                  5       5        UR                  U5        M     U R                  5         g)zAn example of minitaur standing and squatting on the floor.

To validate the accurate motor model we command the robot and sit and stand up
periodically in both simulation and experiment. We compare the measured motor
trajectories, torques and gains.
TFr$   g{Gz?r%   r   g      ?   r-   r   r   N)r   r   r   r   r   r   sinr   r.   r   r/   r0   r   )r   r   	amplitudespeedr2   r3   r4   r5   r6   r   r7   r   s               r    SineStandExampler>   H   s     !22$EJHJMQPT<@<@;@B+ %)
%EllI A#Khhuy!I-!;<q@Fv&++F3KAq{))+,##K0 # r"   c            	         [         R                  " 5       n [        R                  " S[        R
                  SSU SS9nSnSnSnSnSn[        U5       H  nSnXx-  n	Un
UnSnU	S	:  a  SnOU	S
:  a  SnOSn[        R                  " X-  5      X-   -  n[        R                  " X-  [        R                  -   5      X-
  -  n[        R                  " X-  5      U-  n[        R                  " X-  [        R                  -   5      U-  nXXUUUU/nUR                  U5      u  nnnnUU-  nU(       d  M    O   UR                  5         g)z0An example of minitaur walking with a sine gait.TF)r   r	   r
   r   r   r(   r   i N  r)   r+   r-   
      gN)r   r   r   r   r   r   r   r   r;   r   r   r   )r   r   
sum_rewardr   amplitude_1_boundamplitude_2_boundr=   r3   r4   r5   
amplitude1
amplitude2steering_amplitudea1a2a3a4r   r   rewardr   s                        r    SinePolicyExamplerM   m   sW   '==?* 22$HJFJ>CBL;@B+ *
%
%EllI A"J"J2v	
R 
!)	
 ?	@B	!)dgg%	&**I	JB	!)	z	)B	!)dgg%	&	3Bbb"b"-F$))&1AvtQ&Jt/ #0 r"   c                     [         R                  " [         R                  S9n U R                  SS[        SS9  U R                  5       n[        S[        UR                  5      -   5        UR                  S:X  a
  [        5         UR                  S:X  a
  [        5         UR                  S:X  a
  [        5         UR                  S	:X  a  [        5         g g )
N)formatter_classz--envz7environment ID (0==sine, 1==stand, 2=reset, 3=overheat)r   )helptypedefaultz--env=r+   r   r:   )argparseArgumentParserArgumentDefaultsHelpFormatteradd_argumentint
parse_argsprintstrenvrM   r>   r!   r8   )parserargss     r    mainr^      s    ""83Y3YZ&gT  ! 
			$3txx= !
hh!m
hh!m
hh!m
hh!m r"   __main__)__doc__osinspectpathdirnameabspathgetfilecurrentframe
currentdir	parentdirsysinsertr   numpyr   pybullet_envs.bulletr   rS   r   r!   r8   r>   rM   r^   __name__ r"   r    <module>rp      s    
 WW__RWW__W__W=Q=Q=S-TUV
GGOOBGGOOJ78	   1i     1  8( F"J'T& z& r"   