
    ni06                         S SK JrJrJr  S SKrS SKrS SKrS SKrS SK J	r	   " S S\5      r
 " S S\
5      r " S S	\
5      r " S
 S\
5      r " S S\
5      r " S S\
5      rS rS r " S S\5      r " S S\5      rg)    )XmlBasedRobotMJCFBasedRobotURDFBasedRobotN)BodyPartc                   2    \ rS rSrS rS rS rS rS rSr	g)	
WalkerBase	   c                     [         R                  " XX#U5        XPl        SU l        Su  U l        U l        U l        SU l        SU l        / SQU l	        g )Nr   r   r   r   g     @@)
r   __init__powercamera_xstart_pos_xstart_pos_ystart_pos_zwalk_target_xwalk_target_ybody_xyz)selffn
robot_name
action_dimobs_dimr   s         X/home/james-whalen/.local/lib/python3.13/site-packages/pybullet_envs/robot_locomotors.pyr   WalkerBase.__init__   sL    DjgFJDM;B8Dd&(8DDDM    c                    Xl         U R                   H-  nUR                  U R                  R	                  SSS9S5        M/     U R
                   Vs/ s H  o0R                  U   PM     snU l        [        R                  " U R
                   Vs/ s H  nSPM     sn[        R                  S9U l        U R                  R                  U 5        S U l        g s  snf s  snf )Ngg?lowhighr           dtype)_pordered_jointsreset_current_position	np_randomuniform	foot_listpartsfeetnparrayfloat32feet_contactsceneactor_introduce	initial_z)r   bullet_clientjfs       r   robot_specific_resetWalkerBase.robot_specific_reset   s    G  t~~55$S5I1M ! )-71A7DIt~~!>~!#~!>bjjQDJJt$DN 8!>s   CCc                 .   [         R                  " U5      R                  5       (       d   e[        U R                  5       HQ  u  p#UR                  U R                  UR                  -  [        [         R                  " X   SS5      5      -  5        MS     g )N   )
