
    niSS                         S r SSKrSSKrSSKrSSKJr  SSKr/ SQr/ SQr	/ SQr
/ SQrS	rS
r/ SQr/ SQr/ SQr/ SQr/ SQrSr " S S\5      rg)zHThis file implements the functionalities of a minitaur using pybullet.

    N   )motor)r   r   皙?)r   r   r   r   )r   g{Gzt?r   )r   {Gz?r   g@      ?)
front_left	back_leftfront_right
back_right)motor_front_leftL_jointmotor_front_leftR_jointmotor_back_leftL_jointmotor_back_leftR_jointmotor_front_rightL_jointmotor_front_rightR_jointmotor_back_rightL_jointmotor_back_rightR_joint)               	                                 )r         
               )r   r   r   r   r   r   r!   r#   c                   `   \ rS rSrSr\R                  R                  \R                  R                  \	5      S5      SS\
R                  SSSSSSSS4S	 jrS
 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S rS rS rS rS r S r!S r"S  r#S! r$S" r%S# r&S$ r'S% r(S&r)g'))Minitaur   zLThe minitaur class that simulates a quadruped robot from Ghost Robotics.

  z../datar   Fr   g{Gz?g333333?c                    SU l         [        U R                   S-  5      U l        Xl        X l        X@l        XPl        X`l        / SQU l        [        R                  " U R                   5      U l        [        R                  " U R                   5      U l        SU l        Xpl        Xl        Xl        Xl        U R                  (       aF  Xl        Xl        [(        R*                  " U R                  U R$                  U R&                  S9U l        O-U R                  (       a  SU l        Xl        OSU l        SU l        X0l        U R1                  5         g)a  Constructs a minitaur and reset it to the initial states.

Args:
  pybullet_client: The instance of BulletClient to manage different
    simulations.
  urdf_root: The path to the urdf folder.
  time_step: The time step of the simulation.
  self_collision_enabled: Whether to enable self collision.
  motor_velocity_limit: The upper limit of the motor velocity.
  pd_control_enabled: Whether to use PD control for the motors.
  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 acurate 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.
  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.
  kd_for_pd_controllers: kd value for the pd controllers of the motors.
r   r   )r+   r+   r+   r+   r   r   r   r   g      @)torque_control_enabledkpkdr   N)
num_motorsintnum_legs_pybullet_client
_urdf_root_self_collision_enabled_motor_velocity_limit_pd_control_enabled_motor_directionnpzeros_observed_motor_torques_applied_motor_torques
_max_force_accurate_motor_model_enabled_torque_control_enabled_motor_overheat_protection_on_rack_kp_kdr   
MotorModel_motor_model	time_stepReset)selfpybullet_client	urdf_rootrI   self_collision_enabledmotor_velocity_limitpd_control_enabledaccurate_motor_model_enabledmotor_kpmotor_kdr0   motor_overheat_protectionon_rackkd_for_pd_controllerss                 W/home/james-whalen/.local/lib/python3.13/site-packages/pybullet_envs/bullet/minitaur.py__init__Minitaur.__init__!   s    L DO!+,DM+O#9 !518D#%88DOO#<D "$((4??";DDO)E&#9 &?#M))hh**$B^B^.2hh.2hh8d 
	!	!dh&hdhdhNJJL    c                    U R                   R                  U R                  [        5      S   U l        / U l        U R
                  R                  U R                   R                  U R                  [        S   5      S   5        U R
                  R                  U R                   R                  U R                  [        S   5      S   5        g )Nr   )	r6   getDynamicsInfo	quadrupedBASE_LINK_ID_base_mass_urdf_leg_masses_urdfappendLEG_LINK_IDMOTOR_LINK_IDrK   s    rW   _RecordMassInfoFromURDF Minitaur._RecordMassInfoFromURDFe   s    00@@Q]^_`aDD  --dnnk!nMaPR  --dnnmA>NOPQRTrZ   c                    U R                   R                  U R                  5      n0 U l        [	        U5       HM  nU R                   R                  U R                  U5      nUS   U R                  US   R                  S5      '   MO     g )Nr   r   zUTF-8)r6   getNumJointsr]   _joint_name_to_idrangegetJointInfodecode)rK   
num_jointsi
joint_infos       rW   _BuildJointNameToIdDict Minitaur._BuildJointNameToIdDictm   sr    &&33DNNCJD:((55dnnaHj>HmdZ]11':; rZ   c                 ^    [          Vs/ s H  oR                  U   PM     snU l        g s  snf N)MOTOR_NAMESri   _motor_id_list)rK   
motor_names     rW   _BuildMotorIdListMinitaur._BuildMotorIdListt   s'    P[\P[*11*=P[\D\s   *c                    U(       Ga   U R                   (       aF  U R                  R                  SU R                  -  [        U R                  R
                  S9U l        O2U R                  R                  SU R                  -  [        5      U l        U R                  5         U R                  5         U R                  5         U R                  SS9  U R                  (       aF  U R                  R                  U R                  SSSU R                  R                  / SQ/ SQ/ SQ5        OiU R                  R                  U R                  [        [        5        U R                  R!                  U R                  / SQ/ SQ5        U R                  SS9  ["        R$                  " U R&                  5      U l        S/U R&                  -  U l        g	)
zReset the minitaur to its initial states.

Args:
  reload_urdf: Whether to reload the urdf file. If not, Reset() just place
    the minitaur back to its starting position.
z%s/quadruped/minitaur.urdf)flagsT)add_constraintr+   r   r   r   )r   r   r   FN)r8   r6   loadURDFr7   INIT_POSITIONURDF_USE_SELF_COLLISIONr]   rp   rw   re   	ResetPoserD   createConstraintJOINT_FIXEDresetBasePositionAndOrientationINIT_ORIENTATIONresetBaseVelocityr<   r=   r3   _overheat_counter_motor_enabled_list)rK   reload_urdfs     rW   rJ   Minitaur.Resetw   s[    		%	%..77(4??:''?? 8 A
 ..77(4??:MK
""$

""$
nnDn)	..t~~r2r/3/D/D/P/PR[/8)	E ;;DNNM<LN
--dnniS
nnEn*XXdoo6D $v7DrZ   c                 x    U R                   R                  U R                  UU R                   R                  US9  g )N)	bodyIndex
jointIndexcontrolModeforce)r6   setJointMotorControl2r]   TORQUE_CONTROL)rK   motor_idtorques      rW   _SetMotorTorqueByIdMinitaur._SetMotorTorqueById   s6    //$..;C<@<Q<Q<`<`6< 0 >rZ   c           
          U R                   R                  U R                  UU R                   R                  UU R                  U R
                  U R                  S9  g )N)r   r   r   targetPositionpositionGainvelocityGainr   )r6   r   r]   POSITION_CONTROLrE   rF   r@   )rK   r   desired_angles      rW   _SetDesiredMotorAngleById"Minitaur._SetDesiredMotorAngleById   sL    //$..;C<@<Q<Q<b<b?L=AXX=AXX6:oo 0 GrZ   c                 B    U R                  U R                  U   U5        g rs   )r   ri   )rK   rv   r   s      rW   _SetDesiredMotorAngleByName$Minitaur._SetDesiredMotorAngleByName   s    ""4#9#9*#E}UrZ   c                 ^    [        U R                  5       H  nU R                  X!5        M     g)zpReset the pose of the minitaur.

