a
    i‚jML  ã                   @   sò   d Z ddlZddlZddlmZmZmZmZmZ ddl	Z	ddl
m  mZ ddlm  mZ ddlZddlmZ ddlmZ ddlmZ ddlmZ ddlmZ dd	lmZ G d
d„ dejƒZG dd„ deƒZG dd„ deƒZG dd„ deƒZdS )z Objects module for USD exporter.é    N)ÚAnyÚDictÚOptionalÚSequenceÚTuple)ÚGf)ÚSdf)ÚUsd)ÚUsdGeom)ÚUsdShade)ÚVtc                   @   sÌ   e Zd ZdZe g d¢¡dfejej	ej
eejeeeeejf   dœdd„Zejdd„ ƒZejd	d
„ ƒZdd„ Zdd„ Zddd„Zdejejeeeej dœdd„Zeedœdd„Zejedœdd„ZdS )Ú	USDObjecta¶  Abstract interface for all USD objects including meshes and primitives.

  Subclasses must implement:

  * `_get_uv_geometry(self)`: gets the nessecary UV information to
     wrap a texture around an object in USD. Each subclass implements
     their own method to getting UV information as different objects
     are contructed in different ways.

  * `_get_mesh_geometry(self)`: gets the mesh geometry of an object
     in the scene.
  ©é   r   r   r   © )ÚstageÚmodelÚgeomÚobj_nameÚrgbaÚgeom_texturesc                 C   sd   || _ || _|| _|| _|| _|| _d|› | _tj 	|| j¡| _
| j
 ¡ | _| j
 ¡ | _d| _d S )Nz/World/Mesh_Xform_éþÿÿÿ)r   r   r   r   r   r   Ú
xform_pathr
   ÚXformÚDefineÚ	usd_xformZAddTransformOpÚtransform_opÚ
