
    j                     8    d dl Z d dlZddlmZmZ ddlmZ ddZy)    N   )logtol)__version__c                    ddl m} ddlm} t        j
                  j                  |      }t        j
                  j                  |      }t        j
                  j                  |      \  }	}
|
dk7  rt        d      t        j
                  j                  |      st	        j                  |       n*t        j
                  j                  |      st        d      	 | j                         }| j$                  t'        |D cg c]  }|j$                   c}      z  }|j)                  d
d      }d}t+        |      D ]  \  }}| d| }| d}t        j
                  j-                  ||      } |||       | j.                  |_        || j0                  z  |_        d| }| }|j2                  D cg c]!  }|D cg c]  }dj5                  |       c}# }}}|j7                  |d|      }|j7                  |d      }|j7                  |ddd       |j7                  |d|j8                  d       |j7                  |d|d   d   |d   d   |d   d   |d   d   |d   d   |d   d          |j7                  |d      }|j7                  |ddd       |j7                  |d      }|j7                  |d||dd |dd |d!       |j7                  |d"d      }|)|j7                  |d#|d   dd |d   dd |d   dd$%       |j7                  |d&      }|j7                  |ddd       |j7                  |d      }|j7                  |d||dd |dd |d!       |W| d'} |j7                  |d(| d)*      }!|j7                  |!ddd       |j7                  |!d+|,       |j7                  |!d-|,       |} |j;                  |      }"| d.}#|"j=                  t        j
                  j-                  ||#      d/       |j)                  d0      }|j7                  |d1      }$||$_        |j7                  |d2      }%d3|%_        |j7                  |d4d56      }&| d.|&_        |j7                  |d7      }'d8t@         |j7                  |'d1      _        d9|j7                  |'d:      _        |j7                  |d;      }(||(_        |j;                  |      }"tB        jD                  rNdd<l#m$}) |jK                   |)d=      >      }*|*jM                  |"      st        jN                  |*jP                         |"j=                  t        j
                  j-                  |d?             tS        j&                  |      S # t        $ r( t        j                   dd	       | j"                  g}Y w xY wc c}w c c}w c c}}w )@a  
    Convert a Trimesh object into a URDF package for physics
    simulation. This breaks the mesh into convex pieces and
    writes them to the same directory as the .urdf file.

    Parameters
    ---------
    mesh : trimesh.Trimesh
      Input geometry
    directory : str
      The directory path for the URDF package

    Returns
    ---------
    mesh : Trimesh
      Multi-body mesh containing convex decomposition
    r   N   )export_mesh zURDF path must be a directory!z-problem with convex decomposition, using hullT)exc_inforobotroot)name_convex_piece_z.objlink_z{:.2E}linkinertialoriginz0 0 0)xyzrpymassz.2E)valueinertiar   )ixxixyixziyyiyzizzvisualgeometrymeshz.4E )filenamescalematerialcolorz 1)rgba	collision_jointjointfixed)r   typeparent)r   childz.urdf)pretty_printmodelr   versionz1.0sdfz1.4)r1   authorztrimesh zblank@blank.blankemaildescription)
get_streamzschema/urdf.xsd)filezmodel.config)*
lxml.etreeetreeexportr	   ospathabspathbasenamesplitext
ValueErrorexistsmkdirisdirconvex_decompositionBaseExceptionr   errorconvex_hullvolumesumElement	enumeratejoincenter_massdensitymoment_inertiaformat
SubElementr   ElementTreewritetextr   r   strict	resourcesr6   	XMLSchemavalidatedebug	error_lognp)+r!   	directoryr$   r&   kwargsetr	   fullpathr   _extconvex_piecesmeffective_densityr   prev_link_nameipiece
piece_namepiece_filenamepiece_filepath	link_name	geom_namexyIr   r   r   r    r%   r(   
joint_namer*   treeurdf_filenamer0   r1   r2   r3   r5   r6   schemas+                                              B/DATA/.local/lib/python3.12/site-packages/trimesh/exchange/urdf.pyexport_urdfru   	   s   &  $ wwy)H77H%DWWd#FAs
by9:: 77>>(#
WW]]8$9::+113 c]*K]188]*K&LL ::gF:+D Nm,5v^A3/
&<t,h?E>* !,,)DLL8J<(	%&	6;6J6JK6J!,!Qhooa !,6JK }}T6	}: ==z2
hg7C
hC0@B
!Q!Q!Q!Q!Q!Q 	 		
 tX.
fhGA==4
3Kqs1U3K8	 	 	
 =="==MM'58C.%(3qqRUVX(Y  
 MM$4	
iwGD==J7
3Kqs1U3K8	 	 	
 %%;f-JMM$jwMOEMM%wGMDMM%M?MM%yM9"G -L >>$DfENMJJrww||Hm44JH ::gDMM$'EEJmmD),GGL
--eU-
3Cu~CH]]4*F+3K=)ABMM&&!&*=BMM&'"'--m4KK>>$D
zz* :.?#@At$IIf&&'JJrww||Hn5666-  c  +		ADQ))*+
 +L( -Ks0   V  V4
5	V>>V9V> -V10V19V>)g      ?N)	r;   numpyr[   	constantsr   r   r1   r   ru        rt   <module>rz      s    	    !Z!ry   