# Copyright (c) 2022-2025, The Isaac Lab Project Developers.# All rights reserved.## SPDX-License-Identifier: BSD-3-Clause# needed to import for allowing type-hinting: Usd.Stage | Nonefrom__future__importannotationsimportmathimportisaacsim.core.utils.stageasstage_utilsimportomni.logimportomni.physx.scripts.utilsasphysx_utilsfromomni.physx.scriptsimportdeformableUtilsasdeformable_utilsfrompxrimportPhysxSchema,Usd,UsdPhysicsfrom..utilsimport(apply_nested,find_global_fixed_joint_prim,get_all_matching_child_prims,safe_set_attribute_on_usd_schema,)from.importschemas_cfg"""Articulation root properties."""
[docs]defdefine_articulation_root_properties(prim_path:str,cfg:schemas_cfg.ArticulationRootPropertiesCfg,stage:Usd.Stage|None=None):"""Apply the articulation root schema on the input prim and set its properties. See :func:`modify_articulation_root_properties` for more details on how the properties are set. Args: prim_path: The prim path where to apply the articulation root schema. cfg: The configuration for the articulation root. 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. TypeError: When the prim already has conflicting API schemas. """# obtain stageifstageisNone:stage=stage_utils.get_current_stage()# get articulation USD primprim=stage.GetPrimAtPath(prim_path)# check if prim path is validifnotprim.IsValid():raiseValueError(f"Prim path '{prim_path}' is not valid.")# check if prim has articulation applied on itifnotUsdPhysics.ArticulationRootAPI(prim):UsdPhysics.ArticulationRootAPI.Apply(prim)# set articulation root propertiesmodify_articulation_root_properties(prim_path,cfg,stage)
[docs]@apply_nesteddefmodify_articulation_root_properties(prim_path:str,cfg:schemas_cfg.ArticulationRootPropertiesCfg,stage:Usd.Stage|None=None)->bool:"""Modify PhysX parameters for an articulation root prim. The `articulation root`_ marks the root of an articulation tree. For floating articulations, this should be on the root body. For fixed articulations, this API can be on a direct or indirect parent of the root joint which is fixed to the world. The schema comprises of attributes that belong to the `ArticulationRootAPI`_ and `PhysxArticulationAPI`_. schemas. The latter contains the PhysX parameters for the articulation root. The properties are applied to the articulation root prim. The common properties (such as solver position and velocity iteration counts, sleep threshold, stabilization threshold) take precedence over those specified in the rigid body schemas for all the rigid bodies in the articulation. .. caution:: When the attribute :attr:`schemas_cfg.ArticulationRootPropertiesCfg.fix_root_link` is set to True, a fixed joint is created between the root link and the world frame (if it does not already exist). However, to deal with physics parser limitations, the articulation root schema needs to be applied to the parent of the root link. .. note:: This function is decorated with :func:`apply_nested` that set the properties to all the prims (that have the schema applied on them) under the input prim path. .. _articulation root: https://nvidia-omniverse.github.io/PhysX/physx/5.4.1/docs/Articulations.html .. _ArticulationRootAPI: https://openusd.org/dev/api/class_usd_physics_articulation_root_a_p_i.html .. _PhysxArticulationAPI: https://docs.omniverse.nvidia.com/kit/docs/omni_usd_schema_physics/104.2/class_physx_schema_physx_articulation_a_p_i.html Args: prim_path: The prim path to the articulation root. cfg: The configuration for the articulation root. stage: The stage where to find the prim. Defaults to None, in which case the current stage is used. Returns: True if the properties were successfully set, False otherwise. Raises: NotImplementedError: When the root prim is not a rigid body and a fixed joint is to be created. """# obtain stageifstageisNone:stage=stage_utils.get_current_stage()# get articulation USD primarticulation_prim=stage.GetPrimAtPath(prim_path)# check if prim has articulation applied on itifnotUsdPhysics.ArticulationRootAPI(articulation_prim):returnFalse# retrieve the articulation apiphysx_articulation_api=PhysxSchema.PhysxArticulationAPI(articulation_prim)ifnotphysx_articulation_api:physx_articulation_api=PhysxSchema.PhysxArticulationAPI.Apply(articulation_prim)# convert to dictcfg=cfg.to_dict()# extract non-USD propertiesfix_root_link=cfg.pop("fix_root_link",None)# set into physx apiforattr_name,valueincfg.items():safe_set_attribute_on_usd_schema(physx_articulation_api,attr_name,value,camel_case=True)# fix root link based on input# we do the fixed joint processing later to not interfere with setting other propertiesiffix_root_linkisnotNone:# check if a global fixed joint exists under the root primexisting_fixed_joint_prim=find_global_fixed_joint_prim(prim_path)# if we found a fixed joint, enable/disable it based on the input# otherwise, create a fixed joint between the world and the root linkifexisting_fixed_joint_primisnotNone:omni.log.info(f"Found an existing fixed joint for the articulation: '{prim_path}'. Setting it to: {fix_root_link}.")existing_fixed_joint_prim.GetJointEnabledAttr().Set(fix_root_link)eliffix_root_link:omni.log.info(f"Creating a fixed joint for the articulation: '{prim_path}'.")# note: we have to assume that the root prim is a rigid body,# i.e. we don't handle the case where the root prim is not a rigid body but has articulation api on it# Currently, there is no obvious way to get first rigid body link identified by the PhysX parserifnotarticulation_prim.HasAPI(UsdPhysics.RigidBodyAPI):raiseNotImplementedError(f"The articulation prim '{prim_path}' does not have the RigidBodyAPI applied."" To create a fixed joint, we need to determine the first rigid body link in"" the articulation tree. However, this is not implemented yet.")# create a fixed joint between the root link and the world framephysx_utils.createJoint(stage=stage,joint_type="Fixed",from_prim=None,to_prim=articulation_prim)# Having a fixed joint on a rigid body is not treated as "fixed base articulation".# instead, it is treated as a part of the maximal coordinate tree.# Moving the articulation root to the parent solves this issue. This is a limitation of the PhysX parser.# get parent primparent_prim=articulation_prim.GetParent()# apply api to parentUsdPhysics.ArticulationRootAPI.Apply(parent_prim)PhysxSchema.PhysxArticulationAPI.Apply(parent_prim)# copy the attributes# -- usd attributesusd_articulation_api=UsdPhysics.ArticulationRootAPI(articulation_prim)forattr_nameinusd_articulation_api.GetSchemaAttributeNames():attr=articulation_prim.GetAttribute(attr_name)parent_prim.GetAttribute(attr_name).Set(attr.Get())# -- physx attributesphysx_articulation_api=PhysxSchema.PhysxArticulationAPI(articulation_prim)forattr_nameinphysx_articulation_api.GetSchemaAttributeNames():attr=articulation_prim.GetAttribute(attr_name)parent_prim.GetAttribute(attr_name).Set(attr.Get())# remove api from rootarticulation_prim.RemoveAPI(UsdPhysics.ArticulationRootAPI)articulation_prim.RemoveAPI(PhysxSchema.PhysxArticulationAPI)# successreturnTrue
"""Rigid body properties."""
[docs]defdefine_rigid_body_properties(prim_path:str,cfg:schemas_cfg.RigidBodyPropertiesCfg,stage:Usd.Stage|None=None):"""Apply the rigid body schema on the input prim and set its properties. See :func:`modify_rigid_body_properties` for more details on how the properties are set. Args: prim_path: The prim path where to apply the rigid body schema. cfg: The configuration for the rigid body. 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. TypeError: When the prim already has conflicting API schemas. """# obtain stageifstageisNone:stage=stage_utils.get_current_stage()# get USD primprim=stage.GetPrimAtPath(prim_path)# check if prim path is validifnotprim.IsValid():raiseValueError(f"Prim path '{prim_path}' is not valid.")# check if prim has rigid body applied on itifnotUsdPhysics.RigidBodyAPI(prim):UsdPhysics.RigidBodyAPI.Apply(prim)# set rigid body propertiesmodify_rigid_body_properties(prim_path,cfg,stage)
[docs]@apply_nesteddefmodify_rigid_body_properties(prim_path:str,cfg:schemas_cfg.RigidBodyPropertiesCfg,stage:Usd.Stage|None=None)->bool:"""Modify PhysX parameters for a rigid body prim. A `rigid body`_ is a single body that can be simulated by PhysX. It can be either dynamic or kinematic. A dynamic body responds to forces and collisions. A `kinematic body`_ can be moved by the user, but does not respond to forces. They are similar to having static bodies that can be moved around. The schema comprises of attributes that belong to the `RigidBodyAPI`_ and `PhysxRigidBodyAPI`_. schemas. The latter contains the PhysX parameters for the rigid body. .. 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. .. _rigid body: https://nvidia-omniverse.github.io/PhysX/physx/5.4.1/docs/RigidBodyOverview.html .. _kinematic body: https://openusd.org/release/wp_rigid_body_physics.html#kinematic-bodies .. _RigidBodyAPI: https://openusd.org/dev/api/class_usd_physics_rigid_body_a_p_i.html .. _PhysxRigidBodyAPI: https://docs.omniverse.nvidia.com/kit/docs/omni_usd_schema_physics/104.2/class_physx_schema_physx_rigid_body_a_p_i.html Args: prim_path: The prim path to the rigid body. cfg: The configuration for the rigid body. stage: The stage where to find the prim. Defaults to None, in which case the current stage is used. Returns: True if the properties were successfully set, False otherwise. """# obtain stageifstageisNone:stage=stage_utils.get_current_stage()# get rigid-body USD primrigid_body_prim=stage.GetPrimAtPath(prim_path)# check if prim has rigid-body applied on itifnotUsdPhysics.RigidBodyAPI(rigid_body_prim):returnFalse# retrieve the USD rigid-body apiusd_rigid_body_api=UsdPhysics.RigidBodyAPI(rigid_body_prim)# retrieve the physx rigid-body apiphysx_rigid_body_api=PhysxSchema.PhysxRigidBodyAPI(rigid_body_prim)ifnotphysx_rigid_body_api:physx_rigid_body_api=PhysxSchema.PhysxRigidBodyAPI.Apply(rigid_body_prim)# convert to dictcfg=cfg.to_dict()# set into USD APIforattr_namein["rigid_body_enabled","kinematic_enabled"]:value=cfg.pop(attr_name,None)safe_set_attribute_on_usd_schema(usd_rigid_body_api,attr_name,value,camel_case=True)# set into PhysX APIforattr_name,valueincfg.items():safe_set_attribute_on_usd_schema(physx_rigid_body_api,attr_name,value,camel_case=True)# successreturnTrue
"""Collision properties."""
[docs]defdefine_collision_properties(prim_path:str,cfg:schemas_cfg.CollisionPropertiesCfg,stage:Usd.Stage|None=None):"""Apply the collision schema on the input prim and set its properties. See :func:`modify_collision_properties` for more details on how the properties are set. Args: prim_path: The prim path where to apply the rigid body schema. cfg: The configuration for the collider. 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 stageifstageisNone:stage=stage_utils.get_current_stage()# get USD primprim=stage.GetPrimAtPath(prim_path)# check if prim path is validifnotprim.IsValid():raiseValueError(f"Prim path '{prim_path}' is not valid.")# check if prim has collision applied on itifnotUsdPhysics.CollisionAPI(prim):UsdPhysics.CollisionAPI.Apply(prim)# set collision propertiesmodify_collision_properties(prim_path,cfg,stage)
[docs]@apply_nesteddefmodify_collision_properties(prim_path:str,cfg:schemas_cfg.CollisionPropertiesCfg,stage:Usd.Stage|None=None)->bool:"""Modify PhysX properties of collider prim. These properties are based on the `UsdPhysics.CollisionAPI`_ and `PhysxSchema.PhysxCollisionAPI`_ schemas. For more information on the properties, please refer to the official documentation. Tuning these parameters influence the contact behavior of the rigid body. For more information on tune them and their effect on the simulation, please refer to the `PhysX documentation <https://nvidia-omniverse.github.io/PhysX/physx/5.4.1/docs/AdvancedCollisionDetection.html>`__. .. 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.CollisionAPI: https://openusd.org/dev/api/class_usd_physics_collision_a_p_i.html .. _PhysxSchema.PhysxCollisionAPI: https://docs.omniverse.nvidia.com/kit/docs/omni_usd_schema_physics/104.2/class_physx_schema_physx_collision_a_p_i.html Args: prim_path: The prim path of parent. cfg: The configuration for the collider. stage: The stage where to find the prim. Defaults to None, in which case the current stage is used. Returns: True if the properties were successfully set, False otherwise. """# obtain stageifstageisNone:stage=stage_utils.get_current_stage()# get USD primcollider_prim=stage.GetPrimAtPath(prim_path)# check if prim has collision applied on itifnotUsdPhysics.CollisionAPI(collider_prim):returnFalse# retrieve the USD collision apiusd_collision_api=UsdPhysics.CollisionAPI(collider_prim)# retrieve the collision apiphysx_collision_api=PhysxSchema.PhysxCollisionAPI(collider_prim)ifnotphysx_collision_api:physx_collision_api=PhysxSchema.PhysxCollisionAPI.Apply(collider_prim)# convert to dictcfg=cfg.to_dict()# set into USD APIforattr_namein["collision_enabled"]:value=cfg.pop(attr_name,None)safe_set_attribute_on_usd_schema(usd_collision_api,attr_name,value,camel_case=True)# set into PhysX APIforattr_name,valueincfg.items():safe_set_attribute_on_usd_schema(physx_collision_api,attr_name,value,camel_case=True)# successreturnTrue
"""Mass properties."""
[docs]defdefine_mass_properties(prim_path:str,cfg:schemas_cfg.MassPropertiesCfg,stage:Usd.Stage|None=None):"""Apply the mass schema on the input prim and set its properties. See :func:`modify_mass_properties` for more details on how the properties are set. Args: prim_path: The prim path where to apply the rigid body schema. cfg: The configuration for the mass 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 stageifstageisNone:stage=stage_utils.get_current_stage()# get USD primprim=stage.GetPrimAtPath(prim_path)# check if prim path is validifnotprim.IsValid():raiseValueError(f"Prim path '{prim_path}' is not valid.")# check if prim has mass applied on itifnotUsdPhysics.MassAPI(prim):UsdPhysics.MassAPI.Apply(prim)# set mass propertiesmodify_mass_properties(prim_path,cfg,stage)
[docs]@apply_nesteddefmodify_mass_properties(prim_path:str,cfg:schemas_cfg.MassPropertiesCfg,stage:Usd.Stage|None=None)->bool:"""Set properties for the mass of a rigid body prim. These properties are based on the `UsdPhysics.MassAPI` schema. If the mass is not defined, the density is used to compute the mass. However, in that case, a collision approximation of the rigid body is used to compute the density. For more information on the properties, please refer to the `documentation <https://openusd.org/release/wp_rigid_body_physics.html#body-mass-properties>`__. .. caution:: The mass of an object can be specified in multiple ways and have several conflicting settings that are resolved based on precedence. Please make sure to understand the precedence rules before using this property. .. 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.MassAPI: https://openusd.org/dev/api/class_usd_physics_mass_a_p_i.html Args: prim_path: The prim path of the rigid body. cfg: The configuration for the mass properties. stage: The stage where to find the prim. Defaults to None, in which case the current stage is used. Returns: True if the properties were successfully set, False otherwise. """# obtain stageifstageisNone:stage=stage_utils.get_current_stage()# get USD primrigid_prim=stage.GetPrimAtPath(prim_path)# check if prim has mass API applied on itifnotUsdPhysics.MassAPI(rigid_prim):returnFalse# retrieve the USD mass apiusd_physics_mass_api=UsdPhysics.MassAPI(rigid_prim)# convert to dictcfg=cfg.to_dict()# set into USD APIforattr_namein["mass","density"]:value=cfg.pop(attr_name,None)safe_set_attribute_on_usd_schema(usd_physics_mass_api,attr_name,value,camel_case=True)# successreturnTrue
"""Contact sensor."""
[docs]defactivate_contact_sensors(prim_path:str,threshold:float=0.0,stage:Usd.Stage=None):"""Activate the contact sensor on all rigid bodies under a specified prim path. This function adds the PhysX contact report API to all rigid bodies under the specified prim path. It also sets the force threshold beyond which the contact sensor reports the contact. The contact reporting API can only be added to rigid bodies. Args: prim_path: The prim path under which to search and prepare contact sensors. threshold: The threshold for the contact sensor. Defaults to 0.0. stage: The stage where to find the prim. Defaults to None, in which case the current stage is used. Raises: ValueError: If the input prim path is not valid. ValueError: If there are no rigid bodies under the prim path. """# obtain stageifstageisNone:stage=stage_utils.get_current_stage()# get primprim:Usd.Prim=stage.GetPrimAtPath(prim_path)# check if prim is validifnotprim.IsValid():raiseValueError(f"Prim path '{prim_path}' is not valid.")# iterate over all childrennum_contact_sensors=0all_prims=[prim]whilelen(all_prims)>0:# get current primchild_prim=all_prims.pop(0)# check if prim is a rigid body# nested rigid bodies are not allowed by SDK so we can safely assume that# if a prim has a rigid body API, it is a rigid body and we don't need to# check its childrenifchild_prim.HasAPI(UsdPhysics.RigidBodyAPI):# set sleep threshold to zerorb=PhysxSchema.PhysxRigidBodyAPI.Get(stage,prim.GetPrimPath())rb.CreateSleepThresholdAttr().Set(0.0)# add contact report API with threshold of zeroifnotchild_prim.HasAPI(PhysxSchema.PhysxContactReportAPI):omni.log.verbose(f"Adding contact report API to prim: '{child_prim.GetPrimPath()}'")cr_api=PhysxSchema.PhysxContactReportAPI.Apply(child_prim)else:omni.log.verbose(f"Contact report API already exists on prim: '{child_prim.GetPrimPath()}'")cr_api=PhysxSchema.PhysxContactReportAPI.Get(stage,child_prim.GetPrimPath())# set threshold to zerocr_api.CreateThresholdAttr().Set(threshold)# increment number of contact sensorsnum_contact_sensors+=1else:# add all children to treeall_prims+=child_prim.GetChildren()# check if no contact sensors were foundifnum_contact_sensors==0:raiseValueError(f"No contact sensors added to the prim: '{prim_path}'. This means that no rigid bodies"" are present under this prim. Please check the prim path.")# successreturnTrue
"""Joint drive properties."""
[docs]@apply_nesteddefmodify_joint_drive_properties(prim_path:str,cfg:schemas_cfg.JointDrivePropertiesCfg,stage:Usd.Stage|None=None)->bool:"""Modify PhysX parameters for a joint prim. This function checks if the input prim is a prismatic or revolute joint and applies the joint drive schema on it. If the joint is a tendon (i.e., it has the `PhysxTendonAxisAPI`_ schema applied on it), then the joint drive schema is not applied. Based on the configuration, this method modifies the properties of the joint drive. These properties are based on the `UsdPhysics.DriveAPI`_ schema. For more information on the properties, please refer to the official documentation. .. caution:: We highly recommend modifying joint properties of articulations through the functionalities in the :mod:`isaaclab.actuators` module. The methods here are for setting simulation low-level properties only. .. _UsdPhysics.DriveAPI: https://openusd.org/dev/api/class_usd_physics_drive_a_p_i.html .. _PhysxTendonAxisAPI: https://docs.omniverse.nvidia.com/kit/docs/omni_usd_schema_physics/104.2/class_physx_schema_physx_tendon_axis_a_p_i.html Args: prim_path: The prim path where to apply the joint drive schema. cfg: The configuration for the joint drive. stage: The stage where to find the prim. Defaults to None, in which case the current stage is used. Returns: True if the properties were successfully set, False otherwise. Raises: ValueError: If the input prim path is not valid. """# obtain stageifstageisNone:stage=stage_utils.get_current_stage()# get USD primprim=stage.GetPrimAtPath(prim_path)# check if prim path is validifnotprim.IsValid():raiseValueError(f"Prim path '{prim_path}' is not valid.")# check if prim has joint drive applied on itifprim.IsA(UsdPhysics.RevoluteJoint):drive_api_name="angular"elifprim.IsA(UsdPhysics.PrismaticJoint):drive_api_name="linear"else:returnFalse# check that prim is not a tendon child prim# note: root prim is what "controls" the tendon so we still want to apply the drive to itifprim.HasAPI(PhysxSchema.PhysxTendonAxisAPI)andnotprim.HasAPI(PhysxSchema.PhysxTendonAxisRootAPI):returnFalse# check if prim has joint drive applied on itusd_drive_api=UsdPhysics.DriveAPI(prim,drive_api_name)ifnotusd_drive_api:usd_drive_api=UsdPhysics.DriveAPI.Apply(prim,drive_api_name)# check if prim has Physx joint drive applied on itphysx_joint_api=PhysxSchema.PhysxJointAPI(prim)ifnotphysx_joint_api:physx_joint_api=PhysxSchema.PhysxJointAPI.Apply(prim)# mapping from configuration name to USD attribute namecfg_to_usd_map={"max_velocity":"max_joint_velocity","max_effort":"max_force","drive_type":"type",}# convert to dictcfg=cfg.to_dict()# check if linear driveis_linear_drive=prim.IsA(UsdPhysics.PrismaticJoint)# convert values for angular drives from radians to degrees unitsifnotis_linear_drive:ifcfg["max_velocity"]isnotNone:# rad / s --> deg / scfg["max_velocity"]=cfg["max_velocity"]*180.0/math.piifcfg["stiffness"]isnotNone:# N-m/rad --> N-m/degcfg["stiffness"]=cfg["stiffness"]*math.pi/180.0ifcfg["damping"]isnotNone:# N-m-s/rad --> N-m-s/degcfg["damping"]=cfg["damping"]*math.pi/180.0# set into PhysX APIforattr_namein["max_velocity"]:value=cfg.pop(attr_name,None)attr_name=cfg_to_usd_map[attr_name]safe_set_attribute_on_usd_schema(physx_joint_api,attr_name,value,camel_case=True)# set into USD APIforattr_name,attr_valueincfg.items():attr_name=cfg_to_usd_map.get(attr_name,attr_name)safe_set_attribute_on_usd_schema(usd_drive_api,attr_name,attr_value,camel_case=True)returnTrue
"""Fixed tendon properties."""
[docs]@apply_nesteddefmodify_fixed_tendon_properties(prim_path:str,cfg:schemas_cfg.FixedTendonPropertiesCfg,stage:Usd.Stage|None=None)->bool:"""Modify PhysX parameters for a fixed tendon attachment prim. A `fixed tendon`_ can be used to link multiple degrees of freedom of articulation joints through length and limit constraints. For instance, it can be used to set up an equality constraint between a driven and passive revolute joints. The schema comprises of attributes that belong to the `PhysxTendonAxisRootAPI`_ 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. .. _fixed tendon: https://nvidia-omniverse.github.io/PhysX/physx/5.4.1/_api_build/classPxArticulationFixedTendon.html .. _PhysxTendonAxisRootAPI: https://docs.omniverse.nvidia.com/kit/docs/omni_usd_schema_physics/104.2/class_physx_schema_physx_tendon_axis_root_a_p_i.html Args: prim_path: The prim path to the tendon attachment. cfg: The configuration for the tendon attachment. stage: The stage where to find the prim. Defaults to None, in which case the current stage is used. Returns: True if the properties were successfully set, False otherwise. Raises: ValueError: If the input prim path is not valid. """# obtain stageifstageisNone:stage=stage_utils.get_current_stage()# get USD primtendon_prim=stage.GetPrimAtPath(prim_path)# check if prim has fixed tendon applied on ithas_root_fixed_tendon=tendon_prim.HasAPI(PhysxSchema.PhysxTendonAxisRootAPI)ifnothas_root_fixed_tendon:returnFalse# resolve all available instances of the schema since it is multi-instanceforschema_nameintendon_prim.GetAppliedSchemas():# only consider the fixed tendon schemaif"PhysxTendonAxisRootAPI"notinschema_name:continue# retrieve the USD tendon apiinstance_name=schema_name.split(":")[-1]physx_tendon_axis_api=PhysxSchema.PhysxTendonAxisRootAPI(tendon_prim,instance_name)# convert to dictcfg=cfg.to_dict()# set into PhysX APIforattr_name,valueincfg.items():safe_set_attribute_on_usd_schema(physx_tendon_axis_api,attr_name,value,camel_case=True)# successreturnTrue
"""Deformable body properties."""
[docs]defdefine_deformable_body_properties(prim_path:str,cfg:schemas_cfg.DeformableBodyPropertiesCfg,stage:Usd.Stage|None=None):"""Apply the deformable body schema on the input prim and set its properties. See :func:`modify_deformable_body_properties` for more details on how the properties are set. .. note:: If the input prim is not a mesh, this function will traverse the prim and find the first mesh under it. If no mesh or multiple meshes are found, an error is raised. This is because the deformable body schema can only be applied to a single mesh. Args: prim_path: The prim path where to apply the deformable body schema. cfg: The configuration for the deformable body. 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. ValueError: When the prim has no mesh or multiple meshes. """# obtain stageifstageisNone:stage=stage_utils.get_current_stage()# get USD primprim=stage.GetPrimAtPath(prim_path)# check if prim path is validifnotprim.IsValid():raiseValueError(f"Prim path '{prim_path}' is not valid.")# traverse the prim and get the meshmatching_prims=get_all_matching_child_prims(prim_path,lambdap:p.GetTypeName()=="Mesh")# check if the mesh is validiflen(matching_prims)==0:raiseValueError(f"Could not find any mesh in '{prim_path}'. Please check asset.")iflen(matching_prims)>1:# get list of all meshes foundmesh_paths=[p.GetPrimPath()forpinmatching_prims]raiseValueError(f"Found multiple meshes in '{prim_path}': {mesh_paths}."" Deformable body schema can only be applied to one mesh.")# get deformable-body USD primmesh_prim=matching_prims[0]# check if prim has deformable-body applied on itifnotPhysxSchema.PhysxDeformableBodyAPI(mesh_prim):PhysxSchema.PhysxDeformableBodyAPI.Apply(mesh_prim)# set deformable body propertiesmodify_deformable_body_properties(mesh_prim.GetPrimPath(),cfg,stage)
[docs]@apply_nesteddefmodify_deformable_body_properties(prim_path:str,cfg:schemas_cfg.DeformableBodyPropertiesCfg,stage:Usd.Stage|None=None):"""Modify PhysX parameters for a deformable body prim. A `deformable body`_ is a single body that can be simulated by PhysX. Unlike rigid bodies, deformable bodies support relative motion of the nodes in the mesh. Consequently, they can be used to simulate deformations under applied forces. PhysX soft body simulation employs Finite Element Analysis (FEA) to simulate the deformations of the mesh. It uses two tetrahedral meshes to represent the deformable body: 1. **Simulation mesh**: This mesh is used for the simulation and is the one that is deformed by the solver. 2. **Collision mesh**: This mesh only needs to match the surface of the simulation mesh and is used for collision detection. For most applications, we assume that the above two meshes are computed from the "render mesh" of the deformable body. The render mesh is the mesh that is visible in the scene and is used for rendering purposes. It is composed of triangles and is the one that is used to compute the above meshes based on PhysX cookings. The schema comprises of attributes that belong to the `PhysxDeformableBodyAPI`_. schemas containing the PhysX parameters for the deformable body. .. caution:: The deformable body schema is still under development by the Omniverse team. The current implementation works with the PhysX schemas shipped with Isaac Sim 4.0.0 onwards. It may change in future releases. .. 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. .. _deformable body: https://nvidia-omniverse.github.io/PhysX/physx/5.4.1/docs/SoftBodies.html .. _PhysxDeformableBodyAPI: https://docs.omniverse.nvidia.com/kit/docs/omni_usd_schema_physics/104.2/class_physx_schema_physx_deformable_a_p_i.html Args: prim_path: The prim path to the deformable body. cfg: The configuration for the deformable body. stage: The stage where to find the prim. Defaults to None, in which case the current stage is used. Returns: True if the properties were successfully set, False otherwise. """# obtain stageifstageisNone:stage=stage_utils.get_current_stage()# get deformable-body USD primdeformable_body_prim=stage.GetPrimAtPath(prim_path)# check if the prim is valid and has the deformable-body APIifnotdeformable_body_prim.IsValid()ornotPhysxSchema.PhysxDeformableBodyAPI(deformable_body_prim):returnFalse# retrieve the physx deformable-body apiphysx_deformable_body_api=PhysxSchema.PhysxDeformableBodyAPI(deformable_body_prim)# retrieve the physx deformable apiphysx_deformable_api=PhysxSchema.PhysxDeformableAPI(physx_deformable_body_api)# convert to dictcfg=cfg.to_dict()# set into deformable body APIattr_kwargs={attr_name:cfg.pop(attr_name)forattr_namein["kinematic_enabled","collision_simplification","collision_simplification_remeshing","collision_simplification_remeshing_resolution","collision_simplification_target_triangle_count","collision_simplification_force_conforming","simulation_hexahedral_resolution","solver_position_iteration_count","vertex_velocity_damping","sleep_damping","sleep_threshold","settling_threshold","self_collision","self_collision_filter_distance",]}status=deformable_utils.add_physx_deformable_body(stage,prim_path=prim_path,**attr_kwargs)# check if the deformable body was successfully addedifnotstatus:returnFalse# obtain the PhysX collision API (this is set when the deformable body is added)physx_collision_api=PhysxSchema.PhysxCollisionAPI(deformable_body_prim)# set into PhysX APIforattr_name,valueincfg.items():ifattr_namein["rest_offset","contact_offset"]:safe_set_attribute_on_usd_schema(physx_collision_api,attr_name,value,camel_case=True)else:safe_set_attribute_on_usd_schema(physx_deformable_api,attr_name,value,camel_case=True)# successreturnTrue