
    ni>7                        S SK r S SKrS SKrS SKrS SKrS SKrS SKr\R                  R                  \R                  R                  \R                  " \R                  " 5       5      5      5      r\R                  R                  \5      r\R                  R                  R!                  S \5        S SK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 S5      rg)    Nc                   .    \ rS rSrSrSrS rS rS rSr	g)	XmlBasedRobot   *
Base class for mujoco .xml based agents.
Tc                    S U l         / U l        S U l        S U l        S U l        [
        R                  " U/[
        R                  S9n[        R                  R                  U* U[
        R                  S9U l        [
        R                  [
        R                  " U/[
        R                  S9-  n[        R                  R                  U* U[
        R                  S9U l        Xl        X@l        g )N)dtype)partsobjectsjdictordered_joints
robot_bodynponesfloat32gymspacesBoxaction_spaceinfobservation_space
robot_nameself_collision)selfr   
action_dimobs_dimr   highs         S/home/james-whalen/.local/lib/python3.13/site-packages/pybullet_envs/robot_bases.py__init__XmlBasedRobot.__init__   s    DJDLDJDDO77J<rzz2D

ud"**ED66BGGWIRZZ88D ZZ^^TE4rzz^JD !O(    c                    Xl         U R                  b  U R                  nO0 nU R                  b  U R                  nO0 nU R                  b  U R                  nO/ n[        R
                  " U5      (       a  U/nSn[        [        U5      5       GHT  nU R                   R                  X'   5      S:X  a`  U R                   R                  X'   5      u  pU	R                  S5      U l        UR                  S5      n[        U R                   XUS5      X8'   [        U R                   R                  X'   5      5       GH  n
U R                   R                  X'   U
[        R                  SSSS9  U R                   R!                  X'   U
5      nUS   nUS   nUR                  S5      nUR                  S5      nU(       a  [#        SU-  5        U(       a  [#        S	U-  5        [        U R                   XXz5      X8'   XR                  :X  a	  X8   U l        US:X  aU  U
S:X  aO  U R$                  cB  [        U R                   U R                  USS5      X0R                  '   X0R                     U l        US S
 S:X  a(  ['        U R                   XXz5      R)                  5         GMc  US S S:w  d  GMo  ['        U R                   XXz5      XL'   UR+                  XL   5        SXL   l        GM     GMW     X4XPR$                  4$ )Nr   utf8皙?)positionGainvelocityGainforce      zROBOT PART '%s'zROBOT JOINT '%s'   ignore   jointfixg      Y@)_pr	   r   r   r   isscalarrangelengetNumJointsgetBodyInfodecoder   BodyPartsetJointMotorControl2pybulletPOSITION_CONTROLgetJointInfoprintr   Jointdisable_motorappend
power_coef)r   bullet_clientbodiesr	   jointsr   dumpi	part_namer   j	jointInfo
joint_names                r   
addToSceneXmlBasedRobot.addToScene"   s   Gzzjjeezzzzff&**nn	{{6xfD3v;			fi	(A	- $ 3 3FI >	$++F3$$V,	#DGGY2FTWW))&)45!%%fi&'&.&?&?3636,- 	& 	/ GG((A6	q\
bM	&&v.
$$V,	(945
 :- $DGGYE'!,$/6a1f!8#+DGGT__faQS#T%
 !//2$/bq>X%
Q
2
@
@
B
bq>Z'$TWWj!G&



 2
3*/&

'K 6  d .//99r    c                 T    U R                   U R                     R                  X5        g N)r	   r   
reset_poser   positionorientations      r   rL   XmlBasedRobot.reset_posel   s    JJt**8Ar    )
r.   r   r   r
   r   r   r	   r   r   r   N)
__name__
__module____qualname____firstlineno____doc__r   r   rH   rL   __static_attributes__ r    r   r   r      s      .) H:TBr    r   c                   .    \ rS rSrSrSS jrS rS rSrg)	MJCFBasedRobotp   r   c                 L    [         R                  XX4U5        Xl        SU l        g Nr   )r   r   	model_xmldoneLoading)r   r]   r   r   r   r   s         r   r   MJCFBasedRobot.__init__u   s!    4Z.QNDr    c           	         Xl         U R                  S:X  Ga  / U l        SU l        U R                  (       a  U R                   R	                  [
        R                  R                  [        R                  " 5       SU R                  5      [        R                  [        R                  -  [        R                  -  S9U l        U R!                  U R                   U R                  5      u  U l        U l        U l        U l        OU R                   R	                  [
        R                  R                  [        R                  " 5       SU R                  [        R                  S95      U l        U R!                  U R                   U R                  5      u  U l        U l        U l        U l        U R)                  U R                   5        U R+                  5       nU$ )Nr   r(   mjcf)flags)r.   r^   r   r   loadMJCFospathjoinpybullet_datagetDataPathr]   r7   URDF_USE_SELF_COLLISION+URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTSURDF_GOOGLEY_UNDEFINED_COLORSr
   rH   r	   r   r   robot_specific_reset
calc_stater   r?   ss      r   resetMJCFBasedRobot.resetz   sb   GAdd			ww'']5N5N5PRX59^^)E.6.N.N(0(\(\/](0(N(N/O ( Q
 HLGGT\\H#D
DJ 3T_ ww''GGLL224fdnnV^V|V|L}GKGGT\\H#D
DJ 3T_dgg& 	A Hr    c                     gr\   rW   r   s    r   calc_potentialMJCFBasedRobot.calc_potential       r    )r.   r^   r   r]   r
   r   r	   r   N)T	rQ   rR   rS   rT   rU   r   rp   rt   rV   rW   r    r   rY   rY   p   s    
6r    rY   c                   >    \ rS rSrSr/ SQ/ SQSS4S jrS rS rS	rg
)URDFBasedRobot   z(
Base class for URDF .xml based robots.
r   r   r   r   r   r   r(   Fc	                 b    [         R                  XX4U5        Xl        XPl        X`l        Xpl        g rK   )r   r   
model_urdfbasePositionbaseOrientation
fixed_base)	r   r~   r   r   r   r   r   r   r   s	            r   r   URDFBasedRobot.__init__   s,     4Z.Q O$* Or    c                 l   Xl         / U l        [        [        R                  R                  [        R                  R                  [        5      SU R                  5      5        U R                  (       a  U R                  U R                   U R                   R                  [        R                  R                  [        R                  " 5       U R                  5      U R                  U R                  U R                   ["        R$                  ["        R&                  -  S95      u  U l        U l        U l        U l        OU R                  U R                   U R                   R                  [        R                  R                  [        R                  " 5       U R                  5      U R                  U R                  U R                   ["        R&                  S95      u  U l        U l        U l        U l        U R/                  U R                   5        U R1                  5       nU R3                  5       U l        U$ )Ndata)r   r   useFixedBaserb   )r.   r   r:   rd   re   rf   dirname__file__r~   r   rH   loadURDFrg   rh   r   r   r   r7   ri   rk   r	   r   r   rl   rm   rt   	potentialrn   s      r   rp   URDFBasedRobot.reset   s   GD	"'',,rwwx0&$//
JKEI__
''
''

277<<(A(A(CT__U(,(9(9+/+?+?(,!)!A!AHDjDj!j	  lFmBdj$*d14? FJ__
''
''

277<<(A(A(CT__U(,(9(9+/+?+?(,AgAg  iFjBdj$*d14? 	dgg& 	A((*DNHr    c                     gr\   rW   rs   s    r   rt   URDFBasedRobot.calc_potential   rv   r    )
r.   r   r   r   r   r~   r   r	   r   r   Nrw   rW   r    r   ry   ry      s$     &+#! <r    ry   c                   >    \ rS rSrSr/ SQ/ SQSS4S jrS rS rS	rg
)SDFBasedRobot   z'
Base class for SDF robots in a Scene.
r{   r|   Fc	                 J    [         R                  XX4U5        Xl        Xpl        g rK   )r   r   	model_sdfr   )	r   r   r   r   r   r   r   r   r   s	            r   r   SDFBasedRobot.__init__   s      4Z.QN Or    c           	         Xl         / U l        U R                  U R                   U R                   R                  [        R
                  R                  SU R                  5      5      5      u  U l        U l	        U l        U l
        U R                  U R                   5        U R                  5       nU R                  5       U l        U$ )Nmodels_robot)r.   r   rH   loadSDFrd   re   rf   r   r	   r   r   rl   rm   rt   r   rn   s      r   rp   SDFBasedRobot.reset   s    GDCG??^T^^DEDG@DJ
D/ 	dgg& 	A((*DNHr    c                     gr\   rW   rs   s    r   rt   SDFBasedRobot.calc_potential   rv   r    )r.   r   r   r   r   r	   r   r   Nrw   rW   r    r   r   r      s$     &+#!"r    r   c                   ,    \ rS rSrS rS rS rS rSrg)Pose_Helper   c                     Xl         g rK   	body_part)r   r   s     r   r   Pose_Helper.__init__   s    Nr    c                 6    U R                   R                  5       $ rK   )r   current_positionrs   s    r   xyzPose_Helper.xyz   s    >>**,,r    c                 ^    [         R                  " U R                  R                  5       5      $ rK   )r7   getEulerFromQuaternionr   current_orientationrs   s    r   rpyPose_Helper.rpy   s     **4>>+M+M+OPPr    c                 6    U R                   R                  5       $ rK   )r   r   rs   s    r   rO   Pose_Helper.orientation  s    >>--//r    r   N)	rQ   rR   rS   rT   r   r   r   rO   rV   rW   r    r   r   r      s    -Q0r    r   c                   ~    \ 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Q/ SQ4S jrS rS rS rSrg)r5   i  c                     X0l         Xl        X@l        XPl        U R	                  5       U l        U R                  5       U l        [        U 5      U l	        g rK   )
r@   r.   	bodyIndexbodyPartIndexr   initialPositionr   initialOrientationr   bp_pose)r   r?   	body_namer@   r   r   s         r   r   BodyPart.__init__	  sE    KGN&002D"668Dt$DLr    c           	          US:X  a%  U R                   R                  U5      u  u  p4nu  pgpO(U R                   R                  X5      u  u  p4nu  pgp      n
[        R                  " X4XVXxU	/5      $ Nr#   )r.   getBasePositionAndOrientationgetLinkStater   array)r   body_idlink_idxyzabcd_s              r   state_fields_of_pose_of BodyPart.state_fields_of_pose_of  so     "} $ E Eg NiqQ!1,0GG,@,@,R)iqQ!q!Q88Q1q)**r    c                 "    U R                  5       $ rK   )r   rs   s    r   get_positionBodyPart.get_position  s      ""r    c                 h    U R                  U R                  U R                     U R                  5      $ rK   )r   r@   r   r   rs   s    r   get_poseBodyPart.get_pose  s(    ''DNN(CTEWEWXXr    c           
      d   U R                   S:X  a8  U R                  R                  U R                  U R                     5      u  u  pp4OQU R                  R                  U R                  U R                     U R                   SS9u  u  pVnu  pp      nu  pnu  pn[        R                  " XU/5      $ )Nr#   r(   )computeLinkVelocity)r   r.   getBaseVelocityr@   r   r   r   r   )r   vxvyvzr   r   r   r   r   r   r   r   vrvps                 r   speedBodyPart.speed!  s    R//DNN0KLolrrAHLH\H\
++dnn
%t'9'9q I] IREiqQ!q!Q<BB""88RRL!!r    c                 (    U R                  5       S S $ N   r   rs   s    r   r   BodyPart.current_position)  s    ==?2Ar    c                 (    U R                  5       SS  $ r   r   rs   s    r   r   BodyPart.current_orientation,  s    ==?12r    c                 "    U R                  5       $ rK   )r   rs   s    r   get_orientationBodyPart.get_orientation/  s    ##%%r    c                     U R                   R                  U R                  U R                     UU R	                  5       5        g rK   )r.   resetBasePositionAndOrientationr@   r   r   r   rN   s     r   reset_positionBodyPart.reset_position2  s2    GG++DKK,G,0,@,@,BDr    c                     U R                   R                  U R                  U R                     U R	                  5       U5        g rK   )r.   r   r@   r   r   )r   rO   s     r   reset_orientationBodyPart.reset_orientation6  s1    GG++DKK,GIZIZI\,79r    r{   c                 j    U R                   R                  U R                  U R                     X5        g rK   )r.   resetBaseVelocityr@   r   )r   linearVelocityangularVelocitys      r   reset_velocityBodyPart.reset_velocity:  s#    GGdkk$..9>[r    c                 j    U R                   R                  U R                  U R                     X5        g rK   )r.   r   r@   r   rM   s      r   rL   BodyPart.reset_pose=  s#    GG++DKK,G_r    c                     U R                   $ rK   )r   rs   s    r   poseBodyPart.pose@  s    <<r    c                     U R                   R                  U R                  U R                     SU R                  S5      $ r   )r.   getContactPointsr@   r   r   rs   s    r   contact_listBodyPart.contact_listC  s1    77##DKK$?TEWEWY[\\r    )r.   r@   r   r   r   r   r   N)r#   )rQ   rR   rS   rT   r   r   r   r   r   r   r   r   r   r   r   rL   r   r   rV   rW   r    r   r5   r5     sY    % +#Y"&D9 +4Y \`]r    r5   c                   n    \ 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)r;   iG  c                     X0l         Xl        X@l        XPl        X l        U R                  R                  U R                   U R                     U R                  5      nUS   U l        US   U l        SU l        g )Nr,   	   r   )	r@   r.   r   
jointIndexrG   r9   
lowerLimit
upperLimitpower_coeff)r   r?   rG   r@   r   r   rF   s          r   r   Joint.__init__I  s`    KGN O O$$T[[%@$//RIlDOlDODr    c                     U R                   R                  U R                  U R                     U R                  X5        g rK   )r.   resetJointStater@   r   r   )r   r   r   s      r   	set_stateJoint.set_stateV  s)    GGDKK7!Pr    c                 "    U R                  5       $ rK   	get_staters   s    r   r   Joint.current_positionY  s    >>r    c                     U R                  5       u  pSU R                  U R                  -   -  nSX-
  -  U R                  U R                  -
  -  SU-  4$ )Ng      ?   r$   )r  r   r   )r   posvelpos_mids       r   current_relative_positionJoint.current_relative_position\  sO    ~~HCT__t67G4??T__#DEsSyQQr    c                     U R                   R                  U R                  U R                     U R                  5      u  p  nX4$ rK   )r.   getJointStater@   r   r   )r   r   r   r   s       r   r  Joint.get_statea  s6    ''''DNN(CT__UKA1a5Lr    c                 *    U R                  5       u  pU$ rK   r  )r   r   r   s      r   r   Joint.get_positione      >>DAHr    c                 *    U R                  5       u  pU$ rK   r  )r   r   rs      r   r   Joint.get_orientationi  r  r    c                 *    U R                  5       u  pU$ rK   r  )r   r   r   s      r   get_velocityJoint.get_velocitym  s    NNEAIr    c                     U R                   R                  U R                  U R                     U R                  [
        R                  US9  g )N)targetPositionr.   r6   r@   r   r   r7   r8   r   s     r   set_positionJoint.set_positionq  ;    GG!!$++dnn"="&//"*";";19 " ;r    c                     U R                   R                  U R                  U R                     U R                  [
        R                  US9  g )N)targetVelocity)r.   r6   r@   r   r   r7   VELOCITY_CONTROL)r   velocitys     r   set_velocityJoint.set_velocityw  r  r    c                 &    U R                  U5        g rK   )
set_torquer   torques     r   set_motor_torqueJoint.set_motor_torque}  s    OOFr    c                     U R                   R                  U R                  U R                     U R                  [
        R                  US9  g )N)r   r   controlModer'   )r.   r6   r@   r   r   r7   TORQUE_CONTROLr'  s     r   r&  Joint.set_torque  s;    GG!!DKK,G-1__.6.E.E(. " 0r    c                 &    U R                  X5        g rK   )r   r   rN   r"  s      r   reset_current_positionJoint.reset_current_position  s    +r    c                     U R                   R                  U R                  U R                     U R                  UUS9  U R                  5         g )N)targetValuer   )r.   r   r@   r   r   r<   r0  s      r   r   Joint.reset_position  sC    GGDKK7 OO(0+3  5 	r    c                     U R                   R                  U R                  U R                     U R                  [
        R                  SSSSSS9  g )Nr   r$   )r,  r  r   r%   r&   r'   r  rs   s    r   r<   Joint.disable_motor  sG    GG!!$++dnn"="&//.6.G.G1212/2/2() " +r    )r.   r@   r   r   rG   r   r   r   N)rQ   rR   rS   rT   r   r   r   r  r  r   r   r  r  r#  r)  r&  r1  r   r<   rV   rW   r    r   r;   r;   G  sO    QR
;;0,+r    r;   )r7   r   
gym.spaces	gym.utilsnumpyr   rd   inspectre   r   abspathgetfilecurrentframe
currentdir	parentdirsysinsertrg   r   rY   ry   r   r   r5   r;   rW   r    r   <module>rC     s     ! !  WW__RWW__W__W=Q=Q=S-TUV
GGOOJ'	   1i   bB bBJ&] &R4] 4n%M %P0 0=] =]@Q+ Q+r    