
    nil                     `    S SK Jr  S SKJr  S SKrS rS rS r	S r
S rS	 rS
 rS rS rS rg)    )ElementTree)pathNc                     [        SU 5        g )Nzlog: )print)txtlevels     \/home/james-whalen/.local/lib/python3.13/site-packages/pybullet_utils/gazebo_world_parser.pylogr
      s        c                 :    U  H  n[        U5      (       a  M    g   ga  Checks if a list of strings contains int numbers exclusively.

To determine whether the number is an int, the function `is_int` is used.
Args:
  stringlist(list(str): list to check
Returns:
  : bool -- True if every string in the list can be represented as int, False if not
FTis_int
stringlistnums     r	   only_contains_intr           c{{  r   c                 :    U  H  n[        U5      (       a  M    g   g)a  Checks if a list of strings contains float numbers exclusively.

To determine whether the number is a float, the function `is_float` is used.
Args:
  stringlist(list(str): list to check
Returns:
  : bool -- True if every string in the list can be represented as float, False if not
FT)is_floatr   s     r	   only_contains_floatr      s     }}  r   c                 H     [        U 5        g! [        [        4 a     gf = f)zTests if the specified string represents a float number.
Args:
  text(str): text to check
Returns:
  : bool -- True if the text can be parsed to a float, False if not.
TF)float
ValueError	TypeErrortexts    r	   r   r   &   s(    d	" s    !!c                 r    [        U 5      (       a  [        U 5      $ [        U 5      (       a  [        U 5      $ U $ )ak  Returns the specified string parsed to an int or float.

If no number can be parsed, the original string is returned.

To determine whether the number is an int or float, the functions `is_int` and `is_float` are
used.
Args:
  text(string): text to parse to a number
Returns:
  : int/float/str -- depending on successful parsing, a number or a string is returned
)r   intr   r   r   s    r	   parse_numberr    4   s/     d||4y	$T{Kr   c                 <     [        U 5        g! [         a     gf = f)zTests if the specified string represents an integer number.
Args:
  text(str): text to check
Returns:
  : bool -- True if the text can be parsed to an int, False if not.
TF)r   r   r   s    r	   r   r   F   s#    D	 s    
c                 :    U  H  n[        U5      (       a  M    g   gr   r   r   s     r	   r   r   T   r   r   c                 2   U R                  5       nU(       d  g[        U5      S:  a\  [        U5      (       a  U Vs/ s H  n[        U5      PM     nnU$ [	        U5      (       a  U Vs/ s H  n[        U5      PM     nnU$ U$ [        U 5      $ s  snf s  snf )a  Parses a text by splitting up elements separated by whitespace.

The elements are then parsed to int/float-only lists.
Args:
  text(str): text with elements seperated by whitespace
Returns:
  : list(str/float/int) -- list with elements parsed to the same type
N   )splitlenr   r   r   r   r    )r   
numstringsr   numss       r	   
parse_textr)   b   s     J
:Z(((23
CH
D3K ,,*45*3E#J*D5K 4 6s   B)Bc                 R   [        S5        [        S5        U R                   Vs0 s H  o"U R                  U   _M     nn/ US'   U R                  S5      nUb\  UR                  S5      nUbH  UR                  nSnUR	                  U5      (       a  SU[        U5      S  -   S-   n[        S	U5        XcS'   U R                  S
5      nUb  [        UR                  5      n	W	(       a=  [        U	5      S:X  a.  U	S S US'   U	SS  US'   [        SUS   5        [        SUS   5        U$ s  snf )Nz--------------------zparsing modelchildrenincludeurizmodel://zmodels/z
/model.sdfzuri=pose      pose_xyzpose_rpyznewmodel['pose_xyz']=znewmodel['pose_rpy']=)r   attribfindr   
startswithr&   r)   )
	model_xmlurdffilepathanewmodelinclude_xmluri_xmlr-   prefixpose_xmlr.   s
             r	   
parseModelr>      s5   	
 !	/090@0@A0@19##A&&0@HAHZ..+K  'g		ll>>&!!#c&kl++L8#fc~~f%H&dD	1#BQx#ABx%hz&:;%hz&:; O1 Bs   D$c                    [         R                  " U5      nUR                  5       nSUR                  ;   a  [	        SUR                  S   5        0 n[        SS5        UR                  S5       H  n[        SR                  UR                  S   5      S5        [        US	5      nXdUR                  S   '   S
UR                  5       ;   d  M^  US
   c  Mf  U R                  " US
   5      nU Hm  nU R                  " U5      u  pUS   nUS   nU R                  " U5      nU R                  " XX5      u  pU R                  " XU5        U R                  " USSS9  Mo     [	        SU5        M     U$ )Nversionzversion=zParsing models...INFOmodelz Adding link {}.nameDEBUG r-   r1   r2   r   )masszbodies=)ETparsegetrootr3   r   r
   iterformatr>   keysloadSDFgetBasePositionAndOrientationgetQuaternionFromEulermultiplyTransformsresetBasePositionAndOrientationchangeDynamics)pfilepathtreerootmodelsr6   r9   bodiesbold_posold_ornr1   r2   pose_ornnew_posnew_orns                   r	   
parseWorldr`      sP   	(	$	$$++Jt{{9-.&6"99W%i	

#
#I$4$4V$<
=wGIr*h)1Yf%&	(--/	!huo&A8E?+A;;A>/'j)(j)(--h7(11(gV
'

+
+A
@


1bq
)  	i &" 
-r   )	xml.etreer   rH   osr   pybulletrT   r
   r   r   r   r    r   r)   r>   r`    r   r	   <module>re      s@    '  $:<r   