r,   isfiniteall	enumerater%   set_motor_torquer   
power_coeffloatclip)r   anr4   s       r   apply_actionWalkerBase.apply_action   sj    KKN  ! $--.all2U27714R;P5QQR /r   c           
         [         R                  " U R                   Vs/ s H  oR                  5       PM     sn[         R                  S9R                  5       nUSS S2   U l        [         R                  " [         R                  " USS S2   5      S:  5      U l	        U R                  R                  5       n[         R                  " U R                  R                  5        Vs/ s H   o3R                  5       R                  5       PM"     sn5      R                  5       nUSS S2   R                  5       USS S2   R                  5       UR                  5       S   4U l        UR                  5       U l        UR%                  5       U l        U R                   S   nU R(                  S :X  a  XPl        U R&                  u  pcn[         R*                  " U R,                  U R                   S   -
  U R.                  U R                   S   -
  5      U l        [         R2                  R5                  U R,                  U R                   S   -
  U R.                  U R                   S   -
  /5      U l        U R0                  U-
  n[         R                  " [         R8                  " U* 5      [         R:                  " U* 5      * S/[         R:                  " U* 5      [         R8                  " U* 5      S// SQ/5      n	[         R<                  " U	U R                  R?                  5       5      u  pn[         R                  " XPR(                  -
  [         R:                  " U5      [         R8                  " U5      SU
-  SU-  SU-  UU/[         R                  S9n[         R@                  " [         RB                  " U/U/-   U RD                  /-   5      S	S
5      $ s  snf s  snf )Nr"   r:      r   gGz?   )r   r   r:   g333333?   )#r,   r-   r%   current_relative_positionr.   flattenjoint_speedscount_nonzeroabsjoints_at_limit
robot_bodyposer*   valuesxyzmeanr   body_real_xyzrpybody_rpyr2   arctan2r   r   walk_target_thetalinalgnormwalk_target_distcossindotspeedrA   concatenater/   )r   r4   	body_posep	parts_xyzzryawangle_to_target	rot_speedvxvyvzmores                 r   
calc_stateWalkerBase.calc_state#   s   
9L9LM9LA--/9LMzz	##*79  !$Q$D++BFF1QTT7Od,BCD$$&I$**2C2C2EF2EQ&&(,,.2EFGOOQIqt!t_))+Yqt!t_-A-A-CY]]_UVEW DM"DMMODMaA~~nIA#ZZ(:(:T]]1=M(M(,(:(:T]]1=M(MODIINN			dmmA.	.0B0BT]]STEU0UVXD,,s2O2663$<"&&#,:RVVSD\=?VVSD\1=NOXZ [I	--/1JBB 88FF?#FF?#"H"H"H		
 jjD 772>>4&A3,$2C2C1D"DEr2NNQ N Gs   O
%'Oc                    SnU(       a  [        S5        [        U R                  5        [        S5        [        U R                  R                  5        [        S5        [        U R                  R                  5        [        S5        [        U R                  R
                  5        U R                  * U R                  R                  -  $ )Nr   z%calc_potential: self.walk_target_distzself.scene.dtzself.scene.frame_skipzself.scene.timestep)printr]   r0   dt
frame_skiptimestep)r   	debugmodes     r   calc_potentialWalkerBase.calc_potentialN   s     I34D!!"ODJJMM#$DJJ!!"!"DJJ !!!DJJMM11r   )r$   rV   rX   r   r   r+   r/   r2   rM   rP   r   r   r   r   r]   rZ   r   r   N)
__name__
__module____qualname____firstlineno__r   r6   rD   ro   rw   __static_attributes__ r   r   r   r   	   s    S
)OV2r   r   c                   &    \ rS rSrS/rS rS rSrg)Hopper^   footc           	      4    [         R                  U SSSSSS9  g )Nz
hopper.xmltorsorH      g      ?r   r   r   r   r   r   s    r   r   Hopper.__init__a   s    lG2UYZr   c                 4    US:  a  [        U5      S:  a  S$ S$ N皙?      ?r:   r9   rO   r   rf   pitchs      r   alive_bonusHopper.alive_bonusd       SSZ#-2525r   r~   Nry   rz   r{   r|   r)   r   r   r}   r~   r   r   r   r   ^   s    h)[6r   r   c                   .    \ rS rSrSS/rS rS rS rSrg)	Walker2Dh   r   	foot_leftc           	      4    [         R                  U SSSSSS9  g )Nzwalker2d.xmlr         g?r   r   r   s    r   r   Walker2D.__init__k   s    ng!RW[\r   c                 4    US:  a  [        U5      S:  a  S$ S$ r   r   r   s      r   r   Walker2D.alive_bonusn   r   r   c                 h    [         R                  X5        S H  nSU R                  U   l        M     g )N)
foot_jointfoot_left_joint      >@r   r6   jdictr?   )r   r3   rC   s      r   r6   Walker2D.robot_specific_resetq   s*    ##D8.!%djjm /r   r~   N	ry   rz   r{   r|   r)   r   r   r6   r}   r~   r   r   r   r   h   s    {#)]6&r   r   c                   .    \ rS rSr/ SQrS rS rS rSrg)HalfCheetahw   )ffootfshinfthighbfootbshinbthighc           	      4    [         R                  U SSSSSS9  g )Nzhalf_cheetah.xmlr   r      g?r   r   r   s    r   r   HalfCheetah.__init__{   s     0'aQS[_`r   c                     [         R                  " U5      S:  aR  U R                  S   (       d>  U R                  S   (       d*  U R                  S   (       d  U R                  S   (       d  S$ S$ )Nr   r:   rG      rJ   r9   )r,   rO   r/   r   s      r   r   HalfCheetah.alive_bonus~   s_    $T->->q-A$J[J[	K$$Q'0A0A!0D  MJLMr   c                    [         R                  X5        SU R                  S   l        SU R                  S   l        SU R                  S   l        SU R                  S   l        SU R                  S	   l        S
U R                  S   l        g )Ng      ^@r   g     V@r   g      N@r   g     a@r   r   r   r   r   r   r3   s     r   r6    HalfCheetah.robot_specific_reset   sv    ##D8&+DJJx#%)DJJw"%)DJJw"&+DJJx#%)DJJw"%)DJJw"r   r~   Nr   r~   r   r   r   r   w   s    )aM
*r   r   c                   (    \ rS rSr/ SQrS rS rSrg)Ant   )front_left_footfront_right_footleft_back_footright_back_footc           	      4    [         R                  U SSSSSS9  g )Nzant.xmlr         g      @r   r   r   s    r   r   Ant.__init__   s    iQRUVr   c                     US:  a  S$ S$ )Ngp=
ף?r:   r9   r~   r   s      r   r   Ant.alive_bonus       T2!r!r   r~   Nr   r~   r   r   r   r      s    Z)W"r   r   c                   @    \ rS rSrSrSS/rS rS rSrSr	S r
S	 rS
rg)Humanoid   T
right_foot	left_footc           	      4    [         R                  U SSSSSS9  g )Nzhumanoid_symmetric.xmlr      ,   g=
ףp=?r   r   r   s    r   r   Humanoid.__init__   s&    0#% ""  $r   c                 Z   [         R                  X5        / SQU l        / SQU l        U =R                  / SQ-  sl        U =R                  / SQ-  sl        U =R                  / SQ-  sl        U =R                  / SQ-  sl        U =R                  / SQ-  sl        U =R                  / SQ-  sl        U =R                  / SQ-  sl        U =R                  / SQ-  sl        U R                   Vs/ s H  o R                  U   PM     snU l        U R                  (       Ga   / S	Qn/ S	QnU R                  R                  S
SS9nU R                  (       a  U R                  R                  S5      S:X  ao  [        R                  SSS5        U R                  R                  S5      S:X  a  [        R                  S-  n/ SQnO[        R                  S-  S-  n/ SQnSnXvU/nO	/ SQnSSU/nU R                  R!                  U5        U R                  R#                  U5        SU l        g s  snf )N)	abdomen_z	abdomen_y	abdomen_x)d   r   r   )right_hip_xright_hip_zright_hip_y
right_knee)r   r   i,     )
left_hip_x
left_hip_z
left_hip_y	left_knee)right_shoulder1right_shoulder2right_elbow)K   r   r   )left_shoulder1left_shoulder2
left_elbowr   Q	Q	@r   rG   r   ffffff?)r   r   g?rH   )r   r   g      ?)r   r   r   r   )r   r6   motor_namesmotor_powerr   motors
random_yawr'   r(   random_leanrandintcposeset_xyzr,   pirQ   reset_positionreset_orientationr2   )r   r3   rC   positionorientationrh   r   rolls           r   r6   Humanoid.robot_specific_reset   s   ##D8>D&DSS,,OO,,MM$JJ$*.*:*:;*:Q::a=*:;DKhkNN""u4"8c			dnn44Q71<aC >>!!!$)%%!)%!(%%!)a-%!(C(!Sk
oo$$X.
oo''4DN) <s   .H(Fc                 N   [         R                  " U5      R                  5       (       d   eSn[        [	        S5      U R
                  U R                  5       HJ  u  p4nUR                  [        X%-  U R                  -  [         R                  " X   SS5      -  5      5        ML     g )Nr:   r   r9   )r,   r;   r<   zipranger   r   r>   r@   r   rA   )r   rB   
force_gainimr   s         r   rD   Humanoid.apply_action   s~    KKN  ! J59dkk43C3CDez1DJJ>rSUAVVWX Er   c                     US:  a  S$ S$ )Ng(\?rG   r9   r~   r   s      r   r   Humanoid.alive_bonus   r   r   )r2   r   r   r   N)ry   rz   r{   r|   self_collisionr)   r   r6   r   r   rD   r   r}   r~   r   r   r   r      s3    .[))$ D *+Y"r   r   c                    U R                  [        R                  R                  [        R
                  " 5       S5      XU/5      nU R                  USSS9  U R                  U5      u  pVUR                  S5      nU/n[        XUSS5      $ )Nzcube_small.urdfr9   g333333?)massutf8r   )
