From e5e82507a9411ab8bbb684224a006488f6e7c4d4 Mon Sep 17 00:00:00 2001 From: Mayank Mittal Date: Tue, 8 Aug 2023 20:38:05 +0200 Subject: [PATCH] adds schemas --- .../omni/isaac/orbit/sim/schemas/__init__.py | 45 ++++ .../omni/isaac/orbit/sim/schemas/schemas.py | 143 ++++++++++++ .../isaac/orbit/sim/schemas/schemas_cfg.py | 158 +++++++++++++ .../omni/isaac/orbit/sim/utils.py | 145 ++++++++++++ .../omni.isaac.orbit/test/sim/test_schemas.py | 214 ++++++++++++++++++ 5 files changed, 705 insertions(+) create mode 100644 source/extensions/omni.isaac.orbit/omni/isaac/orbit/sim/schemas/__init__.py create mode 100644 source/extensions/omni.isaac.orbit/omni/isaac/orbit/sim/schemas/schemas.py create mode 100644 source/extensions/omni.isaac.orbit/omni/isaac/orbit/sim/schemas/schemas_cfg.py create mode 100644 source/extensions/omni.isaac.orbit/omni/isaac/orbit/sim/utils.py create mode 100644 source/extensions/omni.isaac.orbit/test/sim/test_schemas.py diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/sim/schemas/__init__.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/sim/schemas/__init__.py new file mode 100644 index 0000000000..95b09bf80b --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/sim/schemas/__init__.py @@ -0,0 +1,45 @@ +# Copyright [2023] Boston Dynamics AI Institute, Inc. +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Sub-module containing utilities for schemas used in Omniverse. + +We wrap the USD schemas for PhysX and USD Physics in a more convenient API for setting the parameters from +Python. This is done so that configuration objects can define the schema properties to set and make it easier +to tune the physics parameters without requiring to open Omniverse Kit and manually set the parameters into +the respective USD attributes. + +.. caution:: + + Schema properties cannot be applied on prims that are prototypes as they are read-only prims. This + particularly affects instanced assets where some of the prims (usually the visual and collision meshes) + are prototypes so that the instancing can be done efficiently. In such cases, it is assumed that the + prototypes have sim-ready properties on them that don't need to be modified. + +The schemas are defined in the following files: + +* ``_isaac_sim/kit/extsPhysics/omni.usd.schema.physics/plugins/UsdPhysics/resources/UsdPhysics/schema.usda`` +* ``_isaac_sim/kit/extsPhysics/omni.usd.schema.physx/plugins/PhysxSchema/resources/PhysxSchema/schema.usda`` + +These can also be obtained online from the following links: + +* `UsdPhysics schema `_ +* `PhysxSchema schema `_ + +""" + +from .schemas_cfg import ( + ArticulationRootPropertiesCfg, + CollisionPropertiesCfg, + MassPropertiesCfg, + RigidBodyPropertiesCfg, +) + +__all__ = [ + "ArticulationRootPropertiesCfg", + "RigidBodyPropertiesCfg", + "CollisionPropertiesCfg", + "MassPropertiesCfg", +] diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/sim/schemas/schemas.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/sim/schemas/schemas.py new file mode 100644 index 0000000000..aedbf55d63 --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/sim/schemas/schemas.py @@ -0,0 +1,143 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +from typing import TYPE_CHECKING + +import omni.isaac.core.utils.prims as prim_utils +from pxr import PhysxSchema, UsdPhysics + +from ..utils import apply_nested, safe_set_attribute_on_usd_schema + +if TYPE_CHECKING: + from . import schemas_cfg + + +@apply_nested +def set_articulation_root_properties(prim_path: str, cfg: schemas_cfg.ArticulationRootPropertiesCfg): + """Set PhysX parameters for an articulation root prim. + + Args: + prim_path (str): The prim path to the articulation root. + cfg (schemas_cfg.ArticulationRootPropertiesCfg): The configuration for the articulation root. + + Raises: + ValueError: When no articulation schema found at specified articulation path. + """ + # get articulation USD prim + articulation_prim = prim_utils.get_prim_at_path(prim_path) + # check if prim has articulation applied on it + if not UsdPhysics.ArticulationRootAPI(articulation_prim): + raise ValueError(f"No articulation schema present for prim '{prim_path}'.") + # retrieve the articulation api + physx_articulation_api = PhysxSchema.PhysxArticulationAPI(articulation_prim) + if not physx_articulation_api: + physx_articulation_api = PhysxSchema.PhysxArticulationAPI.Apply(articulation_prim) + + # convert to dict + cfg = cfg.to_dict() + del cfg["func"] + # set into physx api + for attr_name, value in cfg.items(): + safe_set_attribute_on_usd_schema(physx_articulation_api, attr_name, value) + + +@apply_nested +def set_rigid_body_properties(prim_path: str, cfg: schemas_cfg.RigidBodyPropertiesCfg): + """Set PhysX parameters for a rigid body prim. + + Args: + prim_path (str): The prim path to the rigid body. + cfg (schemas_cfg.RigidBodyPropertiesCfg): The configuration for the rigid body. + + Raises: + ValueError: When no rigid-body schema found at specified prim path. + """ + # get rigid-body USD prim + rigid_body_prim = prim_utils.get_prim_at_path(prim_path) + # check if prim has rigid-body applied on it + if not UsdPhysics.RigidBodyAPI(rigid_body_prim): + raise ValueError(f"No rigid body schema present for prim '{prim_path}'.") + # retrieve the USD rigid-body api + usd_rigid_body_api = UsdPhysics.RigidBodyAPI(rigid_body_prim) + # retrieve the physx rigid-body api + physx_rigid_body_api = PhysxSchema.PhysxRigidBodyAPI(rigid_body_prim) + if not physx_rigid_body_api: + physx_rigid_body_api = PhysxSchema.PhysxRigidBodyAPI.Apply(rigid_body_prim) + + # convert to dict + cfg = cfg.to_dict() + del cfg["func"] + # set into USD API + for attr_name in ["rigid_body_enabled", "kinematic_enabled"]: + value = cfg.pop(attr_name, None) + safe_set_attribute_on_usd_schema(usd_rigid_body_api, attr_name, value) + # set into PhysX API + for attr_name, value in cfg.items(): + safe_set_attribute_on_usd_schema(physx_rigid_body_api, attr_name, value) + + +@apply_nested +def set_collision_properties(prim_path: str, cfg: schemas_cfg.CollisionPropertiesCfg): + """Set PhysX properties of collider prim. + + Args: + prim_path (str): The prim path of parent. + cfg (schemas_cfg.CollisionPropertiesCfg): The configuration for the collider. + + Raises: + ValueError: When no collision schema found at specified prim path. + """ + # get USD prim + collider_prim = prim_utils.get_prim_at_path(prim_path) + # check if prim has collision applied on it + if not UsdPhysics.CollisionAPI(collider_prim): + raise ValueError(f"No collider schema present for prim '{prim_path}'.") + # retrieve the USD rigid-body api + usd_collision_api = UsdPhysics.CollisionAPI(collider_prim) + # retrieve the collision api + physx_collision_api = PhysxSchema.PhysxCollisionAPI(collider_prim) + if not physx_collision_api: + physx_collision_api = PhysxSchema.PhysxCollisionAPI.Apply(collider_prim) + + # convert to dict + cfg = cfg.to_dict() + del cfg["func"] + # set into USD API + for attr_name in ["collision_enabled"]: + value = cfg.pop(attr_name, None) + safe_set_attribute_on_usd_schema(usd_collision_api, attr_name, value) + # set into PhysX API + for attr_name, value in cfg.items(): + safe_set_attribute_on_usd_schema(physx_collision_api, attr_name, value) + + +@apply_nested +def set_mass_properties(prim_path: str, cfg: schemas_cfg.MassPropertiesCfg): + """Set properties for the mass of a rigid body prim. + + Args: + prim_path (str): The prim path of the rigid body. + cfg (schemas_cfg.MassPropertiesCfg): The configuration for the mass properties. + + Raises: + ValueError: When no mass schema found at specified prim path. + """ + # get USD prim + rigid_prim = prim_utils.get_prim_at_path(prim_path) + # check if prim has collision applied on it + if not UsdPhysics.MassAPI(rigid_prim): + raise ValueError(f"No mass schema present for prim '{prim_path}'.") + # retrieve the USD rigid-body api + usd_physics_mass_api = UsdPhysics.MassAPI(rigid_prim) + + # convert to dict + cfg = cfg.to_dict() + del cfg["func"] + # set into USD API + for attr_name in ["mass", "density"]: + value = cfg.pop(attr_name, None) + safe_set_attribute_on_usd_schema(usd_physics_mass_api, attr_name, value) diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/sim/schemas/schemas_cfg.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/sim/schemas/schemas_cfg.py new file mode 100644 index 0000000000..2721e92381 --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/sim/schemas/schemas_cfg.py @@ -0,0 +1,158 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +from typing import Callable + +from omni.isaac.orbit.utils import configclass + +from . import schemas + + +@configclass +class ArticulationRootPropertiesCfg: + """Properties to apply to articulation. + + These properties are based on the `PhysxSchema.PhysxArticulationAPI`_ schema. For more information on the + properties, please refer to the official documentation. + + Note: + The properties are applied to the articulation root prim. The common properties (such as solver position + and velocity iteration counts, sleep threshold, stabilization threshold) are applied to all the + rigid bodies in the articulation. + + .. PhysxSchema.PhysxArticulationAPI: https://docs.omniverse.nvidia.com/kit/docs/omni_usd_schema_physics/104.2/class_physx_schema_physx_articulation_a_p_i.html + """ + + func: Callable = schemas.set_articulation_root_properties + """The function to call to apply the properties.""" + + articulation_enabled: bool | None = None + """Whether to enable or disable articulation.""" + enabled_self_collisions: bool | None = None + """Whether to enable or disable self-collisions.""" + solver_position_iteration_count: int | None = None + """Solver position iteration counts for the body.""" + solver_velocity_iteration_count: int | None = None + """Solver position iteration counts for the body.""" + sleep_threshold: float | None = None + """Mass-normalized kinetic energy threshold below which an actor may go to sleep.""" + stabilization_threshold: float | None = None + """The mass-normalized kinetic energy threshold below which an articulation may participate in stabilization.""" + + +@configclass +class RigidBodyPropertiesCfg: + """Properties to apply to a rigid body. + + These properties are based on the `UsdPhysics.RigidBodyAPI` and `PhysxSchema.PhysxRigidBodyAPI`_ schemas. + For more information on the properties, please refer to the official documentation. + + .. UsdPhysics.RigidBodyAPI: https://openusd.org/dev/api/class_usd_physics_rigid_body_a_p_i.html + .. PhysxSchema.PhysxRigidBodyAPI: https://docs.omniverse.nvidia.com/kit/docs/omni_usd_schema_physics/104.2/class_physx_schema_physx_rigid_body_a_p_i.html + """ + + func: Callable = schemas.set_rigid_body_properties + """The function to call to apply the properties.""" + + rigid_body_enabled: bool | None = None + """Whether to enable or disable the rigid body.""" + kinematic_enabled: bool | None = None + """Determines whether the body is kinematic or not. + + A kinematic body is a body that is moved through animated poses or through user defined poses. The simulation + still derives velocities for the kinematic body based on the external motion. + + For more information on kinematic bodies, please refer to the `documentation `_. + """ + disable_gravity: bool | None = False + """Disable gravity for the actor. Defaults to False.""" + linear_damping: float | None = None + """Linear damping for the body.""" + angular_damping: float | None = None + """Angular damping for the body.""" + max_linear_velocity: float | None = 1000.0 + """Maximum linear velocity for rigid bodies (in m/s). Defaults to 1000.0.""" + max_angular_velocity: float | None = 1000.0 + """Maximum angular velocity for rigid bodies (in rad/s). Defaults to 1000.0.""" + max_depenetration_velocity: float | None = 10.0 + """Maximum depenetration velocity permitted to be introduced by the solver (in m/s). + Defaults to 10.0.""" + max_contact_impulse: float | None = None + """The limit on the impulse that may be applied at a contact.""" + enable_gyroscopic_forces: bool | None = None + """Enables computation of gyroscopic forces on the rigid body.""" + retain_accelerations: bool | None = None + """Carries over forces/accelerations over sub-steps.""" + solver_position_iteration_count: int | None = None + """Solver position iteration counts for the body.""" + solver_velocity_iteration_count: int | None = None + """Solver position iteration counts for the body.""" + sleep_threshold: float | None = None + """Mass-normalized kinetic energy threshold below which an actor may go to sleep.""" + stabilization_threshold: float | None = None + """The mass-normalized kinetic energy threshold below which an actor may participate in stabilization.""" + + +@configclass +class CollisionPropertiesCfg: + """Properties to apply to colliders in a rigid body. + + These properties are based on the `UsdPhysics.CollisionAPI` and `PhysxSchema.PhysxCollisionAPI`_ schemas. + For more information on the properties, please refer to the official documentation. + + .. 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 + """ + + func: Callable = schemas.set_collision_properties + """The function to call to apply the properties.""" + + collision_enabled: bool | None = None + """Whether to enable or disable collisions.""" + contact_offset: float | None = None + """Contact offset for the collision shape (in m).""" + rest_offset: float | None = None + """Rest offset for the collision shape (in m).""" + torsional_patch_radius: float | None = None + """Radius of the contact patch for applying torsional friction (in m).""" + min_torsional_patch_radius: float | None = None + """Minimum radius of the contact patch for applying torsional friction (in m).""" + + +@configclass +class MassPropertiesCfg: + """Properties to define explicit mass properties of a rigid body. + + 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 `_. + + .. 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. + + .. UsdPhysics.MassAPI: https://openusd.org/dev/api/class_usd_physics_mass_a_p_i.html + """ + + func: Callable = schemas.set_mass_properties + """The function to call to apply the properties.""" + + mass: float | None = None + """The mass of the rigid body (in kg). + + Note: + If non-zero, the mass is ignored and the density is used to compute the mass. + """ + density: float | None = None + """The density of the rigid body (in kg/m^3). + + The density indirectly defines the mass of the rigid body. It is generally computed using the collision + approximation of the body. + """ diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/sim/utils.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/sim/utils.py new file mode 100644 index 0000000000..a59948bcd3 --- /dev/null +++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/sim/utils.py @@ -0,0 +1,145 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import functools +from typing import Any, Callable + +import carb +import omni.isaac.core.utils.prims as prim_utils +import omni.kit.commands +from pxr import Sdf, Usd + +from omni.isaac.orbit.utils.string import to_camel_case + + +def safe_set_attribute_on_usd_schema( + schema_api: Usd.APISchemaBase, attr_name: str, value: Any, camel_case: bool = True +): + """Set the value of an attribute on its USD schema if it exists. + + A USD API schema serves as an interface or API for authoring and extracting a set of attributes. + They typically derive from the :class:`pxr.Usd.SchemaBase` class. This function checks if the + + Args: + schema_api (Usd.APISchemaBase): The USD schema to set the attribute on. + attr_name (str): The name of the attribute. + value (Any): The value to set the attribute to. + camel_case (bool, optional): Whether to convert the attribute name to camel case. Defaults to True. + + Raises: + TypeError: When the input attribute name does not exist on the provided schema API. + """ + # if value is None, do nothing + if value is None: + return + # convert attribute name to camel case + if camel_case: + attr_name = to_camel_case(attr_name, to="CC") + # retrieve the attribute + # reference: https://openusd.org/dev/api/_usd__page__common_idioms.html#Usd_Create_Or_Get_Property + attr = getattr(schema_api, f"Create{attr_name}Attr", None) + # check if attribute exists + if attr is not None: + attr().Set(value) + else: + # think: do we ever need to create the attribute if it doesn't exist? + # currently, we are not doing this since the schemas are already created with some defaults. + carb.log_error(f"Attribute '{attr_name}' does not exist on prim '{schema_api.GetPath()}'.") + raise TypeError(f"Attribute '{attr_name}' does not exist on prim '{schema_api.GetPath()}'.") + + +def safe_set_attribute_on_usd_prim(prim: Usd.Prim, attr_name: str, value: Any, camel_case: bool = True): + """Set the value of a attribute on its USD prim. + + The function creates a new attribute if it does not exist on the prim. This is because in some cases (such + as with shaders), their attributes are not exposed as USD prim properties that can be altered. This function + allows us to set the value of the attributes in these cases. + + Args: + prim (Usd.Prim): The USD prim to set the attribute on. + attr_name (str): The name of the attribute. + value (Any): The value to set the attribute to. + camel_case (bool, optional): Whether to convert the attribute name to camel case. Defaults to True. + """ + # if value is None, do nothing + if value is None: + return + # convert attribute name to camel case + if camel_case: + attr_name = to_camel_case(attr_name, to="cC") + # resolve sdf type based on value + if isinstance(value, bool): + sdf_type = Sdf.ValueTypeNames.Bool + elif isinstance(value, int): + sdf_type = Sdf.ValueTypeNames.Int + elif isinstance(value, float): + sdf_type = Sdf.ValueTypeNames.Float + elif isinstance(value, (tuple, list)) and len(value) == 3 and any(isinstance(v, float) for v in value): + sdf_type = Sdf.ValueTypeNames.Float3 + elif isinstance(value, (tuple, list)) and len(value) == 2 and any(isinstance(v, float) for v in value): + sdf_type = Sdf.ValueTypeNames.Float2 + else: + raise NotImplementedError( + f"Cannot set attribute '{attr_name}' with value '{value}'. Please modify the code to support this type." + ) + # change property + omni.kit.commands.execute( + "ChangePropertyCommand", + prop_path=Sdf.Path(f"{prim.GetPath()}.{attr_name}"), + value=value, + prev=None, + type_to_create_if_not_exist=sdf_type, + ) + + +def apply_nested(func: Callable) -> Callable: + """Decorator to apply a function to all prims under a specified prim-path. + + The function iterates over the provided prim path and all its children to apply input function + to all prims under the specified prim path. + + If the function succeeds to apply to a prim, it will not look at the children of that prim. + This is based on the assumption that nested schemas are not allowed. For example, a parent prim + and its child prim cannot both have a rigid-body schema applied on them. + + Args: + func (Callable): The function to apply to all prims under a specified prim-path. The function + must take the prim-path as the first argument and the configuration as the second argument. + + Returns: + Callable: The wrapped function that applies the function to all prims under a specified prim-path. + """ + + @functools.wraps(func) + def wrapper(prim_path: str, cfg: object): + # get USD prim + prim = prim_utils.get_prim_at_path(prim_path) + # check if prim is valid + if not prim.IsValid(): + raise ValueError(f"Prim at path '{prim_path}' is not valid.") + # iterate over all prims under prim-path + all_prims = [prim] + while len(all_prims) > 0: + # get current prim + child_prim = all_prims.pop(0) + child_prim_path = prim_utils.get_prim_path(child_prim) + # check if prim is a prototype + # note: we prefer throwing a warning instead of ignoring the prim since the user may + # have intended to set properties on the prototype prim. + if child_prim.IsInstance(): + carb.log_warn(f"Cannot perform '{func.__name__}' on instanced prim: '{child_prim_path}'") + continue + # check if successful + is_successful = True + # set properties + try: + func(child_prim_path, cfg) + except ValueError: + is_successful = False + # if not successful, look at children + if not is_successful: + all_prims += child_prim.GetChildren() + + return wrapper diff --git a/source/extensions/omni.isaac.orbit/test/sim/test_schemas.py b/source/extensions/omni.isaac.orbit/test/sim/test_schemas.py new file mode 100644 index 0000000000..c10020cba0 --- /dev/null +++ b/source/extensions/omni.isaac.orbit/test/sim/test_schemas.py @@ -0,0 +1,214 @@ +# Copyright (c) 2022, NVIDIA CORPORATION & AFFILIATES, ETH Zurich, and University of Toronto +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Launch Isaac Sim Simulator first.""" + +from omni.isaac.orbit.app import AppLauncher + +# launch omniverse app +simulation_app = AppLauncher(headless=True).app + +"""Rest everything follows.""" + +import traceback +import unittest + +import carb +import omni.isaac.core.utils.prims as prim_utils +import omni.isaac.core.utils.stage as stage_utils +from omni.isaac.core.simulation_context import SimulationContext +from pxr import UsdPhysics + +import omni.isaac.orbit.sim as sim_utils +from omni.isaac.orbit.utils.assets import ISAAC_NUCLEUS_DIR +from omni.isaac.orbit.utils.string import to_camel_case + + +class TestPhysicsSchema(unittest.TestCase): + """Test fixture for checking schemas modifications through Orbit.""" + + def setUp(self) -> None: + """Create a blank new stage for each test.""" + # Simulation time-step + self.dt = 0.1 + # Load kit helper + self.sim = SimulationContext(physics_dt=self.dt, rendering_dt=self.dt, backend="numpy") + # Set some default values for test + self.arti_cfg = sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, + articulation_enabled=True, + solver_position_iteration_count=4, + solver_velocity_iteration_count=1, + sleep_threshold=0.0, + stabilization_threshold=0.0, + ) + self.rigid_cfg = sim_utils.RigidBodyPropertiesCfg( + rigid_body_enabled=True, + kinematic_enabled=False, + disable_gravity=False, + linear_damping=0.0, + angular_damping=0.0, + max_linear_velocity=1000.0, + max_angular_velocity=1000.0, + max_depenetration_velocity=10.0, + max_contact_impulse=10.0, + enable_gyroscopic_forces=True, + retain_accelerations=True, + solver_position_iteration_count=8, + solver_velocity_iteration_count=1, + sleep_threshold=0.0, + stabilization_threshold=0.0, + ) + self.collision_cfg = sim_utils.CollisionPropertiesCfg( + collision_enabled=True, + contact_offset=0.05, + rest_offset=0.001, + min_torsional_patch_radius=0.1, + torsional_patch_radius=1.0, + ) + self.mass_cfg = sim_utils.MassPropertiesCfg(mass=1.0) + # Wait for spawning + stage_utils.update_stage() + + def tearDown(self) -> None: + """Stops simulator after each test.""" + # stop simulation + self.sim.stop() + self.sim.clear() + + def test_valid_properties_cfg(self): + """Test that all the config instances have non-None values. + + This is to ensure that we check that all the properties of the schema are set. + """ + for cfg in [self.arti_cfg, self.rigid_cfg, self.collision_cfg]: + # check nothing is none + for k, v in cfg.__dict__.items(): + self.assertIsNotNone(v, f"{cfg.__class__.__name__}:{k} is None. Please make sure schemas are valid.") + + def test_set_properties_on_invalid_prim(self): + """Test setting properties on a prim that does not exist.""" + # set properties + with self.assertRaises(ValueError): + self.rigid_cfg.func("/World/asset_xyz", self.rigid_cfg) + + def test_set_properties_on_articulation_instanced_usd(self): + """Test setting properties on articulation instanced usd. + + In this case, setting collision properties on the articulation instanced usd will fail. + """ + # spawn asset to the stage + asset_usd_file = f"{ISAAC_NUCLEUS_DIR}/Robots/ANYbotics/anymal_instanceable.usd" + prim_utils.create_prim("/World/asset_instanced", usd_path=asset_usd_file, translation=(0.0, 0.0, 0.62)) + + # set properties on the asset and check all properties are set + for cfg in [self.arti_cfg, self.rigid_cfg, self.mass_cfg]: + cfg.func("/World/asset_instanced", cfg) + + # check articulation properties are set correctly + # note: we exploit the hierarchy in the asset to check + root_prim = prim_utils.get_prim_at_path("/World/asset_instanced") + for attr_name, attr_value in self.arti_cfg.__dict__.items(): + # skip names we know are not present + if attr_name in ["func"]: + continue + # convert attribute name in prim to cfg name + prim_prop_name = f"physxArticulation:{to_camel_case(attr_name, to='cC')}" + # validate the values + self.assertEqual(root_prim.GetAttribute(prim_prop_name).Get(), attr_value) + # check rigid body properties are set correctly + # note: we exploit the hierarchy in the asset to check + root_prim = prim_utils.get_prim_at_path("/World/asset_instanced") + for link_prim in root_prim.GetChildren(): + if UsdPhysics.RigidBodyAPI(link_prim): + for attr_name, attr_value in self.rigid_cfg.__dict__.items(): + # skip names we know are not present + if attr_name in ["func", "rigid_body_enabled", "kinematic_enabled"]: + continue + # convert attribute name in prim to cfg name + prim_prop_name = f"physxRigidBody:{to_camel_case(attr_name, to='cC')}" + # validate the values + self.assertEqual(link_prim.GetAttribute(prim_prop_name).Get(), attr_value) + else: + print(f"Skipping prim {link_prim.GetPrimPath()} as it is not a rigid body.") + # check rigid body properties are set correctly + # note: we exploit the hierarchy in the asset to check + root_prim = prim_utils.get_prim_at_path("/World/asset_instanced") + for link_prim in root_prim.GetChildren(): + if UsdPhysics.MassAPI(link_prim): + for attr_name, attr_value in self.rigid_cfg.__dict__.items(): + # skip names we know are not present + if attr_name in ["func"]: + continue + # convert attribute name in prim to cfg name + print(attr_name, link_prim.GetPrimPath()) + # print(link_prim.GetProperties()) + prim_prop_name = f"physics:{to_camel_case(attr_name, to='cC')}" + # validate the values + self.assertEqual(link_prim.GetAttribute(prim_prop_name).Get(), attr_value) + else: + print(f"Skipping prim {link_prim.GetPrimPath()} as it is not a mass api.") + + + def test_set_properties_on_articulation_usd(self): + """Test setting properties on articulation usd.""" + # spawn asset to the stage + asset_usd_file = f"{ISAAC_NUCLEUS_DIR}/Robots/Franka/franka.usd" + prim_utils.create_prim("/World/asset", usd_path=asset_usd_file, translation=(0.0, 0.0, 0.62)) + + # set properties on the asset and check all properties are set + for cfg in [self.arti_cfg, self.rigid_cfg, self.collision_cfg]: + cfg.func("/World/asset", cfg) + + # check articulation properties are set correctly + root_prim = prim_utils.get_prim_at_path("/World/asset") + for attr_name, attr_value in self.arti_cfg.__dict__.items(): + # skip names we know are not present + if attr_name in ["func"]: + continue + # convert attribute name in prim to cfg name + prim_prop_name = f"physxArticulation:{to_camel_case(attr_name, to='cC')}" + # validate the values + self.assertEqual(root_prim.GetAttribute(prim_prop_name).Get(), attr_value) + # check rigid body properties are set correctly + root_prim = prim_utils.get_prim_at_path("/World/asset") + for link_prim in root_prim.GetChildren(): + if UsdPhysics.RigidBodyAPI(link_prim): + for attr_name, attr_value in self.rigid_cfg.__dict__.items(): + # skip names we know are not present + if attr_name in ["func", "rigid_body_enabled", "kinematic_enabled"]: + continue + # convert attribute name in prim to cfg name + prim_prop_name = f"physxRigidBody:{to_camel_case(attr_name, to='cC')}" + # validate the values + self.assertEqual(link_prim.GetAttribute(prim_prop_name).Get(), attr_value) + else: + print(f"Skipping prim {link_prim.GetPrimPath()} as it is not a rigid body.") + # check collision properties are set correctly + # note: we exploit the hierarchy in the asset to check + root_prim = prim_utils.get_prim_at_path("/World/asset") + for link_prim in root_prim.GetChildren(): + for mesh_prim in link_prim.GetChildren(): + if UsdPhysics.CollisionAPI(mesh_prim): + for attr_name, attr_value in self.collision_cfg.__dict__.items(): + # skip names we know are not present + if attr_name in ["func", "collision_enabled"]: + continue + # convert attribute name in prim to cfg name + prim_prop_name = f"physxCollision:{to_camel_case(attr_name, to='cC')}" + # validate the values + self.assertEqual(mesh_prim.GetAttribute(prim_prop_name).Get(), attr_value) + + +if __name__ == "__main__": + try: + unittest.main() + except Exception as err: + carb.log_error(err) + carb.log_error(traceback.format_exc()) + raise + finally: + # close sim app + simulation_app.close()