
    ni|*                     t   S r SSKrSSKrSSKrSSK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SKJr  S	rS
r/ SQr/ SQr/ SQr\R*                  " / SQ5      rSrSrSrSr\R*                  " \\\/S
-  5      r\R8                  rSrSr\R*                  " \
R@                  \
RB                  \
RD                  /\-  5      r#\RH                  " S5      r%\RH                  " S5      r&\RH                  " S5      r'\RH                  " S5      r(Sr)Sr*Sr+Sr,\RZ                   " S S\R\                  5      5       r/g)z'Pybullet simulation of a Laikago robot.    N)transformations)laikago_pose_utilslaikago_constants)laikago_motor)minitaur)robot_config      )FR_hip_motor_2_chassis_jointFR_upper_leg_2_hip_motor_jointFR_lower_leg_2_upper_leg_jointFL_hip_motor_2_chassis_jointFL_upper_leg_2_hip_motor_jointFL_lower_leg_2_upper_leg_jointRR_hip_motor_2_chassis_jointRR_upper_leg_2_hip_motor_jointRR_lower_leg_2_upper_leg_jointRL_hip_motor_2_chassis_jointRL_upper_leg_2_hip_motor_jointRL_lower_leg_2_upper_leg_joint)r   r      )r   r   gQ?)r   r   r   r   r   r   r   r   r   r   r   g        g333333gQ?   gQ?))zG?%䃞r   )r   %䃞?r   )zGʿr   r   )r   r   r   z\w+_chassis_\w+z\w+_hip_motor_\w+z\w+_lower_leg_\w+zjtoe\d*zlaikago.urdfzlaikago_toes_zup.urdf   c                      ^  \ rS rSrSr\4U 4S jjrS rS rS r	S r
U 4S jrS	 rS
 rS rS rS rS rS rS rS rSU 4S jjrS r\S 5       r\S 5       r\S 5       r\S 5       r\S 5       rSrU =r$ )LaikagoD   z#A simulation for the Laikago robot.c                 4  > Xl         SU;  a  SUS'   SU;  a  SUS'   SU;  a  S US'   SU;   a  US   U l        US	 OSU l        [        US'   [        US	'   [        US
'   [
        US'   SUS'   [        R                  US'   S US'   [        [        U ]*  " S0 UD6  g )Nmotor_kpg      Y@motor_kd       @motor_torque_limitsenable_clip_motor_commandsF
num_motorsdofs_per_legmotor_directionmotor_offsetmotor_overheat_protectionmotor_model_classsafety_config )_urdf_filename_enable_clip_motor_commands
NUM_MOTORSDOFS_PER_LEGJOINT_DIRECTIONSJOINT_OFFSETSr   LaikagoMotorModelsuperr!   __init__)selfurdf_filenamekwargs	__class__s      _/home/james-whalen/.local/lib/python3.13/site-packages/pybullet_envs/minitaur/robots/laikago.pyr9   Laikago.__init__H   s    ' fZfZF*&*f"#
 $v-)/0L)Md&