loadURDFospathjoinpybullet_datagetDataPathchangeDynamicsgetBodyInfodecoder   r$   xyrf   body	part_name_bodiess           r   get_cuber     s    	RWW\\-";";"=?PQTUZ[S\	]$D"3'%,)v&)6&	"B	//r   c                     U R                  [        R                  R                  [        R
                  " 5       S5      XU/5      nU R                  U5      u  pVUR                  S5      nU/n[        XUSS5      $ )Nzsphere2red_nocol.urdfr  r   r9   )	r  r  r  r  r  r	  r  r  r   r  s           r   
get_spherer     sj    	RWW\\-";";"=?VWZ[`aYb	c$%,)v&)6&	"B	//r   c                   ,    \ rS rSrS rS rS rS rSrg)HumanoidFlagrun   c                 <    [         R                  U 5        S U l        g N)r   r   flagr   s    r   r   HumanoidFlagrun.__init__   s    dDIr   c                 N    [         R                  X5        U R                  5         g r  )r   r6   flag_repositionr   s     r   r6   $HumanoidFlagrun.robot_specific_reset   s    !!$6r   c                    U R                   R                  U R                  R                  * U R                  R                  7S9U l        U R                   R                  U R                  R
                  * U R                  R
                  7S9U l        SnU =R                  U-  sl        U =R                  U-  sl        U R                  (       aN  U R                  R                  U R                  R                  S   U R                  U R                  S// SQ5        O1[        U R                  U R                  U R                  S5      U l        SU R                  R                  -  U l        g )Nr   g      ?r   gffffff?r   r   r   r:   iX  )r'   r(   r0   stadium_halflenr   stadium_halfwidthr   r  r$   resetBasePositionAndOrientationr  r  rt   flag_timeout)r   more_compacts     r   r  HumanoidFlagrun.flag_reposition   s"   //TZZ5O5O4O6:jj6P6P5P 0 RD//TZZ5Q5Q4Q6:jj6R6R5R 0 TDL,&,&		 gg--dii.>.>q.A/3/A/A4CUCUWZ.[.:< TWWd&8&8$:L:LcRdidjj333Dr   c                    U =R                   S-  sl         [        R                  U 5      nU R                  S:  d  U R                   S::  a:  U R	                  5         [        R                  U 5      nU R                  5       U l        U$ )Nr:   r   )r&  r   ro   r]   r  rw   	potential)r   states     r   ro   HumanoidFlagrun.calc_state  so    %Eq D$5$5$:
!!$'e**,dnLr   )r  r&  r*  r   r   N)	ry   rz   r{   r|   r   r6   r  ro   r}   r~   r   r   r  r     s    4(r   r  c                   2    \ rS rSrS rS rS rS rS rSr	g)	HumanoidFlagrunHarderi  c                 X    [         R                  U 5        S U l        S U l        SU l        g )Nr   )r  r   r  aggressive_cubeframer   s    r   r   HumanoidFlagrunHarder.__init__  s&    T"DIDDJr   c                 B   [         R                  X5        SU l        U R                  (       a9  U R                  R                  U R                  R                  S   / SQ/ SQ5        O[        U R                  SSS5      U l        SU l        S U l	        SU l
        SU l        g )Nr   )      r   皙?r"  r4  r5  r!   r   )r  r6   r1  r0  r$   r%  r  r  on_ground_frame_countercrawl_start_potentialcrawl_ignored_potentialr2   r   s     r   r6   *HumanoidFlagrunHarder.robot_specific_reset  s    ((=DJ
