
    Ti                     :    d dl Z d dlZddlmZmZ ddlmZ ddZdS )    N   )logtol)__version__      ?c                 R   ddl m} ddlm} t          j                            |          }t          j                            |          }t          j                            |          \  }	}
|
dk    rt          d          t          j        
                    |          st	          j        |           n.t          j                            |          st          d          	 |                                 }n.# t          $ r! t          j        dd	           | j        g}Y nw xY w| j        t'          d
 |D                       z  }|                    dd          }d}t+          |          D ]\  }}| d| }| d}t          j                            ||          } |||           | j        |_        || j        z  |_        d| }| }d |j        D             }|                    |d|          }|                    |d          }|                    |ddd           |                    |d|j        d           |                    |d|d         d         |d         d         |d         d         |d         d         |d         d         |d         d                    |                    |d          }|                    |ddd           |                    |d          }|                    |d||d d!|d d!|d "           |                    |d#d          }|6|                    |d$|d         dd!|d         dd!|d         dd%&           |                    |d'          }|                    |ddd           |                    |d          }|                    |d||d d!|d d!|d "           |g| d(}|                    |d)|d*+          }|                    |ddd           |                    |d,|-           |                    |d.|-           |}|                    |          }| d/} |                    t          j                            ||           d0           |                    d1          }|                    |d2          }!||!_        |                    |d3          }"d4|"_        |                    |d5d67          }#| d/|#_        |                    |d8          }$d9t>           |                    |$d2          _        d:|                    |$d;          _        |                    |d<          }%||%_        |                    |          }t@          j!        rSdd=l"m#}& |$                     |&d>          ?          }'|'%                    |          st          j&        |'j'                   |                    t          j                            |d@                     tQ          j        |          S )Aa  
    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_infoc                     g | ]	}|j         
S  )volume).0ms     O/DATA/AppData/hermes/venv/lib/python3.11/site-packages/trimesh/exchange/urdf.py
<listcomp>zexport_urdf.<locals>.<listcomp>7   s    *K*K*K18*K*K*K    robotroot)name_convex_piece_z.objlink_c                 &    g | ]}d  |D             S )c                 8    g | ]}d                      |          S )z{:.2E})format)r   ys     r   r   z*export_urdf.<locals>.<listcomp>.<listcomp>K   s$    ,,,Qhooa  ,,,r   r   )r   xs     r   r   zexport_urdf.<locals>.<listcomp>K   s'    KKK,,!,,,KKKr   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)r?   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_hullr   sumElement	enumeratejoincenter_massdensitymoment_inertia
SubElementr$   ElementTreewritetextr   r   strict	resourcesrD   	XMLSchemavalidatedebug	error_lognp)(r/   	directoryr2   r4   kwargsetr
   fullpathr   _extconvex_pieceseffective_densityr   prev_link_nameipiece
piece_namepiece_filenamepiece_filepath	link_name	geom_nameIr   r    r-   r.   r3   r6   
joint_namer8   treeurdf_filenamer>   r?   r@   rA   rC   rD   schemas(                                           r   export_urdfr}   	   s   &  $##### wy))H7H%%DWd##FAs
byy9::: 7>>(## ;
W]]8$$ ;9:::+1133 + + +	ADQQQQ)*+
 c*K*K]*K*K*K&L&LL ::gF:++D Nm,, C# C#5//A//
&,,,h??E>*** !,)DL8(J((	%'	KKe6JKKK }}T6	}:: ==z22
hg7CCC
h.A.ABBB
!Q!Q!Q!Q!Q!Q 	 		
 		
 		
 tX..
fhGAAA==44
888888U888	 	 	
 	
 	
 =="===MM'58(Y(Y(Y%((Y(Y(Yq(Y(Y(Y(Y    
 MM$44	
iwGDDD==J77
888888U888	 	 	
 	
 	
 %%---JMM$jwMOOEMM%wGMDDDMM%M???MM%yM999" >>$DNNNMJJrw||Hm444JHHH ::gDMM$''EEJmmD),,GGL
--eU-
3
3C~~~CH]]4**F)AK)A)ABMM&&!!&*=BMM&'""'--m44KK>>$D
z (****** ::.?#@#@AAt$$ 	(If&'''JJrw||Hn556666-   s   %C: :(D%$D%)r   N)	rI   numpyrg   	constantsr   r   r?   r   r}   r   r   r   <module>r      sh    				                     ! ! ! ! ! !Z! Z! Z! Z! Z! Z!r   