Args:
  add_constraint: Whether to add a constraint at the joints of two feet.
N)rj   r5   _ResetPoseForLeg)rK   r{   rn   s      rW   r   Minitaur.ResetPose   s%     4==!
A. "rZ   c           
         Sn[         R                  S-  nSn[        U   nU R                  R	                  U R
                  U R                  SU-   S-      U R                  SU-     U-  SS9  U R                  R	                  U R
                  U R                  SU-   S	-      U R                  SU-     U-  SS9  U R                  R	                  U R
                  U R                  SU-   S
-      U R                  SU-  S-      U-  SS9  U R                  R	                  U R
                  U R                  SU-   S-      U R                  SU-  S-      U-  SS9  U(       az  U R                  R                  U R
                  U R                  SU-   S-      U R
                  U R                  SU-   S	-      U R                  R                  / SQ[        [        5        U R                  (       d  U R                  (       a  U R                  R                  U R
                  U R                  SU-   S-      U R                  R                  SUS9  U R                  R                  U R
                  U R                  SU-   S
-      U R                  R                  SUS9  OYU R!                  SU-   S-   U R                  SU-     U-  5        U R!                  SU-   S
-   U R                  SU-  S-      U-  5        U R                  R                  U R
                  U R                  SU-   S	-      U R                  R                  SUS9  U R                  R                  U R
                  U R                  SU-   S-      U R                  R                  SUS9  g)zReset the initial pose for the leg.

Args:
  leg_id: It should be 0, 1, 2, or 3, which represents the leg at
    front_left, back_left, front_right and back_right.
  add_constraint: Whether to add a constraint at the joints of two feet.
r   g       @gPkwmotor_L_jointr   )targetVelocityknee_L_linkR_jointr   R_linkr|   )r   r   r   r   r   N)mathpiLEG_POSITIONr6   resetJointStater]   ri   r;   r   JOINT_POINT2POINTKNEE_CONSTRAINT_POINT_RIGHTKNEE_CONSTRAINT_POINT_LEFTrA   r:   r   VELOCITY_CONTROLr   )rK   leg_idr{   knee_friction_forcehalf_pi
knee_angleleg_positions          rW   r   Minitaur._ResetPoseForLeg   s    ggmGJ'L))$..*.*@*@LAXAJBK +L*.*?*?F
*Kg*U9:	 * <
 	))$..*.*@*@<AWAIBJ +K*.*?*?F
*Kj*X9:	 * <
 	))$..*.*@*@LAXAJBK +L*.*?*?F