gg--d.B.B.I.I!.Lo.:< &dggtQ=d#$D !%D#&D DNr   c                 "   U R                   S-  S:X  Ga  U R                   S:  Gas  U R                  S:X  Gab  [        R                  " U R                  5      n[        R                  " U R
                  R                  5       5      nU R                  R                  SSS9nSnU R                  R                  SS	S9nXg-  nX4U-  -  nUS   U[        R                  " U5      -  -   US
   U[        R                  " U5      -  -   US   S-   /n	U[        R                  " U	5      -
  n
X[        R                  R                  U
5      -  -  n
XR                  R                  SSSS9-  n
U R                  R                  U	5        U R                  R                  U
S9  US:  a  U =R                  S
-  sl        O%U R                  S:  a  U =R                  S
-  sl        U =R                   S
-  sl         U R                  S:  a  U R!                  5       $ S$ )N   r   r   r   r   r   g      @g      4@r   r:   rG   r   g      )rH   )r   r    size)linearVelocityr      r9   )r1  r6  r,   r-   r   rQ   ra   r'   r(   r^   r_   r[   r\   r0  r   reset_velocitypotential_leak)r   rf   r   
target_xyzrobot_speedangle	from_distattack_speedtime_to_travelr   attack_speed_vectors              r   r   !HumanoidFlagrunHarder.alive_bonus#  s   zzB!

S 0T5Q5QUV5V88DMM*jHHT__2245knn$$T$:ei^^++ , l /n.00j
Q-)bffUm3
3Z]YQSQWQWX]Q^E^5^
Q-#
h '();;BIINN;N,OOO^^334d3SS
))(3
))9L)M3w
""a'"		%	%	)
""a'"JJ!OJ$($@$@3$F4 NBNr   c                 `    U R                   S   n[        R                  " USS5      nUS-  S-   $ )NrG   r   r   r   )r   r,   rA   )r   rf   s     r   r@  $HumanoidFlagrunHarder.potential_leak>  s0    aA
1cAs7S=r   c                 &   [         R                  U 5      nU R                  S   S:  a@  U R                  c  XR                  -
  U l        XR                  -
  U l        U R                  nOXR                  -  nS U l        XR                  5       S-  -   $ )NrG   r   r   )r   rw   r   r7  r8  r@  )r   flag_running_progresss     r   rw   $HumanoidFlagrunHarder.calc_potentialC  s     %33D9 }}Q#		#	#	+%:=Y=Y%Y"%:=W=W%Wd""88 ;;;#'d  #6#6#83#>>>r   )r0  r8  r7  r  r1  r2   r6  N)
ry   rz   r{   r|   r   r6   r   r@  rw   r}   r~   r   r   r.  r.    s    O6
?r   r.  )robot_basesr   r   r   numpyr,   pybulletr  r  r   r   r   r   r   r   r   r  r  r  r.  r~   r   r   <module>rQ     s    E E   	   R2 R2j6Z 6&z &** *,"* "9"z 9"x00%h %PM?O M?r   