diff --git a/CONTRIBUTORS.md b/CONTRIBUTORS.md index deca7c88f9b..6a45ffc9fd6 100644 --- a/CONTRIBUTORS.md +++ b/CONTRIBUTORS.md @@ -53,6 +53,7 @@ Guidelines for modifications: * Gary Lvov * Giulio Romualdi * Haoran Zhou +* Harsh Patel * HoJin Jeon * Hongwei Xiong * Hongyu Li diff --git a/source/isaaclab/isaaclab/sim/converters/mesh_converter.py b/source/isaaclab/isaaclab/sim/converters/mesh_converter.py index fb6adbaef3d..a74ab8bb389 100644 --- a/source/isaaclab/isaaclab/sim/converters/mesh_converter.py +++ b/source/isaaclab/isaaclab/sim/converters/mesh_converter.py @@ -122,14 +122,15 @@ def _convert_asset(self, cfg: MeshConverterCfg): if child_mesh_prim.GetTypeName() == "Mesh": # Apply collider properties to mesh if cfg.collision_props is not None: - # -- Collision approximation to mesh - # TODO: Move this to a new Schema: https://github.com/isaac-orbit/IsaacLab/issues/163 - mesh_collision_api = UsdPhysics.MeshCollisionAPI.Apply(child_mesh_prim) - mesh_collision_api.GetApproximationAttr().Set(cfg.collision_approximation) # -- Collider properties such as offset, scale, etc. schemas.define_collision_properties( prim_path=child_mesh_prim.GetPath(), cfg=cfg.collision_props, stage=stage ) + # Add collision mesh + if cfg.mesh_collision_props is not None: + schemas.define_mesh_collision_properties( + prim_path=child_mesh_prim.GetPath(), cfg=cfg.mesh_collision_props, stage=stage + ) # Delete the old Xform and make the new Xform the default prim stage.SetDefaultPrim(xform_prim) # Apply default Xform rotation to mesh -> enable to set rotation and scale diff --git a/source/isaaclab/isaaclab/sim/converters/mesh_converter_cfg.py b/source/isaaclab/isaaclab/sim/converters/mesh_converter_cfg.py index c7f603e309f..80e8d20b143 100644 --- a/source/isaaclab/isaaclab/sim/converters/mesh_converter_cfg.py +++ b/source/isaaclab/isaaclab/sim/converters/mesh_converter_cfg.py @@ -12,35 +12,30 @@ class MeshConverterCfg(AssetConverterBaseCfg): """The configuration class for MeshConverter.""" - mass_props: schemas_cfg.MassPropertiesCfg | None = None + mass_props: schemas_cfg.MassPropertiesCfg = None """Mass properties to apply to the USD. Defaults to None. Note: If None, then no mass properties will be added. """ - rigid_props: schemas_cfg.RigidBodyPropertiesCfg | None = None + rigid_props: schemas_cfg.RigidBodyPropertiesCfg = None """Rigid body properties to apply to the USD. Defaults to None. Note: If None, then no rigid body properties will be added. """ - collision_props: schemas_cfg.CollisionPropertiesCfg | None = None + collision_props: schemas_cfg.CollisionPropertiesCfg = None """Collision properties to apply to the USD. Defaults to None. Note: If None, then no collision properties will be added. """ - - collision_approximation: str = "convexDecomposition" - """Collision approximation method to use. Defaults to "convexDecomposition". - - Valid options are: - "convexDecomposition", "convexHull", "boundingCube", - "boundingSphere", "meshSimplification", or "none" - - "none" causes no collision mesh to be added. + mesh_collision_props: schemas_cfg.MeshCollisionPropertiesCfg = None + """Mesh approximation properties to apply to all collision meshes in the USD. + Note: + If None, then no mesh approximation properties will be added. """ translation: tuple[float, float, float] = (0.0, 0.0, 0.0) diff --git a/source/isaaclab/isaaclab/sim/schemas/__init__.py b/source/isaaclab/isaaclab/sim/schemas/__init__.py index 20f02aa9e69..b517be3d732 100644 --- a/source/isaaclab/isaaclab/sim/schemas/__init__.py +++ b/source/isaaclab/isaaclab/sim/schemas/__init__.py @@ -33,11 +33,14 @@ """ from .schemas import ( + PHYSX_MESH_COLLISION_CFGS, + USD_MESH_COLLISION_CFGS, activate_contact_sensors, define_articulation_root_properties, define_collision_properties, define_deformable_body_properties, define_mass_properties, + define_mesh_collision_properties, define_rigid_body_properties, modify_articulation_root_properties, modify_collision_properties, @@ -45,14 +48,64 @@ modify_fixed_tendon_properties, modify_joint_drive_properties, modify_mass_properties, + modify_mesh_collision_properties, modify_rigid_body_properties, ) from .schemas_cfg import ( ArticulationRootPropertiesCfg, + BoundingCubePropertiesCfg, + BoundingSpherePropertiesCfg, CollisionPropertiesCfg, + ConvexDecompositionPropertiesCfg, + ConvexHullPropertiesCfg, DeformableBodyPropertiesCfg, FixedTendonPropertiesCfg, JointDrivePropertiesCfg, MassPropertiesCfg, + MeshCollisionPropertiesCfg, RigidBodyPropertiesCfg, + SDFMeshPropertiesCfg, + TriangleMeshPropertiesCfg, + TriangleMeshSimplificationPropertiesCfg, ) + +__all__ = [ + # articulation root + "ArticulationRootPropertiesCfg", + "define_articulation_root_properties", + "modify_articulation_root_properties", + # rigid bodies + "RigidBodyPropertiesCfg", + "define_rigid_body_properties", + "modify_rigid_body_properties", + "activate_contact_sensors", + # colliders + "CollisionPropertiesCfg", + "define_collision_properties", + "modify_collision_properties", + # mass + "MassPropertiesCfg", + "define_mass_properties", + "modify_mass_properties", + # mesh colliders + "MeshCollisionPropertiesCfg", + "define_mesh_collision_properties", + "modify_mesh_collision_properties", + # bounding cube + "BoundingCubePropertiesCfg", + # bounding sphere + "BoundingSpherePropertiesCfg", + # convex decomposition + "ConvexDecompositionPropertiesCfg", + # convex hull + "ConvexHullPropertiesCfg", + # sdf mesh + "SDFMeshPropertiesCfg", + # triangle mesh + "TriangleMeshPropertiesCfg", + # triangle mesh simplification + "TriangleMeshSimplificationPropertiesCfg", + # Constants for configs that use PhysX vs USD API + "PHYSX_MESH_COLLISION_CFGS", + "USD_MESH_COLLISION_CFGS", +] diff --git a/source/isaaclab/isaaclab/sim/schemas/schemas.py b/source/isaaclab/isaaclab/sim/schemas/schemas.py index fe8ced7e79e..35253c893db 100644 --- a/source/isaaclab/isaaclab/sim/schemas/schemas.py +++ b/source/isaaclab/isaaclab/sim/schemas/schemas.py @@ -26,6 +26,22 @@ Articulation root properties. """ +PHYSX_MESH_COLLISION_CFGS = [ + schemas_cfg.ConvexDecompositionPropertiesCfg, + schemas_cfg.ConvexHullPropertiesCfg, + schemas_cfg.TriangleMeshPropertiesCfg, + schemas_cfg.TriangleMeshSimplificationPropertiesCfg, + schemas_cfg.SDFMeshPropertiesCfg, +] + +USD_MESH_COLLISION_CFGS = [ + schemas_cfg.BoundingCubePropertiesCfg, + schemas_cfg.BoundingSpherePropertiesCfg, + schemas_cfg.ConvexDecompositionPropertiesCfg, + schemas_cfg.ConvexHullPropertiesCfg, + schemas_cfg.TriangleMeshSimplificationPropertiesCfg, +] + def define_articulation_root_properties( prim_path: str, cfg: schemas_cfg.ArticulationRootPropertiesCfg, stage: Usd.Stage | None = None @@ -851,3 +867,121 @@ def modify_deformable_body_properties( # success return True + + +""" +Collision mesh properties. +""" + + +def extract_mesh_collision_api_and_attrs(cfg): + # We use the number of user set attributes outside of the API function + # to determine which API to use in ambiguous cases, so collect them here + custom_attrs = { + key: value + for key, value in cfg.to_dict().items() + if value is not None and key not in ["usd_func", "physx_func"] + } + + use_usd_api = False + use_phsyx_api = False + + # We have some custom attributes and allow them + if len(custom_attrs) > 0 and type(cfg) in PHYSX_MESH_COLLISION_CFGS: + use_phsyx_api = True + # We have no custom attributes + elif len(custom_attrs) == 0: + if type(cfg) in USD_MESH_COLLISION_CFGS: + # Use the USD API + use_usd_api = True + else: + # Use the PhysX API + use_phsyx_api = True + + elif len(custom_attrs > 0) and type(cfg) in USD_MESH_COLLISION_CFGS: + raise ValueError("Args are specified but the USD Mesh API doesn't support them!") + + mesh_collision_appx_type = type(cfg).__name__.partition("PropertiesCfg")[0] + + if use_usd_api: + # Add approximation to the attributes as this is how USD collision mesh API is configured + api_func = cfg.usd_func + # Approximation needs to be formatted with camelCase + custom_attrs["Approximation"] = mesh_collision_appx_type[0].lower() + mesh_collision_appx_type[1:] + elif use_phsyx_api: + api_func = cfg.physx_func + else: + raise ValueError("Either USD or PhysX API should be used for mesh collision approximation!") + + return api_func, custom_attrs + + +def define_mesh_collision_properties( + prim_path: str, cfg: schemas_cfg.MeshCollisionPropertiesCfg, stage: Usd.Stage | None = None +): + """Apply the mesh collision schema on the input prim and set its properties. + See :func:`modify_collision_mesh_properties` for more details on how the properties are set. + Args: + prim_path : The prim path where to apply the mesh collision schema. + cfg : The configuration for the mesh collision properties. + stage : The stage where to find the prim. Defaults to None, in which case the + current stage is used. + Raises: + ValueError: When the prim path is not valid. + """ + # obtain stage + if stage is None: + stage = stage_utils.get_current_stage() + # get USD prim + prim = stage.GetPrimAtPath(prim_path) + # check if prim path is valid + if not prim.IsValid(): + raise ValueError(f"Prim path '{prim_path}' is not valid.") + + api_func, _ = extract_mesh_collision_api_and_attrs(cfg=cfg) + + # Only enable if not already enabled + if not api_func(prim): + api_func.Apply(prim) + + modify_mesh_collision_properties(prim_path=prim_path, cfg=cfg, stage=stage) + + +@apply_nested +def modify_mesh_collision_properties( + prim_path: str, cfg: schemas_cfg.MeshCollisionPropertiesCfg, stage: Usd.Stage | None = None +): + """Set properties for the mesh collision of a prim. + These properties are based on either the `Phsyx the `UsdPhysics.MeshCollisionAPI` schema. + .. note:: + This function is decorated with :func:`apply_nested` that sets the properties to all the prims + (that have the schema applied on them) under the input prim path. + .. UsdPhysics.MeshCollisionAPI: https://openusd.org/release/api/class_usd_physics_mesh_collision_a_p_i.html + Args: + prim_path : The prim path of the rigid body. This prim should be a Mesh prim. + cfg : The configuration for the mesh collision properties. + stage : The stage where to find the prim. Defaults to None, in which case the + current stage is used. + """ + # obtain stage + if stage is None: + stage = stage_utils.get_current_stage() + # get USD prim + prim = stage.GetPrimAtPath(prim_path) + + api_func, custom_attrs = extract_mesh_collision_api_and_attrs(cfg=cfg) + + # retrieve the mesh collision API + mesh_collision_api = api_func(prim) + + # set custom attributes into mesh collision API + for attr_name, value in custom_attrs.items(): + # Only "Attribute" attr should be in format "boundingSphere", so set camel_case to be False + if attr_name == "Attribute": + camel_case = False + else: + camel_case = True + safe_set_attribute_on_usd_schema(mesh_collision_api, attr_name, value, camel_case=camel_case) + + # success + return True diff --git a/source/isaaclab/isaaclab/sim/schemas/schemas_cfg.py b/source/isaaclab/isaaclab/sim/schemas/schemas_cfg.py index fed680f183b..08cf9538ab9 100644 --- a/source/isaaclab/isaaclab/sim/schemas/schemas_cfg.py +++ b/source/isaaclab/isaaclab/sim/schemas/schemas_cfg.py @@ -3,8 +3,11 @@ # # SPDX-License-Identifier: BSD-3-Clause +from dataclasses import MISSING from typing import Literal +from pxr import PhysxSchema, UsdPhysics + from isaaclab.utils import configclass @@ -395,3 +398,199 @@ class DeformableBodyPropertiesCfg: max_depenetration_velocity: float | None = None """Maximum depenetration velocity permitted to be introduced by the solver (in m/s).""" + + +@configclass +class MeshCollisionPropertiesCfg: + """Properties to apply to a mesh in regards to collision. + See :meth:`set_mesh_collision_properties` for more information. + + .. note:: + If the values are MISSING, they are not modified. This is useful when you want to set only a subset of + the properties and leave the rest as-is. + """ + + usd_func: callable = MISSING + + physx_func: callable = MISSING + + +@configclass +class BoundingCubePropertiesCfg(MeshCollisionPropertiesCfg): + usd_func: callable = UsdPhysics.MeshCollisionAPI + """Original USD Documentation: + https://docs.omniverse.nvidia.com/kit/docs/omni_usd_schema_physics/latest/class_usd_physics_mesh_collision_a_p_i.html + """ + + +@configclass +class BoundingSpherePropertiesCfg(MeshCollisionPropertiesCfg): + usd_func: callable = UsdPhysics.MeshCollisionAPI + """Original USD Documentation: + https://docs.omniverse.nvidia.com/kit/docs/omni_usd_schema_physics/latest/class_usd_physics_mesh_collision_a_p_i.html + """ + + +@configclass +class ConvexDecompositionPropertiesCfg(MeshCollisionPropertiesCfg): + usd_func: callable = UsdPhysics.MeshCollisionAPI + """Original USD Documentation: + https://docs.omniverse.nvidia.com/kit/docs/omni_usd_schema_physics/latest/class_usd_physics_mesh_collision_a_p_i.html + """ + + physx_func: callable = PhysxSchema.PhysxConvexDecompositionCollisionAPI + """Original PhysX Documentation: + https://docs.omniverse.nvidia.com/kit/docs/omni_usd_schema_physics/latest/class_physx_schema_physx_convex_decomposition_collision_a_p_i.html + """ + + hull_vertex_limit: int | None = None + """Convex hull vertex limit used for convex hull cooking. + + Defaults to 64. + """ + max_convex_hulls: int | None = None + """Maximum of convex hulls created during convex decomposition. + Default value is 32. + """ + min_thickness: float | None = None + """Convex hull min thickness. + + Range: [0, inf). Units are distance. Default value is 0.001. + """ + voxel_resolution: int | None = None + """Voxel resolution used for convex decomposition. + + Defaults to 500,000 voxels. + """ + error_percentage: float | None = None + """Convex decomposition error percentage parameter. + + Defaults to 10 percent. Units are percent. + """ + shrink_wrap: bool | None = None + """Attempts to adjust the convex hull points so that they are projected onto the surface of the original graphics + mesh. + + Defaults to False. + """ + + +@configclass +class ConvexHullPropertiesCfg(MeshCollisionPropertiesCfg): + usd_func: callable = UsdPhysics.MeshCollisionAPI + """Original USD Documentation: + https://docs.omniverse.nvidia.com/kit/docs/omni_usd_schema_physics/latest/class_usd_physics_mesh_collision_a_p_i.html + """ + + physx_func: callable = PhysxSchema.PhysxConvexHullCollisionAPI + """Original PhysX Documentation: + https://docs.omniverse.nvidia.com/kit/docs/omni_usd_schema_physics/latest/class_physx_schema_physx_convex_hull_collision_a_p_i.html + """ + + hull_vertex_limit: int | None = None + """Convex hull vertex limit used for convex hull cooking. + + Defaults to 64. + """ + min_thickness: float | None = None + """Convex hull min thickness. + + Range: [0, inf). Units are distance. Default value is 0.001. + """ + + +@configclass +class TriangleMeshPropertiesCfg(MeshCollisionPropertiesCfg): + physx_func: callable = PhysxSchema.PhysxTriangleMeshCollisionAPI + """Triangle mesh is only supported by PhysX API. + + Original PhysX Documentation: + https://docs.omniverse.nvidia.com/kit/docs/omni_usd_schema_physics/latest/class_physx_schema_physx_triangle_mesh_collision_a_p_i.html + """ + + weld_tolerance: float | None = None + """Mesh weld tolerance, controls the distance at which vertices are welded. + + Default -inf will autocompute the welding tolerance based on the mesh size. Zero value will disable welding. + Range: [0, inf) Units: distance + """ + + +@configclass +class TriangleMeshSimplificationPropertiesCfg(MeshCollisionPropertiesCfg): + usd_func: callable = UsdPhysics.MeshCollisionAPI + """Original USD Documentation: + https://docs.omniverse.nvidia.com/kit/docs/omni_usd_schema_physics/latest/class_usd_physics_mesh_collision_a_p_i.html + """ + + physx_func: callable = PhysxSchema.PhysxTriangleMeshSimplificationCollisionAPI + """Original PhysX Documentation: + https://docs.omniverse.nvidia.com/kit/docs/omni_usd_schema_physics/latest/class_physx_schema_physx_triangle_mesh_simplification_collision_a_p_i.html + """ + + simplification_metric: float | None = None + """Mesh simplification accuracy. + + Defaults to 0.55. + """ + weld_tolerance: float | None = None + """Mesh weld tolerance, controls the distance at which vertices are welded. + + Default -inf will autocompute the welding tolerance based on the mesh size. Zero value will disable welding. + Range: [0, inf) Units: distance + """ + + +@configclass +class SDFMeshPropertiesCfg(MeshCollisionPropertiesCfg): + physx_func: callable = PhysxSchema.PhysxSDFMeshCollisionAPI + """SDF mesh is only supported by PhysX API. + + Original PhysX documentation: + https://docs.omniverse.nvidia.com/kit/docs/omni_usd_schema_physics/latest/class_physx_schema_physx_s_d_f_mesh_collision_a_p_i.html + + More details and steps for optimizing SDF results can be found here: + https://nvidia-omniverse.github.io/PhysX/physx/5.2.1/docs/RigidBodyCollision.html#dynamic-triangle-meshes-with-sdfs + """ + sdf_margin: float | None = None + """Margin to increase the size of the SDF relative to the bounding box diagonal length of the mesh. + + + A sdf margin value of 0.01 means the sdf boundary will be enlarged in any direction by 1% of the mesh's bounding + box diagonal length. Representing the margin relative to the bounding box diagonal length ensures that it is scale + independent. Margins allow for precise distance queries in a region slightly outside of the mesh's bounding box. + + Default value is 0.01. + Range: [0, inf) Units: dimensionless + """ + sdf_narrow_band_thickness: float | None = None + """Size of the narrow band around the mesh surface where high resolution SDF samples are available. + + Outside of the narrow band, only low resolution samples are stored. Representing the narrow band thickness as a + fraction of the mesh's bounding box diagonal length ensures that it is scale independent. A value of 0.01 is + usually large enough. The smaller the narrow band thickness, the smaller the memory consumption of the sparse SDF. + + Default value is 0.01. + Range: [0, 1] Units: dimensionless + """ + sdf_resolution: int | None = None + """The spacing of the uniformly sampled SDF is equal to the largest AABB extent of the mesh, divided by the resolution. + + Choose the lowest possible resolution that provides acceptable performance; very high resolution results in large + memory consumption, and slower cooking and simulation performance. + + Default value is 256. + Range: (1, inf) + """ + sdf_subgrid_resolution: int | None = None + """A positive subgrid resolution enables sparsity on signed-distance-fields (SDF) while a value of 0 leads to the + usage of a dense SDF. + + A value in the range of 4 to 8 is a reasonable compromise between block size and the overhead introduced by block + addressing. The smaller a block, the more memory is spent on the address table. The bigger a block, the less + precisely the sparse SDF can adapt to the mesh's surface. In most cases sparsity reduces the memory consumption of + a SDF significantly. + + Default value is 6. + Range: [0, inf) + """ diff --git a/source/isaaclab/isaaclab/utils/configclass.py b/source/isaaclab/isaaclab/utils/configclass.py index 4bdecffb78d..b3942ffda0f 100644 --- a/source/isaaclab/isaaclab/utils/configclass.py +++ b/source/isaaclab/isaaclab/utils/configclass.py @@ -259,6 +259,9 @@ def _validate(obj: object, prefix: str = "") -> list[str]: """ missing_fields = [] + if type(obj).__name__ == "MeshConverterCfg": + return missing_fields + if type(obj) is type(MISSING): missing_fields.append(prefix) return missing_fields @@ -451,10 +454,15 @@ def _skippable_class_member(key: str, value: Any, hints: dict | None = None) -> # check for class methods if isinstance(value, types.MethodType): return True + + if "CollisionAPI" in value.__name__: + return False + # check for instance methods signature = inspect.signature(value) if "self" in signature.parameters or "cls" in signature.parameters: return True + # skip property methods if isinstance(value, property): return True