Q*ORY*Y9:	 * <
 	))$..*.*@*@<AWAIBJ +K*.*?*?F
Q*OR\*\9:	 * <
 
,,
..$00<1G(1RS
..$00<1G(1RS



1
19>Y
$	& ))T-E-E
11NN,,X-Dy-PQ++<<# 2 % 11NN,,X-Dy-PQ++<<# 2 % &&x,'>'J'+'<'<QZ'H7'RT
&&x,'>'J'+'<'<QZ!^'Lw'VX 	//..**7\+AH+LM))::! 0 # 	//..**7\+AH+LM))::! 0 #rZ   c                 T    U R                   R                  U R                  5      u  pU$ )zRGet the position of minitaur's base.

Returns:
  The position of minitaur's base.
r6   getBasePositionAndOrientationr]   )rK   position_s      rW   GetBasePositionMinitaur.GetBasePosition   s%     ((FFt~~VKHOrZ   c                 T    U R                   R                  U R                  5      u  pU$ )zsGet the orientation of minitaur's base, represented as quaternion.

Returns:
  The orientation of minitaur's base.
r   )rK   r   orientations      rW   GetBaseOrientationMinitaur.GetBaseOrientation  s&     ++II$..YNArZ   c                     U R                   $ )zNGet the length of the action list.

