-
Notifications
You must be signed in to change notification settings - Fork 857
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
- Loading branch information
Showing
5 changed files
with
705 additions
and
0 deletions.
There are no files selected for viewing
45 changes: 45 additions & 0 deletions
45
source/extensions/omni.isaac.orbit/omni/isaac/orbit/sim/schemas/__init__.py
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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 <https://openusd.org/dev/api/usd_physics_page_front.html>`_ | ||
* `PhysxSchema schema <https://docs.omniverse.nvidia.com/kit/docs/omni_usd_schema_physics/104.2/index.html>`_ | ||
""" | ||
|
||
from .schemas_cfg import ( | ||
ArticulationRootPropertiesCfg, | ||
CollisionPropertiesCfg, | ||
MassPropertiesCfg, | ||
RigidBodyPropertiesCfg, | ||
) | ||
|
||
__all__ = [ | ||
"ArticulationRootPropertiesCfg", | ||
"RigidBodyPropertiesCfg", | ||
"CollisionPropertiesCfg", | ||
"MassPropertiesCfg", | ||
] |
143 changes: 143 additions & 0 deletions
143
source/extensions/omni.isaac.orbit/omni/isaac/orbit/sim/schemas/schemas.py
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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) |
158 changes: 158 additions & 0 deletions
158
source/extensions/omni.isaac.orbit/omni/isaac/orbit/sim/schemas/schemas_cfg.py
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -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 <https://openusd.org/release/wp_rigid_body_physics.html#kinematic-bodies>`_. | ||
""" | ||
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 <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. | ||
.. 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. | ||
""" |
Oops, something went wrong.