AddScaleOpÚscale_opÚlast_visible_frame)Úselfr   r   r   r   r   r   r   r   úR/home/ghrups/robot-bench/.venv/lib64/python3.9/site-packages/mujoco/usd/objects.pyÚ__init__3   s    	zUSDObject.__init__c                 C   s   t ‚dS )z/Gets UV information for an object in the scene.N©ÚNotImplementedError©r    r   r   r!   Ú_get_uv_geometryL   s    zUSDObject._get_uv_geometryc                 C   s   t ‚dS )z)Gets structure of an object in the scene.Nr#   r%   r   r   r!   Ú_get_mesh_geometryQ   s    zUSDObject._get_mesh_geometryc                 C   sü  t  d| j› ¡}tj | j|¡}tj | j| d¡¡}tj | j| d¡¡}tj | j| d¡¡}| 	d¡ | 
dt jj¡ | ¡ d¡ | 
dt jj¡ t| jd	 ƒ¡ | 
d
t jj¡ | jj¡ | 
dt jj¡ d| jj ¡ | ¡  | ¡ d¡ | 	d¡ | 
dt jj¡ | jtjjj d ¡ | 
dt jj¡ d¡ | 
dt jj¡ d¡ | 
dt jj¡ d¡ | 
dt jj¡ | ¡ d¡ | dt jj¡ | 	d¡ | 
dt jj¡ d¡ | dt jj¡ | ¡  | ¡ d¡ |  ¡  !tj"¡ t "|¡ #|¡ dS )z9Attaches an image texture to a material for a USD object.ú/World/_materials/Material_ÚPrincipled_BSDFZImage_TextureZuvmapÚUsdPreviewSurfaceÚdiffuseColorÚrgbÚopacityéÿÿÿÿÚmetallicÚ	roughnessç      ð?ÚsurfaceZUsdUVTextureÚfiler   ZsourceColorSpaceZsRGBZwrapSÚrepeatZwrapTÚstÚresultZUsdPrimvarReader_float2ÚvarnameÚUVMapÚresultsN)$r   ÚPathr   r   ÚMaterialr   r   ÚShaderÚ
AppendPathÚCreateIdAttrÚCreateInputÚValueTypeNamesÚColor3fÚConnectToSourceÚConnectableAPIÚFloatÚSetÚfloatr   r   Ú	shininessÚCreateSurfaceOutputZAssetr   ÚmujocoÚmjtTextureRoleÚmjTEXROLE_RGBÚvalueÚTokenZFloat2ZCreateOutputZFloat3ÚGetPrimÚApplyAPIÚMaterialBindingAPIÚBind)r    Úusd_meshÚmtl_pathÚmtlÚbsdf_shaderZimage_shaderZuvmap_shaderr   r   r!   Úattach_image_materialV   sd    ÿÿÿ
ÿþÿÿ
ÿÿ
ÿÿÿ
ÿzUSDObject.attach_image_materialc                 C   sö   t  d| j› ¡}tj | j|¡}tj | j| d¡¡}| 	d¡ | 
dt jj¡ t| jdd… ƒ¡ | 
dt jj¡ t| jd ƒ¡ | 
d	t jj¡ | jj¡ | 
d
t jj¡ d| jj ¡ | ¡  | ¡ d¡ | ¡  tj¡ t |¡ |¡ dS )z9Attaches an solid texture to a material for a USD object.r(   r)   r*   r+   r   é   r-   r.   r/   r0   r1   r2   N)r   r:   r   r   r;   r   r   r<   r=   r>   r?   r@   rA   rE   Útupler   rD   rF   r   rG   rH   rB   rC   rN   rO   rP   rQ   )r    rR   rS   rT   rU   r   r   r!   Úattach_solid_material“   s.    ÿ
ÿÿÿ
ÿÿzUSDObject.attach_solid_materialÚnonec                 C   s   |  d¡ |¡ d S )NZsubdivisionScheme)ZGetAttributerE   )r    Úusd_primÚschemer   r   r!   Ú_set_refinement_properties³   s    z$USDObject._set_refinement_propertiesN©ÚposÚmatÚvisibleÚframeÚscalec                 C   s|   t j||dj}| j t | ¡ ¡|¡ |rZ|| j dkrZ|  	dt
d| jƒ¡ |  	d|¡ |rd|| _|durx|  ||¡ dS )z2Updates the position and orientation of an object.)Zrotation_matrixZtranslation_vectorr   Fr   TN)Úutils_moduleZcreate_transform_matrixÚTr   rE   r   ZMatrix4dÚtolistr   Úupdate_visibilityÚmaxÚupdate_scale)r    r_   r`   ra   rb   rc   Ztransformation_matr   r   r!   Úupdate¶   s    	ÿzUSDObject.update)ra   rb   c                 C   s"   |rdnd}| j  ¡  ||¡ dS )zAUpdates the visibility of an object in a scene for a given frame.Z	inheritedZ	invisibleN)r   ZGetVisibilityAttrrE   )r    ra   rb   Zvisibility_settingr   r   r!   rg   Ï   s    zUSDObject.update_visibility©rc   rb   c                 C   s   | j  t | ¡ ¡|¡ dS )z>Updates the scale of an object in the scene for a given frame.N)r   rE   r   ÚVec3frf   )r    rc   rb   r   r   r!   ri   Ô   s    zUSDObject.update_scale)rZ   )N)Ú__name__Ú
__module__Ú__qualname__Ú__doc__ÚnpÚarrayr	   ÚStagerI   ÚMjModelÚMjvGeomÚstrÚndarrayr   r   r   Ú
mjtTexturer"   ÚabcÚabstractmethodr&   r'   rV   rY   r]   ÚboolÚintrj   rg   ri   r   r   r   r!   r   %   s8   ùù

= 
	 úúr   c                       st   e Zd ZdZe g d¢¡dfejej	ej
eeejeeeeejf   dœ‡ fdd„Zdd„ Zd	d
„ Zdd„ Z‡  ZS )ÚUSDMeshz6Class that handles predefined meshes in the USD scene.r   r   )r   r   r   r   Údataidr   r   c                    s  t ƒ  ||||||¡ || _| j› d|› }tj ||¡| _| |¡| _	|  
¡ \}	}
}| j ¡  |	¡ | j ¡  dd„ t|ƒD ƒ¡ | j ¡  |
¡ |jdkr| jtjjj r|  ¡ \}}t | j¡ dtjjtjj¡| _| j |¡ | j t  !| "¡ ¡¡ |  #| j¡ n|  $| j¡ d S )Nú/Mesh_c                 S   s   g | ]}d ‘qS ©rW   r   ©Ú.0Ú_r   r   r!   Ú
<listcomp>ò   ó    z$USDMesh.__init__.<locals>.<listcomp>r.   r8   )%Úsuperr"   r~   r   r
   ÚMeshr   rR   ÚGetPrimAtPathr[   r'   ÚGetPointsAttrrE   ÚGetFaceVertexCountsAttrÚrangeÚGetFaceVertexIndicesAttrÚmatidr   rI   rJ   rK   rL   r&   ÚPrimvarsAPIÚCreatePrimvarr   r@   ÚTexCoord2fArrayÚTokensÚfaceVaryingÚ	texcoordsÚ
SetIndicesr   ÚIntArrayrf   rV   rY   )r    r   r   r   r   r~   r   r   Ú	mesh_pathÚ	mesh_vertÚ	mesh_faceÚmesh_facenumÚmesh_texcoordÚmesh_facetexcoord©Ú	__class__r   r!   r"   Ü   s2    

ÿÿþýzUSDMesh.__init__c                 C   s6   dg}d}t |ƒD ]}||| d 7 }| |¡ q|S )Nr   rW   )r‹   Úappend)r    ÚnmeshZarrZfacetexcoords_rangesZrunning_sumÚir   r   r!   Ú_get_facetexcoord_ranges  s    z USDMesh._get_facetexcoord_rangesc                 C   s¢   | j j| j }| j| j jd k r2| j j| jd  n
t| j jƒ}| j j||… }|  | j j| j j¡}| j j 	¡ }||| j || jd  … }d||t|ƒk< ||fS )Nr   r   )
r   Zmesh_texcoordadrr~   rŸ   Úlenrš   r¡   r™   r›   Úflatten)r    Zmesh_texcoord_adr_fromZmesh_texcoord_adr_torš   Zmesh_facetexcoord_rangesr›   r   r   r!   r&     s&    ÿ
ýÿÿ
ÿÿzUSDMesh._get_uv_geometryc                 C   s´   | j j| j }| j| j jd k r2| j j| jd  n
t| j jƒ}| j j||… }| j j| j }| j| j jd k r€| j j| jd  n
t| j jƒ}| j j||… }| j j| j }|||fS )Nr   )	r   Zmesh_vertadrr~   rŸ   r¢   r—   Zmesh_faceadrr˜   r™   )r    Zmesh_vert_adr_fromZmesh_vert_adr_tor—   Zmesh_face_adr_fromZmesh_face_adr_tor˜   r™   r   r   r!   r'   (  s    ÿ
ýÿ
ýzUSDMesh._get_mesh_geometry)rm   rn   ro   rp   rq   rr   r	   rs   rI   rt   ru   rv   r|   rw   r   r   r   rx   r"   r¡   r&   r'   Ú__classcell__r   r   rœ   r!   r}   Ù   s   	øø+r}   c                       s|   e Zd ZdZe g d¢¡dfeeef ej	e
je
jeejeeeee
jf   dœ‡ fdd„Zdd„ Zd	d
„ Zdd„ Z‡  ZS )ÚUSDPrimitiveMeshz2Class to handle primitive shapes in the USD scene.r   r   ©Úmesh_configr   r   r   r   r   r   c                    s<  t ƒ  ||||||¡ || _|  ¡ | _| j› d|› }tj ||¡| _	| 
|¡| _|  ¡ \}	}
}| j	 ¡  |	¡ | j	 ¡  dd„ t|ƒD ƒ¡ | j	 ¡  |
¡ |  | j¡ |jdkr,| jtjjj r,|  ¡ \}}t | j	¡ dtjjtjj ¡| _!| j! |¡ | j! "t# $t%t|d ƒƒ¡¡ |  &| j	¡ n|  '| j	¡ d S )Nr   c                 S   s   g | ]}d ‘qS r€   r   r   r   r   r!   r„   U  r…   z-USDPrimitiveMesh.__init__.<locals>.<listcomp>r.   r8   rW   )(r†   r"   r§   Úgenerate_primitive_meshÚ	prim_meshr   r
   r‡   r   rR   rˆ   r[   r'   r‰   rE   rŠ   r‹   rŒ   r]   r   r   rI   rJ   rK   rL   r&   rŽ   r   r   r@   r   r‘   r’   r“   r”   r   r•   ÚlistrV   rY   )r    r§   r   r   r   r   r   r   r–   r—   r˜   r™   rš   rƒ   rœ   r   r!   r"   ?  s6    


ÿÿþýzUSDPrimitiveMesh.__init__c                 C   sT   t j}| jr| j|jj nd}|r*|d nd}t | j|¡\}}| | 	¡  ¡ |S )z0Generates the mesh for the primitive USD object.Nr   )
rI   rJ   r   rK   rL   Úshapes_moduleÚmesh_factoryr§   Ú	translateÚ
get_center)r    Útex_roleÚgeom_rgb_textureÚtexture_typerƒ   r©   r   r   r!   r¨   m  s    ÿýz(USDPrimitiveMesh.generate_primitive_meshc           	      C   s`  | j r| j jd usJ ‚t | j j¡}t | j j¡}tj}| j|j	j
 d }|tjjkrT| jj| jj \}}| jj| jj r¼| jjd dkrœ|| jjd 9 }| jjd dkr¼|| jjd 9 }| jjd d… \}}| jjtjjkr|dkrî|nd}|dkrþ|nd}| jj| jj rT|d d …df  ||d  9  < |d d …df  ||d  9  < || ¡ fS )Nr   r   é   )r©   Útriangle_uvsrq   rr   ÚasarrayÚ	trianglesrI   rJ   r   rK   rL   rx   ZmjTEXTURE_2Dr   Zmat_texrepeatr   r   Zmat_texuniformÚsizeÚtypeZmjtGeomZmjGEOM_PLANEr£   )	r    rš   r›   r¯   r°   Zs_scaleZt_scaleZs_sizeZt_sizer   r   r!   r&   z  s(      z!USDPrimitiveMesh._get_uv_geometryc                 C   s4   | j s
J ‚t | j j¡}t | j j¡}||t|ƒfS )N)r©   rq   r´   Úverticesrµ   r¢   )r    r—   r˜   r   r   r!   r'   –  s    
z#USDPrimitiveMesh._get_mesh_geometry)rm   rn   ro   rp   rq   rr   r   r   r	   rs   rI   rt   ru   rv   rw   r   r   r   rx   r"   r¨   r&   r'   r¤   r   r   rœ   r!   r¥   <  s   	ø
ø.r¥   c                       s´   e Zd ZdZe g d¢¡dfeeef ej	e
je
jeejeeeee
jf   dœ‡ fdd„Zdd„ Zd	d
„ Zdd„ Zdejejeeeej dœ‡ fdd„Zejedœdd„Z‡  ZS )Ú	USDTendonz)Class to handle tendons in the USD scene.r   r   r¦   c              	      sR  t ƒ  ||||||¡ || _|  ¡ | _t t¡| _| j 	¡ D ]’\}}	| j
› d|› }
|
› d|› }tj ||
¡}|| j| d< tj ||¡| j| d< | |¡| j| d< | ¡ | j| d< | ¡ | j| d< q<|  ¡ }d }| 	¡ D ]j\}}| j| d  ¡  |d ¡ | j| d  ¡  d	d
„ t|d ƒD ƒ¡ | j| d  ¡  |d ¡ qätj}|jdkr| j|jj r|  ¡ }| 	¡ D ]–\}}t | j| d ¡  dt!j"j#tj$j%¡| _&| j& |d ¡ | j& 't( )t*t|d d ƒƒ¡¡ | j 	¡ D ]&\}	}|  +|d ¡ |  ,|d ¡ qîq‚n2| j 	¡ D ]&\}	}|  +|d ¡ |  -|d ¡ q&d S )Nz/Mesh_Xform_r   r   rR   r[   Útranslate_opr   r—   c                 S   s   g | ]}d ‘qS r€   r   r   r   r   r!   r„   Æ  r…   z&USDTendon.__init__.<locals>.<listcomp>r™   r˜   r.   r8   rš   rW   ).r†   r"   r§   r¨   Útendon_partsÚcollectionsÚdefaultdictÚdictÚusd_refsÚitemsr   r
   r   r   r‡   rˆ   ZAddTranslateOpr   r'   r‰   rE   rŠ   r‹   rŒ   rI   rJ   r   r   rK   rL   r&   rŽ   r   r   r@   r   r‘   r’   r“   r”   r   r•   rª   r]   rV   rY   )r    r§   r   r   r   r   r   r   Únamerƒ   Zpart_xform_pathr–   r   Úpart_geometriesZpart_geometryr¯   Úpart_uv_geometriesZpart_uv_geometryÚrefrœ   r   r!   r"   £  s\    