Returns:
  The length of the action list.
)r3   rd   s    rW   GetActionDimensionMinitaur.GetActionDimension  s     ??rZ   c                 `   [         R                  " S/U R                  5       -  5      n[        R                  USU R
                  & [        R                  XR
                  SU R
                  -  & [        R                  USU R
                  -  SU R
                  -  & SUSU R
                  -  S& U$ )zGet the upper bound of the observation.

Returns:
  The upper bound of an observation. See GetObservation() for the details
    of each element of an observation.
g        r   r   r   r   N)	r<   arrayGetObservationDimensionr   r   r3   r   MOTOR_SPEED_LIMITOBSERVED_TORQUE_LIMIT)rK   upper_bounds     rW   GetObservationUpperBound!Minitaur.GetObservationUpperBound  s     ((C54#?#?#AABK%)WWK$//"8=8O8OKDOO 34<A<W<WKDOO#A$78(+KDOO#$%rZ   c                 $    U R                  5       * $ )z'Get the lower bound of the observation.)r   rd   s    rW   GetObservationLowerBound!Minitaur.GetObservationLowerBound&  s    ))+++rZ   c                 4    [        U R                  5       5      $ )zXGet the length of the observation list.

Returns:
  The length of the observation list.
)lenGetObservationrd   s    rW   r    Minitaur.GetObservationDimension*  s     t""$%%rZ   c                 h   / nUR                  U R                  5       R                  5       5        UR                  U R                  5       R                  5       5        UR                  U R	                  5       R                  5       5        UR                  [        U R                  5       5      5        U$ )aN  Get the observations of minitaur.

It includes the angles, velocities, torques and the orientation of the base.

Returns:
  The observation list. observation[0:8] are motor angles. observation[8:16]
  are motor velocities, observation[16:24] are motor torques.
  observation[24:28] is the orientation of the base, in quaternion form.
)extendGetMotorAnglestolistGetMotorVelocitiesGetMotorTorqueslistr   )rK   observations     rW   r   Minitaur.GetObservation2  s     Kt**,3356t..0779:t++-4467tD33567rZ   c                    U R                   [        R                  :  a]  U R                  5       nX R                  U R                   -  -   nX R                  U R                   -  -
  n[        R
                  " XU5      nU R                  (       d  U R                  (       Ga  U R                  5       nU R                  5       nU R                  (       GaI  U R                  R                  XU5      u  pxU R                  (       a  [        U R                  5       Ht  n	[        Xy   5      [        :  a  U R                   U	==   S-  ss'   OSU R                   U	'   U R                   U	   ["        U R                  -  :  d  Me  SU R$                  U	'   Mv     Xl        [        R(                  " XpR*                  5      U l        [/        U R0                  U R,                  U R$                  5       H2  u  pnU(       a  U R3                  X5        M   U R3                  U
S5        M4     gU R4                  * XQ-
  -  U R6                  U-  -
  nXl        [        R(                  " U R&                  U R*                  5      U l        [/        U R0                  U R8                  5       H  u  pU R3                  X5        M     g[        R(                  " XR*                  5      n[/        U R0                  U5       H  u  pU R;                  X5        M     g)a  Set the desired motor angles to the motors of the minitaur.

The desired motor angles are clipped based on the maximum allowed velocity.
If the pd_control_enabled is True, a torque is calculated according to
the difference between current and desired joint angle, as well as the joint
velocity. This torque is exerted to the motor. For more information about
PD control, please refer to: https://en.wikipedia.org/wiki/PID_controller.

Args:
  motor_commands: The eight desired motor angles.
r   r   FN)r9   r<   infr   rI   cliprA   r:   r   rH   convert_to_torquerC   rj   r3   absOVERHEAT_SHUTDOWN_TORQUEr   OVERHEAT_SHUTDOWN_TIMEr   r>   multiplyr;   _applied_motor_torquezipru   r   rE   rF   r?   r   )rK   motor_commandscurrent_motor_anglemotor_commands_maxmotor_commands_minqqdotactual_torqueobserved_torquern   r   motor_torquemotor_enabledtorque_commandsmotor_commands_with_directionmotor_command_with_directions                   rW   ApplyActionMinitaur.ApplyActionC  sx    !!BFF* //1/..4C]C]2]]/..4C]C]2]]ww~CUVn))T-E-E-E



a$$&d		+	+	+)-):):)L)Lt*%&**)a=#$'??$$Q'1,'*+d$$Q'&&q),BT^^,SS,1d&&q) * (7$ &([[@U@U%V"589L9L9=9S9S9=9Q9Q6S1HM $$X<$$Xq16S  88)q'9:TXX_L (7$ ')kk$2N2N262G2G'I# '*$*=*=t?Z?Z&["H

"
"8
: '\ ')kk.BWBW&X#478K8K8U5W
0(&&xN5WrZ   c                     U R                    Vs/ s H,  nU R                  R                  U R                  U5      S   PM.     nn[        R
                  " X R                  5      nU$ s  snf )zLGet the eight motor angles at the current moment.

Returns:
  Motor angles.
r   ru   r6   getJointStater]   r<   r   r;   )rK   r   motor_angless      rW   r   Minitaur.GetMotorAngles  sg     +++H 	++DNNHEaH+   ;;|-B-BCL   3A&c                     U R                    Vs/ s H,  nU R                  R                  U R                  U5      S   PM.     nn[        R
                  " X R                  5      nU$ s  snf )zRGet the velocity of all eight motors.

Returns:
  Velocities of all eight motors.
r   r   )rK   r   motor_velocitiess      rW   r   Minitaur.GetMotorVelocities  si     +++H 	++DNNHEaH+   {{#35J5JKr   c                 2   U R                   (       d  U R                  (       a  U R                  $ U R                   Vs/ s H,  nU R                  R                  U R                  U5      S   PM.     nn[        R                  " X R                  5      nU$ s  snf )zbGet the amount of torques the motors are exerting.

Returns:
  Motor torques of all eight motors.
r   )
rA   r:   r>   ru   r6   r   r]   r<   r   r;   )rK   r   motor_torquess      rW   r   Minitaur.GetMotorTorques  s     ))T-E-E))) ---h 


-
-dnnh
G
J-   kk-1F1FGms   3Bc                 N   [         R                  " U5      nSnSn[        U R                  S-  5      n[        R
                  S-  n[        U R                  5       HF  nUS-  nU* U-  XU-      U-   -  n	SU-  U-  X   -  n
Xu:  a  U
* n
[        R
                  U	-   U
-   X''   MH     U$ )zConvert the actions that use leg model to the real motor actions.

Args:
  actions: The theta, phi of the leg model.
Returns:
  The eight desired motor angles that can be used in ApplyActions().
r   g      ?r   r$   r+   )copydeepcopyr4   r3   r   r   rj   )rK   actionsmotor_anglescale_for_singularityoffset_for_singularityhalf_num_motors	quater_pirn   
action_idxforward_backward_componentextension_components              rW   ConvertFromLegModelMinitaur.ConvertFromLegModel  s     --(K $//A-.O!I4??#6j 
 9
,/03IIK !  !Gi/'2EE	
	22"<<?RRkn $ rZ   c                     U R                   $ )z,Get the mass of the base from the URDF file.)r_   rd   s    rW   GetBaseMassFromURDFMinitaur.GetBaseMassFromURDF  s    rZ   c                     U R                   $ )z,Get the mass of the legs from the URDF file.)r`   rd   s    rW   GetLegMassesFromURDFMinitaur.GetLegMassesFromURDF  s       rZ   c                 V    U R                   R                  U R                  [        US9  g )Nmass)r6   changeDynamicsr]   r^   )rK   	base_masss     rW   SetBaseMassMinitaur.SetBaseMass  s!    ((I(VrZ   c                     [          H*  nU R                  R                  U R                  X!S   S9  M,     [         H*  nU R                  R                  U R                  X!S   S9  M,     g)a3  Set the mass of the legs.

A leg includes leg_link and motor. All four leg_links have the same mass,
which is leg_masses[0]. All four motors have the same mass, which is
leg_mass[1].

Args:
  leg_masses: The leg masses. leg_masses[0] is the mass of the leg link.
    leg_masses[1] is the mass of the motor.
r   r  r   N)rb   r6   r  r]   rc   )rK   
leg_masseslink_ids      rW   SetLegMassesMinitaur.SetLegMasses  s\     
**4>>7TU*W  
**4>>7TU*W !rZ   c                 f    [          H'  nU R                  R                  U R                  X!S9  M)     g)zSet the lateral friction of the feet.

Args:
  foot_friction: The lateral friction coefficient of the foot. This value is
    shared by all four feet.
)lateralFrictionN)FOOT_LINK_IDr6   r  r]   )rK   foot_frictionr!  s      rW   SetFootFrictionMinitaur.SetFootFriction  s+      
**4>>7*b  rZ   c                 ^    U R                   (       a  U R                  R                  U5        g g rs   )rA   rH   set_voltage)rK   voltages     rW   SetBatteryVoltageMinitaur.SetBatteryVoltage  s$    ))
##G, *rZ   c                 ^    U R                   (       a  U R                  R                  U5        g g rs   )rA   rH   set_viscous_damping)rK   viscous_dampings     rW   SetMotorViscousDampingMinitaur.SetMotorViscousDamping  s$    ))
++O< *rZ   )rA   r   r?   r_   ri   rF   rE   r`   r@   r;   r   ru   rH   rC   r9   r>   rD   r   r:   r6   r8   rB   r7   r5   r3   r]   rI   N)T)*__name__
__module____qualname____firstlineno____doc__ospathjoindirname__file__r<   r   rX   re   rp   rw   rJ   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r   r  r  r  r  r"  r(  r-  r2  __static_attributes__ rZ   rW   r-   r-      s     bggooh&?K&+$&FF"',1&+).%(BHTL]8B>GV/H#T,&"@OD 2 !WX c-=rZ   r-   )r8  r  r   numpyr<    r   r9  r~   r   r   r   r   r   r   rt   rb   rc   r&  r^   objectr-   r?  rZ   rW   <module>rC     so        	 - +   G
 I-,W=v W=rZ   