-
.).d& &F<)F> 0F*F>*/F&'"/"A"AF"F?	'4!+F+    c                 d   U R                  5       nU R                  (       aR  U R                  R                  UU R	                  5       U R                  5       U R                  R                  S9U l        g U R                  R                  XR	                  5       U R                  5       5      U l        g )N)flags)GetURDFFile_self_collision_enabled_pybullet_clientloadURDF_GetDefaultInitPosition_GetDefaultInitOrientationURDF_USE_SELF_COLLISION	quadruped)r:   laikago_urdf_paths     r>   _LoadRobotURDFLaikago._LoadRobotURDFe   s    ((*##,,55


&
&
(

)
)
+%%==	 6 ?dn ,,55
99;

)
)
+-dnr@   c                 X   U R                  5         US::  a  g [        S5       H/  nU R                  [        [        R
                  R                  S9  M1     UbR  [        X R                  -  5      n[        U5       H+  nU R                  U[        R
                  R                  S9  M-     g g )Nr   i  )motor_control_mode)	ReceiveObservationrange_StepInternalINIT_MOTOR_ANGLESr	   MotorControlModePOSITIONint	time_step)r:   default_motor_angles
reset_time_num_steps_to_resets        r>   _SettleDownForResetLaikago._SettleDownForResetr   s    Q3Z

)::CC  E  'zNN:;'(! +<<EE 	 	G ) (r@   c                     [         $ N)_DEFAULT_HIP_POSITIONSr:   s    r>   GetHipPositionsInBaseFrame"Laikago.GetHipPositionsInBaseFrame   s    !!r@   c                    U R                   R                  U R                  S9n/ SQnU HC  nU[           U R                  :X  a  M   U R                  R                  U[           5      nSX$'   ME     U$ ! [         a     MV  f = f)N)bodyA)FFFFT)rE   getContactPointsrJ   _BODY_B_FIELD_NUMBER_foot_link_idsindex_LINK_A_FIELD_NUMBER
ValueError)r:   all_contactscontactscontacttoe_link_indexs        r>   GetFootContactsLaikago.GetFootContacts   s    ((999OL+H	%	&$..	8,,22()+#'    O  s   &A33
B Bc                    > U R                   [        :X  a  [        [        U ]  U5      $ [        [        U ]  U5      SSS24   $ )z%Compute the Jacobian for a given leg.)r   r   r   N)r1   URDF_WITH_TOESr8   r!   ComputeJacobian)r:   leg_idr=   s     r>   rt   Laikago.ComputeJacobian   s@     n,7D1&997D1&9)Q,GGr@   c           	      *   AU R                    HM  nU R                   U   nU R                  R                  U R                  UU R                  R                  SSS9  MO     [        [        [        [        [        5      5      5       H  u  p$SU;   a  [        U   [        -   nO<SU;   a  [        U   [        -   nO%SU;   a  [        U   [        -   nO[        SU-  5      eU R                  R                  U R                  U R                   U   USS9  M     g )Nr   )	bodyIndex
jointIndexcontrolModetargetVelocityforcehip_motor_2_chassis_jointupper_leg_2_hip_motor_jointlower_leg_2_upper_leg_jointz/The name %s is not recognized as a motor joint.)r{   )_joint_name_to_idrE   setJointMotorControl2rJ   VELOCITY_CONTROLzipMOTOR_NAMESrQ   lenrS   HIP_JOINT_OFFSETUPPER_LEG_JOINT_OFFSETKNEE_JOINT_OFFSETrk   resetJointState)r:   add_constraintnamejoint_idiangles         r>   	ResetPoseLaikago.ResetPose   s   &&''-h
11NN++<< 2  ' {E#k*:$;<	$	,!!$'77(D0!!$'==(D0!!$'88J  	
++
..$006a , Q =r@   c                 p    [         R                  R                  U R                  SU R                  -   5      $ )Nzlaikago/)ospathjoin
_urdf_rootr1   ra   s    r>   rC   Laikago.GetURDFFile   s&    77<<d6I6I)IJJr@   c                    U R                   R                  U R                  5      nS/U l        / U l        / U l        / U l        / U l        [        U5       GHK  nU R                   R                  U R                  U5      nUS   R                  S5      nU R                  U   n[        R                  U5      (       a  U R                  R                  U5        M  [        R                  U5      (       a  U R
                  R                  U5        M  [         R                  U5      (       a  U R                  R                  U5        M  ["        R                  U5      (       a4  U R$                  [&        :X  d   eU R                  R                  U5        GM@  [)        SU-  5      e   U R                  R+                  U R                  5        U R                  R+                  U R                  5        U R$                  [,        :X  a%  U R                  R+                  U R                  5        [/        U R                  5      [0        :X  d   eU R                  R3                  5         U R
                  R3                  5         U R                  R3                  5         U R                  R3                  5         U R                  R3                  5         g)znBuild the link Ids from its name in the URDF file.

Raises:
  ValueError: Unknown category of the joint name.
r   r   zUTF-8zUnknown category of joint %sN)rE   getNumJointsrJ   _chassis_link_ids_leg_link_ids_motor_link_ids_knee_link_idsrh   rQ   getJointInfodecoder   CHASSIS_NAME_PATTERNmatchappendMOTOR_NAME_PATTERNKNEE_NAME_PATTERNTOE_NAME_PATTERNr1   rs   rk   extendURDF_NO_TOESr   NUM_LEGSsort)r:   
num_jointsr   
joint_info
joint_namer   s         r>   _BuildUrdfIdsLaikago._BuildUrdfIds   s&    &&33DNNCJ TDDDDD:((55dnnaHja=''0j''
3h		#	#J	/	/%%h/##J//##H- "":..""8,!!*--""n444""8,7*DEE! $ 	d112d112l*
  !4!45t""#x///!
r@   c                     [         $ r_   )r   ra   s    r>   _GetMotorNamesLaikago._GetMotorNames   s    r@   c                 <    U R                   (       a  [        $ [        $ r_   )_on_rackINIT_RACK_POSITIONINIT_POSITIONra   s    r>   rG   Laikago._GetDefaultInitPosition   s    }}r@   c                     U R                   [        :X  a  / SQ$ [        R                  " [        R
                  S-  S[        R
                  S-  SS9$ )N)r   r   r   r   r&   r   sxyz)aiajakaxes)r1   rs   r   quaternion_from_eulermathpira   s    r>   rH   "Laikago._GetDefaultInitOrientation   sF    
 n,22WWs]qTWWs]A Ar@   c                 "    U R                  5       $ )z"Get default initial base position.)rG   ra   s    r>   GetDefaultInitPositionLaikago.GetDefaultInitPosition       ''))r@   c                 "    U R                  5       $ )z%Get default initial base orientation.)rH   ra   s    r>   GetDefaultInitOrientation!Laikago.GetDefaultInitOrientation   s    **,,r@   c                 .    [         [        -   [        -  nU$ )zGet default initial joint pose.)rS   r6   r5   )r:   
joint_poses     r>   GetDefaultInitJointPoseLaikago.GetDefaultInitJointPose  s    #m37GGJr@   c                 p   > U R                   (       a  U R                  U5      n[        [        U ]  X5        g)zClips and then apply the motor commands using the motor model.

Args:
  motor_commands: np.array. Can be motor angles, torques, hybrid commands,
    or motor pwms (for Minitaur only).N
  motor_control_mode: A MotorControlMode enum.
N)r2   _ClipMotorCommandsr8   r!   ApplyAction)r:   motor_commandsrO   r=   s      r>   r   Laikago.ApplyAction  s0     ''..~>n	'4$^H
r@   c                 j    [         nU R                  5       n[        R                  " UX2-
  X2-   5      nU$ )zClips motor commands.

Args:
  motor_commands: np.array. Can be motor angles, torques, hybrid commands,
    or motor pwms (for Minitaur only).

Returns:
  Clipped motor commands.
)MAX_MOTOR_ANGLE_CHANGE_PER_STEPGetMotorAnglesnpclip)r:   r   max_angle_changecurrent_motor_angless       r>   r   Laikago._ClipMotorCommands  s>     7..0WW^1D1DFN r@   c                     A [         $ r_   r   )clss    r>   GetConstantsLaikago.GetConstants)  s    r@   c                     U R                   $ r_   )rJ   ra   s    r>   robot_idLaikago.robot_id1  s    >>r@   c                 "    U R                  5       $ r_   )GetBasePositionra   s    r>   base_positionLaikago.base_position5  s    !!r@   c                 "    U R                  5       $ r_   )GetTrueBaseRollPitchYawra   s    r>   base_roll_pitch_yawLaikago.base_roll_pitch_yaw9  r   r@   c                 "    U R                  5       $ r_   )GetTimeSinceResetra   s    r>   	timestampLaikago.timestamp=  s    !!##r@   )r   r2   rh   r   r   r   r1   rJ   r_   ) __name__
__module____qualname____firstlineno____doc__rs   r9   rL   r\   rb   rp   rt   r   rC   r   r   rG   rH   r   r   r   r   r   classmethodr   propertyr   r   r   r   __static_attributes____classcell__)r=   s   @r>   r!   r!   D   s    +#1 ,:-G"" HQ.K,\	A*-
&     " " * * $ $r@   r!   )0r   r   r   reginnumpyr   pybullet_utilsr   (pybullet_envs.minitaur.envs_v2.utilitiesr   pybullet_envs.minitaur.robotsr   r   r   r	   r3   r   r   r   r   arrayr5   r   r   r   r4   r6   r   PIr   r`   LAIKAGO_DEFAULT_ABDUCTION_ANGLELAIKAGO_DEFAULT_HIP_ANGLELAIKAGO_DEFAULT_KNEE_ANGLErS   compiler   r   r   r   r   rs   rg   rj   configurableMinitaurr!   r0   r@   r>   <module>r     sa   -  	 	 
  * G ; 7 2 6
  88BC    -/@AAEG	WW"&   HH660011 	   zz"45 ZZ 45 JJ34 ::j) (   z$h z$ z$r@   