ÿÿÿÿûÿzUSDTendon.__init__c                 C   sf   i }| j r| j tjjj nd}|r*|d nd}| jD ],}t ||¡\}}| | 	¡  ¡ |||< q4|S )z+Generates the tendon mesh using primitives.Nr   )
r   rI   rJ   rK   rL   r§   r«   r¬   r­   r®   )r    Z
mesh_partsr°   r±   Zpart_configZ	mesh_namer©   r   r   r!   r¨   ä  s    ÿý
ÿ
z!USDTendon.generate_primitive_meshc                 C   sV   t  t¡}| j ¡ D ]<\}}|jd us*J ‚t |j¡}t |j	¡}||dœ||< q|S )N)rš   r›   )
r¼   r½   r¾   r»   rÀ   r³   rq   rr   r´   rµ   )r    rÃ   rÁ   Úmeshrš   r›   r   r   r!   r&   õ  s    
þzUSDTendon._get_uv_geometryc                 C   sN   t  t¡}| j ¡ D ]4\}}t |j¡}t |j¡}||t	|ƒdœ||< q|S )N)r—   r˜   r™   )
r¼   r½   r¾   r»   rÀ   rq   r´   r¸   rµ   r¢   )r    rÂ   rÁ   rÅ   r—   r˜   r   r   r!   r'     s    
ýzUSDTendon._get_mesh_geometryNr^   c                    s¤   t ƒ  |||||¡ | jD ]„}d|v r`dd|d  |d d  g}| j| d  t |¡|¡ qd|v rdd|d |d d  g}| j| d  t |¡|¡ qdS )z?Updates the position and orientation of an object in the scene.Úleftr   r²   rº   ÚrightN)r†   rj   r»   r¿   rE   r   rl   )r    r_   r`   ra   rb   rc   rÁ   r­   rœ   r   r!   rj     s    	
zUSDTendon.updaterk   c                 C   sj   | j D ]^}d|v r4| j| d  t | ¡ ¡|¡ q| ¡ }|d |d< | j| d  t |¡|¡ qdS )z Updates the scale of the tendon.Zcylinderr   r   r²   N)r»   r¿   rE   r   rl   rf   )r    rc   rb   rÁ   Zhemisphere_scaler   r   r!   ri      s    
"zUSDTendon.update_scale)N)rm   rn   ro   rp   rq   rr   r   r   r	   rs   rI   rt   ru   rv   rw   r   r   r   rx   r"   r¨   r&   r'   r{   r|   rj   ri   r¤   r   r   rœ   r!   r¹      s0   	ø
øA úúr¹   )rp   ry   r¼   Útypingr   r   r   r   r   rI   Zmujoco.usd.shapesÚusdZshapesr«   Zmujoco.usd.utilsÚutilsrd   Únumpyrq   Zpxrr   r   r	   r
   r   r   ÚABCr   r}   r¥   r¹   r   r   r   r!   Ú<module>   s$    5cd