From d6644762d4267c3ee100dd076ef33920832ab51b Mon Sep 17 00:00:00 2001 From: Pascal Roth Date: Sun, 2 Nov 2025 12:45:27 +0100 Subject: [PATCH 1/5] copy first utils --- source/isaaclab/isaaclab/assets/asset_base.py | 4 +- .../isaaclab/isaaclab/controllers/rmp_flow.py | 3 +- source/isaaclab/isaaclab/envs/mdp/events.py | 2 +- .../isaaclab/envs/ui/base_env_window.py | 2 +- .../isaaclab/markers/visualization_markers.py | 4 +- .../isaaclab/scene/interactive_scene.py | 2 +- .../isaaclab/sensors/camera/camera.py | 2 +- source/isaaclab/isaaclab/sensors/imu/imu.py | 2 +- .../sensors/ray_caster/ray_caster_camera.py | 2 +- .../isaaclab/isaaclab/sensors/sensor_base.py | 2 +- .../isaaclab/isaaclab/sim/schemas/schemas.py | 3 +- .../isaaclab/sim/simulation_context.py | 2 +- .../sim/spawners/from_files/from_files.py | 6 +- .../isaaclab/sim/spawners/lights/lights.py | 2 +- .../sim/spawners/materials/__init__.py | 2 +- .../spawners/materials/physics_materials.py | 4 +- .../spawners/materials/visual_materials.py | 2 +- .../isaaclab/sim/spawners/meshes/meshes.py | 2 +- .../isaaclab/sim/spawners/sensors/sensors.py | 2 +- .../isaaclab/sim/spawners/shapes/shapes.py | 2 +- .../sim/spawners/wrappers/wrappers.py | 6 +- source/isaaclab/isaaclab/sim/utils.py | 5 +- source/isaaclab/isaaclab/terrains/utils.py | 2 +- .../ui/xr_widgets/instruction_widget.py | 3 +- source/isaaclab/isaaclab/utils/prims.py | 1022 +++++++++++++++++ source/isaaclab/isaaclab/utils/semantics.py | 475 ++++++++ source/isaaclab/isaaclab/utils/stage.py | 785 +++++++++++++ source/isaaclab/isaaclab/utils/string.py | 49 + source/isaaclab/isaaclab/utils/types.py | 27 + .../test/assets/check_fixed_base_assets.py | 3 +- .../isaaclab/test/assets/test_articulation.py | 2 +- .../test/assets/test_deformable_object.py | 2 +- .../isaaclab/test/assets/test_rigid_object.py | 2 +- .../assets/test_rigid_object_collection.py | 2 +- .../test/assets/test_surface_gripper.py | 2 +- .../test/controllers/test_differential_ik.py | 4 +- .../controllers/test_operational_space.py | 4 +- .../test/deps/isaacsim/check_camera.py | 3 +- .../check_floating_base_made_fixed.py | 5 +- .../deps/isaacsim/check_legged_robot_clone.py | 3 +- .../test/deps/isaacsim/check_ref_count.py | 3 +- .../isaacsim/check_rep_texture_randomizer.py | 3 +- .../markers/test_visualization_markers.py | 2 +- .../test/sensors/check_contact_sensor.py | 2 +- .../isaaclab/test/sensors/check_ray_caster.py | 2 +- source/isaaclab/test/sensors/test_camera.py | 4 +- .../test/sensors/test_frame_transformer.py | 2 +- source/isaaclab/test/sensors/test_imu.py | 2 +- .../test/sensors/test_multi_tiled_camera.py | 4 +- .../test/sensors/test_ray_caster_camera.py | 4 +- .../isaaclab/test/sensors/test_sensor_base.py | 4 +- .../test/sensors/test_tiled_camera.py | 5 +- source/isaaclab/test/sim/check_meshes.py | 3 +- .../test_build_simulation_context_headless.py | 2 +- ...st_build_simulation_context_nonheadless.py | 2 +- .../isaaclab/test/sim/test_mesh_converter.py | 4 +- .../isaaclab/test/sim/test_mjcf_converter.py | 4 +- source/isaaclab/test/sim/test_schemas.py | 4 +- .../test/sim/test_simulation_context.py | 2 +- .../test/sim/test_spawn_from_files.py | 4 +- source/isaaclab/test/sim/test_spawn_lights.py | 4 +- .../isaaclab/test/sim/test_spawn_materials.py | 4 +- source/isaaclab/test/sim/test_spawn_meshes.py | 4 +- .../isaaclab/test/sim/test_spawn_sensors.py | 4 +- source/isaaclab/test/sim/test_spawn_shapes.py | 4 +- .../isaaclab/test/sim/test_spawn_wrappers.py | 4 +- .../isaaclab/test/sim/test_stage_in_memory.py | 4 +- .../isaaclab/test/sim/test_urdf_converter.py | 4 +- source/isaaclab/test/sim/test_utils.py | 4 +- .../test/terrains/check_terrain_importer.py | 2 +- .../test/terrains/test_terrain_importer.py | 2 +- .../check_scene_xr_visualization.py | 3 +- .../franka_cabinet/franka_cabinet_env.py | 2 +- .../shadow_hand/shadow_hand_vision_env.py | 3 +- .../manipulation/dexsuite/mdp/utils.py | 2 +- 75 files changed, 2468 insertions(+), 102 deletions(-) create mode 100644 source/isaaclab/isaaclab/utils/prims.py create mode 100644 source/isaaclab/isaaclab/utils/semantics.py create mode 100644 source/isaaclab/isaaclab/utils/stage.py diff --git a/source/isaaclab/isaaclab/assets/asset_base.py b/source/isaaclab/isaaclab/assets/asset_base.py index a618ddc0e13..b5c7e269318 100644 --- a/source/isaaclab/isaaclab/assets/asset_base.py +++ b/source/isaaclab/isaaclab/assets/asset_base.py @@ -14,13 +14,13 @@ from collections.abc import Sequence from typing import TYPE_CHECKING, Any -import isaacsim.core.utils.prims as prim_utils import omni.kit.app import omni.timeline from isaacsim.core.simulation_manager import IsaacEvents, SimulationManager -from isaacsim.core.utils.stage import get_current_stage import isaaclab.sim as sim_utils +import isaaclab.utils.prims as prim_utils +from isaaclab.utils.stage import get_current_stage if TYPE_CHECKING: from .asset_base_cfg import AssetBaseCfg diff --git a/source/isaaclab/isaaclab/controllers/rmp_flow.py b/source/isaaclab/isaaclab/controllers/rmp_flow.py index b9ce875c390..c1c740a4924 100644 --- a/source/isaaclab/isaaclab/controllers/rmp_flow.py +++ b/source/isaaclab/isaaclab/controllers/rmp_flow.py @@ -6,13 +6,14 @@ import torch from dataclasses import MISSING -import isaacsim.core.utils.prims as prim_utils from isaacsim.core.api.simulation_context import SimulationContext from isaacsim.core.prims import SingleArticulation # enable motion generation extensions from isaacsim.core.utils.extensions import enable_extension +import isaaclab.utils.prims as prim_utils + enable_extension("isaacsim.robot_motion.lula") enable_extension("isaacsim.robot_motion.motion_generation") diff --git a/source/isaaclab/isaaclab/envs/mdp/events.py b/source/isaaclab/isaaclab/envs/mdp/events.py index 923fd1597ab..ac0014db310 100644 --- a/source/isaaclab/isaaclab/envs/mdp/events.py +++ b/source/isaaclab/isaaclab/envs/mdp/events.py @@ -22,7 +22,6 @@ import carb import omni.physics.tensors.impl.api as physx from isaacsim.core.utils.extensions import enable_extension -from isaacsim.core.utils.stage import get_current_stage from pxr import Gf, Sdf, UsdGeom, Vt import isaaclab.sim as sim_utils @@ -31,6 +30,7 @@ from isaaclab.assets import Articulation, DeformableObject, RigidObject from isaaclab.managers import EventTermCfg, ManagerTermBase, SceneEntityCfg from isaaclab.terrains import TerrainImporter +from isaaclab.utils.stage import get_current_stage from isaaclab.utils.version import compare_versions if TYPE_CHECKING: diff --git a/source/isaaclab/isaaclab/envs/ui/base_env_window.py b/source/isaaclab/isaaclab/envs/ui/base_env_window.py index 6744238b5a9..f6ecf74cbe6 100644 --- a/source/isaaclab/isaaclab/envs/ui/base_env_window.py +++ b/source/isaaclab/isaaclab/envs/ui/base_env_window.py @@ -15,10 +15,10 @@ import omni.kit.app import omni.kit.commands import omni.usd -from isaacsim.core.utils.stage import get_current_stage from pxr import PhysxSchema, Sdf, Usd, UsdGeom, UsdPhysics from isaaclab.ui.widgets import ManagerLiveVisualizer +from isaaclab.utils.stage import get_current_stage if TYPE_CHECKING: import omni.ui diff --git a/source/isaaclab/isaaclab/markers/visualization_markers.py b/source/isaaclab/isaaclab/markers/visualization_markers.py index ce4611289bc..c43831fdebd 100644 --- a/source/isaaclab/isaaclab/markers/visualization_markers.py +++ b/source/isaaclab/isaaclab/markers/visualization_markers.py @@ -23,18 +23,18 @@ import torch from dataclasses import MISSING -import isaacsim.core.utils.stage as stage_utils import omni.kit.commands import omni.log import omni.physx.scripts.utils as physx_utils -from isaacsim.core.utils.stage import get_current_stage from pxr import Gf, PhysxSchema, Sdf, Usd, UsdGeom, UsdPhysics, Vt import isaaclab.sim as sim_utils +import isaaclab.utils.stage as stage_utils from isaaclab.sim.spawners import SpawnerCfg from isaaclab.sim.utils import attach_stage_to_usd_context from isaaclab.utils.configclass import configclass from isaaclab.utils.math import convert_quat +from isaaclab.utils.stage import get_current_stage @configclass diff --git a/source/isaaclab/isaaclab/scene/interactive_scene.py b/source/isaaclab/isaaclab/scene/interactive_scene.py index 15739c33ad7..75dc49308d8 100644 --- a/source/isaaclab/isaaclab/scene/interactive_scene.py +++ b/source/isaaclab/isaaclab/scene/interactive_scene.py @@ -12,7 +12,6 @@ import omni.usd from isaacsim.core.cloner import GridCloner from isaacsim.core.prims import XFormPrim -from isaacsim.core.utils.stage import get_current_stage from isaacsim.core.version import get_version from pxr import PhysxSchema @@ -34,6 +33,7 @@ from isaaclab.sim import SimulationContext from isaaclab.sim.utils import get_current_stage_id from isaaclab.terrains import TerrainImporter, TerrainImporterCfg +from isaaclab.utils.stage import get_current_stage from .interactive_scene_cfg import InteractiveSceneCfg diff --git a/source/isaaclab/isaaclab/sensors/camera/camera.py b/source/isaaclab/isaaclab/sensors/camera/camera.py index 8d3fe257df6..6fa7a7204f6 100644 --- a/source/isaaclab/isaaclab/sensors/camera/camera.py +++ b/source/isaaclab/isaaclab/sensors/camera/camera.py @@ -13,7 +13,6 @@ from typing import TYPE_CHECKING, Any, Literal import carb -import isaacsim.core.utils.stage as stage_utils import omni.kit.commands import omni.usd from isaacsim.core.prims import XFormPrim @@ -22,6 +21,7 @@ import isaaclab.sim as sim_utils import isaaclab.utils.sensors as sensor_utils +import isaaclab.utils.stage as stage_utils from isaaclab.utils import to_camel_case from isaaclab.utils.array import convert_to_torch from isaaclab.utils.math import ( diff --git a/source/isaaclab/isaaclab/sensors/imu/imu.py b/source/isaaclab/isaaclab/sensors/imu/imu.py index 1c700eeedb2..ef721fb38ac 100644 --- a/source/isaaclab/isaaclab/sensors/imu/imu.py +++ b/source/isaaclab/isaaclab/sensors/imu/imu.py @@ -9,12 +9,12 @@ from collections.abc import Sequence from typing import TYPE_CHECKING -import isaacsim.core.utils.stage as stage_utils from isaacsim.core.simulation_manager import SimulationManager from pxr import UsdPhysics import isaaclab.sim as sim_utils import isaaclab.utils.math as math_utils +import isaaclab.utils.stage as stage_utils from isaaclab.markers import VisualizationMarkers from ..sensor_base import SensorBase diff --git a/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster_camera.py b/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster_camera.py index 3bf8729b78b..704d1590f79 100644 --- a/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster_camera.py +++ b/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster_camera.py @@ -9,11 +9,11 @@ from collections.abc import Sequence from typing import TYPE_CHECKING, ClassVar, Literal -import isaacsim.core.utils.stage as stage_utils import omni.physics.tensors.impl.api as physx from isaacsim.core.prims import XFormPrim import isaaclab.utils.math as math_utils +import isaaclab.utils.stage as stage_utils from isaaclab.sensors.camera import CameraData from isaaclab.utils.warp import raycast_mesh diff --git a/source/isaaclab/isaaclab/sensors/sensor_base.py b/source/isaaclab/isaaclab/sensors/sensor_base.py index aab993cc526..eb8128c9afa 100644 --- a/source/isaaclab/isaaclab/sensors/sensor_base.py +++ b/source/isaaclab/isaaclab/sensors/sensor_base.py @@ -23,9 +23,9 @@ import omni.kit.app import omni.timeline from isaacsim.core.simulation_manager import IsaacEvents, SimulationManager -from isaacsim.core.utils.stage import get_current_stage import isaaclab.sim as sim_utils +from isaaclab.utils.stage import get_current_stage if TYPE_CHECKING: from .sensor_base_cfg import SensorBaseCfg diff --git a/source/isaaclab/isaaclab/sim/schemas/schemas.py b/source/isaaclab/isaaclab/sim/schemas/schemas.py index 482b6745842..ecc814e0555 100644 --- a/source/isaaclab/isaaclab/sim/schemas/schemas.py +++ b/source/isaaclab/isaaclab/sim/schemas/schemas.py @@ -10,10 +10,11 @@ import omni.log import omni.physx.scripts.utils as physx_utils -from isaacsim.core.utils.stage import get_current_stage from omni.physx.scripts import deformableUtils as deformable_utils from pxr import PhysxSchema, Usd, UsdPhysics +from isaaclab.utils.stage import get_current_stage + from ..utils import ( apply_nested, find_global_fixed_joint_prim, diff --git a/source/isaaclab/isaaclab/sim/simulation_context.py b/source/isaaclab/isaaclab/sim/simulation_context.py index 83277635acf..1a0bcf11965 100644 --- a/source/isaaclab/isaaclab/sim/simulation_context.py +++ b/source/isaaclab/isaaclab/sim/simulation_context.py @@ -21,7 +21,6 @@ import carb import flatdict -import isaacsim.core.utils.stage as stage_utils import omni.log import omni.physx import omni.usd @@ -32,6 +31,7 @@ from isaacsim.core.version import get_version from pxr import Gf, PhysxSchema, Sdf, Usd, UsdPhysics +import isaaclab.utils.stage as stage_utils from isaaclab.sim.utils import create_new_stage_in_memory, use_stage from .simulation_cfg import SimulationCfg diff --git a/source/isaaclab/isaaclab/sim/spawners/from_files/from_files.py b/source/isaaclab/isaaclab/sim/spawners/from_files/from_files.py index a3c8a44015a..3d72db01587 100644 --- a/source/isaaclab/isaaclab/sim/spawners/from_files/from_files.py +++ b/source/isaaclab/isaaclab/sim/spawners/from_files/from_files.py @@ -7,19 +7,18 @@ from typing import TYPE_CHECKING -import isaacsim.core.utils.prims as prim_utils import omni.kit.commands import omni.log from pxr import Gf, Sdf, Usd +import isaaclab.utils.prims as prim_utils + # from Isaac Sim 4.2 onwards, pxr.Semantics is deprecated try: import Semantics except ModuleNotFoundError: from pxr import Semantics -from isaacsim.core.utils.stage import get_current_stage - from isaaclab.sim import converters, schemas from isaaclab.sim.utils import ( bind_physics_material, @@ -29,6 +28,7 @@ select_usd_variants, ) from isaaclab.utils.assets import check_usd_path_with_timeout +from isaaclab.utils.stage import get_current_stage if TYPE_CHECKING: from . import from_files_cfg diff --git a/source/isaaclab/isaaclab/sim/spawners/lights/lights.py b/source/isaaclab/isaaclab/sim/spawners/lights/lights.py index dccd00f4bca..024dfd59ea6 100644 --- a/source/isaaclab/isaaclab/sim/spawners/lights/lights.py +++ b/source/isaaclab/isaaclab/sim/spawners/lights/lights.py @@ -7,9 +7,9 @@ from typing import TYPE_CHECKING -import isaacsim.core.utils.prims as prim_utils from pxr import Usd, UsdLux +import isaaclab.utils.prims as prim_utils from isaaclab.sim.utils import clone, safe_set_attribute_on_usd_prim if TYPE_CHECKING: diff --git a/source/isaaclab/isaaclab/sim/spawners/materials/__init__.py b/source/isaaclab/isaaclab/sim/spawners/materials/__init__.py index 966efec76b8..061d764e978 100644 --- a/source/isaaclab/isaaclab/sim/spawners/materials/__init__.py +++ b/source/isaaclab/isaaclab/sim/spawners/materials/__init__.py @@ -31,7 +31,7 @@ Usage: .. code-block:: python - import isaacsim.core.utils.prims as prim_utils + import isaaclab.utils.prims as prim_utils import isaaclab.sim as sim_utils diff --git a/source/isaaclab/isaaclab/sim/spawners/materials/physics_materials.py b/source/isaaclab/isaaclab/sim/spawners/materials/physics_materials.py index e8977a14fd2..b8ad02338bb 100644 --- a/source/isaaclab/isaaclab/sim/spawners/materials/physics_materials.py +++ b/source/isaaclab/isaaclab/sim/spawners/materials/physics_materials.py @@ -7,11 +7,11 @@ from typing import TYPE_CHECKING -import isaacsim.core.utils.prims as prim_utils -from isaacsim.core.utils.stage import get_current_stage from pxr import PhysxSchema, Usd, UsdPhysics, UsdShade +import isaaclab.utils.prims as prim_utils from isaaclab.sim.utils import clone, safe_set_attribute_on_usd_schema +from isaaclab.utils.stage import get_current_stage if TYPE_CHECKING: from . import physics_materials_cfg diff --git a/source/isaaclab/isaaclab/sim/spawners/materials/visual_materials.py b/source/isaaclab/isaaclab/sim/spawners/materials/visual_materials.py index 3c79f6f679e..2e827518628 100644 --- a/source/isaaclab/isaaclab/sim/spawners/materials/visual_materials.py +++ b/source/isaaclab/isaaclab/sim/spawners/materials/visual_materials.py @@ -7,11 +7,11 @@ from typing import TYPE_CHECKING -import isaacsim.core.utils.prims as prim_utils import omni.kit.commands import omni.log from pxr import Usd +import isaaclab.utils.prims as prim_utils from isaaclab.sim.utils import attach_stage_to_usd_context, clone, safe_set_attribute_on_usd_prim from isaaclab.utils.assets import NVIDIA_NUCLEUS_DIR diff --git a/source/isaaclab/isaaclab/sim/spawners/meshes/meshes.py b/source/isaaclab/isaaclab/sim/spawners/meshes/meshes.py index 17c23202ed6..f82567d1a3e 100644 --- a/source/isaaclab/isaaclab/sim/spawners/meshes/meshes.py +++ b/source/isaaclab/isaaclab/sim/spawners/meshes/meshes.py @@ -10,9 +10,9 @@ import trimesh.transformations from typing import TYPE_CHECKING -import isaacsim.core.utils.prims as prim_utils from pxr import Usd, UsdPhysics +import isaaclab.utils.prims as prim_utils from isaaclab.sim import schemas from isaaclab.sim.utils import bind_physics_material, bind_visual_material, clone diff --git a/source/isaaclab/isaaclab/sim/spawners/sensors/sensors.py b/source/isaaclab/isaaclab/sim/spawners/sensors/sensors.py index 0a385902a55..dfd8fc7b393 100644 --- a/source/isaaclab/isaaclab/sim/spawners/sensors/sensors.py +++ b/source/isaaclab/isaaclab/sim/spawners/sensors/sensors.py @@ -7,11 +7,11 @@ from typing import TYPE_CHECKING -import isaacsim.core.utils.prims as prim_utils import omni.kit.commands import omni.log from pxr import Sdf, Usd +import isaaclab.utils.prims as prim_utils from isaaclab.sim.utils import attach_stage_to_usd_context, clone from isaaclab.utils import to_camel_case diff --git a/source/isaaclab/isaaclab/sim/spawners/shapes/shapes.py b/source/isaaclab/isaaclab/sim/spawners/shapes/shapes.py index f4fa156704a..0979cedfdb5 100644 --- a/source/isaaclab/isaaclab/sim/spawners/shapes/shapes.py +++ b/source/isaaclab/isaaclab/sim/spawners/shapes/shapes.py @@ -7,9 +7,9 @@ from typing import TYPE_CHECKING -import isaacsim.core.utils.prims as prim_utils from pxr import Usd +import isaaclab.utils.prims as prim_utils from isaaclab.sim import schemas from isaaclab.sim.utils import bind_physics_material, bind_visual_material, clone diff --git a/source/isaaclab/isaaclab/sim/spawners/wrappers/wrappers.py b/source/isaaclab/isaaclab/sim/spawners/wrappers/wrappers.py index 9f339aa70c7..ace71e072db 100644 --- a/source/isaaclab/isaaclab/sim/spawners/wrappers/wrappers.py +++ b/source/isaaclab/isaaclab/sim/spawners/wrappers/wrappers.py @@ -10,13 +10,13 @@ from typing import TYPE_CHECKING import carb -import isaacsim.core.utils.prims as prim_utils -import isaacsim.core.utils.stage as stage_utils -from isaacsim.core.utils.stage import get_current_stage from pxr import Sdf, Usd import isaaclab.sim as sim_utils +import isaaclab.utils.prims as prim_utils +import isaaclab.utils.stage as stage_utils from isaaclab.sim.spawners.from_files import UsdFileCfg +from isaaclab.utils.stage import get_current_stage if TYPE_CHECKING: from . import wrappers_cfg diff --git a/source/isaaclab/isaaclab/sim/utils.py b/source/isaaclab/isaaclab/sim/utils.py index 338ec5d843a..d6a5e2c410a 100644 --- a/source/isaaclab/isaaclab/sim/utils.py +++ b/source/isaaclab/isaaclab/sim/utils.py @@ -15,16 +15,17 @@ from typing import TYPE_CHECKING, Any import carb -import isaacsim.core.utils.stage as stage_utils import omni import omni.kit.commands import omni.log from isaacsim.core.cloner import Cloner from isaacsim.core.utils.carb import get_carb_setting -from isaacsim.core.utils.stage import get_current_stage from isaacsim.core.version import get_version from pxr import PhysxSchema, Sdf, Usd, UsdGeom, UsdPhysics, UsdShade, UsdUtils +import isaaclab.utils.stage as stage_utils +from isaaclab.utils.stage import get_current_stage + # from Isaac Sim 4.2 onwards, pxr.Semantics is deprecated try: import Semantics diff --git a/source/isaaclab/isaaclab/terrains/utils.py b/source/isaaclab/isaaclab/terrains/utils.py index 1c55a9325b2..b0893be9227 100644 --- a/source/isaaclab/isaaclab/terrains/utils.py +++ b/source/isaaclab/isaaclab/terrains/utils.py @@ -80,10 +80,10 @@ def create_prim_from_mesh(prim_path: str, mesh: trimesh.Trimesh, **kwargs): physics_material: The physics material to apply. Defaults to None. """ # need to import these here to prevent isaacsim launching when importing this module - import isaacsim.core.utils.prims as prim_utils from pxr import UsdGeom import isaaclab.sim as sim_utils + import isaaclab.utils.prims as prim_utils # create parent prim prim_utils.create_prim(prim_path, "Xform") diff --git a/source/isaaclab/isaaclab/ui/xr_widgets/instruction_widget.py b/source/isaaclab/isaaclab/ui/xr_widgets/instruction_widget.py index ec084098dcb..6a30f773b78 100644 --- a/source/isaaclab/isaaclab/ui/xr_widgets/instruction_widget.py +++ b/source/isaaclab/isaaclab/ui/xr_widgets/instruction_widget.py @@ -10,11 +10,12 @@ import omni.kit.commands import omni.ui as ui -from isaacsim.core.utils.prims import delete_prim, get_prim_at_path from omni.kit.xr.scene_view.utils import UiContainer, WidgetComponent from omni.kit.xr.scene_view.utils.spatial_source import SpatialSource from pxr import Gf +from isaaclab.utils.prims import delete_prim, get_prim_at_path + Vec3Type: TypeAlias = Gf.Vec3f | Gf.Vec3d camera_facing_widget_container = {} diff --git a/source/isaaclab/isaaclab/utils/prims.py b/source/isaaclab/isaaclab/utils/prims.py new file mode 100644 index 00000000000..bdb816cb700 --- /dev/null +++ b/source/isaaclab/isaaclab/utils/prims.py @@ -0,0 +1,1022 @@ +import re + +# python +import typing + +import numpy as np +import omni.usd +import usdrt + +# isaacsim +from isaacsim.core.utils._isaac_utils import _find_matching_prim_paths +from isaaclab.utils.semantics import add_labels +from isaaclab.utils.stage import add_reference_to_stage, get_current_stage, get_current_stage_id +from isaaclab.utils.types import SDF_type_to_Gf +from omni.usd.commands import DeletePrimsCommand, MovePrimCommand + +# omniverse +from pxr import Sdf, Usd, UsdGeom, UsdPhysics + + +def get_prim_at_path(prim_path: str, fabric: bool = False) -> typing.Union[Usd.Prim, usdrt.Usd._Usd.Prim]: + """Get the USD or Fabric Prim at a given path string + + Args: + prim_path (str): path of the prim in the stage. + fabric (bool, optional): True for fabric stage and False for USD stage. Defaults to False. + + Returns: + typing.Union[Usd.Prim, usdrt.Usd._Usd.Prim]: USD or Fabric Prim object at the given path in the current stage. + + Example: + + .. code-block:: python + + >>> import isaaclab.utils.prims as prims_utils + >>> + >>> prims_utils.get_prim_at_path("/World/Cube") + Usd.Prim() + """ + + current_stage = get_current_stage(fabric=fabric) + if current_stage: + return current_stage.GetPrimAtPath(prim_path) + else: + return None + + +def is_prim_path_valid(prim_path: str, fabric: bool = False) -> bool: + """Check if a path has a valid USD Prim at it + + Args: + prim_path (str): path of the prim in the stage + fabric (bool, optional): True for fabric stage and False for USD stage. Defaults to False. + + Returns: + bool: True if the path points to a valid prim + + Example: + + .. code-block:: python + + >>> import isaaclab.utils.prims as prims_utils + >>> + >>> # given the stage: /World/Cube + >>> prims_utils.is_prim_path_valid("/World/Cube") + True + >>> prims_utils.is_prim_path_valid("/World/Cube/") + False + >>> prims_utils.is_prim_path_valid("/World/Sphere") # it doesn't exist + False + """ + prim = get_prim_at_path(prim_path, fabric=fabric) + if prim: + return prim.IsValid() + else: + return False + + +def get_prim_attribute_names(prim_path: str, fabric: bool = False) -> typing.List[str]: + """Get all the attribute names of a prim at the path + + Args: + prim_path (str): path of the prim in the stage + fabric (bool, optional): True for fabric stage and False for USD stage. Defaults to False. + + Raises: + Exception: If there is not a valid prim at the given path + + Returns: + typing.List[str]: List of the prim attribute names + + Example: + + .. code-block:: python + + >>> import isaaclab.utils.prims as prims_utils + >>> + >>> prims_utils.get_prim_attribute_names("/World/Cube") + ['doubleSided', 'extent', 'orientation', 'primvars:displayColor', 'primvars:displayOpacity', + 'purpose', 'size', 'visibility', 'xformOp:orient', 'xformOp:scale', 'xformOp:translate', 'xformOpOrder'] + """ + return [attr.GetName() for attr in get_prim_at_path(prim_path=prim_path, fabric=fabric).GetAttributes()] + + +def get_prim_attribute_value(prim_path: str, attribute_name: str, fabric: bool = False) -> typing.Any: + """Get a prim attribute value + + Args: + prim_path (str): path of the prim in the stage + attribute_name (str): name of the attribute to get + fabric (bool, optional): True for fabric stage and False for USD stage. Defaults to False. + + Raises: + Exception: If there is not a valid prim at the given path + + Returns: + typing.Any: Prim attribute value + + Example: + + .. code-block:: python + + >>> import isaaclab.utils.prims as prims_utils + >>> + >>> prims_utils.get_prim_attribute_value("/World/Cube", attribute_name="size") + 1.0 + """ + attr = get_prim_at_path(prim_path=prim_path, fabric=fabric).GetAttribute(attribute_name) + if fabric: + type_name = str(attr.GetTypeName().GetAsString()) + else: + type_name = str(attr.GetTypeName()) + if type_name in SDF_type_to_Gf: + return list(attr.Get()) + else: + return attr.Get() + + +def set_prim_attribute_value(prim_path: str, attribute_name: str, value: typing.Any, fabric: bool = False): + """Set a prim attribute value + + Args: + prim_path (str): path of the prim in the stage + attribute_name (str): name of the attribute to set + value (typing.Any): value to set the attribute to + fabric (bool, optional): True for fabric stage and False for USD stage. Defaults to False. + + Example: + + .. code-block:: python + + >>> import isaaclab.utils.prims as prims_utils + >>> + >>> # given the stage: /World/Cube. Set the Cube size to 5.0 + >>> prims_utils.set_prim_attribute_value("/World/Cube", attribute_name="size", value=5.0) + """ + attr = get_prim_at_path(prim_path=prim_path, fabric=fabric).GetAttribute(attribute_name) + if fabric: + type_name = str(attr.GetTypeName().GetAsString()) + else: + type_name = str(attr.GetTypeName()) + if isinstance(value, np.ndarray): + value = value.tolist() + if type_name in SDF_type_to_Gf: + value = np.array(value).flatten().tolist() + if fabric: + eval("attr.Set(usdrt." + SDF_type_to_Gf[type_name] + "(*value))") + else: + eval("attr.Set(" + SDF_type_to_Gf[type_name] + "(*value))") + else: + attr.Set(value) + + +def define_prim(prim_path: str, prim_type: str = "Xform", fabric: bool = False) -> Usd.Prim: + """Create a USD Prim at the given prim_path of type prim_type unless one already exists + + .. note:: + + This method will create a prim of the specified type in the specified path. + To apply a transformation (position, orientation, scale), set attributes or + load an USD file while creating the prim use the ``create_prim`` function. + + Args: + prim_path (str): path of the prim in the stage + prim_type (str, optional): The type of the prim to create. Defaults to "Xform". + fabric (bool, optional): True for fabric stage and False for USD stage. Defaults to False. + + Raises: + Exception: If there is already a prim at the prim_path + + Returns: + Usd.Prim: The created USD prim. + + Example: + + .. code-block:: python + + >>> import isaaclab.utils.prims as prims_utils + >>> + >>> prims_utils.define_prim("/World/Shapes", prim_type="Xform") + Usd.Prim() + """ + if is_prim_path_valid(prim_path, fabric=fabric): + raise Exception("A prim already exists at prim path: {}".format(prim_path)) + return get_current_stage(fabric=fabric).DefinePrim(prim_path, prim_type) + + +def get_prim_type_name(prim_path: str, fabric: bool = False) -> str: + """Get the TypeName of the USD Prim at the path if it is valid + + Args: + prim_path (str): path of the prim in the stage + fabric (bool, optional): True for fabric stage and False for USD stage. Defaults to False. + + Raises: + Exception: If there is not a valid prim at the given path + + Returns: + str: The TypeName of the USD Prim at the path string + + Example: + + .. code-block:: python + + >>> import isaaclab.utils.prims as prims_utils + >>> + >>> prims_utils.get_prim_type_name("/World/Cube") + Cube + """ + if not is_prim_path_valid(prim_path, fabric=fabric): + raise Exception("A prim does not exist at prim path: {}".format(prim_path)) + prim = get_prim_at_path(prim_path, fabric=fabric) + if fabric: + return prim.GetTypeName() + else: + return prim.GetPrimTypeInfo().GetTypeName() + + +def move_prim(path_from: str, path_to: str) -> None: + """Run the Move command to change a prims USD Path in the stage + + Args: + path_from (str): Path of the USD Prim you wish to move + path_to (str): Final destination of the prim + + Example: + + .. code-block:: python + + >>> import isaaclab.utils.prims as prims_utils + >>> + >>> # given the stage: /World/Cube. Move the prim Cube outside the prim World + >>> prims_utils.move_prim("/World/Cube", "/Cube") + """ + MovePrimCommand(path_from=path_from, path_to=path_to).do() + + +def get_first_matching_child_prim( + prim_path: str, predicate: typing.Callable[[str], bool], fabric: bool = False +) -> Usd.Prim: + """Recursively get the first USD Prim at the path string that passes the predicate function + + Args: + prim_path (str): path of the prim in the stage + predicate (typing.Callable[[str], bool]): Function to test the prims against + fabric (bool, optional): True for fabric stage and False for USD stage. Defaults to False. + + Returns: + Usd.Prim: The first prim or child of the prim, as defined by GetChildren, that passes the predicate + + Example: + + .. code-block:: python + + >>> import isaaclab.utils.prims as prims_utils + >>> + >>> # given the stage: /World/Cube, /World/Cube_01, /World/Cube_02. + >>> # Get the first child prim of type Cube + >>> predicate = lambda path: prims_utils.get_prim_type_name(path) == "Cube" + >>> prims_utils.get_first_matching_child_prim("/", predicate) + Usd.Prim() + """ + prim = get_current_stage(fabric=fabric).GetPrimAtPath(prim_path) + children_stack = [prim] + out = prim.GetChildren() + while len(children_stack) > 0: + prim = children_stack.pop(0) + if predicate(get_prim_path(prim)): + return prim + children = prim.GetChildren() + children_stack = children_stack + children + out = out + children + return None + + +def get_first_matching_parent_prim(prim_path: str, predicate: typing.Callable[[str], bool]) -> Usd.Prim: + """Recursively get the first USD Prim at the parent path string that passes the predicate function + + Args: + prim_path (str): path of the prim in the stage + predicate (typing.Callable[[str], bool]): Function to test the prims against + + Returns: + str: The first prim on the parent path, as defined by GetParent, that passes the predicate + + Example: + + .. code-block:: python + + >>> import isaaclab.utils.prims as prims_utils + >>> + >>> # given the stage: /World/Cube. Get the first parent of Cube prim of type Xform + >>> predicate = lambda path: prims_utils.get_prim_type_name(path) == "Xform" + >>> prims_utils.get_first_matching_parent_prim("/World/Cube", predicate) + Usd.Prim() + """ + current_prim_path = get_prim_path(get_prim_parent(get_prim_at_path(prim_path))) + while not is_prim_root_path(current_prim_path): + if predicate(current_prim_path): + return get_prim_at_path(current_prim_path) + current_prim_path = get_prim_path(get_prim_parent(get_prim_at_path(current_prim_path))) + return None + + +def get_all_matching_child_prims( + prim_path: str, predicate: typing.Callable[[str], bool] = lambda x: True, depth: typing.Optional[int] = None +) -> typing.List[Usd.Prim]: + """Performs a breadth-first search starting from the root and returns all the prims matching the predicate. + + Args: + prim_path (str): root prim path to start traversal from. + predicate (typing.Callable[[str], bool]): predicate that checks the prim path of a prim and returns a boolean. + depth (typing.Optional[int]): maximum depth for traversal, should be bigger than zero if specified. + Defaults to None (i.e: traversal till the end of the tree). + + Returns: + typing.List[Usd.Prim]: A list containing the root and children prims matching specified predicate. + + Example: + + .. code-block:: python + + >>> import isaaclab.utils.prims as prims_utils + >>> + >>> # get all hidden prims + >>> predicate = lambda path: prims_utils.is_prim_hidden_in_stage(path) # True if the prim at path is hidden + >>> prims_utils.get_all_matching_child_prims("/", predicate) + [Usd.Prim(), + Usd.Prim(), + Usd.Prim(), + Usd.Prim(), + Usd.Prim()] + """ + prim = get_prim_at_path(prim_path) + traversal_queue = [(prim, 0)] + out = [] + while len(traversal_queue) > 0: + prim, current_depth = traversal_queue.pop(0) + if is_prim_path_valid(get_prim_path(prim)): + if predicate(get_prim_path(prim)): + out.append(prim) + if depth is None or current_depth < depth: + children = get_prim_children(prim) + traversal_queue = traversal_queue + [(child, current_depth + 1) for child in children] + return out + + +def find_matching_prim_paths(prim_path_regex: str, prim_type: typing.Optional[str] = None) -> typing.List[str]: + """Find all the matching prim paths in the stage based on Regex expression. + + Args: + prim_path_regex (str): The Regex expression for prim path. + prim_type (typing.Optional[str]): The type of the prims to filter, only supports articulation and rigid_body currently. Defaults to None. + + Returns: + typing.List[str]: List of prim paths that match input expression. + + Example: + + .. code-block:: python + + >>> import isaaclab.utils.prims as prims_utils + >>> + >>> # given the stage: /World/env/Cube, /World/env_01/Cube, /World/env_02/Cube + >>> # get only the prim Cube paths from env_01 and env_02 + >>> prims_utils.find_matching_prim_paths("/World/env_.*/Cube") + ['/World/env_01/Cube', '/World/env_02/Cube'] + """ + stage_id = get_current_stage_id() + if prim_type is None: + prim_type = "" + return _find_matching_prim_paths(prim_path_regex.replace(".*", "*"), stage_id, prim_type) + + +def get_prim_children(prim: Usd.Prim) -> typing.List[Usd.Prim]: + """Return the call of the USD Prim's GetChildren member function + + Args: + prim (Usd.Prim): The parent USD Prim + + Returns: + typing.List[Usd.Prim]: A list of the prim's children. + + Example: + + .. code-block:: python + + >>> import isaaclab.utils.prims as prims_utils + >>> + >>> # given the stage: /World/Cube, /World/Cube_01, /World/Cube_02. + >>> # Get all prims under the prim World + >>> prim = prims_utils.get_prim_at_path("/World") + >>> prims_utils.get_prim_children(prim) + [Usd.Prim(), Usd.Prim(), Usd.Prim()] + """ + return prim.GetChildren() + + +def get_prim_parent(prim: Usd.Prim) -> Usd.Prim: + """Return the call of the USD Prim's GetChildren member function + + Args: + prim (Usd.Prim): The USD Prim to call GetParent on + + Returns: + Usd.Prim: The prim's parent returned from GetParent + + Example: + + .. code-block:: python + + >>> import isaaclab.utils.prims as prims_utils + >>> + >>> # given the stage: /World/Cube. Get the prim Cube's parent + >>> prim = prims_utils.get_prim_at_path("/World/Cube") + >>> prims_utils.get_prim_parent(prim) + Usd.Prim() + """ + return prim.GetParent() + + +def query_parent_path(prim_path: str, predicate: typing.Callable[[str], bool]) -> bool: + """Check if one of the ancestors of the prim at the prim_path can pass the predicate + + Args: + prim_path (str): path to the USD Prim for which to check the ancestors + predicate (typing.Callable[[str], bool]): The condition that must be True about the ancestors + + Returns: + bool: True if there is an ancestor that can pass the predicate, False otherwise + + Example: + + .. code-block:: python + + >>> import isaaclab.utils.prims as prims_utils + >>> + >>> # given the stage: /World/Cube. Check is the prim Cube has an ancestor of type Xform + >>> predicate = lambda path: prims_utils.get_prim_type_name(path) == "Xform" + >>> prims_utils.query_parent_path("/World/Cube", predicate) + True + """ + current_prim_path = get_prim_path(get_prim_parent(get_prim_at_path(prim_path))) + while not is_prim_root_path(current_prim_path): + if predicate(current_prim_path): + return True + current_prim_path = get_prim_path(get_prim_parent(get_prim_at_path(current_prim_path))) + return False + + +def is_prim_ancestral(prim_path: str) -> bool: + """Check if any of the prims ancestors were brought in as a reference + + Args: + prim_path (str): The path to the USD prim. + + Returns: + True if prim is part of a referenced prim, false otherwise. + + Example: + + .. code-block:: python + + >>> import isaaclab.utils.prims as prims_utils + >>> + >>> # /World/Cube is a prim created + >>> prims_utils.is_prim_ancestral("/World/Cube") + False + >>> # /World/panda is an USD file loaded (as reference) under that path + >>> prims_utils.is_prim_ancestral("/World/panda") + False + >>> prims_utils.is_prim_ancestral("/World/panda/panda_link0") + True + """ + return omni.usd.check_ancestral(get_prim_at_path(prim_path)) + + +def is_prim_root_path(prim_path: str) -> bool: + """Checks if the input prim path is root or not. + + Args: + prim_path (str): The path to the USD prim. + + Returns: + True if the prim path is "/", False otherwise + + Example: + + .. code-block:: python + + >>> import isaaclab.utils.prims as prims_utils + >>> + >>> # given the stage: /World/Cube + >>> prims_utils.is_prim_root_path("/") + True + >>> prims_utils.is_prim_root_path("/World") + False + >>> prims_utils.is_prim_root_path("/World/Cube") + False + """ + if prim_path == "/": + return True + else: + return False + + +def is_prim_no_delete(prim_path: str) -> bool: + """Checks whether a prim can be deleted or not from USD stage. + + .. note :: + + This function checks for the ``no_delete`` prim metadata. A prim with this + metadata set to True cannot be deleted by using the edit menu, the context menu, + or by calling the ``delete_prim`` function, for example. + + Args: + prim_path (str): The path to the USD prim. + + Returns: + True if prim cannot be deleted, False if it can + + Example: + + .. code-block:: python + + >>> import isaaclab.utils.prims as prims_utils + >>> + >>> # prim without the 'no_delete' metadata + >>> prims_utils.is_prim_no_delete("/World/Cube") + None + >>> # prim with the 'no_delete' metadata set to True + >>> prims_utils.is_prim_no_delete("/World/Cube") + True + """ + return get_prim_at_path(prim_path).GetMetadata("no_delete") + + +def is_prim_hidden_in_stage(prim_path: str) -> bool: + """Checks if the prim is hidden in the USd stage or not. + + .. warning :: + + This function checks for the ``hide_in_stage_window`` prim metadata. + This metadata is not related to the visibility of the prim. + + Args: + prim_path (str): The path to the USD prim. + + Returns: + True if prim is hidden from stage window, False if not hidden. + + Example: + + .. code-block:: python + + >>> import isaaclab.utils.prims as prims_utils + >>> + >>> # prim without the 'hide_in_stage_window' metadata + >>> prims_utils.is_prim_hidden_in_stage("/World/Cube") + None + >>> # prim with the 'hide_in_stage_window' metadata set to True + >>> prims_utils.is_prim_hidden_in_stage("/World/Cube") + True + """ + return get_prim_at_path(prim_path).GetMetadata("hide_in_stage_window") + + +def get_prim_path(prim: Usd.Prim) -> str: + """Get the path of a given USD prim. + + Args: + prim (Usd.Prim): The input USD prim. + + Returns: + str: The path to the input prim. + + Example: + + .. code-block:: python + + >>> import isaaclab.utils.prims as prims_utils + >>> + >>> prim = prims_utils.get_prim_at_path("/World/Cube") # Usd.Prim() + >>> prims_utils.get_prim_path(prim) + /World/Cube + """ + if prim: + if isinstance(prim, Usd.Prim): + return prim.GetPath().pathString + else: + return prim.GetPath() + else: + return None + + +def set_prim_visibility(prim: Usd.Prim, visible: bool) -> None: + """Sets the visibility of the prim in the opened stage. + + .. note:: + + The method does this through the USD API. + + Args: + prim (Usd.Prim): the USD prim + visible (bool): flag to set the visibility of the usd prim in stage. + + Example: + + .. code-block:: python + + >>> import isaaclab.utils.prims as prims_utils + >>> + >>> # given the stage: /World/Cube. Make the Cube not visible + >>> prim = prims_utils.get_prim_at_path("/World/Cube") + >>> prims_utils.set_prim_visibility(prim, False) + """ + imageable = UsdGeom.Imageable(prim) + if visible: + imageable.MakeVisible() + else: + imageable.MakeInvisible() + + +def create_prim( + prim_path: str, + prim_type: str = "Xform", + position: typing.Optional[typing.Sequence[float]] = None, + translation: typing.Optional[typing.Sequence[float]] = None, + orientation: typing.Optional[typing.Sequence[float]] = None, + scale: typing.Optional[typing.Sequence[float]] = None, + usd_path: typing.Optional[str] = None, + semantic_label: typing.Optional[str] = None, + semantic_type: str = "class", + attributes: typing.Optional[dict] = None, +) -> Usd.Prim: + """Create a prim into current USD stage. + + The method applies specified transforms, the semantic label and set specified attributes. + + Args: + prim_path (str): The path of the new prim. + prim_type (str): Prim type name + position (typing.Sequence[float], optional): prim position (applied last) + translation (typing.Sequence[float], optional): prim translation (applied last) + orientation (typing.Sequence[float], optional): prim rotation as quaternion + scale (np.ndarray (3), optional): scaling factor in x, y, z. + usd_path (str, optional): Path to the USD that this prim will reference. + semantic_label (str, optional): Semantic label. + semantic_type (str, optional): set to "class" unless otherwise specified. + attributes (dict, optional): Key-value pairs of prim attributes to set. + + Raises: + Exception: If there is already a prim at the prim_path + + Returns: + Usd.Prim: The created USD prim. + + Example: + + .. code-block:: python + + >>> import numpy as np + >>> import isaaclab.utils.prims as prims_utils + >>> + >>> # create a cube (/World/Cube) of size 2 centered at (1.0, 0.5, 0.0) + >>> prims_utils.create_prim( + ... prim_path="/World/Cube", + ... prim_type="Cube", + ... position=np.array([1.0, 0.5, 0.0]), + ... attributes={"size": 2.0} + ... ) + Usd.Prim() + + .. code-block:: python + + >>> import isaaclab.utils.prims as prims_utils + >>> + >>> # load an USD file (franka.usd) to the stage under the path /World/panda + >>> prims_utils.create_prim( + ... prim_path="/World/panda", + ... prim_type="Xform", + ... usd_path="/home//Documents/Assets/Robots/FrankaRobotics/FrankaPanda/franka.usd" + ... ) + Usd.Prim() + """ + # Note: Imported here to prevent cyclic dependency in the module. + from isaacsim.core.prims import XFormPrim + + # create prim in stage + prim = define_prim(prim_path=prim_path, prim_type=prim_type) + if not prim: + return None + # apply attributes into prim + if attributes is not None: + for k, v in attributes.items(): + prim.GetAttribute(k).Set(v) + # add reference to USD file + if usd_path is not None: + add_reference_to_stage(usd_path=usd_path, prim_path=prim_path) + # add semantic label to prim + if semantic_label is not None: + add_labels(prim, labels=[semantic_label], instance_name=semantic_type) + # apply the transformations + from isaacsim.core.api.simulation_context.simulation_context import SimulationContext + + if SimulationContext.instance() is None: + import isaacsim.core.utils.numpy as backend_utils + + device = "cpu" + else: + backend_utils = SimulationContext.instance().backend_utils + device = SimulationContext.instance().device + if position is not None: + position = backend_utils.expand_dims(backend_utils.convert(position, device), 0) + if translation is not None: + translation = backend_utils.expand_dims(backend_utils.convert(translation, device), 0) + if orientation is not None: + orientation = backend_utils.expand_dims(backend_utils.convert(orientation, device), 0) + if scale is not None: + scale = backend_utils.expand_dims(backend_utils.convert(scale, device), 0) + XFormPrim(prim_path, positions=position, translations=translation, orientations=orientation, scales=scale) + + return prim + + +def delete_prim(prim_path: str) -> None: + """Remove the USD Prim and its descendants from the scene if able + + Args: + prim_path (str): path of the prim in the stage + + Example: + + .. code-block:: python + + >>> import isaaclab.utils.prims as prims_utils + >>> + >>> prims_utils.delete_prim("/World/Cube") + """ + DeletePrimsCommand([prim_path]).do() + + +def get_prim_property(prim_path: str, property_name: str) -> typing.Any: + """Get the attribute of the USD Prim at the given path + + Args: + prim_path (str): path of the prim in the stage + property_name (str): name of the attribute to get + + Returns: + typing.Any: The attribute if it exists, None otherwise + + Example: + + .. code-block:: python + + >>> import isaaclab.utils.prims as prims_utils + >>> + >>> prims_utils.get_prim_property("/World/Cube", property_name="size") + 1.0 + """ + prim = get_prim_at_path(prim_path=prim_path) + return prim.GetAttribute(property_name).Get() + + +def set_prim_property(prim_path: str, property_name: str, property_value: typing.Any) -> None: + """Set the attribute of the USD Prim at the path + + Args: + prim_path (str): path of the prim in the stage + property_name (str): name of the attribute to set + property_value (typing.Any): value to set the attribute to + + Example: + + .. code-block:: python + + >>> import isaaclab.utils.prims as prims_utils + >>> + >>> # given the stage: /World/Cube. Set the Cube size to 5.0 + >>> prims_utils.set_prim_property("/World/Cube", property_name="size", property_value=5.0) + """ + prim = get_prim_at_path(prim_path=prim_path) + prim.GetAttribute(property_name).Set(property_value) + + +def get_prim_object_type(prim_path: str) -> typing.Union[str, None]: + """Get the dynamic control object type of the USD Prim at the given path. + + If the prim at the path is of Dynamic Control type e.g.: rigid_body, joint, dof, articulation, attractor, d6joint, + then the corresponding string returned. If is an Xformable prim, then "xform" is returned. Otherwise None + is returned. + + Args: + prim_path (str): path of the prim in the stage + + Raises: + Exception: If the USD Prim is not a supported type. + + Returns: + str: String corresponding to the object type. + + Example: + + .. code-block:: python + + >>> import isaaclab.utils.prims as prims_utils + >>> + >>> prims_utils.get_prim_object_type("/World/Cube") + xform + """ + prim = get_prim_at_path(prim_path) + if prim.HasAPI(UsdPhysics.ArticulationRootAPI): + return "articulation" + elif prim.HasAPI(UsdPhysics.RigidBodyAPI): + return "rigid_body" + elif ( + prim.IsA(UsdPhysics.PrismaticJoint) or prim.IsA(UsdPhysics.RevoluteJoint) or prim.IsA(UsdPhysics.SphericalJoint) + ): + return "joint" + elif prim.IsA(UsdPhysics.Joint): + return "d6joint" + elif prim.IsA(UsdGeom.Xformable): + return "xform" + else: + return None + + +def is_prim_non_root_articulation_link(prim_path: str) -> bool: + """Used to query if the prim_path corresponds to a link in an articulation which is a non root link. + + Args: + prim_path (str): prim_path to query + + Returns: + bool: True if the prim path corresponds to a link in an articulation which is a non root link + and can't have a transformation applied to it. + + Example: + + .. code-block:: python + + >>> import isaaclab.utils.prims as prims_utils + >>> + >>> # /World/panda contains the prim tree for the Franka panda robot. + >>> # The prim on this path has the Physics Articulation Root property applied + >>> prims_utils.is_prim_non_root_articulation_link("/World/panda") + False + >>> prims_utils.is_prim_non_root_articulation_link("/World/panda/panda_link0") + True + """ + parent_articulation_root = get_first_matching_parent_prim( + prim_path=prim_path, predicate=lambda a: get_prim_at_path(a).HasAPI(UsdPhysics.ArticulationRootAPI) + ) + if parent_articulation_root is None: + return False + + has_physics_apis = get_prim_at_path(prim_path).HasAPI(UsdPhysics.RigidBodyAPI) + if not has_physics_apis: + return False + + # get all joints under ArticulationRoot + joint_prims = get_all_matching_child_prims( + prim_path=get_prim_path(parent_articulation_root), predicate=lambda a: "Joint" in get_prim_type_name(a) + ) + # this assumes if that the first link is a root articulation link + for joint_prim in joint_prims: + joint = UsdPhysics.Joint(joint_prim) + if joint.GetExcludeFromArticulationAttr().Get(): + continue + body_targets = joint.GetBody0Rel().GetTargets() + joint.GetBody1Rel().GetTargets() + for target in body_targets: + if prim_path == str(target): + return True + return False + + +def set_prim_hide_in_stage_window(prim: Usd.Prim, hide: bool): + """Set ``hide_in_stage_window`` metadata for a prim + + .. warning :: + + This metadata is unrelated to the visibility of the prim. + Use the ``set_prim_visibility`` function for the latter purpose + + Args: + prim (Usd.Prim): Prim to set + hide (bool): True to hide in stage window, false to show + + Example: + + .. code-block:: python + + >>> import isaaclab.utils.prims as prims_utils + >>> + >>> prim = prims_utils.get_prim_at_path("/World/Cube") + >>> prims_utils.set_prim_hide_in_stage_window(prim, True) + """ + prim.SetMetadata("hide_in_stage_window", hide) + + +def set_prim_no_delete(prim: Usd.Prim, no_delete: bool): + """Set ``no_delete`` metadata for a prim + + .. note :: + + A prim with this metadata set to True cannot be deleted by using the edit menu, + the context menu, or by calling the ``delete_prim`` function, for example. + + Args: + prim (Usd.Prim): Prim to set + no_delete (bool):True to make prim undeletable in stage window, false to allow deletion + + Example: + + .. code-block:: python + + >>> import isaaclab.utils.prims as prims_utils + >>> + >>> prim = prims_utils.get_prim_at_path("/World/Cube") + >>> prims_utils.set_prim_no_delete(prim, True) + """ + prim.SetMetadata("no_delete", no_delete) + + +def set_targets(prim: Usd.Prim, attribute: str, target_prim_paths: list): + """Set targets for a prim relationship attribute + + Args: + prim (Usd.Prim): Prim to create and set attribute on + attribute (str): Relationship attribute to create + target_prim_paths (list): list of targets to set + + Example: + + .. code-block:: python + + >>> import isaaclab.utils.prims as prims_utils + >>> + >>> # given the stage: /World/Cube, /World/Cube_01, /World/Cube_02. + >>> # Set each prim Cube to the relationship targetPrim of the prim World + >>> prim = prims_utils.get_prim_at_path("/World") + >>> targets = ["/World/Cube", "/World/Cube_01", "/World/Cube_02"] + >>> prims_utils.set_targets(prim, "targetPrim", targets) + """ + try: + input_rel = prim.CreateRelationship(attribute) + input_rel.SetTargets(target_prim_paths) + except Exception as e: + print(e, prim.GetPath()) + + +def get_articulation_root_api_prim_path(prim_path): + """Get the prim path that has the Articulation Root API + + .. note:: + + This function assumes that all prims defined by a regular expression correspond to the same articulation type + + Args: + prim_path (str): path or regex of the prim(s) on which to search for the prim containing the API + + Returns: + str: path or regex of the prim(s) that has the Articulation Root API. + If no prim has been found, the same input value is returned + + Example: + + .. code-block:: python + + >>> import isaaclab.utils.prims as prims_utils + >>> + >>> # given the stage: /World/env/Ant, /World/env_01/Ant, /World/env_02/Ant + >>> # search specifying the prim with the Articulation Root API + >>> prims_utils.get_articulation_root_api_prim_path("/World/env/Ant/torso") + /World/env/Ant/torso + >>> # search specifying some ancestor prim that does not have the Articulation Root API + >>> prims_utils.get_articulation_root_api_prim_path("/World/env/Ant") + /World/env/Ant/torso + >>> # regular expression search + >>> prims_utils.get_articulation_root_api_prim_path("/World/env.*/Ant") + /World/env.*/Ant/torso + """ + predicate = lambda path: get_prim_at_path(path).HasAPI(UsdPhysics.ArticulationRootAPI) + # single prim + if Sdf.Path.IsValidPathString(prim_path) and is_prim_path_valid(prim_path): + prim = get_first_matching_child_prim(prim_path, predicate) + if prim is not None: + return get_prim_path(prim) + # regular expression + else: + paths = find_matching_prim_paths(prim_path) + if len(paths): + prim = get_first_matching_child_prim(paths[0], predicate) + if prim is not None: + path = get_prim_path(prim) + remainder_path = "/".join(path.split("/")[prim_path.count("/") + 1 :]) + if remainder_path != "": + return prim_path + "/" + remainder_path + else: + return prim_path + return prim_path diff --git a/source/isaaclab/isaaclab/utils/semantics.py b/source/isaaclab/isaaclab/utils/semantics.py new file mode 100644 index 00000000000..4e09d9e7275 --- /dev/null +++ b/source/isaaclab/isaaclab/utils/semantics.py @@ -0,0 +1,475 @@ +# SPDX-FileCopyrightText: Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. +# SPDX-License-Identifier: Apache-2.0 +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +from typing import Dict, List, Optional, Tuple + +import carb +import isaaclab.utils.prims as prim_utils +import omni.usd +import Semantics +from isaaclab.utils.stage import get_current_stage, get_current_stage_id +from pxr import Usd, UsdGeom, UsdSemantics + + +def add_update_semantics(prim: Usd.Prim, semantic_label: str, type_label: str = "class", suffix="") -> None: + """[DEPRECATED] Apply a semantic label to a prim or update an existing label using the old SemanticsAPI. + + Args: + prim (Usd.Prim): Usd Prim to add or update semantics on + semantic_label (str): The label we want to apply + type_label (str): The type of semantic information we are specifying (default = "class") + suffix (str): Additional suffix used to specify multiple semantic attribute names. + """ + carb.log_warn( + "Function 'add_update_semantics' uses the deprecated SemanticsAPI. " + "Consider upgrading the semantics with `upgrade_prim_semantics_to_labels(prim)` first and then using `add_labels` instead." + ) + # Apply or acquire the existing SemanticAPI + semantic_api = Semantics.SemanticsAPI.Get(prim, "Semantics" + suffix) + if not semantic_api: + semantic_api = Semantics.SemanticsAPI.Apply(prim, "Semantics" + suffix) + semantic_api.CreateSemanticTypeAttr() + semantic_api.CreateSemanticDataAttr() + + type_attr = semantic_api.GetSemanticTypeAttr() + data_attr = semantic_api.GetSemanticDataAttr() + + # Set the type and data for the SemanticAPI + if type_label is not None: + type_attr.Set(type_label) + if semantic_label is not None: + data_attr.Set(semantic_label) + return + + +def remove_all_semantics(prim: Usd.Prim, recursive: bool = False) -> None: + """[DEPRECATED] Removes all semantic tags (old SemanticsAPI) from a given prim and its children + + Args: + prim (Usd.Prim): Prim to remove any applied semantic APIs on + recursive (bool, optional): Also traverse children and remove semantics recursively. Defaults to False. + """ + carb.log_warn( + "Function 'remove_all_semantics' uses the deprecated SemanticsAPI. " + "Consider upgrading the semantics with `upgrade_prim_semantics_to_labels(prim, recursive=True)` first and then using `remove_labels` instead." + ) + + def remove_semantics(input_prim: Usd.Prim): + for prop in input_prim.GetProperties(): + is_semantic = Semantics.SemanticsAPI.IsSemanticsAPIPath(prop.GetPath()) + if is_semantic: + name = prop.SplitName()[1] + sem = Semantics.SemanticsAPI.Get(input_prim, name) + + typeAttr = sem.GetSemanticTypeAttr() + dataAttr = sem.GetSemanticDataAttr() + input_prim.RemoveProperty(typeAttr.GetName()) + input_prim.RemoveProperty(dataAttr.GetName()) + input_prim.RemoveAPI(Semantics.SemanticsAPI, name) + + if recursive: + for p in Usd.PrimRange(prim.GetPrim()): + remove_semantics(p) + else: + remove_semantics(prim) + + +def get_semantics(prim: Usd.Prim) -> Dict[str, Tuple[str, str]]: + """[DEPRECATED] Returns semantics (old SemanticsAPI) that are applied to a prim + + Args: + prim (Usd.Prim): Prim to return semantics for + + Returns: + Dict[str, Tuple[str,str]]: Dictionary containing the name of the applied semantic, and the type and data associated with that semantic. + """ + carb.log_warn( + "Function 'get_semantics' reads the deprecated SemanticsAPI. " + "Consider upgrading the semantics with `upgrade_prim_semantics_to_labels(prim)` first and then using `get_labels` instead." + ) + result = {} + for prop in prim.GetProperties(): + is_semantic = Semantics.SemanticsAPI.IsSemanticsAPIPath(prop.GetPath()) + if is_semantic: + name = prop.SplitName()[1] + sem = Semantics.SemanticsAPI.Get(prim, name) + + typeAttr = sem.GetSemanticTypeAttr() + dataAttr = sem.GetSemanticDataAttr() + result[name] = (typeAttr.Get(), dataAttr.Get()) + return result + + +def check_missing_semantics(prim_path: str = None) -> List[str]: + """[DEPRECATED] Returns a list of prim path of meshes with missing semantics (old SemanticsAPI) + + Args: + prim_path (str): This will check Prim path and its childrens' semantics + + Returns: + List[str]: Prim paths + """ + carb.log_warn( + "Function 'check_missing_semantics' uses the deprecated SemanticsAPI via get_semantics. " + "Consider upgrading the semantics with `upgrade_prim_semantics_to_labels(prim, recursive=True)` first and then using `check_missing_labels` instead." + ) + prim_paths = [] + prims = [] + + if prim_path is None: + stage = get_current_stage() + prims = stage.Traverse() + else: + prims = prim_utils.get_all_matching_child_prims(prim_path, lambda prim: True) + + for prim in prims: + if prim.IsA(UsdGeom.Mesh): + semantics = get_semantics(prim) + if not semantics: + prim_paths.append(prim.GetPath().pathString) + return prim_paths + + +def check_incorrect_semantics(prim_path: str = None) -> List[List[str]]: + """[DEPRECATED] Returns a list of prim path of meshes with different semantics labels (old SemanticsAPI) than their prim path and their semantic labels + + Args: + prim_path (str): This will check Prim path and its childrens' semantics + + Returns: + List[List[str]]: List of prim path and semantic label + """ + carb.log_warn( + "Function 'check_incorrect_semantics' uses the deprecated SemanticsAPI via get_semantics. " + "Consider upgrading the semantics with `upgrade_prim_semantics_to_labels(prim, recursive=True)` first and then using `check_incorrect_labels` instead." + ) + incorrect_pairs = [] + if prim_path is None: + stage = get_current_stage() + prims = stage.Traverse() + else: + prims = prim_utils.get_all_matching_child_prims(prim_path, lambda prim: True) + + for prim in prims: + if prim.IsA(UsdGeom.Mesh): + semantics = get_semantics(prim) + prim_path = prim.GetPath().pathString + if semantics: + for key in list(semantics.keys()): + semantic_label = semantics[key][1].lower() + + if ( + semantic_label not in prim_path.lower() + and semantic_label.replace("_", "") not in prim_path.lower() + and semantic_label.replace("-", "") not in prim_path.lower() + ): + incorrect_pair = [prim_path, semantics[key][1]] + incorrect_pairs.append(incorrect_pair) + break + return incorrect_pairs + + +def count_semantics_in_scene(prim_path: str = None) -> Dict[str, int]: + """[DEPRECATED] Returns a dictionary of labels (old SemanticsAPI) and the corresponding count + + Args: + prim_path (str): This will check Prim path and its childrens' semantics + + Returns: + Dict[str, int]: labels and count + """ + carb.log_warn( + "Function 'count_semantics_in_scene' uses the deprecated SemanticsAPI via get_semantics. " + "Consider upgrading the semantics with `upgrade_prim_semantics_to_labels(prim, recursive=True)` first and then using `count_labels_in_scene` instead." + ) + semantics_counter = {"missing": 0} + if prim_path is None: + stage = get_current_stage() + prims = stage.Traverse() + else: + prims = prim_utils.get_all_matching_child_prims(prim_path, lambda prim: True) + print(len(prims)) + for prim in prims: + if prim.IsA(UsdGeom.Mesh): + semantics = get_semantics(prim) + if not semantics: + semantics_counter["missing"] += 1 + else: + for key in list(semantics.keys()): + if semantics[key][1] not in semantics_counter: + semantics_counter[semantics[key][1]] = 1 + else: + semantics_counter[semantics[key][1]] += 1 + + return semantics_counter + + +def add_labels(prim: Usd.Prim, labels: list[str], instance_name: str = "class", overwrite: bool = True) -> None: + """Apply semantic labels to a prim using the UsdSemantics.LabelsAPI. + + Args: + prim (Usd.Prim): Usd Prim to add or update labels on. + labels (list): The list of labels to apply. + instance_name (str, optional): The name of the semantic instance. Defaults to "class". + overwrite (bool, optional): If True (default), existing labels for this instance are replaced. + If False, the new labels are appended to existing ones (if any). + """ + import omni.replicator.core.functional as F + + mode = "replace" if overwrite else "add" + F.modify.semantics(prim, {instance_name: labels}, mode=mode) + + +def get_labels(prim: Usd.Prim) -> dict[str, list[str]]: + """Returns semantic labels (UsdSemantics.LabelsAPI) applied to a prim. + + Args: + prim (Usd.Prim): Prim to return labels for. + + Returns: + dict[str, list[str]]: Dictionary mapping instance names to a list of labels. + Returns an empty dict if no LabelsAPI instances are found. + """ + result = {} + for schema_name in prim.GetAppliedSchemas(): + if schema_name.startswith("SemanticsLabelsAPI:"): + instance_name = schema_name.split(":", 1)[1] + sem_api = UsdSemantics.LabelsAPI(prim, instance_name) + labels_attr = sem_api.GetLabelsAttr() + if labels_attr: + labels = labels_attr.Get() + result[instance_name] = list(labels) if labels is not None else [] + else: + result[instance_name] = [] + return result + + +def remove_labels(prim: Usd.Prim, instance_name: str | None = None, include_descendants: bool = False) -> None: + """Removes semantic labels (UsdSemantics.LabelsAPI) from a prim. + + Args: + prim (Usd.Prim): Prim to remove labels from. + instance_name (str | None, optional): Specific instance name to remove. + If None (default), removes *all* LabelsAPI instances. + include_descendants (bool, optional): Also traverse children and remove labels recursively. Defaults to False. + """ + + def remove_single_prim_labels(target_prim: Usd.Prim): + schemas_to_remove = [] + for schema_name in target_prim.GetAppliedSchemas(): + if schema_name.startswith("SemanticsLabelsAPI:"): + current_instance = schema_name.split(":", 1)[1] + if instance_name is None or current_instance == instance_name: + schemas_to_remove.append(current_instance) + + for inst_to_remove in schemas_to_remove: + target_prim.RemoveAPI(UsdSemantics.LabelsAPI, inst_to_remove) + + if include_descendants: + for p in Usd.PrimRange(prim): + remove_single_prim_labels(p) + else: + remove_single_prim_labels(prim) + + +def check_missing_labels(prim_path: str | None = None) -> list[str]: + """Returns a list of prim paths of meshes with missing semantic labels (UsdSemantics.LabelsAPI). + + Args: + prim_path (str | None): This will check Prim path and its childrens' labels. If None, checks the whole stage. + + Returns: + list[str]: Prim paths of meshes with no LabelsAPI applied. + """ + prim_paths = [] + stage = get_current_stage() + if stage is None: + carb.log_warn("Invalid stage, skipping label check") + return prim_paths + + start_prim = stage.GetPrimAtPath(prim_path) if prim_path else stage.GetPseudoRoot() + if not start_prim: + # Allow None prim_path for whole stage check, warn if path specified but not found + if prim_path: + carb.log_warn(f"Prim path not found: {prim_path}") + return prim_paths + + prims_to_check = Usd.PrimRange(start_prim) + + for prim in prims_to_check: + if prim.IsA(UsdGeom.Mesh): + has_any_label = False + for schema_name in prim.GetAppliedSchemas(): + if schema_name.startswith("SemanticsLabelsAPI:"): + has_any_label = True + break + if not has_any_label: + prim_paths.append(prim.GetPath().pathString) + return prim_paths + + +def check_incorrect_labels(prim_path: str | None = None) -> list[list[str]]: + """Returns a list of [prim_path, label] for meshes where at least one semantic label (LabelsAPI) + is not found within the prim's path string (case-insensitive, ignoring '_' and '-'). + + Args: + prim_path (str | None): This will check Prim path and its childrens' labels. If None, checks the whole stage. + + Returns: + list[list[str]]: List containing pairs of [prim_path, first_incorrect_label]. + """ + incorrect_pairs = [] + stage = get_current_stage() + if stage is None: + carb.log_warn("Invalid stage, skipping label check") + return incorrect_pairs + + start_prim = stage.GetPrimAtPath(prim_path) if prim_path else stage.GetPseudoRoot() + if not start_prim: + if prim_path: + carb.log_warn(f"Prim path not found: {prim_path}") + return incorrect_pairs + + prims_to_check = Usd.PrimRange(start_prim) + + for prim in prims_to_check: + if prim.IsA(UsdGeom.Mesh): + labels_dict = get_labels(prim) + if labels_dict: + prim_path_str = prim.GetPath().pathString.lower() + all_labels = [ + label for sublist in labels_dict.values() for label in sublist if label + ] # Flatten and filter None/empty + for label in all_labels: + label_lower = label.lower() + # Check if label (or label without separators) is in path + if ( + label_lower not in prim_path_str + and label_lower.replace("_", "") not in prim_path_str + and label_lower.replace("-", "") not in prim_path_str + ): + incorrect_pair = [prim.GetPath().pathString, label] + incorrect_pairs.append(incorrect_pair) + break # Only report first incorrect label per prim + return incorrect_pairs + + +def count_labels_in_scene(prim_path: str | None = None) -> dict[str, int]: + """Returns a dictionary of semantic labels (UsdSemantics.LabelsAPI) and their corresponding count. + + Args: + prim_path (str | None): This will check Prim path and its childrens' labels. If None, checks the whole stage. + + Returns: + dict[str, int]: Dictionary mapping individual labels to their total count across all instances. + Includes a 'missing_labels' count for meshes with no LabelsAPI. + """ + labels_counter = {"missing_labels": 0} + stage = get_current_stage() + if stage is None: + carb.log_warn("Invalid stage, skipping label check") + return labels_counter + + start_prim = stage.GetPrimAtPath(prim_path) if prim_path else stage.GetPseudoRoot() + if not start_prim: + if prim_path: + carb.log_warn(f"Prim path not found: {prim_path}") + return labels_counter + + prims_to_check = Usd.PrimRange(start_prim) + + for prim in prims_to_check: + if prim.IsA(UsdGeom.Mesh): + labels_dict = get_labels(prim) + if not labels_dict: + labels_counter["missing_labels"] += 1 + else: + # Iterate through all labels from all instances on the prim + all_labels = [label for sublist in labels_dict.values() for label in sublist if label] + for label in all_labels: + labels_counter[label] = labels_counter.get(label, 0) + 1 + + return labels_counter + + +def upgrade_prim_semantics_to_labels(prim: Usd.Prim, include_descendants: bool = False) -> int: + """Upgrades a prim and optionally its descendants from the deprecated SemanticsAPI + to the new UsdSemantics.LabelsAPI. + + Converts each found SemanticsAPI instance on the processed prim(s) to a corresponding + LabelsAPI instance. The old 'semanticType' becomes the new LabelsAPI + 'instance_name', and the old 'semanticData' becomes the single label in the + new 'labels' list. The old SemanticsAPI is always removed after upgrading. + + Args: + prim (Usd.Prim): The starting prim to upgrade. + include_descendants (bool, optional): If True, upgrades the prim and all its descendants. + If False (default), upgrades only the specified prim. + + Returns: + int: The total number of SemanticsAPI instances successfully upgraded to LabelsAPI. + """ + total_upgraded = 0 + + prims_to_process = Usd.PrimRange(prim) if include_descendants else [prim] + + for current_prim in prims_to_process: + if not current_prim: + continue + + old_semantics = {} + for prop in current_prim.GetProperties(): + if Semantics.SemanticsAPI.IsSemanticsAPIPath(prop.GetPath()): + instance_name = prop.SplitName()[1] # Get instance name (e.g., 'Semantics', 'Semantics_a') + sem_api = Semantics.SemanticsAPI.Get(current_prim, instance_name) + if sem_api: + typeAttr = sem_api.GetSemanticTypeAttr() + dataAttr = sem_api.GetSemanticDataAttr() + if typeAttr and dataAttr and instance_name not in old_semantics: + old_semantics[instance_name] = (typeAttr.Get(), dataAttr.Get()) + + if not old_semantics: + continue + + for old_instance_name, (old_type, old_data) in old_semantics.items(): + + if not old_type or not old_data: + carb.log_warn( + f"[upgrade_prim] Skipping instance '{old_instance_name}' on {current_prim.GetPath()} due to missing type or data." + ) + continue + + new_instance_name = old_type + new_labels = [old_data] + + try: + old_sem_api_to_remove = Semantics.SemanticsAPI.Get(current_prim, old_instance_name) + if old_sem_api_to_remove: + typeAttr = old_sem_api_to_remove.GetSemanticTypeAttr() + dataAttr = old_sem_api_to_remove.GetSemanticDataAttr() + # Ensure attributes are valid before trying to remove them by name + if typeAttr and typeAttr.IsDefined(): + current_prim.RemoveProperty(typeAttr.GetName()) + if dataAttr and dataAttr.IsDefined(): + current_prim.RemoveProperty(dataAttr.GetName()) + current_prim.RemoveAPI(Semantics.SemanticsAPI, old_instance_name) + + add_labels(current_prim, new_labels, instance_name=new_instance_name, overwrite=False) + + total_upgraded += 1 + + except Exception as e: + carb.log_warn(f"Failed to upgrade instance '{old_instance_name}' on {current_prim.GetPath()}: {e}") + continue + return total_upgraded diff --git a/source/isaaclab/isaaclab/utils/stage.py b/source/isaaclab/isaaclab/utils/stage.py new file mode 100644 index 00000000000..54e1ef63cc2 --- /dev/null +++ b/source/isaaclab/isaaclab/utils/stage.py @@ -0,0 +1,785 @@ +import builtins +import contextlib +import threading + +# python +import typing + +# omniverse +import omni +# import omni.log +import omni.kit.app +import usdrt + +# isaacsim +from isaacsim.core.utils.constants import AXES_TOKEN +from omni.kit.usd import layers +from omni.metrics.assembler.core import get_metrics_assembler_interface +from omni.usd.commands import DeletePrimsCommand +from pxr import Sdf, Usd, UsdGeom, UsdUtils + +_context = threading.local() # thread-local storage to handle nested contexts and concurrent access + + +@contextlib.contextmanager +def use_stage(stage: Usd.Stage) -> None: + """Context manager that sets a thread-local stage. + + Args: + stage: The stage to set in the context. + + Raises: + AssertionError: If the stage is not a USD stage instance. + + Example: + + .. code-block:: python + + >>> from pxr import Usd + >>> import isaaclab.utils.stage as stage_utils + >>> + >>> stage_in_memory = Usd.Stage.CreateInMemory() + >>> with stage_utils.use_stage(stage_in_memory): + ... # operate on the specified stage + ... pass + >>> # operate on the default stage attached to the USD context + """ + # check stage + assert isinstance(stage, Usd.Stage), f"Expected a USD stage instance, got: {type(stage)}" + # store previous context value if it exists + previous_stage = getattr(_context, "stage", None) + # set new context value + try: + _context.stage = stage + yield + # remove context value or restore previous one if it exists + finally: + if previous_stage is None: + delattr(_context, "stage") + else: + _context.stage = previous_stage + + +def get_current_stage(fabric: bool = False) -> typing.Union[Usd.Stage, usdrt.Usd._Usd.Stage]: + """Get the current open USD or Fabric stage + + Args: + fabric (bool, optional): True to get the fabric stage. False to get the USD stage. Defaults to False. + + Returns: + typing.Union[Usd.Stage, usdrt.Usd._Usd.Stage]: The USD or Fabric stage as specified by the input arg fabric. + + Example: + + .. code-block:: python + + >>> import isaaclab.utils.stage as stage_utils + >>> + >>> stage_utils.get_current_stage() + Usd.Stage.Open(rootLayer=Sdf.Find('anon:0x7fba6c04f840:World7.usd'), + sessionLayer=Sdf.Find('anon:0x7fba6c01c5c0:World7-session.usda'), + pathResolverContext=) + """ + stage = getattr(_context, "stage", omni.usd.get_context().get_stage()) + if fabric: + stage_cache = UsdUtils.StageCache.Get() + stage_id = stage_cache.GetId(stage).ToLongInt() + if stage_id < 0: + stage_id = stage_cache.Insert(stage).ToLongInt() + return usdrt.Usd.Stage.Attach(stage_id) + return stage + + +def get_current_stage_id() -> int: + """Get the current open stage id + + Returns: + int: The stage id. + + Example: + + .. code-block:: python + + >>> import isaaclab.utils.stage as stage_utils + >>> + >>> stage_utils.get_current_stage_id() + 1234567890 + """ + stage = get_current_stage() + stage_cache = UsdUtils.StageCache.Get() + stage_id = stage_cache.GetId(stage).ToLongInt() + if stage_id < 0: + stage_id = stage_cache.Insert(stage).ToLongInt() + return stage_id + + +def update_stage() -> None: + """Update the current USD stage. + + Example: + + .. code-block:: python + + >>> import isaaclab.utils.stage as stage_utils + >>> + >>> stage_utils.update_stage() + """ + omni.kit.app.get_app_interface().update() + + +async def update_stage_async() -> None: + """Update the current USD stage (asynchronous version). + + Example: + + .. code-block:: python + + >>> import asyncio + >>> import isaaclab.utils.stage as stage_utils + >>> from omni.kit.async_engine import run_coroutine + >>> + >>> async def task(): + ... await stage_utils.update_stage_async() + ... + >>> run_coroutine(task()) + """ + await omni.kit.app.get_app().next_update_async() + + +# TODO: make a generic util for setting all layer properties +def set_stage_up_axis(axis: str = "z") -> None: + """Change the up axis of the current stage + + Args: + axis (UsdGeom.Tokens, optional): valid values are ``"x"``, ``"y"`` and ``"z"`` + + Example: + + .. code-block:: python + + >>> import isaaclab.utils.stage as stage_utils + >>> + >>> # set stage up axis to Y-up + >>> stage_utils.set_stage_up_axis("y") + """ + stage = get_current_stage() + if stage is None: + raise Exception("There is no stage currently opened") + rootLayer = stage.GetRootLayer() + rootLayer.SetPermissionToEdit(True) + with Usd.EditContext(stage, rootLayer): + UsdGeom.SetStageUpAxis(stage, AXES_TOKEN[axis]) + + +def get_stage_up_axis() -> str: + """Get the current up-axis of USD stage. + + Returns: + str: The up-axis of the stage. + + Example: + + .. code-block:: python + + >>> import isaaclab.utils.stage as stage_utils + >>> + >>> stage_utils.get_stage_up_axis() + Z + """ + stage = get_current_stage() + return UsdGeom.GetStageUpAxis(stage) + + +def clear_stage(predicate: typing.Optional[typing.Callable[[str], bool]] = None) -> None: + """Deletes all prims in the stage without populating the undo command buffer + + Args: + predicate (typing.Optional[typing.Callable[[str], bool]], optional): + user defined function that takes a prim_path (str) as input and returns True/False if the prim + should/shouldn't be deleted. If predicate is None, a default is used that deletes all prims + + Example: + + .. code-block:: python + + >>> import isaaclab.utils.stage as stage_utils + >>> + >>> # clear the whole stage + >>> stage_utils.clear_stage() + >>> + >>> # given the stage: /World/Cube, /World/Cube_01, /World/Cube_02. + >>> # Delete only the prims of type Cube + >>> predicate = lambda path: prims_utils.get_prim_type_name(path) == "Cube" + >>> stage_utils.clear_stage(predicate) # after the execution the stage will be /World + """ + # Note: Need to import this here to prevent circular dependencies. + from isaaclab.utils.prims import ( + get_all_matching_child_prims, + get_prim_path, + is_prim_ancestral, + is_prim_hidden_in_stage, + is_prim_no_delete, + ) + + def default_predicate(prim_path: str): + # prim = get_prim_at_path(prim_path) + # skip prims that we cannot delete + if is_prim_no_delete(prim_path): + return False + if is_prim_hidden_in_stage(prim_path): + return False + if is_prim_ancestral(prim_path): + return False + if prim_path == "/": + return False + if prim_path.startswith("/Render"): + return False + return True + + if predicate is None: + prims = get_all_matching_child_prims("/", default_predicate) + prim_paths_to_delete = [get_prim_path(prim) for prim in prims] + DeletePrimsCommand(prim_paths_to_delete).do() + else: + prims = get_all_matching_child_prims("/", predicate) + prim_paths_to_delete = [get_prim_path(prim) for prim in prims] + DeletePrimsCommand(prim_paths_to_delete).do() + + if builtins.ISAAC_LAUNCHED_FROM_TERMINAL is False: + omni.kit.app.get_app_interface().update() + + +def print_stage_prim_paths(fabric: bool = False) -> None: + """Traverses the stage and prints all prim (hidden or not) paths. + + Example: + + .. code-block:: python + + >>> import isaaclab.utils.stage as stage_utils + >>> + >>> # given the stage: /World/Cube, /World/Cube_01, /World/Cube_02. + >>> stage_utils.print_stage_prim_paths() + /Render + /World + /World/Cube + /World/Cube_01 + /World/Cube_02 + /OmniverseKit_Persp + /OmniverseKit_Front + /OmniverseKit_Top + /OmniverseKit_Right + """ + # Note: Need to import this here to prevent circular dependencies. + from isaaclab.utils.prims import get_prim_path + + for prim in traverse_stage(fabric=fabric): + prim_path = get_prim_path(prim) + print(prim_path) + + +def add_reference_to_stage(usd_path: str, prim_path: str, prim_type: str = "Xform") -> Usd.Prim: + """Add USD reference to the opened stage at specified prim path. + + Adds a reference to an external USD file at the specified prim path on the current stage. + If the prim does not exist, it will be created with the specified type. + This function also handles stage units verification to ensure compatibility. + + Args: + usd_path: The path to USD file to reference. + prim_path: The prim path where the reference will be attached. + prim_type: The type of prim to create if it doesn't exist. Defaults to "Xform". + + Returns: + The USD prim at the specified prim path. + + Raises: + FileNotFoundError: When the input USD file is not found at the specified path. + + Example: + + .. code-block:: python + + >>> import isaaclab.utils.stage as stage_utils + >>> + >>> # load an USD file (franka.usd) to the stage under the path /World/panda + >>> prim = stage_utils.add_reference_to_stage( + ... usd_path="/home//Documents/Assets/Robots/FrankaRobotics/FrankaPanda/franka.usd", + ... prim_path="/World/panda" + ... ) + >>> prim + Usd.Prim() + """ + stage = get_current_stage() + prim = stage.GetPrimAtPath(prim_path) + if not prim.IsValid(): + prim = stage.DefinePrim(prim_path, prim_type) + # omni.log.info("Loading Asset from path {} ".format(usd_path)) + # Handle units + sdf_layer = Sdf.Layer.FindOrOpen(usd_path) + if not sdf_layer: + pass + # omni.log.info(f"Could not get Sdf layer for {usd_path}") + else: + stage_id = UsdUtils.StageCache.Get().GetId(stage).ToLongInt() + ret_val = get_metrics_assembler_interface().check_layers( + stage.GetRootLayer().identifier, sdf_layer.identifier, stage_id + ) + if ret_val["ret_val"]: + try: + import omni.metrics.assembler.ui + + payref = Sdf.Reference(usd_path) + omni.kit.commands.execute("AddReference", stage=stage, prim_path=prim.GetPath(), reference=payref) + except Exception as exc: + # omni.log.warn( + # f"The USD file {usd_path} used for a reference does have divergent units, please either enable omni.usd.metrics.assembler.ui or convert the file into right units." + # ) + success_bool = prim.GetReferences().AddReference(usd_path) + if not success_bool: + raise FileNotFoundError("The usd file at path {} provided wasn't found".format(usd_path)) + else: + success_bool = prim.GetReferences().AddReference(usd_path) + if not success_bool: + raise FileNotFoundError("The usd file at path {} provided wasn't found".format(usd_path)) + + return prim + + +def create_new_stage() -> Usd.Stage: + """Create a new stage attached to the USD context. + + Returns: + Usd.Stage: The created USD stage. + + Example: + + .. code-block:: python + + >>> import isaaclab.utils.stage as stage_utils + >>> + >>> stage_utils.create_new_stage() + Usd.Stage.Open(rootLayer=Sdf.Find('anon:0x7fba6c04f840:World7.usd'), + sessionLayer=Sdf.Find('anon:0x7fba6c01c5c0:World7-session.usda'), + pathResolverContext=) + """ + return omni.usd.get_context().new_stage() + + +def create_new_stage_in_memory() -> Usd.Stage: + """Create a new stage in memory. + + Returns: + Usd.Stage: The created USD stage. + + Example: + + .. code-block:: python + + >>> import isaaclab.utils.stage as stage_utils + >>> + >>> stage_utils.create_new_stage_in_memory() + Usd.Stage.Open(rootLayer=Sdf.Find('anon:0xf7b00e0:tmp.usda'), + sessionLayer=Sdf.Find('anon:0xf7cd2e0:tmp-session.usda'), + pathResolverContext=) + """ + return Usd.Stage.CreateInMemory() + + +async def create_new_stage_async() -> None: + """Create a new stage (asynchronous version). + + Example: + + .. code-block:: python + + >>> import asyncio + >>> import isaaclab.utils.stage as stage_utils + >>> from omni.kit.async_engine import run_coroutine + >>> + >>> async def task(): + ... await stage_utils.create_new_stage_async() + ... + >>> run_coroutine(task()) + """ + await omni.usd.get_context().new_stage_async() + await omni.kit.app.get_app().next_update_async() + + +def open_stage(usd_path: str) -> bool: + """Open the given usd file and replace currently opened stage. + + Args: + usd_path (str): Path to the USD file to open. + + Raises: + ValueError: When input path is not a supported file type by USD. + + Returns: + bool: True if operation is successful, otherwise false. + + Example: + + .. code-block:: python + + >>> import isaaclab.utils.stage as stage_utils + >>> + >>> stage_utils.open_stage("/home//Documents/Assets/Robots/FrankaRobotics/FrankaPanda/franka.usd") + True + """ + if not Usd.Stage.IsSupportedFile(usd_path): + raise ValueError("Only USD files can be loaded with this method") + usd_context = omni.usd.get_context() + usd_context.disable_save_to_recent_files() + result = omni.usd.get_context().open_stage(usd_path) + usd_context.enable_save_to_recent_files() + return result + + +async def open_stage_async(usd_path: str) -> typing.Tuple[bool, int]: + """Open the given usd file and replace currently opened stage (asynchronous version). + + Args: + usd_path (str): Path to the USD file to open. + + Raises: + ValueError: When input path is not a supported file type by USD. + + Returns: + bool: True if operation is successful, otherwise false. + + Example: + + .. code-block:: python + + >>> import asyncio + >>> import isaaclab.utils.stage as stage_utils + >>> from omni.kit.async_engine import run_coroutine + >>> + >>> async def task(): + ... await stage_utils.open_stage_async("/home//Documents/Assets/Robots/FrankaRobotics/FrankaPanda/franka.usd") + ... + >>> run_coroutine(task()) + """ + if not Usd.Stage.IsSupportedFile(usd_path): + raise ValueError("Only USD files can be loaded with this method") + usd_context = omni.usd.get_context() + usd_context.disable_save_to_recent_files() + (result, error) = await omni.usd.get_context().open_stage_async(usd_path) + usd_context.enable_save_to_recent_files() + return (result, error) + + +def save_stage(usd_path: str, save_and_reload_in_place=True) -> bool: + """Save usd file to path, it will be overwritten with the current stage + + Args: + usd_path (str): File path to save the current stage to + save_and_reload_in_place (bool, optional): use ``save_as_stage`` to save and reload the root layer in place. Defaults to True. + + Raises: + ValueError: When input path is not a supported file type by USD. + + Returns: + bool: True if operation is successful, otherwise false. + + Example: + + .. code-block:: python + + >>> import isaaclab.utils.stage as stage_utils + >>> + >>> stage_utils.save_stage("/home//Documents/Save/stage.usd") + True + """ + if not Usd.Stage.IsSupportedFile(usd_path): + raise ValueError("Only USD files can be saved with this method") + + layer = Sdf.Layer.CreateNew(usd_path) + root_layer = get_current_stage().GetRootLayer() + layer.TransferContent(root_layer) + omni.usd.resolve_paths(root_layer.identifier, layer.identifier) + result = layer.Save() + if save_and_reload_in_place: + open_stage(usd_path) + + return result + + +def close_stage(callback_fn: typing.Callable = None) -> bool: + """Closes the current opened USD stage. + + .. note:: + + Once the stage is closed, it is necessary to open a new stage or create a new one in order to work on it. + + Args: + callback_fn (typing.Callable, optional): Callback function to call while closing. Defaults to None. + + Returns: + bool: True if operation is successful, otherwise false. + + Example: + + .. code-block:: python + + >>> import isaaclab.utils.stage as stage_utils + >>> + >>> stage_utils.close_stage() + True + + .. code-block:: python + + >>> import isaaclab.utils.stage as stage_utils + >>> + >>> def callback(*args, **kwargs): + ... print("callback:", args, kwargs) + ... + >>> stage_utils.close_stage(callback) + True + >>> stage_utils.close_stage(callback) + callback: (False, 'Stage opening or closing already in progress!!') {} + False + """ + if callback_fn is None: + result = omni.usd.get_context().close_stage() + else: + result = omni.usd.get_context().close_stage_with_callback(callback_fn) + return result + + +def set_livesync_stage(usd_path: str, enable: bool) -> bool: + """Save the stage and set the Live Sync mode for real-time live editing of shared files on a Nucleus server + + Args: + usd_path (str): path to enable live sync for, it will be overwritten with the current stage + enable (bool): True to enable livesync, false to disable livesync + + Returns: + bool: True if operation is successful, otherwise false. + + Example: + + .. code-block:: python + + >>> import isaaclab.utils.stage as stage_utils + >>> + >>> stage_utils.set_livesync_stage("omniverse://localhost/Users/Live/stage.usd", enable=True) + server omniverse://localhost: ConnectionStatus.CONNECTING + server omniverse://localhost: ConnectionStatus.CONNECTED + True + """ + # TODO: Check that the provided usd_path exists + # TODO: Check that the provided usd_path exists + if save_stage(usd_path): + if enable: + usd_path_split = usd_path.split("/") + live_session = layers.get_live_syncing().find_live_session_by_name(usd_path, "Default") + if live_session is None: + live_session = layers.get_live_syncing().create_live_session(name="Default") + result = layers.get_live_syncing().join_live_session(live_session) + return True + else: + layers.get_live_syncing().stop_live_session(usd_path) + return True + else: + return False + + +def traverse_stage(fabric=False) -> typing.Iterable: + """Traverse through prims (hidden or not) in the opened Usd stage. + + Returns: + typing.Iterable: Generator which yields prims from the stage in depth-first-traversal order. + + Example: + + .. code-block:: python + + >>> import isaaclab.utils.stage as stage_utils + >>> + >>> # given the stage: /World/Cube, /World/Cube_01, /World/Cube_02. + >>> # Traverse through prims in the stage + >>> for prim in stage_utils.traverse_stage(): + >>> print(prim) + Usd.Prim() + Usd.Prim() + Usd.Prim() + Usd.Prim() + Usd.Prim() + Usd.Prim() + Usd.Prim() + Usd.Prim() + Usd.Prim() + """ + return get_current_stage(fabric=fabric).Traverse() + + +def is_stage_loading() -> bool: + """Convenience function to see if any files are being loaded. + + Returns: + bool: True if loading, False otherwise + + Example: + + .. code-block:: python + + >>> import isaaclab.utils.stage as stage_utils + >>> + >>> stage_utils.is_stage_loading() + False + """ + context = omni.usd.get_context() + if context is None: + return False + else: + _, _, loading = context.get_stage_loading_status() + return loading > 0 + + +def set_stage_units(stage_units_in_meters: float) -> None: + """Set the stage meters per unit + + The most common units and their values are listed in the following table: + + +------------------+--------+ + | Unit | Value | + +==================+========+ + | kilometer (km) | 1000.0 | + +------------------+--------+ + | meters (m) | 1.0 | + +------------------+--------+ + | inch (in) | 0.0254 | + +------------------+--------+ + | centimeters (cm) | 0.01 | + +------------------+--------+ + | millimeter (mm) | 0.001 | + +------------------+--------+ + + Args: + stage_units_in_meters (float): units for stage + + Example: + + .. code-block:: python + + >>> import isaaclab.utils.stage as stage_utils + >>> + >>> stage_utils.set_stage_units(1.0) + """ + if get_current_stage() is None: + raise Exception("There is no stage currently opened, init_stage needed before calling this func") + with Usd.EditContext(get_current_stage(), get_current_stage().GetRootLayer()): + UsdGeom.SetStageMetersPerUnit(get_current_stage(), stage_units_in_meters) + + +def get_stage_units() -> float: + """Get the stage meters per unit currently set + + The most common units and their values are listed in the following table: + + +------------------+--------+ + | Unit | Value | + +==================+========+ + | kilometer (km) | 1000.0 | + +------------------+--------+ + | meters (m) | 1.0 | + +------------------+--------+ + | inch (in) | 0.0254 | + +------------------+--------+ + | centimeters (cm) | 0.01 | + +------------------+--------+ + | millimeter (mm) | 0.001 | + +------------------+--------+ + + Returns: + float: current stage meters per unit + + Example: + + .. code-block:: python + + >>> import isaaclab.utils.stage as stage_utils + >>> + >>> stage_utils.get_stage_units() + 1.0 + """ + return UsdGeom.GetStageMetersPerUnit(get_current_stage()) + + +def get_next_free_path(path: str, parent: str = None) -> str: + """Returns the next free usd path for the current stage + + Args: + path (str): path we want to check + parent (str, optional): Parent prim for the given path. Defaults to None. + + Returns: + str: a new path that is guaranteed to not exist on the current stage + + Example: + + .. code-block:: python + + >>> import isaaclab.utils.stage as stage_utils + >>> + >>> # given the stage: /World/Cube, /World/Cube_01. + >>> # Get the next available path for /World/Cube + >>> stage_utils.get_next_free_path("/World/Cube") + /World/Cube_02 + """ + if parent is not None: + # remove trailing slash from parent and leading slash from path + path = omni.usd.get_stage_next_free_path( + get_current_stage(), parent.rstrip("/") + "/" + path.lstrip("/"), False + ) + else: + path = omni.usd.get_stage_next_free_path(get_current_stage(), path, True) + return path + + +def remove_deleted_references(): + """Clean up deleted references in the current USD stage. + + Removes any deleted items from both payload and references lists + for all prims in the stage's root layer. Prints information about + any deleted items that were cleaned up. + + Example: + + .. code-block:: python + + >>> import isaaclab.utils.stage as stage_utils + >>> stage_utils.remove_deleted_references() + Removed 2 deleted payload items from + Removed 1 deleted reference items from + """ + stage = get_current_stage() + deleted_count = 0 + + for prim in stage.Traverse(): + prim_spec = stage.GetRootLayer().GetPrimAtPath(prim.GetPath()) + if not prim_spec: + continue + + # Clean payload references + payload_list = prim_spec.GetInfo("payload") + if payload_list.deletedItems: + deleted_payload_count = len(payload_list.deletedItems) + print(f"Removed {deleted_payload_count} deleted payload items from {prim.GetPath()}") + payload_list.deletedItems = [] + prim_spec.SetInfo("payload", payload_list) + deleted_count += deleted_payload_count + + # Clean prim references + references_list = prim_spec.GetInfo("references") + if references_list.deletedItems: + deleted_ref_count = len(references_list.deletedItems) + print(f"Removed {deleted_ref_count} deleted reference items from {prim.GetPath()}") + references_list.deletedItems = [] + prim_spec.SetInfo("references", references_list) + deleted_count += deleted_ref_count + + if deleted_count == 0: + print("No deleted references or payloads found in the stage.") diff --git a/source/isaaclab/isaaclab/utils/string.py b/source/isaaclab/isaaclab/utils/string.py index 43a2fa0b310..ca04c468840 100644 --- a/source/isaaclab/isaaclab/utils/string.py +++ b/source/isaaclab/isaaclab/utils/string.py @@ -370,3 +370,52 @@ def resolve_matching_names_values( ) # return return index_list, names_list, values_list + + +def find_unique_string_name(initial_name: str, is_unique_fn: Callable[[str], bool]) -> str: + """Find a unique string name based on the predicate function provided. + + The string is appended with "_N", where N is a natural number till the resultant string + is unique. + + Args: + initial_name (str): The initial string name. + is_unique_fn (Callable[[str], bool]): The predicate function to validate against. + + Returns: + str: A unique string based on input function. + """ + if is_unique_fn(initial_name): + return initial_name + iterator = 1 + result = initial_name + "_" + str(iterator) + while not is_unique_fn(result): + result = initial_name + "_" + str(iterator) + iterator += 1 + return result + + +def find_root_prim_path_from_regex(prim_path_regex: str) -> tuple[str, int]: + """Find the first prim above the regex pattern prim and its position. + + Args: + prim_path_regex (str): full prim path including the regex pattern prim. + + Returns: + Tuple[str, int]: First position is the prim path to the parent of the regex prim. + Second position represents the level of the regex prim in the USD stage tree representation. + + """ + prim_paths_list = str(prim_path_regex).split("/") + root_idx = None + for prim_path_idx in range(len(prim_paths_list)): + chars = set("[]*|^") + if any((c in chars) for c in prim_paths_list[prim_path_idx]): + root_idx = prim_path_idx + break + root_prim_path = None + tree_level = None + if root_idx is not None: + root_prim_path = "/".join(prim_paths_list[:root_idx]) + tree_level = root_idx + return root_prim_path, tree_level diff --git a/source/isaaclab/isaaclab/utils/types.py b/source/isaaclab/isaaclab/utils/types.py index aa6f1fbfd45..2f55e4267b8 100644 --- a/source/isaaclab/isaaclab/utils/types.py +++ b/source/isaaclab/isaaclab/utils/types.py @@ -11,6 +11,33 @@ from collections.abc import Sequence from dataclasses import dataclass +SDF_type_to_Gf = { + "matrix3d": "Gf.Matrix3d", + "matrix3f": "Gf.Matrix3f", + "matrix4d": "Gf.Matrix4d", + "matrix4f": "Gf.Matrix4f", + "range1d": "Gf.Range1d", + "range1f": "Gf.Range1f", + "range2d": "Gf.Range2d", + "range2f": "Gf.Range2f", + "range3d": "Gf.Range3d", + "range3f": "Gf.Range3f", + "rect2i": "Gf.Rect2i", + "vec2d": "Gf.Vec2d", + "vec2f": "Gf.Vec2f", + "vec2h": "Gf.Vec2h", + "vec2i": "Gf.Vec2i", + "vec3d": "Gf.Vec3d", + "double3": "Gf.Vec3d", + "vec3f": "Gf.Vec3f", + "vec3h": "Gf.Vec3h", + "vec3i": "Gf.Vec3i", + "vec4d": "Gf.Vec4d", + "vec4f": "Gf.Vec4f", + "vec4h": "Gf.Vec4h", + "vec4i": "Gf.Vec4i", +} + @dataclass class ArticulationActions: diff --git a/source/isaaclab/test/assets/check_fixed_base_assets.py b/source/isaaclab/test/assets/check_fixed_base_assets.py index 8a07be1c413..dacd3b13ed2 100644 --- a/source/isaaclab/test/assets/check_fixed_base_assets.py +++ b/source/isaaclab/test/assets/check_fixed_base_assets.py @@ -35,9 +35,8 @@ import numpy as np import torch -import isaacsim.core.utils.prims as prim_utils - import isaaclab.sim as sim_utils +import isaaclab.utils.prims as prim_utils from isaaclab.assets import Articulation ## diff --git a/source/isaaclab/test/assets/test_articulation.py b/source/isaaclab/test/assets/test_articulation.py index dfacff5d2ec..6264fee6f45 100644 --- a/source/isaaclab/test/assets/test_articulation.py +++ b/source/isaaclab/test/assets/test_articulation.py @@ -20,12 +20,12 @@ import ctypes import torch -import isaacsim.core.utils.prims as prim_utils import pytest from isaacsim.core.version import get_version import isaaclab.sim as sim_utils import isaaclab.utils.math as math_utils +import isaaclab.utils.prims as prim_utils import isaaclab.utils.string as string_utils from isaaclab.actuators import ActuatorBase, IdealPDActuatorCfg, ImplicitActuatorCfg from isaaclab.assets import Articulation, ArticulationCfg diff --git a/source/isaaclab/test/assets/test_deformable_object.py b/source/isaaclab/test/assets/test_deformable_object.py index 2d589573e69..9bd5524d5c8 100644 --- a/source/isaaclab/test/assets/test_deformable_object.py +++ b/source/isaaclab/test/assets/test_deformable_object.py @@ -20,12 +20,12 @@ import torch import carb -import isaacsim.core.utils.prims as prim_utils import pytest from flaky import flaky import isaaclab.sim as sim_utils import isaaclab.utils.math as math_utils +import isaaclab.utils.prims as prim_utils from isaaclab.assets import DeformableObject, DeformableObjectCfg from isaaclab.sim import build_simulation_context diff --git a/source/isaaclab/test/assets/test_rigid_object.py b/source/isaaclab/test/assets/test_rigid_object.py index 6a0dc77b861..912ce976d09 100644 --- a/source/isaaclab/test/assets/test_rigid_object.py +++ b/source/isaaclab/test/assets/test_rigid_object.py @@ -20,11 +20,11 @@ import torch from typing import Literal -import isaacsim.core.utils.prims as prim_utils import pytest from flaky import flaky import isaaclab.sim as sim_utils +import isaaclab.utils.prims as prim_utils from isaaclab.assets import RigidObject, RigidObjectCfg from isaaclab.sim import build_simulation_context from isaaclab.sim.spawners import materials diff --git a/source/isaaclab/test/assets/test_rigid_object_collection.py b/source/isaaclab/test/assets/test_rigid_object_collection.py index 876a2904bf1..b08f9559bc6 100644 --- a/source/isaaclab/test/assets/test_rigid_object_collection.py +++ b/source/isaaclab/test/assets/test_rigid_object_collection.py @@ -19,10 +19,10 @@ import ctypes import torch -import isaacsim.core.utils.prims as prim_utils import pytest import isaaclab.sim as sim_utils +import isaaclab.utils.prims as prim_utils from isaaclab.assets import RigidObjectCfg, RigidObjectCollection, RigidObjectCollectionCfg from isaaclab.sim import build_simulation_context from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR diff --git a/source/isaaclab/test/assets/test_surface_gripper.py b/source/isaaclab/test/assets/test_surface_gripper.py index c2f81143f59..155dab1130c 100644 --- a/source/isaaclab/test/assets/test_surface_gripper.py +++ b/source/isaaclab/test/assets/test_surface_gripper.py @@ -18,11 +18,11 @@ import torch -import isaacsim.core.utils.prims as prim_utils import pytest from isaacsim.core.version import get_version import isaaclab.sim as sim_utils +import isaaclab.utils.prims as prim_utils from isaaclab.actuators import ImplicitActuatorCfg from isaaclab.assets import ( Articulation, diff --git a/source/isaaclab/test/controllers/test_differential_ik.py b/source/isaaclab/test/controllers/test_differential_ik.py index 0b84e09eff2..598c1e24643 100644 --- a/source/isaaclab/test/controllers/test_differential_ik.py +++ b/source/isaaclab/test/controllers/test_differential_ik.py @@ -14,12 +14,12 @@ import torch -import isaacsim.core.utils.prims as prim_utils -import isaacsim.core.utils.stage as stage_utils import pytest from isaacsim.core.cloner import GridCloner import isaaclab.sim as sim_utils +import isaaclab.utils.prims as prim_utils +import isaaclab.utils.stage as stage_utils from isaaclab.assets import Articulation from isaaclab.controllers import DifferentialIKController, DifferentialIKControllerCfg diff --git a/source/isaaclab/test/controllers/test_operational_space.py b/source/isaaclab/test/controllers/test_operational_space.py index b708b357218..c012e06b01d 100644 --- a/source/isaaclab/test/controllers/test_operational_space.py +++ b/source/isaaclab/test/controllers/test_operational_space.py @@ -14,12 +14,12 @@ import torch -import isaacsim.core.utils.prims as prim_utils -import isaacsim.core.utils.stage as stage_utils import pytest from isaacsim.core.cloner import GridCloner import isaaclab.sim as sim_utils +import isaaclab.utils.prims as prim_utils +import isaaclab.utils.stage as stage_utils from isaaclab.assets import Articulation from isaaclab.controllers import OperationalSpaceController, OperationalSpaceControllerCfg from isaaclab.markers import VisualizationMarkers diff --git a/source/isaaclab/test/deps/isaacsim/check_camera.py b/source/isaaclab/test/deps/isaacsim/check_camera.py index c9e0374fc92..e5585121c76 100644 --- a/source/isaaclab/test/deps/isaacsim/check_camera.py +++ b/source/isaaclab/test/deps/isaacsim/check_camera.py @@ -50,7 +50,6 @@ except ModuleNotFoundError: import isaacsim.core.utils.nucleus as nucleus_utils -import isaacsim.core.utils.prims as prim_utils import omni.replicator.core as rep from isaacsim.core.api.world import World from isaacsim.core.prims import Articulation, RigidPrim, SingleGeometryPrim, SingleRigidPrim @@ -59,6 +58,8 @@ from PIL import Image, ImageChops from pxr import Gf, UsdGeom +import isaaclab.utils.prims as prim_utils + # check nucleus connection if nucleus_utils.get_assets_root_path() is None: msg = ( diff --git a/source/isaaclab/test/deps/isaacsim/check_floating_base_made_fixed.py b/source/isaaclab/test/deps/isaacsim/check_floating_base_made_fixed.py index dbe12e8265f..487df23cc38 100644 --- a/source/isaaclab/test/deps/isaacsim/check_floating_base_made_fixed.py +++ b/source/isaaclab/test/deps/isaacsim/check_floating_base_made_fixed.py @@ -33,8 +33,6 @@ import torch import isaacsim.core.utils.nucleus as nucleus_utils -import isaacsim.core.utils.prims as prim_utils -import isaacsim.core.utils.stage as stage_utils import omni.kit.commands import omni.log import omni.physx @@ -44,6 +42,9 @@ from isaacsim.core.utils.viewports import set_camera_view from pxr import PhysxSchema, UsdPhysics +import isaaclab.utils.prims as prim_utils +import isaaclab.utils.stage as stage_utils + # check nucleus connection if nucleus_utils.get_assets_root_path() is None: msg = ( diff --git a/source/isaaclab/test/deps/isaacsim/check_legged_robot_clone.py b/source/isaaclab/test/deps/isaacsim/check_legged_robot_clone.py index c26f627a220..a157a4ce4e3 100644 --- a/source/isaaclab/test/deps/isaacsim/check_legged_robot_clone.py +++ b/source/isaaclab/test/deps/isaacsim/check_legged_robot_clone.py @@ -50,13 +50,14 @@ except ModuleNotFoundError: import isaacsim.core.utils.nucleus as nucleus_utils -import isaacsim.core.utils.prims as prim_utils from isaacsim.core.api.world import World from isaacsim.core.cloner import GridCloner from isaacsim.core.prims import Articulation from isaacsim.core.utils.carb import set_carb_setting from isaacsim.core.utils.viewports import set_camera_view +import isaaclab.utils.prims as prim_utils + # check nucleus connection if nucleus_utils.get_assets_root_path() is None: msg = ( diff --git a/source/isaaclab/test/deps/isaacsim/check_ref_count.py b/source/isaaclab/test/deps/isaacsim/check_ref_count.py index 2683bd3e989..9f4a9d7720f 100644 --- a/source/isaaclab/test/deps/isaacsim/check_ref_count.py +++ b/source/isaaclab/test/deps/isaacsim/check_ref_count.py @@ -44,11 +44,12 @@ except ModuleNotFoundError: import isaacsim.core.utils.nucleus as nucleus_utils -import isaacsim.core.utils.prims as prim_utils from isaacsim.core.api.simulation_context import SimulationContext from isaacsim.core.prims import Articulation from isaacsim.core.utils.carb import set_carb_setting +import isaaclab.utils.prims as prim_utils + # check nucleus connection if nucleus_utils.get_assets_root_path() is None: msg = ( diff --git a/source/isaaclab/test/deps/isaacsim/check_rep_texture_randomizer.py b/source/isaaclab/test/deps/isaacsim/check_rep_texture_randomizer.py index 5700b5c9044..495aa060f33 100644 --- a/source/isaaclab/test/deps/isaacsim/check_rep_texture_randomizer.py +++ b/source/isaaclab/test/deps/isaacsim/check_rep_texture_randomizer.py @@ -45,7 +45,6 @@ import numpy as np import torch -import isaacsim.core.utils.prims as prim_utils import omni.replicator.core as rep from isaacsim.core.api.simulation_context import SimulationContext from isaacsim.core.cloner import GridCloner @@ -53,6 +52,8 @@ from isaacsim.core.prims import RigidPrim from isaacsim.core.utils.viewports import set_camera_view +import isaaclab.utils.prims as prim_utils + def main(): """Spawn a bunch of balls and randomly change their textures.""" diff --git a/source/isaaclab/test/markers/test_visualization_markers.py b/source/isaaclab/test/markers/test_visualization_markers.py index 03955076b6e..ce93a97d61a 100644 --- a/source/isaaclab/test/markers/test_visualization_markers.py +++ b/source/isaaclab/test/markers/test_visualization_markers.py @@ -14,11 +14,11 @@ import torch -import isaacsim.core.utils.stage as stage_utils import pytest from isaacsim.core.api.simulation_context import SimulationContext import isaaclab.sim as sim_utils +import isaaclab.utils.stage as stage_utils from isaaclab.markers import VisualizationMarkers, VisualizationMarkersCfg from isaaclab.markers.config import FRAME_MARKER_CFG, POSITION_GOAL_MARKER_CFG from isaaclab.utils.math import random_orientation diff --git a/source/isaaclab/test/sensors/check_contact_sensor.py b/source/isaaclab/test/sensors/check_contact_sensor.py index 30d2c9be437..0b7cac6fe21 100644 --- a/source/isaaclab/test/sensors/check_contact_sensor.py +++ b/source/isaaclab/test/sensors/check_contact_sensor.py @@ -36,13 +36,13 @@ import torch -import isaacsim.core.utils.prims as prim_utils from isaacsim.core.api.simulation_context import SimulationContext from isaacsim.core.cloner import GridCloner from isaacsim.core.utils.carb import set_carb_setting from isaacsim.core.utils.viewports import set_camera_view import isaaclab.sim as sim_utils +import isaaclab.utils.prims as prim_utils from isaaclab.assets import Articulation from isaaclab.sensors.contact_sensor import ContactSensor, ContactSensorCfg from isaaclab.utils.timer import Timer diff --git a/source/isaaclab/test/sensors/check_ray_caster.py b/source/isaaclab/test/sensors/check_ray_caster.py index e1d3473ecc4..52344890275 100644 --- a/source/isaaclab/test/sensors/check_ray_caster.py +++ b/source/isaaclab/test/sensors/check_ray_caster.py @@ -41,7 +41,6 @@ import torch -import isaacsim.core.utils.prims as prim_utils from isaacsim.core.api.simulation_context import SimulationContext from isaacsim.core.cloner import GridCloner from isaacsim.core.prims import RigidPrim @@ -49,6 +48,7 @@ import isaaclab.sim as sim_utils import isaaclab.terrains as terrain_gen +import isaaclab.utils.prims as prim_utils from isaaclab.sensors.ray_caster import RayCaster, RayCasterCfg, patterns from isaaclab.terrains.config.rough import ROUGH_TERRAINS_CFG from isaaclab.terrains.terrain_importer import TerrainImporter diff --git a/source/isaaclab/test/sensors/test_camera.py b/source/isaaclab/test/sensors/test_camera.py index e660274b862..43ba761477f 100644 --- a/source/isaaclab/test/sensors/test_camera.py +++ b/source/isaaclab/test/sensors/test_camera.py @@ -22,14 +22,14 @@ import scipy.spatial.transform as tf import torch -import isaacsim.core.utils.prims as prim_utils -import isaacsim.core.utils.stage as stage_utils import omni.replicator.core as rep import pytest from isaacsim.core.prims import SingleGeometryPrim, SingleRigidPrim from pxr import Gf, Usd, UsdGeom import isaaclab.sim as sim_utils +import isaaclab.utils.prims as prim_utils +import isaaclab.utils.stage as stage_utils from isaaclab.sensors.camera import Camera, CameraCfg from isaaclab.utils import convert_dict_to_backend from isaaclab.utils.math import convert_quat diff --git a/source/isaaclab/test/sensors/test_frame_transformer.py b/source/isaaclab/test/sensors/test_frame_transformer.py index eda9f6019ab..40f4851142b 100644 --- a/source/isaaclab/test/sensors/test_frame_transformer.py +++ b/source/isaaclab/test/sensors/test_frame_transformer.py @@ -16,11 +16,11 @@ import scipy.spatial.transform as tf import torch -import isaacsim.core.utils.stage as stage_utils import pytest import isaaclab.sim as sim_utils import isaaclab.utils.math as math_utils +import isaaclab.utils.stage as stage_utils from isaaclab.assets import RigidObjectCfg from isaaclab.scene import InteractiveScene, InteractiveSceneCfg from isaaclab.sensors import FrameTransformerCfg, OffsetCfg diff --git a/source/isaaclab/test/sensors/test_imu.py b/source/isaaclab/test/sensors/test_imu.py index 7f621a36574..f150edaddf8 100644 --- a/source/isaaclab/test/sensors/test_imu.py +++ b/source/isaaclab/test/sensors/test_imu.py @@ -17,11 +17,11 @@ import pathlib import torch -import isaacsim.core.utils.stage as stage_utils import pytest import isaaclab.sim as sim_utils import isaaclab.utils.math as math_utils +import isaaclab.utils.stage as stage_utils from isaaclab.actuators import ImplicitActuatorCfg from isaaclab.assets import ArticulationCfg, RigidObjectCfg from isaaclab.markers.config import GREEN_ARROW_X_MARKER_CFG, RED_ARROW_X_MARKER_CFG diff --git a/source/isaaclab/test/sensors/test_multi_tiled_camera.py b/source/isaaclab/test/sensors/test_multi_tiled_camera.py index 7408ae06b75..6c3bc59a0e0 100644 --- a/source/isaaclab/test/sensors/test_multi_tiled_camera.py +++ b/source/isaaclab/test/sensors/test_multi_tiled_camera.py @@ -20,8 +20,6 @@ import random import torch -import isaacsim.core.utils.prims as prim_utils -import isaacsim.core.utils.stage as stage_utils import omni.replicator.core as rep import pytest from flaky import flaky @@ -29,6 +27,8 @@ from pxr import Gf, UsdGeom import isaaclab.sim as sim_utils +import isaaclab.utils.prims as prim_utils +import isaaclab.utils.stage as stage_utils from isaaclab.sensors.camera import TiledCamera, TiledCameraCfg diff --git a/source/isaaclab/test/sensors/test_ray_caster_camera.py b/source/isaaclab/test/sensors/test_ray_caster_camera.py index dd693bbc3f1..62363f1f9c2 100644 --- a/source/isaaclab/test/sensors/test_ray_caster_camera.py +++ b/source/isaaclab/test/sensors/test_ray_caster_camera.py @@ -20,13 +20,13 @@ import os import torch -import isaacsim.core.utils.prims as prim_utils -import isaacsim.core.utils.stage as stage_utils import omni.replicator.core as rep import pytest from pxr import Gf import isaaclab.sim as sim_utils +import isaaclab.utils.prims as prim_utils +import isaaclab.utils.stage as stage_utils from isaaclab.sensors.camera import Camera, CameraCfg from isaaclab.sensors.ray_caster import RayCasterCamera, RayCasterCameraCfg, patterns from isaaclab.sim import PinholeCameraCfg diff --git a/source/isaaclab/test/sensors/test_sensor_base.py b/source/isaaclab/test/sensors/test_sensor_base.py index 9f82198214c..93a3d65da2e 100644 --- a/source/isaaclab/test/sensors/test_sensor_base.py +++ b/source/isaaclab/test/sensors/test_sensor_base.py @@ -18,11 +18,11 @@ from collections.abc import Sequence from dataclasses import dataclass -import isaacsim.core.utils.prims as prim_utils -import isaacsim.core.utils.stage as stage_utils import pytest import isaaclab.sim as sim_utils +import isaaclab.utils.prims as prim_utils +import isaaclab.utils.stage as stage_utils from isaaclab.sensors import SensorBase, SensorBaseCfg from isaaclab.utils import configclass diff --git a/source/isaaclab/test/sensors/test_tiled_camera.py b/source/isaaclab/test/sensors/test_tiled_camera.py index fdef7a3ae5c..2e52631c136 100644 --- a/source/isaaclab/test/sensors/test_tiled_camera.py +++ b/source/isaaclab/test/sensors/test_tiled_camera.py @@ -20,13 +20,14 @@ import random import torch -import isaacsim.core.utils.prims as prim_utils -import isaacsim.core.utils.stage as stage_utils import omni.replicator.core as rep import pytest from isaacsim.core.prims import SingleGeometryPrim, SingleRigidPrim from pxr import Gf, UsdGeom +import isaaclab.utils.prims as prim_utils +import isaaclab.utils.stage as stage_utils + # from Isaac Sim 4.2 onwards, pxr.Semantics is deprecated try: import Semantics diff --git a/source/isaaclab/test/sim/check_meshes.py b/source/isaaclab/test/sim/check_meshes.py index 8595605b747..0aad9b38e1c 100644 --- a/source/isaaclab/test/sim/check_meshes.py +++ b/source/isaaclab/test/sim/check_meshes.py @@ -40,9 +40,8 @@ import torch import tqdm -import isaacsim.core.utils.prims as prim_utils - import isaaclab.sim as sim_utils +import isaaclab.utils.prims as prim_utils def define_origins(num_origins: int, spacing: float) -> list[list[float]]: diff --git a/source/isaaclab/test/sim/test_build_simulation_context_headless.py b/source/isaaclab/test/sim/test_build_simulation_context_headless.py index af29346f9ee..e21c23892c0 100644 --- a/source/isaaclab/test/sim/test_build_simulation_context_headless.py +++ b/source/isaaclab/test/sim/test_build_simulation_context_headless.py @@ -21,10 +21,10 @@ """Rest everything follows.""" import pytest -from isaacsim.core.utils.prims import is_prim_path_valid from isaaclab.sim.simulation_cfg import SimulationCfg from isaaclab.sim.simulation_context import build_simulation_context +from isaaclab.utils.prims import is_prim_path_valid @pytest.mark.parametrize("gravity_enabled", [True, False]) diff --git a/source/isaaclab/test/sim/test_build_simulation_context_nonheadless.py b/source/isaaclab/test/sim/test_build_simulation_context_nonheadless.py index 1c1bf480da4..9f14ffc8ff6 100644 --- a/source/isaaclab/test/sim/test_build_simulation_context_nonheadless.py +++ b/source/isaaclab/test/sim/test_build_simulation_context_nonheadless.py @@ -20,10 +20,10 @@ """Rest everything follows.""" import pytest -from isaacsim.core.utils.prims import is_prim_path_valid from isaaclab.sim.simulation_cfg import SimulationCfg from isaaclab.sim.simulation_context import build_simulation_context +from isaaclab.utils.prims import is_prim_path_valid @pytest.mark.parametrize("gravity_enabled", [True, False]) diff --git a/source/isaaclab/test/sim/test_mesh_converter.py b/source/isaaclab/test/sim/test_mesh_converter.py index 90bfc557c78..69bd24d427d 100644 --- a/source/isaaclab/test/sim/test_mesh_converter.py +++ b/source/isaaclab/test/sim/test_mesh_converter.py @@ -17,13 +17,13 @@ import random import tempfile -import isaacsim.core.utils.prims as prim_utils -import isaacsim.core.utils.stage as stage_utils import omni import pytest from isaacsim.core.api.simulation_context import SimulationContext from pxr import UsdGeom, UsdPhysics +import isaaclab.utils.prims as prim_utils +import isaaclab.utils.stage as stage_utils from isaaclab.sim.converters import MeshConverter, MeshConverterCfg from isaaclab.sim.schemas import schemas_cfg from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR, retrieve_file_path diff --git a/source/isaaclab/test/sim/test_mjcf_converter.py b/source/isaaclab/test/sim/test_mjcf_converter.py index 5921b12fc6c..82a0d0da2da 100644 --- a/source/isaaclab/test/sim/test_mjcf_converter.py +++ b/source/isaaclab/test/sim/test_mjcf_converter.py @@ -14,12 +14,12 @@ import os -import isaacsim.core.utils.prims as prim_utils -import isaacsim.core.utils.stage as stage_utils import pytest from isaacsim.core.api.simulation_context import SimulationContext from isaacsim.core.utils.extensions import enable_extension, get_extension_path_from_name +import isaaclab.utils.prims as prim_utils +import isaaclab.utils.stage as stage_utils from isaaclab.sim.converters import MjcfConverter, MjcfConverterCfg diff --git a/source/isaaclab/test/sim/test_schemas.py b/source/isaaclab/test/sim/test_schemas.py index 29b451f4214..25dbbf091bc 100644 --- a/source/isaaclab/test/sim/test_schemas.py +++ b/source/isaaclab/test/sim/test_schemas.py @@ -14,13 +14,13 @@ import math -import isaacsim.core.utils.prims as prim_utils -import isaacsim.core.utils.stage as stage_utils import pytest from isaacsim.core.api.simulation_context import SimulationContext from pxr import UsdPhysics import isaaclab.sim.schemas as schemas +import isaaclab.utils.prims as prim_utils +import isaaclab.utils.stage as stage_utils from isaaclab.sim.utils import find_global_fixed_joint_prim from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR from isaaclab.utils.string import to_camel_case diff --git a/source/isaaclab/test/sim/test_simulation_context.py b/source/isaaclab/test/sim/test_simulation_context.py index f0f783463d2..60b61779953 100644 --- a/source/isaaclab/test/sim/test_simulation_context.py +++ b/source/isaaclab/test/sim/test_simulation_context.py @@ -14,10 +14,10 @@ import numpy as np -import isaacsim.core.utils.prims as prim_utils import pytest from isaacsim.core.api.simulation_context import SimulationContext as IsaacSimulationContext +import isaaclab.utils.prims as prim_utils from isaaclab.sim import SimulationCfg, SimulationContext diff --git a/source/isaaclab/test/sim/test_spawn_from_files.py b/source/isaaclab/test/sim/test_spawn_from_files.py index 59b2741e4ee..6dfb9d6bd65 100644 --- a/source/isaaclab/test/sim/test_spawn_from_files.py +++ b/source/isaaclab/test/sim/test_spawn_from_files.py @@ -12,13 +12,13 @@ """Rest everything follows.""" -import isaacsim.core.utils.prims as prim_utils -import isaacsim.core.utils.stage as stage_utils import pytest from isaacsim.core.api.simulation_context import SimulationContext from isaacsim.core.utils.extensions import enable_extension, get_extension_path_from_name import isaaclab.sim as sim_utils +import isaaclab.utils.prims as prim_utils +import isaaclab.utils.stage as stage_utils from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR diff --git a/source/isaaclab/test/sim/test_spawn_lights.py b/source/isaaclab/test/sim/test_spawn_lights.py index ec178244e1b..e1846b331dc 100644 --- a/source/isaaclab/test/sim/test_spawn_lights.py +++ b/source/isaaclab/test/sim/test_spawn_lights.py @@ -12,13 +12,13 @@ """Rest everything follows.""" -import isaacsim.core.utils.prims as prim_utils -import isaacsim.core.utils.stage as stage_utils import pytest from isaacsim.core.api.simulation_context import SimulationContext from pxr import UsdLux import isaaclab.sim as sim_utils +import isaaclab.utils.prims as prim_utils +import isaaclab.utils.stage as stage_utils from isaaclab.utils.string import to_camel_case diff --git a/source/isaaclab/test/sim/test_spawn_materials.py b/source/isaaclab/test/sim/test_spawn_materials.py index e95ee6e3724..dcceff892d3 100644 --- a/source/isaaclab/test/sim/test_spawn_materials.py +++ b/source/isaaclab/test/sim/test_spawn_materials.py @@ -12,13 +12,13 @@ """Rest everything follows.""" -import isaacsim.core.utils.prims as prim_utils -import isaacsim.core.utils.stage as stage_utils import pytest from isaacsim.core.api.simulation_context import SimulationContext from pxr import UsdPhysics, UsdShade import isaaclab.sim as sim_utils +import isaaclab.utils.prims as prim_utils +import isaaclab.utils.stage as stage_utils from isaaclab.utils.assets import NVIDIA_NUCLEUS_DIR diff --git a/source/isaaclab/test/sim/test_spawn_meshes.py b/source/isaaclab/test/sim/test_spawn_meshes.py index b2297255d97..b4a4bb1fc0e 100644 --- a/source/isaaclab/test/sim/test_spawn_meshes.py +++ b/source/isaaclab/test/sim/test_spawn_meshes.py @@ -12,12 +12,12 @@ """Rest everything follows.""" -import isaacsim.core.utils.prims as prim_utils -import isaacsim.core.utils.stage as stage_utils import pytest from isaacsim.core.api.simulation_context import SimulationContext import isaaclab.sim as sim_utils +import isaaclab.utils.prims as prim_utils +import isaaclab.utils.stage as stage_utils @pytest.fixture diff --git a/source/isaaclab/test/sim/test_spawn_sensors.py b/source/isaaclab/test/sim/test_spawn_sensors.py index ac0cab828ad..c516ad1ecb3 100644 --- a/source/isaaclab/test/sim/test_spawn_sensors.py +++ b/source/isaaclab/test/sim/test_spawn_sensors.py @@ -12,12 +12,12 @@ """Rest everything follows.""" -import isaacsim.core.utils.prims as prim_utils -import isaacsim.core.utils.stage as stage_utils import pytest from isaacsim.core.api.simulation_context import SimulationContext import isaaclab.sim as sim_utils +import isaaclab.utils.prims as prim_utils +import isaaclab.utils.stage as stage_utils from isaaclab.sim.spawners.sensors.sensors import CUSTOM_FISHEYE_CAMERA_ATTRIBUTES, CUSTOM_PINHOLE_CAMERA_ATTRIBUTES from isaaclab.utils.string import to_camel_case diff --git a/source/isaaclab/test/sim/test_spawn_shapes.py b/source/isaaclab/test/sim/test_spawn_shapes.py index c889a4ab818..63747470983 100644 --- a/source/isaaclab/test/sim/test_spawn_shapes.py +++ b/source/isaaclab/test/sim/test_spawn_shapes.py @@ -12,12 +12,12 @@ """Rest everything follows.""" -import isaacsim.core.utils.prims as prim_utils -import isaacsim.core.utils.stage as stage_utils import pytest from isaacsim.core.api.simulation_context import SimulationContext import isaaclab.sim as sim_utils +import isaaclab.utils.prims as prim_utils +import isaaclab.utils.stage as stage_utils @pytest.fixture diff --git a/source/isaaclab/test/sim/test_spawn_wrappers.py b/source/isaaclab/test/sim/test_spawn_wrappers.py index 5edae7a79d6..57d7bb2ce34 100644 --- a/source/isaaclab/test/sim/test_spawn_wrappers.py +++ b/source/isaaclab/test/sim/test_spawn_wrappers.py @@ -12,12 +12,12 @@ """Rest everything follows.""" -import isaacsim.core.utils.prims as prim_utils -import isaacsim.core.utils.stage as stage_utils import pytest from isaacsim.core.api.simulation_context import SimulationContext import isaaclab.sim as sim_utils +import isaaclab.utils.prims as prim_utils +import isaaclab.utils.stage as stage_utils from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR diff --git a/source/isaaclab/test/sim/test_stage_in_memory.py b/source/isaaclab/test/sim/test_stage_in_memory.py index d114185862a..5770e1ed776 100644 --- a/source/isaaclab/test/sim/test_stage_in_memory.py +++ b/source/isaaclab/test/sim/test_stage_in_memory.py @@ -12,8 +12,6 @@ """Rest everything follows.""" -import isaacsim.core.utils.prims as prim_utils -import isaacsim.core.utils.stage as stage_utils import omni import omni.physx import omni.usd @@ -23,6 +21,8 @@ from isaacsim.core.version import get_version import isaaclab.sim as sim_utils +import isaaclab.utils.prims as prim_utils +import isaaclab.utils.stage as stage_utils from isaaclab.sim.simulation_context import SimulationCfg, SimulationContext from isaaclab.utils.assets import ISAACLAB_NUCLEUS_DIR diff --git a/source/isaaclab/test/sim/test_urdf_converter.py b/source/isaaclab/test/sim/test_urdf_converter.py index f238fd02408..5eb7992dca7 100644 --- a/source/isaaclab/test/sim/test_urdf_converter.py +++ b/source/isaaclab/test/sim/test_urdf_converter.py @@ -15,13 +15,13 @@ import numpy as np import os -import isaacsim.core.utils.prims as prim_utils -import isaacsim.core.utils.stage as stage_utils import pytest from isaacsim.core.api.simulation_context import SimulationContext from isaacsim.core.prims import Articulation from isaacsim.core.utils.extensions import enable_extension, get_extension_path_from_name +import isaaclab.utils.prims as prim_utils +import isaaclab.utils.stage as stage_utils from isaaclab.sim.converters import UrdfConverter, UrdfConverterCfg diff --git a/source/isaaclab/test/sim/test_utils.py b/source/isaaclab/test/sim/test_utils.py index a18e0534294..ef01f9bc379 100644 --- a/source/isaaclab/test/sim/test_utils.py +++ b/source/isaaclab/test/sim/test_utils.py @@ -15,13 +15,13 @@ import numpy as np import torch -import isaacsim.core.utils.prims as prim_utils -import isaacsim.core.utils.stage as stage_utils import pytest from pxr import Sdf, Usd, UsdGeom, UsdPhysics import isaaclab.sim as sim_utils import isaaclab.utils.math as math_utils +import isaaclab.utils.prims as prim_utils +import isaaclab.utils.stage as stage_utils from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR, ISAACLAB_NUCLEUS_DIR diff --git a/source/isaaclab/test/terrains/check_terrain_importer.py b/source/isaaclab/test/terrains/check_terrain_importer.py index 2de8b457e32..c140063e466 100644 --- a/source/isaaclab/test/terrains/check_terrain_importer.py +++ b/source/isaaclab/test/terrains/check_terrain_importer.py @@ -64,7 +64,6 @@ import numpy as np -import isaacsim.core.utils.prims as prim_utils import omni.kit import omni.kit.commands from isaacsim.core.api.materials import PhysicsMaterial @@ -78,6 +77,7 @@ import isaaclab.sim as sim_utils import isaaclab.terrains as terrain_gen +import isaaclab.utils.prims as prim_utils from isaaclab.terrains.config.rough import ROUGH_TERRAINS_CFG from isaaclab.terrains.terrain_importer import TerrainImporter from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR diff --git a/source/isaaclab/test/terrains/test_terrain_importer.py b/source/isaaclab/test/terrains/test_terrain_importer.py index 26bacac387c..0eaad8b77df 100644 --- a/source/isaaclab/test/terrains/test_terrain_importer.py +++ b/source/isaaclab/test/terrains/test_terrain_importer.py @@ -17,7 +17,6 @@ import trimesh from typing import Literal -import isaacsim.core.utils.prims as prim_utils import omni.kit import omni.kit.commands import pytest @@ -29,6 +28,7 @@ from pxr import UsdGeom import isaaclab.terrains as terrain_gen +import isaaclab.utils.prims as prim_utils from isaaclab.sim import PreviewSurfaceCfg, SimulationContext, build_simulation_context, get_first_matching_child_prim from isaaclab.terrains import TerrainImporter, TerrainImporterCfg from isaaclab.terrains.config.rough import ROUGH_TERRAINS_CFG diff --git a/source/isaaclab/test/visualization/check_scene_xr_visualization.py b/source/isaaclab/test/visualization/check_scene_xr_visualization.py index dd614082b8e..12b7e6f1977 100644 --- a/source/isaaclab/test/visualization/check_scene_xr_visualization.py +++ b/source/isaaclab/test/visualization/check_scene_xr_visualization.py @@ -70,9 +70,10 @@ def get_camera_position(): tuple: (x, y, z) camera position or None if not available """ try: - import isaacsim.core.utils.stage as stage_utils from pxr import UsdGeom + import isaaclab.utils.stage as stage_utils + stage = stage_utils.get_current_stage() if stage is not None: # Get the viewport camera prim diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/franka_cabinet/franka_cabinet_env.py b/source/isaaclab_tasks/isaaclab_tasks/direct/franka_cabinet/franka_cabinet_env.py index 8e0aab5b0c7..4fb0da01d82 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/franka_cabinet/franka_cabinet_env.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/franka_cabinet/franka_cabinet_env.py @@ -7,7 +7,6 @@ import torch -from isaacsim.core.utils.stage import get_current_stage from isaacsim.core.utils.torch.transformations import tf_combine, tf_inverse, tf_vector from pxr import UsdGeom @@ -21,6 +20,7 @@ from isaaclab.utils import configclass from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR, ISAACLAB_NUCLEUS_DIR from isaaclab.utils.math import sample_uniform +from isaaclab.utils.stage import get_current_stage @configclass diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand/shadow_hand_vision_env.py b/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand/shadow_hand_vision_env.py index 42e8c4f03c4..12448271704 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand/shadow_hand_vision_env.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/shadow_hand/shadow_hand_vision_env.py @@ -14,14 +14,13 @@ except ModuleNotFoundError: from pxr import Semantics -from isaacsim.core.utils.stage import get_current_stage - import isaaclab.sim as sim_utils from isaaclab.assets import Articulation, RigidObject from isaaclab.scene import InteractiveSceneCfg from isaaclab.sensors import TiledCamera, TiledCameraCfg from isaaclab.utils import configclass from isaaclab.utils.math import quat_apply +from isaaclab.utils.stage import get_current_stage from isaaclab_tasks.direct.inhand_manipulation.inhand_manipulation_env import InHandManipulationEnv, unscale diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/mdp/utils.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/mdp/utils.py index f7b8e9db59b..6e155ae29f2 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/mdp/utils.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/manipulation/dexsuite/mdp/utils.py @@ -10,9 +10,9 @@ import trimesh from trimesh.sample import sample_surface -import isaacsim.core.utils.prims as prim_utils from pxr import UsdGeom +import isaaclab.utils.prims as prim_utils from isaaclab.sim.utils import get_all_matching_child_prims # ---- module-scope caches ---- From 0b44cc30419a87173879da588c40453ae1f2fc2a Mon Sep 17 00:00:00 2001 From: Pascal Roth Date: Sun, 2 Nov 2025 18:04:53 +0100 Subject: [PATCH 2/5] wip --- .../isaaclab/isaaclab/envs/direct_marl_env.py | 4 +- .../isaaclab/isaaclab/envs/direct_rl_env.py | 4 +- .../isaaclab/envs/manager_based_env.py | 4 +- source/isaaclab/isaaclab/utils/__init__.py | 4 + source/isaaclab/isaaclab/utils/prims.py | 46 ++--- source/isaaclab/isaaclab/utils/seed.py | 35 ++++ source/isaaclab/isaaclab/utils/semantics.py | 48 +++-- source/isaaclab/isaaclab/utils/stage.py | 169 ++++-------------- .../test/terrains/test_terrain_generator.py | 8 +- 9 files changed, 141 insertions(+), 181 deletions(-) create mode 100644 source/isaaclab/isaaclab/utils/seed.py diff --git a/source/isaaclab/isaaclab/envs/direct_marl_env.py b/source/isaaclab/isaaclab/envs/direct_marl_env.py index 0e7429117fc..637b7b65985 100644 --- a/source/isaaclab/isaaclab/envs/direct_marl_env.py +++ b/source/isaaclab/isaaclab/envs/direct_marl_env.py @@ -17,7 +17,6 @@ from dataclasses import MISSING from typing import Any, ClassVar -import isaacsim.core.utils.torch as torch_utils import omni.kit.app import omni.log import omni.physx @@ -29,6 +28,7 @@ from isaaclab.sim.utils import attach_stage_to_usd_context, use_stage from isaaclab.utils.noise import NoiseModel from isaaclab.utils.timer import Timer +from isaaclab.utils.seed import set_seed from .common import ActionType, AgentID, EnvStepReturn, ObsType, StateType from .direct_marl_env_cfg import DirectMARLEnvCfg @@ -462,7 +462,7 @@ def seed(seed: int = -1) -> int: except ModuleNotFoundError: pass # set seed for torch and other libraries - return torch_utils.set_seed(seed) + return set_seed(seed) def render(self, recompute: bool = False) -> np.ndarray | None: """Run rendering without stepping through the physics. diff --git a/source/isaaclab/isaaclab/envs/direct_rl_env.py b/source/isaaclab/isaaclab/envs/direct_rl_env.py index e43c4db7a28..28155ba0c34 100644 --- a/source/isaaclab/isaaclab/envs/direct_rl_env.py +++ b/source/isaaclab/isaaclab/envs/direct_rl_env.py @@ -17,7 +17,6 @@ from dataclasses import MISSING from typing import Any, ClassVar -import isaacsim.core.utils.torch as torch_utils import omni.kit.app import omni.log import omni.physx @@ -30,6 +29,7 @@ from isaaclab.sim.utils import attach_stage_to_usd_context, use_stage from isaaclab.utils.noise import NoiseModel from isaaclab.utils.timer import Timer +from isaaclab.utils.seed import set_seed from .common import VecEnvObs, VecEnvStepReturn from .direct_rl_env_cfg import DirectRLEnvCfg @@ -414,7 +414,7 @@ def seed(seed: int = -1) -> int: except ModuleNotFoundError: pass # set seed for torch and other libraries - return torch_utils.set_seed(seed) + return set_seed(seed) def render(self, recompute: bool = False) -> np.ndarray | None: """Run rendering without stepping through the physics. diff --git a/source/isaaclab/isaaclab/envs/manager_based_env.py b/source/isaaclab/isaaclab/envs/manager_based_env.py index 9ddc538aa41..636ef3fb778 100644 --- a/source/isaaclab/isaaclab/envs/manager_based_env.py +++ b/source/isaaclab/isaaclab/envs/manager_based_env.py @@ -8,7 +8,6 @@ from collections.abc import Sequence from typing import Any -import isaacsim.core.utils.torch as torch_utils import omni.log import omni.physx from isaacsim.core.simulation_manager import SimulationManager @@ -20,6 +19,7 @@ from isaaclab.sim.utils import attach_stage_to_usd_context, use_stage from isaaclab.ui.widgets import ManagerLiveVisualizer from isaaclab.utils.timer import Timer +from isaaclab.utils.seed import set_seed from .common import VecEnvObs from .manager_based_env_cfg import ManagerBasedEnvCfg @@ -495,7 +495,7 @@ def seed(seed: int = -1) -> int: except ModuleNotFoundError: pass # set seed for torch and other libraries - return torch_utils.set_seed(seed) + return set_seed(seed) def close(self): """Cleanup for the environment.""" diff --git a/source/isaaclab/isaaclab/utils/__init__.py b/source/isaaclab/isaaclab/utils/__init__.py index a5365948699..01756018c29 100644 --- a/source/isaaclab/isaaclab/utils/__init__.py +++ b/source/isaaclab/isaaclab/utils/__init__.py @@ -15,3 +15,7 @@ from .timer import Timer from .types import * from .version import * +from .stage import * +from .prims import * +from .semantics import * +from .seed import * diff --git a/source/isaaclab/isaaclab/utils/prims.py b/source/isaaclab/isaaclab/utils/prims.py index bdb816cb700..e1d83f4bc81 100644 --- a/source/isaaclab/isaaclab/utils/prims.py +++ b/source/isaaclab/isaaclab/utils/prims.py @@ -1,24 +1,30 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +import numpy as np import re # python import typing -import numpy as np import omni.usd import usdrt # isaacsim from isaacsim.core.utils._isaac_utils import _find_matching_prim_paths -from isaaclab.utils.semantics import add_labels -from isaaclab.utils.stage import add_reference_to_stage, get_current_stage, get_current_stage_id -from isaaclab.utils.types import SDF_type_to_Gf from omni.usd.commands import DeletePrimsCommand, MovePrimCommand # omniverse from pxr import Sdf, Usd, UsdGeom, UsdPhysics +from isaaclab.utils.semantics import add_labels +from isaaclab.utils.stage import add_reference_to_stage, get_current_stage, get_current_stage_id +from isaaclab.utils.types import SDF_type_to_Gf + -def get_prim_at_path(prim_path: str, fabric: bool = False) -> typing.Union[Usd.Prim, usdrt.Usd._Usd.Prim]: +def get_prim_at_path(prim_path: str, fabric: bool = False) -> Usd.Prim | usdrt.Usd._Usd.Prim: """Get the USD or Fabric Prim at a given path string Args: @@ -76,7 +82,7 @@ def is_prim_path_valid(prim_path: str, fabric: bool = False) -> bool: return False -def get_prim_attribute_names(prim_path: str, fabric: bool = False) -> typing.List[str]: +def get_prim_attribute_names(prim_path: str, fabric: bool = False) -> list[str]: """Get all the attribute names of a prim at the path Args: @@ -201,7 +207,7 @@ def define_prim(prim_path: str, prim_type: str = "Xform", fabric: bool = False) Usd.Prim() """ if is_prim_path_valid(prim_path, fabric=fabric): - raise Exception("A prim already exists at prim path: {}".format(prim_path)) + raise Exception(f"A prim already exists at prim path: {prim_path}") return get_current_stage(fabric=fabric).DefinePrim(prim_path, prim_type) @@ -228,7 +234,7 @@ def get_prim_type_name(prim_path: str, fabric: bool = False) -> str: Cube """ if not is_prim_path_valid(prim_path, fabric=fabric): - raise Exception("A prim does not exist at prim path: {}".format(prim_path)) + raise Exception(f"A prim does not exist at prim path: {prim_path}") prim = get_prim_at_path(prim_path, fabric=fabric) if fabric: return prim.GetTypeName() @@ -323,8 +329,8 @@ def get_first_matching_parent_prim(prim_path: str, predicate: typing.Callable[[s def get_all_matching_child_prims( - prim_path: str, predicate: typing.Callable[[str], bool] = lambda x: True, depth: typing.Optional[int] = None -) -> typing.List[Usd.Prim]: + prim_path: str, predicate: typing.Callable[[str], bool] = lambda x: True, depth: int | None = None +) -> list[Usd.Prim]: """Performs a breadth-first search starting from the root and returns all the prims matching the predicate. Args: @@ -365,7 +371,7 @@ def get_all_matching_child_prims( return out -def find_matching_prim_paths(prim_path_regex: str, prim_type: typing.Optional[str] = None) -> typing.List[str]: +def find_matching_prim_paths(prim_path_regex: str, prim_type: str | None = None) -> list[str]: """Find all the matching prim paths in the stage based on Regex expression. Args: @@ -392,7 +398,7 @@ def find_matching_prim_paths(prim_path_regex: str, prim_type: typing.Optional[st return _find_matching_prim_paths(prim_path_regex.replace(".*", "*"), stage_id, prim_type) -def get_prim_children(prim: Usd.Prim) -> typing.List[Usd.Prim]: +def get_prim_children(prim: Usd.Prim) -> list[Usd.Prim]: """Return the call of the USD Prim's GetChildren member function Args: @@ -644,14 +650,14 @@ def set_prim_visibility(prim: Usd.Prim, visible: bool) -> None: def create_prim( prim_path: str, prim_type: str = "Xform", - position: typing.Optional[typing.Sequence[float]] = None, - translation: typing.Optional[typing.Sequence[float]] = None, - orientation: typing.Optional[typing.Sequence[float]] = None, - scale: typing.Optional[typing.Sequence[float]] = None, - usd_path: typing.Optional[str] = None, - semantic_label: typing.Optional[str] = None, + position: typing.Sequence[float] | None = None, + translation: typing.Sequence[float] | None = None, + orientation: typing.Sequence[float] | None = None, + scale: typing.Sequence[float] | None = None, + usd_path: str | None = None, + semantic_label: str | None = None, semantic_type: str = "class", - attributes: typing.Optional[dict] = None, + attributes: dict | None = None, ) -> Usd.Prim: """Create a prim into current USD stage. @@ -804,7 +810,7 @@ def set_prim_property(prim_path: str, property_name: str, property_value: typing prim.GetAttribute(property_name).Set(property_value) -def get_prim_object_type(prim_path: str) -> typing.Union[str, None]: +def get_prim_object_type(prim_path: str) -> str | None: """Get the dynamic control object type of the USD Prim at the given path. If the prim at the path is of Dynamic Control type e.g.: rigid_body, joint, dof, articulation, attractor, d6joint, diff --git a/source/isaaclab/isaaclab/utils/seed.py b/source/isaaclab/isaaclab/utils/seed.py new file mode 100644 index 00000000000..8f24228864e --- /dev/null +++ b/source/isaaclab/isaaclab/utils/seed.py @@ -0,0 +1,35 @@ +import os +import random + +import numpy as np +import torch +import warp as wp + + +def set_seed(seed: int, torch_deterministic: bool = False) -> int: + """set seed across modules""" + if seed == -1 and torch_deterministic: + seed = 42 + elif seed == -1: + seed = np.random.randint(0, 10000) + print("Setting seed: {}".format(seed)) + + random.seed(seed) + np.random.seed(seed) + torch.manual_seed(seed) + os.environ["PYTHONHASHSEED"] = str(seed) + torch.cuda.manual_seed(seed) + torch.cuda.manual_seed_all(seed) + wp.rand_init(seed) + + if torch_deterministic: + # refer to https://docs.nvidia.com/cuda/cublas/index.html#cublasApi_reproducibility + os.environ["CUBLAS_WORKSPACE_CONFIG"] = ":4096:8" + torch.backends.cudnn.benchmark = False + torch.backends.cudnn.deterministic = True + torch.use_deterministic_algorithms(True) + else: + torch.backends.cudnn.benchmark = True + torch.backends.cudnn.deterministic = False + + return seed \ No newline at end of file diff --git a/source/isaaclab/isaaclab/utils/semantics.py b/source/isaaclab/isaaclab/utils/semantics.py index 4e09d9e7275..30073811cec 100644 --- a/source/isaaclab/isaaclab/utils/semantics.py +++ b/source/isaaclab/isaaclab/utils/semantics.py @@ -1,3 +1,8 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + # SPDX-FileCopyrightText: Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. # SPDX-License-Identifier: Apache-2.0 # @@ -15,12 +20,13 @@ from typing import Dict, List, Optional, Tuple import carb -import isaaclab.utils.prims as prim_utils import omni.usd import Semantics -from isaaclab.utils.stage import get_current_stage, get_current_stage_id from pxr import Usd, UsdGeom, UsdSemantics +import isaaclab.utils.prims as prim_utils +from isaaclab.utils.stage import get_current_stage, get_current_stage_id + def add_update_semantics(prim: Usd.Prim, semantic_label: str, type_label: str = "class", suffix="") -> None: """[DEPRECATED] Apply a semantic label to a prim or update an existing label using the old SemanticsAPI. @@ -32,8 +38,8 @@ def add_update_semantics(prim: Usd.Prim, semantic_label: str, type_label: str = suffix (str): Additional suffix used to specify multiple semantic attribute names. """ carb.log_warn( - "Function 'add_update_semantics' uses the deprecated SemanticsAPI. " - "Consider upgrading the semantics with `upgrade_prim_semantics_to_labels(prim)` first and then using `add_labels` instead." + "Function 'add_update_semantics' uses the deprecated SemanticsAPI. Consider upgrading the semantics with" + " `upgrade_prim_semantics_to_labels(prim)` first and then using `add_labels` instead." ) # Apply or acquire the existing SemanticAPI semantic_api = Semantics.SemanticsAPI.Get(prim, "Semantics" + suffix) @@ -61,8 +67,8 @@ def remove_all_semantics(prim: Usd.Prim, recursive: bool = False) -> None: recursive (bool, optional): Also traverse children and remove semantics recursively. Defaults to False. """ carb.log_warn( - "Function 'remove_all_semantics' uses the deprecated SemanticsAPI. " - "Consider upgrading the semantics with `upgrade_prim_semantics_to_labels(prim, recursive=True)` first and then using `remove_labels` instead." + "Function 'remove_all_semantics' uses the deprecated SemanticsAPI. Consider upgrading the semantics with" + " `upgrade_prim_semantics_to_labels(prim, recursive=True)` first and then using `remove_labels` instead." ) def remove_semantics(input_prim: Usd.Prim): @@ -85,7 +91,7 @@ def remove_semantics(input_prim: Usd.Prim): remove_semantics(prim) -def get_semantics(prim: Usd.Prim) -> Dict[str, Tuple[str, str]]: +def get_semantics(prim: Usd.Prim) -> dict[str, tuple[str, str]]: """[DEPRECATED] Returns semantics (old SemanticsAPI) that are applied to a prim Args: @@ -95,8 +101,8 @@ def get_semantics(prim: Usd.Prim) -> Dict[str, Tuple[str, str]]: Dict[str, Tuple[str,str]]: Dictionary containing the name of the applied semantic, and the type and data associated with that semantic. """ carb.log_warn( - "Function 'get_semantics' reads the deprecated SemanticsAPI. " - "Consider upgrading the semantics with `upgrade_prim_semantics_to_labels(prim)` first and then using `get_labels` instead." + "Function 'get_semantics' reads the deprecated SemanticsAPI. Consider upgrading the semantics with" + " `upgrade_prim_semantics_to_labels(prim)` first and then using `get_labels` instead." ) result = {} for prop in prim.GetProperties(): @@ -111,7 +117,7 @@ def get_semantics(prim: Usd.Prim) -> Dict[str, Tuple[str, str]]: return result -def check_missing_semantics(prim_path: str = None) -> List[str]: +def check_missing_semantics(prim_path: str = None) -> list[str]: """[DEPRECATED] Returns a list of prim path of meshes with missing semantics (old SemanticsAPI) Args: @@ -121,8 +127,9 @@ def check_missing_semantics(prim_path: str = None) -> List[str]: List[str]: Prim paths """ carb.log_warn( - "Function 'check_missing_semantics' uses the deprecated SemanticsAPI via get_semantics. " - "Consider upgrading the semantics with `upgrade_prim_semantics_to_labels(prim, recursive=True)` first and then using `check_missing_labels` instead." + "Function 'check_missing_semantics' uses the deprecated SemanticsAPI via get_semantics. Consider upgrading the" + " semantics with `upgrade_prim_semantics_to_labels(prim, recursive=True)` first and then using" + " `check_missing_labels` instead." ) prim_paths = [] prims = [] @@ -141,7 +148,7 @@ def check_missing_semantics(prim_path: str = None) -> List[str]: return prim_paths -def check_incorrect_semantics(prim_path: str = None) -> List[List[str]]: +def check_incorrect_semantics(prim_path: str = None) -> list[list[str]]: """[DEPRECATED] Returns a list of prim path of meshes with different semantics labels (old SemanticsAPI) than their prim path and their semantic labels Args: @@ -151,8 +158,9 @@ def check_incorrect_semantics(prim_path: str = None) -> List[List[str]]: List[List[str]]: List of prim path and semantic label """ carb.log_warn( - "Function 'check_incorrect_semantics' uses the deprecated SemanticsAPI via get_semantics. " - "Consider upgrading the semantics with `upgrade_prim_semantics_to_labels(prim, recursive=True)` first and then using `check_incorrect_labels` instead." + "Function 'check_incorrect_semantics' uses the deprecated SemanticsAPI via get_semantics. Consider upgrading" + " the semantics with `upgrade_prim_semantics_to_labels(prim, recursive=True)` first and then using" + " `check_incorrect_labels` instead." ) incorrect_pairs = [] if prim_path is None: @@ -180,7 +188,7 @@ def check_incorrect_semantics(prim_path: str = None) -> List[List[str]]: return incorrect_pairs -def count_semantics_in_scene(prim_path: str = None) -> Dict[str, int]: +def count_semantics_in_scene(prim_path: str = None) -> dict[str, int]: """[DEPRECATED] Returns a dictionary of labels (old SemanticsAPI) and the corresponding count Args: @@ -190,8 +198,9 @@ def count_semantics_in_scene(prim_path: str = None) -> Dict[str, int]: Dict[str, int]: labels and count """ carb.log_warn( - "Function 'count_semantics_in_scene' uses the deprecated SemanticsAPI via get_semantics. " - "Consider upgrading the semantics with `upgrade_prim_semantics_to_labels(prim, recursive=True)` first and then using `count_labels_in_scene` instead." + "Function 'count_semantics_in_scene' uses the deprecated SemanticsAPI via get_semantics. Consider upgrading the" + " semantics with `upgrade_prim_semantics_to_labels(prim, recursive=True)` first and then using" + " `count_labels_in_scene` instead." ) semantics_counter = {"missing": 0} if prim_path is None: @@ -446,7 +455,8 @@ def upgrade_prim_semantics_to_labels(prim: Usd.Prim, include_descendants: bool = if not old_type or not old_data: carb.log_warn( - f"[upgrade_prim] Skipping instance '{old_instance_name}' on {current_prim.GetPath()} due to missing type or data." + f"[upgrade_prim] Skipping instance '{old_instance_name}' on {current_prim.GetPath()} due to missing" + " type or data." ) continue diff --git a/source/isaaclab/isaaclab/utils/stage.py b/source/isaaclab/isaaclab/utils/stage.py index 54e1ef63cc2..540f8fdd97a 100644 --- a/source/isaaclab/isaaclab/utils/stage.py +++ b/source/isaaclab/isaaclab/utils/stage.py @@ -1,28 +1,45 @@ +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + import builtins import contextlib import threading - -# python import typing -# omniverse import omni -# import omni.log import omni.kit.app import usdrt - -# isaacsim -from isaacsim.core.utils.constants import AXES_TOKEN -from omni.kit.usd import layers +from pxr import Sdf, Usd, UsdGeom, UsdUtils from omni.metrics.assembler.core import get_metrics_assembler_interface from omni.usd.commands import DeletePrimsCommand -from pxr import Sdf, Usd, UsdGeom, UsdUtils _context = threading.local() # thread-local storage to handle nested contexts and concurrent access +AXES_TOKEN = { + "X": UsdGeom.Tokens.x, + "x": UsdGeom.Tokens.x, + "Y": UsdGeom.Tokens.y, + "y": UsdGeom.Tokens.y, + "Z": UsdGeom.Tokens.z, + "z": UsdGeom.Tokens.z, +} +"""Mapping from axis name to axis USD token + + >>> import isaacsim.core.utils.constants as constants_utils + >>> + >>> # get the x-axis USD token + >>> constants_utils.AXES_TOKEN['x'] + X + >>> constants_utils.AXES_TOKEN['X'] + X +""" + + @contextlib.contextmanager -def use_stage(stage: Usd.Stage) -> None: +def use_stage(stage: Usd.Stage): """Context manager that sets a thread-local stage. Args: @@ -60,14 +77,14 @@ def use_stage(stage: Usd.Stage) -> None: _context.stage = previous_stage -def get_current_stage(fabric: bool = False) -> typing.Union[Usd.Stage, usdrt.Usd._Usd.Stage]: +def get_current_stage(fabric: bool = False) -> Usd.Stage | usdrt.Usd._Usd.Stage: """Get the current open USD or Fabric stage Args: - fabric (bool, optional): True to get the fabric stage. False to get the USD stage. Defaults to False. + fabric: True to get the fabric stage. False to get the USD stage. Defaults to False. Returns: - typing.Union[Usd.Stage, usdrt.Usd._Usd.Stage]: The USD or Fabric stage as specified by the input arg fabric. + The USD or Fabric stage as specified by the input arg fabric. Example: @@ -127,25 +144,6 @@ def update_stage() -> None: omni.kit.app.get_app_interface().update() -async def update_stage_async() -> None: - """Update the current USD stage (asynchronous version). - - Example: - - .. code-block:: python - - >>> import asyncio - >>> import isaaclab.utils.stage as stage_utils - >>> from omni.kit.async_engine import run_coroutine - >>> - >>> async def task(): - ... await stage_utils.update_stage_async() - ... - >>> run_coroutine(task()) - """ - await omni.kit.app.get_app().next_update_async() - - # TODO: make a generic util for setting all layer properties def set_stage_up_axis(axis: str = "z") -> None: """Change the up axis of the current stage @@ -190,12 +188,11 @@ def get_stage_up_axis() -> str: return UsdGeom.GetStageUpAxis(stage) -def clear_stage(predicate: typing.Optional[typing.Callable[[str], bool]] = None) -> None: +def clear_stage(predicate: typing.Callable[[str], bool] | None = None) -> None: """Deletes all prims in the stage without populating the undo command buffer Args: - predicate (typing.Optional[typing.Callable[[str], bool]], optional): - user defined function that takes a prim_path (str) as input and returns True/False if the prim + predicate: user defined function that takes a prim_path (str) as input and returns True/False if the prim should/shouldn't be deleted. If predicate is None, a default is used that deletes all prims Example: @@ -337,11 +334,11 @@ def add_reference_to_stage(usd_path: str, prim_path: str, prim_type: str = "Xfor # ) success_bool = prim.GetReferences().AddReference(usd_path) if not success_bool: - raise FileNotFoundError("The usd file at path {} provided wasn't found".format(usd_path)) + raise FileNotFoundError(f"The usd file at path {usd_path} provided wasn't found") else: success_bool = prim.GetReferences().AddReference(usd_path) if not success_bool: - raise FileNotFoundError("The usd file at path {} provided wasn't found".format(usd_path)) + raise FileNotFoundError(f"The usd file at path {usd_path} provided wasn't found") return prim @@ -386,26 +383,6 @@ def create_new_stage_in_memory() -> Usd.Stage: return Usd.Stage.CreateInMemory() -async def create_new_stage_async() -> None: - """Create a new stage (asynchronous version). - - Example: - - .. code-block:: python - - >>> import asyncio - >>> import isaaclab.utils.stage as stage_utils - >>> from omni.kit.async_engine import run_coroutine - >>> - >>> async def task(): - ... await stage_utils.create_new_stage_async() - ... - >>> run_coroutine(task()) - """ - await omni.usd.get_context().new_stage_async() - await omni.kit.app.get_app().next_update_async() - - def open_stage(usd_path: str) -> bool: """Open the given usd file and replace currently opened stage. @@ -436,40 +413,6 @@ def open_stage(usd_path: str) -> bool: return result -async def open_stage_async(usd_path: str) -> typing.Tuple[bool, int]: - """Open the given usd file and replace currently opened stage (asynchronous version). - - Args: - usd_path (str): Path to the USD file to open. - - Raises: - ValueError: When input path is not a supported file type by USD. - - Returns: - bool: True if operation is successful, otherwise false. - - Example: - - .. code-block:: python - - >>> import asyncio - >>> import isaaclab.utils.stage as stage_utils - >>> from omni.kit.async_engine import run_coroutine - >>> - >>> async def task(): - ... await stage_utils.open_stage_async("/home//Documents/Assets/Robots/FrankaRobotics/FrankaPanda/franka.usd") - ... - >>> run_coroutine(task()) - """ - if not Usd.Stage.IsSupportedFile(usd_path): - raise ValueError("Only USD files can be loaded with this method") - usd_context = omni.usd.get_context() - usd_context.disable_save_to_recent_files() - (result, error) = await omni.usd.get_context().open_stage_async(usd_path) - usd_context.enable_save_to_recent_files() - return (result, error) - - def save_stage(usd_path: str, save_and_reload_in_place=True) -> bool: """Save usd file to path, it will be overwritten with the current stage @@ -506,7 +449,7 @@ def save_stage(usd_path: str, save_and_reload_in_place=True) -> bool: return result -def close_stage(callback_fn: typing.Callable = None) -> bool: +def close_stage(callback_fn: typing.Callable | None = None) -> bool: """Closes the current opened USD stage. .. note:: @@ -514,7 +457,7 @@ def close_stage(callback_fn: typing.Callable = None) -> bool: Once the stage is closed, it is necessary to open a new stage or create a new one in order to work on it. Args: - callback_fn (typing.Callable, optional): Callback function to call while closing. Defaults to None. + callback_fn: Callback function to call while closing. Defaults to None. Returns: bool: True if operation is successful, otherwise false. @@ -548,49 +491,11 @@ def close_stage(callback_fn: typing.Callable = None) -> bool: return result -def set_livesync_stage(usd_path: str, enable: bool) -> bool: - """Save the stage and set the Live Sync mode for real-time live editing of shared files on a Nucleus server - - Args: - usd_path (str): path to enable live sync for, it will be overwritten with the current stage - enable (bool): True to enable livesync, false to disable livesync - - Returns: - bool: True if operation is successful, otherwise false. - - Example: - - .. code-block:: python - - >>> import isaaclab.utils.stage as stage_utils - >>> - >>> stage_utils.set_livesync_stage("omniverse://localhost/Users/Live/stage.usd", enable=True) - server omniverse://localhost: ConnectionStatus.CONNECTING - server omniverse://localhost: ConnectionStatus.CONNECTED - True - """ - # TODO: Check that the provided usd_path exists - # TODO: Check that the provided usd_path exists - if save_stage(usd_path): - if enable: - usd_path_split = usd_path.split("/") - live_session = layers.get_live_syncing().find_live_session_by_name(usd_path, "Default") - if live_session is None: - live_session = layers.get_live_syncing().create_live_session(name="Default") - result = layers.get_live_syncing().join_live_session(live_session) - return True - else: - layers.get_live_syncing().stop_live_session(usd_path) - return True - else: - return False - - def traverse_stage(fabric=False) -> typing.Iterable: """Traverse through prims (hidden or not) in the opened Usd stage. Returns: - typing.Iterable: Generator which yields prims from the stage in depth-first-traversal order. + Generator which yields prims from the stage in depth-first-traversal order. Example: diff --git a/source/isaaclab/test/terrains/test_terrain_generator.py b/source/isaaclab/test/terrains/test_terrain_generator.py index 46f029ab7c9..78ed6f10da3 100644 --- a/source/isaaclab/test/terrains/test_terrain_generator.py +++ b/source/isaaclab/test/terrains/test_terrain_generator.py @@ -17,11 +17,11 @@ import shutil import torch -import isaacsim.core.utils.torch as torch_utils import pytest from isaaclab.terrains import FlatPatchSamplingCfg, TerrainGenerator, TerrainGeneratorCfg from isaaclab.terrains.config.rough import ROUGH_TERRAINS_CFG +from isaaclab.utils.seed import set_seed @pytest.fixture @@ -65,7 +65,7 @@ def test_generation_reproducibility(use_global_seed, seed): Setting only locally is not tested as it is not supported. """ # set initial seed - torch_utils.set_seed(seed) + set_seed(seed) # create terrain generator cfg = ROUGH_TERRAINS_CFG @@ -77,7 +77,7 @@ def test_generation_reproducibility(use_global_seed, seed): terrain_mesh_1 = terrain_generator.terrain_mesh.copy() # set seed again - torch_utils.set_seed(seed) + set_seed(seed) # create terrain generator terrain_generator = TerrainGenerator(cfg=cfg) @@ -116,7 +116,7 @@ def test_generation_cache(output_dir, curriculum): # set a random seed to disturb the process # this is to ensure that the seed inside the terrain generator makes deterministic results - torch_utils.set_seed(12456) + set_seed(12456) # create terrain generator with cache enabled terrain_generator = TerrainGenerator(cfg=cfg) From c0eaf1e1ff63fb96495a12a161722e758220ea53 Mon Sep 17 00:00:00 2001 From: Pascal Roth Date: Sun, 2 Nov 2025 19:12:31 +0100 Subject: [PATCH 3/5] remove stage utils double definition --- .../assets/rigid_object/rigid_object_data.py | 2 +- .../rigid_object_collection_data.py | 2 +- .../isaaclab/isaaclab/envs/direct_marl_env.py | 4 +- .../isaaclab/isaaclab/envs/direct_rl_env.py | 4 +- .../isaaclab/envs/manager_based_env.py | 4 +- .../isaaclab/markers/visualization_markers.py | 5 +- .../isaaclab/scene/interactive_scene.py | 3 +- .../isaaclab/sim/simulation_context.py | 5 +- .../sim/spawners/from_files/from_files.py | 10 +- .../spawners/materials/visual_materials.py | 3 +- .../isaaclab/sim/spawners/sensors/sensors.py | 3 +- source/isaaclab/isaaclab/sim/utils.py | 156 +----------------- source/isaaclab/isaaclab/utils/__init__.py | 8 +- source/isaaclab/isaaclab/utils/prims.py | 12 +- source/isaaclab/isaaclab/utils/seed.py | 13 +- source/isaaclab/isaaclab/utils/semantics.py | 26 +-- source/isaaclab/isaaclab/utils/stage.py | 149 ++++++++++++++--- .../isaaclab/test/sim/test_stage_in_memory.py | 24 +-- 18 files changed, 182 insertions(+), 251 deletions(-) diff --git a/source/isaaclab/isaaclab/assets/rigid_object/rigid_object_data.py b/source/isaaclab/isaaclab/assets/rigid_object/rigid_object_data.py index ee83900376f..1eef4d60f00 100644 --- a/source/isaaclab/isaaclab/assets/rigid_object/rigid_object_data.py +++ b/source/isaaclab/isaaclab/assets/rigid_object/rigid_object_data.py @@ -9,8 +9,8 @@ import omni.physics.tensors.impl.api as physx import isaaclab.utils.math as math_utils -from isaaclab.sim.utils import get_current_stage_id from isaaclab.utils.buffers import TimestampedBuffer +from isaaclab.utils.stage import get_current_stage_id class RigidObjectData: diff --git a/source/isaaclab/isaaclab/assets/rigid_object_collection/rigid_object_collection_data.py b/source/isaaclab/isaaclab/assets/rigid_object_collection/rigid_object_collection_data.py index 328010bb14f..8c2311c5aa4 100644 --- a/source/isaaclab/isaaclab/assets/rigid_object_collection/rigid_object_collection_data.py +++ b/source/isaaclab/isaaclab/assets/rigid_object_collection/rigid_object_collection_data.py @@ -9,8 +9,8 @@ import omni.physics.tensors.impl.api as physx import isaaclab.utils.math as math_utils -from isaaclab.sim.utils import get_current_stage_id from isaaclab.utils.buffers import TimestampedBuffer +from isaaclab.utils.stage import get_current_stage_id class RigidObjectCollectionData: diff --git a/source/isaaclab/isaaclab/envs/direct_marl_env.py b/source/isaaclab/isaaclab/envs/direct_marl_env.py index 637b7b65985..87f8055c627 100644 --- a/source/isaaclab/isaaclab/envs/direct_marl_env.py +++ b/source/isaaclab/isaaclab/envs/direct_marl_env.py @@ -25,10 +25,10 @@ from isaaclab.managers import EventManager from isaaclab.scene import InteractiveScene from isaaclab.sim import SimulationContext -from isaaclab.sim.utils import attach_stage_to_usd_context, use_stage from isaaclab.utils.noise import NoiseModel -from isaaclab.utils.timer import Timer from isaaclab.utils.seed import set_seed +from isaaclab.utils.stage import attach_stage_to_usd_context, use_stage +from isaaclab.utils.timer import Timer from .common import ActionType, AgentID, EnvStepReturn, ObsType, StateType from .direct_marl_env_cfg import DirectMARLEnvCfg diff --git a/source/isaaclab/isaaclab/envs/direct_rl_env.py b/source/isaaclab/isaaclab/envs/direct_rl_env.py index 28155ba0c34..4da7d90b2b1 100644 --- a/source/isaaclab/isaaclab/envs/direct_rl_env.py +++ b/source/isaaclab/isaaclab/envs/direct_rl_env.py @@ -26,10 +26,10 @@ from isaaclab.managers import EventManager from isaaclab.scene import InteractiveScene from isaaclab.sim import SimulationContext -from isaaclab.sim.utils import attach_stage_to_usd_context, use_stage from isaaclab.utils.noise import NoiseModel -from isaaclab.utils.timer import Timer from isaaclab.utils.seed import set_seed +from isaaclab.utils.stage import attach_stage_to_usd_context, use_stage +from isaaclab.utils.timer import Timer from .common import VecEnvObs, VecEnvStepReturn from .direct_rl_env_cfg import DirectRLEnvCfg diff --git a/source/isaaclab/isaaclab/envs/manager_based_env.py b/source/isaaclab/isaaclab/envs/manager_based_env.py index 636ef3fb778..04c722aa104 100644 --- a/source/isaaclab/isaaclab/envs/manager_based_env.py +++ b/source/isaaclab/isaaclab/envs/manager_based_env.py @@ -16,10 +16,10 @@ from isaaclab.managers import ActionManager, EventManager, ObservationManager, RecorderManager from isaaclab.scene import InteractiveScene from isaaclab.sim import SimulationContext -from isaaclab.sim.utils import attach_stage_to_usd_context, use_stage from isaaclab.ui.widgets import ManagerLiveVisualizer -from isaaclab.utils.timer import Timer from isaaclab.utils.seed import set_seed +from isaaclab.utils.stage import attach_stage_to_usd_context, use_stage +from isaaclab.utils.timer import Timer from .common import VecEnvObs from .manager_based_env_cfg import ManagerBasedEnvCfg diff --git a/source/isaaclab/isaaclab/markers/visualization_markers.py b/source/isaaclab/isaaclab/markers/visualization_markers.py index c43831fdebd..b8cab0b8537 100644 --- a/source/isaaclab/isaaclab/markers/visualization_markers.py +++ b/source/isaaclab/isaaclab/markers/visualization_markers.py @@ -31,10 +31,9 @@ import isaaclab.sim as sim_utils import isaaclab.utils.stage as stage_utils from isaaclab.sim.spawners import SpawnerCfg -from isaaclab.sim.utils import attach_stage_to_usd_context from isaaclab.utils.configclass import configclass from isaaclab.utils.math import convert_quat -from isaaclab.utils.stage import get_current_stage +from isaaclab.utils.stage import attach_stage_to_usd_context, get_current_stage @configclass @@ -67,7 +66,7 @@ class VisualizationMarkers: The class parses the configuration to create different the marker prototypes into the stage. Each marker prototype prim is created as a child of the :class:`UsdGeom.PointInstancer` prim. The prim path for the marker prim is resolved using the key of the marker in the :attr:`VisualizationMarkersCfg.markers` - dictionary. The marker prototypes are created using the :meth:`isaacsim.core.utils.create_prim` + dictionary. The marker prototypes are created using the :meth:`isaaclab.utils.prims.create_prim` function, and then instanced using :class:`UsdGeom.PointInstancer` prim to allow creating multiple instances of the marker prims. diff --git a/source/isaaclab/isaaclab/scene/interactive_scene.py b/source/isaaclab/isaaclab/scene/interactive_scene.py index 75dc49308d8..b745cfa3ff1 100644 --- a/source/isaaclab/isaaclab/scene/interactive_scene.py +++ b/source/isaaclab/isaaclab/scene/interactive_scene.py @@ -31,9 +31,8 @@ ) from isaaclab.sensors import ContactSensorCfg, FrameTransformerCfg, SensorBase, SensorBaseCfg from isaaclab.sim import SimulationContext -from isaaclab.sim.utils import get_current_stage_id from isaaclab.terrains import TerrainImporter, TerrainImporterCfg -from isaaclab.utils.stage import get_current_stage +from isaaclab.utils.stage import get_current_stage, get_current_stage_id from .interactive_scene_cfg import InteractiveSceneCfg diff --git a/source/isaaclab/isaaclab/sim/simulation_context.py b/source/isaaclab/isaaclab/sim/simulation_context.py index 1a0bcf11965..0581762a60f 100644 --- a/source/isaaclab/isaaclab/sim/simulation_context.py +++ b/source/isaaclab/isaaclab/sim/simulation_context.py @@ -32,7 +32,6 @@ from pxr import Gf, PhysxSchema, Sdf, Usd, UsdPhysics import isaaclab.utils.stage as stage_utils -from isaaclab.sim.utils import create_new_stage_in_memory, use_stage from .simulation_cfg import SimulationCfg from .spawners import DomeLightCfg, GroundPlaneCfg @@ -134,7 +133,7 @@ def __init__(self, cfg: SimulationCfg | None = None): # create stage in memory if requested if self.cfg.create_stage_in_memory: - self._initial_stage = create_new_stage_in_memory() + self._initial_stage = stage_utils.create_new_stage_in_memory() else: self._initial_stage = omni.usd.get_context().get_stage() @@ -613,7 +612,7 @@ async def reset_async(self, soft: bool = False): def _init_stage(self, *args, **kwargs) -> Usd.Stage: _ = super()._init_stage(*args, **kwargs) - with use_stage(self.get_initial_stage()): + with stage_utils.use_stage(self.get_initial_stage()): # a stage update here is needed for the case when physics_dt != rendering_dt, otherwise the app crashes # when in headless mode self.set_setting("/app/player/playSimulations", False) diff --git a/source/isaaclab/isaaclab/sim/spawners/from_files/from_files.py b/source/isaaclab/isaaclab/sim/spawners/from_files/from_files.py index 3d72db01587..d5d5f789d80 100644 --- a/source/isaaclab/isaaclab/sim/spawners/from_files/from_files.py +++ b/source/isaaclab/isaaclab/sim/spawners/from_files/from_files.py @@ -20,15 +20,9 @@ from pxr import Semantics from isaaclab.sim import converters, schemas -from isaaclab.sim.utils import ( - bind_physics_material, - bind_visual_material, - clone, - is_current_stage_in_memory, - select_usd_variants, -) +from isaaclab.sim.utils import bind_physics_material, bind_visual_material, clone, select_usd_variants from isaaclab.utils.assets import check_usd_path_with_timeout -from isaaclab.utils.stage import get_current_stage +from isaaclab.utils.stage import get_current_stage, is_current_stage_in_memory if TYPE_CHECKING: from . import from_files_cfg diff --git a/source/isaaclab/isaaclab/sim/spawners/materials/visual_materials.py b/source/isaaclab/isaaclab/sim/spawners/materials/visual_materials.py index 2e827518628..454ff0f54b2 100644 --- a/source/isaaclab/isaaclab/sim/spawners/materials/visual_materials.py +++ b/source/isaaclab/isaaclab/sim/spawners/materials/visual_materials.py @@ -12,8 +12,9 @@ from pxr import Usd import isaaclab.utils.prims as prim_utils -from isaaclab.sim.utils import attach_stage_to_usd_context, clone, safe_set_attribute_on_usd_prim +from isaaclab.sim.utils import clone, safe_set_attribute_on_usd_prim from isaaclab.utils.assets import NVIDIA_NUCLEUS_DIR +from isaaclab.utils.stage import attach_stage_to_usd_context if TYPE_CHECKING: from . import visual_materials_cfg diff --git a/source/isaaclab/isaaclab/sim/spawners/sensors/sensors.py b/source/isaaclab/isaaclab/sim/spawners/sensors/sensors.py index dfd8fc7b393..7d241f3ed88 100644 --- a/source/isaaclab/isaaclab/sim/spawners/sensors/sensors.py +++ b/source/isaaclab/isaaclab/sim/spawners/sensors/sensors.py @@ -12,8 +12,9 @@ from pxr import Sdf, Usd import isaaclab.utils.prims as prim_utils -from isaaclab.sim.utils import attach_stage_to_usd_context, clone +from isaaclab.sim.utils import clone from isaaclab.utils import to_camel_case +from isaaclab.utils.stage import attach_stage_to_usd_context if TYPE_CHECKING: from . import sensors_cfg diff --git a/source/isaaclab/isaaclab/sim/utils.py b/source/isaaclab/isaaclab/sim/utils.py index d6a5e2c410a..7254ff95cf6 100644 --- a/source/isaaclab/isaaclab/sim/utils.py +++ b/source/isaaclab/isaaclab/sim/utils.py @@ -7,24 +7,20 @@ from __future__ import annotations -import contextlib import functools import inspect import re -from collections.abc import Callable, Generator +from collections.abc import Callable from typing import TYPE_CHECKING, Any -import carb import omni import omni.kit.commands import omni.log from isaacsim.core.cloner import Cloner -from isaacsim.core.utils.carb import get_carb_setting from isaacsim.core.version import get_version -from pxr import PhysxSchema, Sdf, Usd, UsdGeom, UsdPhysics, UsdShade, UsdUtils +from pxr import PhysxSchema, Sdf, Usd, UsdGeom, UsdPhysics, UsdShade -import isaaclab.utils.stage as stage_utils -from isaaclab.utils.stage import get_current_stage +from isaaclab.utils.stage import attach_stage_to_usd_context, get_current_stage # from Isaac Sim 4.2 onwards, pxr.Semantics is deprecated try: @@ -1034,149 +1030,3 @@ class TableVariants: f"Setting variant selection '{variant_selection}' for variant set '{variant_set_name}' on" f" prim '{prim_path}'." ) - - -""" -Stage management. -""" - - -def attach_stage_to_usd_context(attaching_early: bool = False): - """Attaches the current USD stage in memory to the USD context. - - This function should be called during or after scene is created and before stage is simulated or rendered. - - Note: - If the stage is not in memory or rendering is not enabled, this function will return without attaching. - - Args: - attaching_early: Whether to attach the stage to the usd context before stage is created. Defaults to False. - """ - - from isaacsim.core.simulation_manager import SimulationManager - - from isaaclab.sim.simulation_context import SimulationContext - - # if Isaac Sim version is less than 5.0, stage in memory is not supported - isaac_sim_version = float(".".join(get_version()[2])) - if isaac_sim_version < 5: - return - - # if stage is not in memory, we can return early - if not is_current_stage_in_memory(): - return - - # attach stage to physx - stage_id = get_current_stage_id() - physx_sim_interface = omni.physx.get_physx_simulation_interface() - physx_sim_interface.attach_stage(stage_id) - - # this carb flag is equivalent to if rendering is enabled - carb_setting = carb.settings.get_settings() - is_rendering_enabled = get_carb_setting(carb_setting, "/physics/fabricUpdateTransformations") - - # if rendering is not enabled, we don't need to attach it - if not is_rendering_enabled: - return - - # early attach warning msg - if attaching_early: - omni.log.warn( - "Attaching stage in memory to USD context early to support an operation which doesn't support stage in" - " memory." - ) - - # skip this callback to avoid wiping the stage after attachment - SimulationContext.instance().skip_next_stage_open_callback() - - # disable stage open callback to avoid clearing callbacks - SimulationManager.enable_stage_open_callback(False) - - # enable physics fabric - SimulationContext.instance()._physics_context.enable_fabric(True) - - # attach stage to usd context - omni.usd.get_context().attach_stage_with_callback(stage_id) - - # attach stage to physx - physx_sim_interface = omni.physx.get_physx_simulation_interface() - physx_sim_interface.attach_stage(stage_id) - - # re-enable stage open callback - SimulationManager.enable_stage_open_callback(True) - - -def is_current_stage_in_memory() -> bool: - """Checks if the current stage is in memory. - - This function compares the stage id of the current USD stage with the stage id of the USD context stage. - - Returns: - Whether the current stage is in memory. - """ - - # grab current stage id - stage_id = get_current_stage_id() - - # grab context stage id - context_stage = omni.usd.get_context().get_stage() - with use_stage(context_stage): - context_stage_id = get_current_stage_id() - - # check if stage ids are the same - return stage_id != context_stage_id - - -@contextlib.contextmanager -def use_stage(stage: Usd.Stage) -> Generator[None, None, None]: - """Context manager that sets a thread-local stage, if supported. - - In Isaac Sim < 5.0, this is a no-op to maintain compatibility. - - Args: - stage: The stage to set temporarily. - - Yields: - None - """ - isaac_sim_version = float(".".join(get_version()[2])) - if isaac_sim_version < 5: - omni.log.warn("[Compat] Isaac Sim < 5.0 does not support thread-local stage contexts. Skipping use_stage().") - yield # no-op - else: - with stage_utils.use_stage(stage): - yield - - -def create_new_stage_in_memory() -> Usd.Stage: - """Creates a new stage in memory, if supported. - - Returns: - The new stage in memory. - """ - isaac_sim_version = float(".".join(get_version()[2])) - if isaac_sim_version < 5: - omni.log.warn( - "[Compat] Isaac Sim < 5.0 does not support creating a new stage in memory. Falling back to creating a new" - " stage attached to USD context." - ) - return stage_utils.create_new_stage() - else: - return stage_utils.create_new_stage_in_memory() - - -def get_current_stage_id() -> int: - """Gets the current open stage id. - - This function is a reimplementation of :meth:`isaacsim.core.utils.stage.get_current_stage_id` for - backwards compatibility to Isaac Sim < 5.0. - - Returns: - The current open stage id. - """ - stage = get_current_stage() - stage_cache = UsdUtils.StageCache.Get() - stage_id = stage_cache.GetId(stage).ToLongInt() - if stage_id < 0: - stage_id = stage_cache.Insert(stage).ToLongInt() - return stage_id diff --git a/source/isaaclab/isaaclab/utils/__init__.py b/source/isaaclab/isaaclab/utils/__init__.py index 01756018c29..8ce24995001 100644 --- a/source/isaaclab/isaaclab/utils/__init__.py +++ b/source/isaaclab/isaaclab/utils/__init__.py @@ -11,11 +11,11 @@ from .dict import * from .interpolation import * from .modifiers import * +from .prims import * +from .seed import * +from .semantics import * +from .stage import * from .string import * from .timer import Timer from .types import * from .version import * -from .stage import * -from .prims import * -from .semantics import * -from .seed import * diff --git a/source/isaaclab/isaaclab/utils/prims.py b/source/isaaclab/isaaclab/utils/prims.py index e1d83f4bc81..3baec73bbb5 100644 --- a/source/isaaclab/isaaclab/utils/prims.py +++ b/source/isaaclab/isaaclab/utils/prims.py @@ -4,19 +4,12 @@ # SPDX-License-Identifier: BSD-3-Clause import numpy as np -import re - -# python import typing import omni.usd import usdrt - -# isaacsim from isaacsim.core.utils._isaac_utils import _find_matching_prim_paths from omni.usd.commands import DeletePrimsCommand, MovePrimCommand - -# omniverse from pxr import Sdf, Usd, UsdGeom, UsdPhysics from isaaclab.utils.semantics import add_labels @@ -524,10 +517,7 @@ def is_prim_root_path(prim_path: str) -> bool: >>> prims_utils.is_prim_root_path("/World/Cube") False """ - if prim_path == "/": - return True - else: - return False + return prim_path == "/" def is_prim_no_delete(prim_path: str) -> bool: diff --git a/source/isaaclab/isaaclab/utils/seed.py b/source/isaaclab/isaaclab/utils/seed.py index 8f24228864e..bfe6fa9ada4 100644 --- a/source/isaaclab/isaaclab/utils/seed.py +++ b/source/isaaclab/isaaclab/utils/seed.py @@ -1,8 +1,13 @@ -import os -import random +# Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause import numpy as np +import os +import random import torch + import warp as wp @@ -12,7 +17,7 @@ def set_seed(seed: int, torch_deterministic: bool = False) -> int: seed = 42 elif seed == -1: seed = np.random.randint(0, 10000) - print("Setting seed: {}".format(seed)) + print(f"Setting seed: {seed}") random.seed(seed) np.random.seed(seed) @@ -32,4 +37,4 @@ def set_seed(seed: int, torch_deterministic: bool = False) -> int: torch.backends.cudnn.benchmark = True torch.backends.cudnn.deterministic = False - return seed \ No newline at end of file + return seed diff --git a/source/isaaclab/isaaclab/utils/semantics.py b/source/isaaclab/isaaclab/utils/semantics.py index 30073811cec..2e1e03b2ca5 100644 --- a/source/isaaclab/isaaclab/utils/semantics.py +++ b/source/isaaclab/isaaclab/utils/semantics.py @@ -3,29 +3,17 @@ # # SPDX-License-Identifier: BSD-3-Clause -# SPDX-FileCopyrightText: Copyright (c) 2021-2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved. -# SPDX-License-Identifier: Apache-2.0 -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -from typing import Dict, List, Optional, Tuple - import carb -import omni.usd -import Semantics from pxr import Usd, UsdGeom, UsdSemantics import isaaclab.utils.prims as prim_utils -from isaaclab.utils.stage import get_current_stage, get_current_stage_id +from isaaclab.utils.stage import get_current_stage + +# from Isaac Sim 4.2 onwards, pxr.Semantics is deprecated +try: + import Semantics +except ModuleNotFoundError: + from pxr import Semantics def add_update_semantics(prim: Usd.Prim, semantic_label: str, type_label: str = "class", suffix="") -> None: diff --git a/source/isaaclab/isaaclab/utils/stage.py b/source/isaaclab/isaaclab/utils/stage.py index 540f8fdd97a..3e6dc9c03c4 100644 --- a/source/isaaclab/isaaclab/utils/stage.py +++ b/source/isaaclab/isaaclab/utils/stage.py @@ -7,13 +7,17 @@ import contextlib import threading import typing +from collections.abc import Generator +import carb import omni import omni.kit.app import usdrt -from pxr import Sdf, Usd, UsdGeom, UsdUtils +from isaacsim.core.utils.carb import get_carb_setting +from isaacsim.core.version import get_version from omni.metrics.assembler.core import get_metrics_assembler_interface from omni.usd.commands import DeletePrimsCommand +from pxr import Sdf, Usd, UsdGeom, UsdUtils _context = threading.local() # thread-local storage to handle nested contexts and concurrent access @@ -38,12 +42,100 @@ """ +def attach_stage_to_usd_context(attaching_early: bool = False): + """Attaches the current USD stage in memory to the USD context. + + This function should be called during or after scene is created and before stage is simulated or rendered. + + Note: + If the stage is not in memory or rendering is not enabled, this function will return without attaching. + + Args: + attaching_early: Whether to attach the stage to the usd context before stage is created. Defaults to False. + """ + + from isaacsim.core.simulation_manager import SimulationManager + + from isaaclab.sim.simulation_context import SimulationContext + + # if Isaac Sim version is less than 5.0, stage in memory is not supported + isaac_sim_version = float(".".join(get_version()[2])) + if isaac_sim_version < 5: + return + + # if stage is not in memory, we can return early + if not is_current_stage_in_memory(): + return + + # attach stage to physx + stage_id = get_current_stage_id() + physx_sim_interface = omni.physx.get_physx_simulation_interface() + physx_sim_interface.attach_stage(stage_id) + + # this carb flag is equivalent to if rendering is enabled + carb_setting = carb.settings.get_settings() + is_rendering_enabled = get_carb_setting(carb_setting, "/physics/fabricUpdateTransformations") + + # if rendering is not enabled, we don't need to attach it + if not is_rendering_enabled: + return + + # early attach warning msg + if attaching_early: + omni.log.warn( + "Attaching stage in memory to USD context early to support an operation which doesn't support stage in" + " memory." + ) + + # skip this callback to avoid wiping the stage after attachment + SimulationContext.instance().skip_next_stage_open_callback() + + # disable stage open callback to avoid clearing callbacks + SimulationManager.enable_stage_open_callback(False) + + # enable physics fabric + SimulationContext.instance()._physics_context.enable_fabric(True) + + # attach stage to usd context + omni.usd.get_context().attach_stage_with_callback(stage_id) + + # attach stage to physx + physx_sim_interface = omni.physx.get_physx_simulation_interface() + physx_sim_interface.attach_stage(stage_id) + + # re-enable stage open callback + SimulationManager.enable_stage_open_callback(True) + + +def is_current_stage_in_memory() -> bool: + """Checks if the current stage is in memory. + + This function compares the stage id of the current USD stage with the stage id of the USD context stage. + + Returns: + Whether the current stage is in memory. + """ + + # grab current stage id + stage_id = get_current_stage_id() + + # grab context stage id + context_stage = omni.usd.get_context().get_stage() + with use_stage(context_stage): + context_stage_id = get_current_stage_id() + + # check if stage ids are the same + return stage_id != context_stage_id + + @contextlib.contextmanager -def use_stage(stage: Usd.Stage): - """Context manager that sets a thread-local stage. +def use_stage(stage: Usd.Stage) -> Generator[None, None, None]: + """Context manager that sets a thread-local stage, if supported. + + In Isaac Sim < 5.0, this is a no-op to maintain compatibility. Args: - stage: The stage to set in the context. + stage: The stage to set temporarily. Raises: AssertionError: If the stage is not a USD stage instance. @@ -61,20 +153,25 @@ def use_stage(stage: Usd.Stage): ... pass >>> # operate on the default stage attached to the USD context """ - # check stage - assert isinstance(stage, Usd.Stage), f"Expected a USD stage instance, got: {type(stage)}" - # store previous context value if it exists - previous_stage = getattr(_context, "stage", None) - # set new context value - try: - _context.stage = stage - yield - # remove context value or restore previous one if it exists - finally: - if previous_stage is None: - delattr(_context, "stage") - else: - _context.stage = previous_stage + isaac_sim_version = float(".".join(get_version()[2])) + if isaac_sim_version < 5: + omni.log.warn("[Compat] Isaac Sim < 5.0 does not support thread-local stage contexts. Skipping use_stage().") + yield # no-op + else: + # check stage + assert isinstance(stage, Usd.Stage), f"Expected a USD stage instance, got: {type(stage)}" + # store previous context value if it exists + previous_stage = getattr(_context, "stage", None) + # set new context value + try: + _context.stage = stage + yield + # remove context value or restore previous one if it exists + finally: + if previous_stage is None: + delattr(_context, "stage") + else: + _context.stage = previous_stage def get_current_stage(fabric: bool = False) -> Usd.Stage | usdrt.Usd._Usd.Stage: @@ -111,7 +208,7 @@ def get_current_stage_id() -> int: """Get the current open stage id Returns: - int: The stage id. + The current open stage id. Example: @@ -364,10 +461,10 @@ def create_new_stage() -> Usd.Stage: def create_new_stage_in_memory() -> Usd.Stage: - """Create a new stage in memory. + """Creates a new stage in memory, if supported. Returns: - Usd.Stage: The created USD stage. + The new stage in memory. Example: @@ -380,7 +477,15 @@ def create_new_stage_in_memory() -> Usd.Stage: sessionLayer=Sdf.Find('anon:0xf7cd2e0:tmp-session.usda'), pathResolverContext=) """ - return Usd.Stage.CreateInMemory() + isaac_sim_version = float(".".join(get_version()[2])) + if isaac_sim_version < 5: + omni.log.warn( + "[Compat] Isaac Sim < 5.0 does not support creating a new stage in memory. Falling back to creating a new" + " stage attached to USD context." + ) + return create_new_stage() + else: + return Usd.Stage.CreateInMemory() def open_stage(usd_path: str) -> bool: diff --git a/source/isaaclab/test/sim/test_stage_in_memory.py b/source/isaaclab/test/sim/test_stage_in_memory.py index 5770e1ed776..1010ac7bc79 100644 --- a/source/isaaclab/test/sim/test_stage_in_memory.py +++ b/source/isaaclab/test/sim/test_stage_in_memory.py @@ -59,7 +59,7 @@ def test_stage_in_memory_with_shapes(sim): # grab stage in memory and set as current stage via the with statement stage_in_memory = sim.get_initial_stage() - with sim_utils.use_stage(stage_in_memory): + with stage_utils.use_stage(stage_in_memory): # create cloned cone stage for i in range(num_clones): prim_utils.create_prim(f"/World/env_{i}", "Xform", translation=(i, i, 0)) @@ -88,7 +88,7 @@ def test_stage_in_memory_with_shapes(sim): cfg.func(prim_path_regex, cfg) # verify stage is in memory - assert sim_utils.is_current_stage_in_memory() + assert stage_utils.is_current_stage_in_memory() # verify prims exist in stage in memory prims = prim_utils.find_matching_prim_paths(prim_path_regex) @@ -96,7 +96,7 @@ def test_stage_in_memory_with_shapes(sim): # verify prims do not exist in context stage context_stage = omni.usd.get_context().get_stage() - with sim_utils.use_stage(context_stage): + with stage_utils.use_stage(context_stage): prims = prim_utils.find_matching_prim_paths(prim_path_regex) assert len(prims) != num_clones @@ -104,7 +104,7 @@ def test_stage_in_memory_with_shapes(sim): sim_utils.attach_stage_to_usd_context() # verify stage is no longer in memory - assert not sim_utils.is_current_stage_in_memory() + assert not stage_utils.is_current_stage_in_memory() # verify prims now exist in context stage prims = prim_utils.find_matching_prim_paths(prim_path_regex) @@ -128,7 +128,7 @@ def test_stage_in_memory_with_usds(sim): # grab stage in memory and set as current stage via the with statement stage_in_memory = sim.get_initial_stage() - with sim_utils.use_stage(stage_in_memory): + with stage_utils.use_stage(stage_in_memory): # create cloned robot stage for i in range(num_clones): prim_utils.create_prim(f"/World/env_{i}", "Xform", translation=(i, i, 0)) @@ -154,7 +154,7 @@ def test_stage_in_memory_with_usds(sim): cfg.func(prim_path_regex, cfg) # verify stage is in memory - assert sim_utils.is_current_stage_in_memory() + assert stage_utils.is_current_stage_in_memory() # verify prims exist in stage in memory prims = prim_utils.find_matching_prim_paths(prim_path_regex) @@ -162,7 +162,7 @@ def test_stage_in_memory_with_usds(sim): # verify prims do not exist in context stage context_stage = omni.usd.get_context().get_stage() - with sim_utils.use_stage(context_stage): + with stage_utils.use_stage(context_stage): prims = prim_utils.find_matching_prim_paths(prim_path_regex) assert len(prims) != num_clones @@ -170,7 +170,7 @@ def test_stage_in_memory_with_usds(sim): sim_utils.attach_stage_to_usd_context() # verify stage is no longer in memory - assert not sim_utils.is_current_stage_in_memory() + assert not stage_utils.is_current_stage_in_memory() # verify prims now exist in context stage prims = prim_utils.find_matching_prim_paths(prim_path_regex) @@ -191,7 +191,7 @@ def test_stage_in_memory_with_clone_in_fabric(sim): # grab stage in memory and set as current stage via the with statement stage_in_memory = sim.get_initial_stage() - with sim_utils.use_stage(stage_in_memory): + with stage_utils.use_stage(stage_in_memory): # set up paths base_env_path = "/World/envs" source_prim_path = f"{base_env_path}/env_0" @@ -218,7 +218,7 @@ def test_stage_in_memory_with_clone_in_fabric(sim): # verify prims do not exist in context stage context_stage = omni.usd.get_context().get_stage() - with sim_utils.use_stage(context_stage): + with stage_utils.use_stage(context_stage): prims = prim_utils.find_matching_prim_paths(prim_path_regex) assert len(prims) != num_clones @@ -226,10 +226,10 @@ def test_stage_in_memory_with_clone_in_fabric(sim): sim_utils.attach_stage_to_usd_context() # verify stage is no longer in memory - assert not sim_utils.is_current_stage_in_memory() + assert not stage_utils.is_current_stage_in_memory() # verify prims now exist in fabric stage using usdrt apis - stage_id = sim_utils.get_current_stage_id() + stage_id = stage_utils.get_current_stage_id() usdrt_stage = usdrt.Usd.Stage.Attach(stage_id) for i in range(num_clones): prim = usdrt_stage.GetPrimAtPath(f"/World/envs/env_{i}/Robot") From d6b4c9372ae5075d92f1ffd4250d232f12a1ac14 Mon Sep 17 00:00:00 2001 From: Pascal Roth Date: Sun, 2 Nov 2025 19:23:58 +0100 Subject: [PATCH 4/5] find_matching_prim_paths removed from prim_utils --- .../isaaclab/isaaclab/controllers/rmp_flow.py | 4 +-- source/isaaclab/isaaclab/utils/prims.py | 33 ++----------------- source/isaaclab/test/sim/test_spawn_shapes.py | 6 ++-- .../isaaclab/test/sim/test_spawn_wrappers.py | 6 ++-- source/isaaclab/test/sim/test_utils.py | 30 ----------------- 5 files changed, 11 insertions(+), 68 deletions(-) diff --git a/source/isaaclab/isaaclab/controllers/rmp_flow.py b/source/isaaclab/isaaclab/controllers/rmp_flow.py index c1c740a4924..ad9108d05cb 100644 --- a/source/isaaclab/isaaclab/controllers/rmp_flow.py +++ b/source/isaaclab/isaaclab/controllers/rmp_flow.py @@ -12,7 +12,7 @@ # enable motion generation extensions from isaacsim.core.utils.extensions import enable_extension -import isaaclab.utils.prims as prim_utils +import isaaclab.sim as sim_utils enable_extension("isaacsim.robot_motion.lula") enable_extension("isaacsim.robot_motion.motion_generation") @@ -82,7 +82,7 @@ def initialize(self, prim_paths_expr: str): # obtain the simulation time physics_dt = SimulationContext.instance().get_physics_dt() # find all prims - self._prim_paths = prim_utils.find_matching_prim_paths(prim_paths_expr) + self._prim_paths = sim_utils.find_matching_prim_paths(prim_paths_expr) self.num_robots = len(self._prim_paths) # resolve controller if self.cfg.name == "rmp_flow": diff --git a/source/isaaclab/isaaclab/utils/prims.py b/source/isaaclab/isaaclab/utils/prims.py index 3baec73bbb5..de6041cfb04 100644 --- a/source/isaaclab/isaaclab/utils/prims.py +++ b/source/isaaclab/isaaclab/utils/prims.py @@ -8,12 +8,12 @@ import omni.usd import usdrt -from isaacsim.core.utils._isaac_utils import _find_matching_prim_paths from omni.usd.commands import DeletePrimsCommand, MovePrimCommand from pxr import Sdf, Usd, UsdGeom, UsdPhysics +import isaaclab.sim as sim_utils from isaaclab.utils.semantics import add_labels -from isaaclab.utils.stage import add_reference_to_stage, get_current_stage, get_current_stage_id +from isaaclab.utils.stage import add_reference_to_stage, get_current_stage from isaaclab.utils.types import SDF_type_to_Gf @@ -364,33 +364,6 @@ def get_all_matching_child_prims( return out -def find_matching_prim_paths(prim_path_regex: str, prim_type: str | None = None) -> list[str]: - """Find all the matching prim paths in the stage based on Regex expression. - - Args: - prim_path_regex (str): The Regex expression for prim path. - prim_type (typing.Optional[str]): The type of the prims to filter, only supports articulation and rigid_body currently. Defaults to None. - - Returns: - typing.List[str]: List of prim paths that match input expression. - - Example: - - .. code-block:: python - - >>> import isaaclab.utils.prims as prims_utils - >>> - >>> # given the stage: /World/env/Cube, /World/env_01/Cube, /World/env_02/Cube - >>> # get only the prim Cube paths from env_01 and env_02 - >>> prims_utils.find_matching_prim_paths("/World/env_.*/Cube") - ['/World/env_01/Cube', '/World/env_02/Cube'] - """ - stage_id = get_current_stage_id() - if prim_type is None: - prim_type = "" - return _find_matching_prim_paths(prim_path_regex.replace(".*", "*"), stage_id, prim_type) - - def get_prim_children(prim: Usd.Prim) -> list[Usd.Prim]: """Return the call of the USD Prim's GetChildren member function @@ -1005,7 +978,7 @@ def get_articulation_root_api_prim_path(prim_path): return get_prim_path(prim) # regular expression else: - paths = find_matching_prim_paths(prim_path) + paths = sim_utils.find_matching_prim_paths(prim_path) if len(paths): prim = get_first_matching_child_prim(paths[0], predicate) if prim is not None: diff --git a/source/isaaclab/test/sim/test_spawn_shapes.py b/source/isaaclab/test/sim/test_spawn_shapes.py index 63747470983..df8b87357a7 100644 --- a/source/isaaclab/test/sim/test_spawn_shapes.py +++ b/source/isaaclab/test/sim/test_spawn_shapes.py @@ -259,7 +259,7 @@ def test_spawn_cone_clones(sim): assert prim.IsValid() assert prim_utils.get_prim_path(prim) == "/World/env_0/Cone" # find matching prims - prims = prim_utils.find_matching_prim_paths("/World/env_*/Cone") + prims = sim_utils.find_matching_prim_paths("/World/env_*/Cone") assert len(prims) == num_clones @@ -285,8 +285,8 @@ def test_spawn_cone_clone_with_all_props_global_material(sim): assert prim.IsValid() assert prim_utils.get_prim_path(prim) == "/World/env_0/Cone" # find matching prims - prims = prim_utils.find_matching_prim_paths("/World/env_*/Cone") + prims = sim_utils.find_matching_prim_paths("/World/env_*/Cone") assert len(prims) == num_clones # find matching material prims - prims = prim_utils.find_matching_prim_paths("/Looks/visualMaterial.*") + prims = sim_utils.find_matching_prim_paths("/Looks/visualMaterial.*") assert len(prims) == 1 diff --git a/source/isaaclab/test/sim/test_spawn_wrappers.py b/source/isaaclab/test/sim/test_spawn_wrappers.py index 57d7bb2ce34..858336eef0d 100644 --- a/source/isaaclab/test/sim/test_spawn_wrappers.py +++ b/source/isaaclab/test/sim/test_spawn_wrappers.py @@ -69,7 +69,7 @@ def test_spawn_multiple_shapes_with_global_settings(sim): assert prim.IsValid() assert prim_utils.get_prim_path(prim) == "/World/env_0/Cone" - prim_paths = prim_utils.find_matching_prim_paths("/World/env_*/Cone") + prim_paths = sim_utils.find_matching_prim_paths("/World/env_*/Cone") assert len(prim_paths) == num_clones for prim_path in prim_paths: @@ -115,7 +115,7 @@ def test_spawn_multiple_shapes_with_individual_settings(sim): assert prim.IsValid() assert prim_utils.get_prim_path(prim) == "/World/env_0/Cone" - prim_paths = prim_utils.find_matching_prim_paths("/World/env_*/Cone") + prim_paths = sim_utils.find_matching_prim_paths("/World/env_*/Cone") assert len(prim_paths) == num_clones for prim_path in prim_paths: @@ -158,5 +158,5 @@ def test_spawn_multiple_files_with_global_settings(sim): assert prim.IsValid() assert prim_utils.get_prim_path(prim) == "/World/env_0/Robot" - prim_paths = prim_utils.find_matching_prim_paths("/World/env_*/Robot") + prim_paths = sim_utils.find_matching_prim_paths("/World/env_*/Robot") assert len(prim_paths) == num_clones diff --git a/source/isaaclab/test/sim/test_utils.py b/source/isaaclab/test/sim/test_utils.py index ef01f9bc379..f75758217b4 100644 --- a/source/isaaclab/test/sim/test_utils.py +++ b/source/isaaclab/test/sim/test_utils.py @@ -104,36 +104,6 @@ def test_get_first_matching_child_prim(): assert isaaclab_result.GetPrimPath() == "/World/env_1/Franka/panda_link0/visuals/panda_link0" -def test_find_matching_prim_paths(): - """Test find_matching_prim_paths() function.""" - # create scene - for index in range(2048): - random_pos = np.random.uniform(-100, 100, size=3) - prim_utils.create_prim(f"/World/Floor_{index}", "Cube", position=random_pos, attributes={"size": 2.0}) - prim_utils.create_prim(f"/World/Floor_{index}/Sphere", "Sphere", attributes={"radius": 10}) - prim_utils.create_prim(f"/World/Floor_{index}/Sphere/childSphere", "Sphere", attributes={"radius": 1}) - prim_utils.create_prim(f"/World/Floor_{index}/Sphere/childSphere2", "Sphere", attributes={"radius": 1}) - - # test leaf paths - isaac_sim_result = prim_utils.find_matching_prim_paths("/World/Floor_.*/Sphere") - isaaclab_result = sim_utils.find_matching_prim_paths("/World/Floor_.*/Sphere") - assert isaac_sim_result == isaaclab_result - - # test non-leaf paths - isaac_sim_result = prim_utils.find_matching_prim_paths("/World/Floor_.*") - isaaclab_result = sim_utils.find_matching_prim_paths("/World/Floor_.*") - assert isaac_sim_result == isaaclab_result - - # test child-leaf paths - isaac_sim_result = prim_utils.find_matching_prim_paths("/World/Floor_.*/Sphere/childSphere.*") - isaaclab_result = sim_utils.find_matching_prim_paths("/World/Floor_.*/Sphere/childSphere.*") - assert isaac_sim_result == isaaclab_result - - # test valid path - with pytest.raises(ValueError): - sim_utils.get_all_matching_child_prims("World/Floor_.*") - - def test_find_global_fixed_joint_prim(): """Test find_global_fixed_joint_prim() function.""" # create scene From dc3bff1a79a2abaa5bb4d69903ddc9ae1e1ae8b5 Mon Sep 17 00:00:00 2001 From: Pascal Roth Date: Sun, 2 Nov 2025 19:35:57 +0100 Subject: [PATCH 5/5] remove dependency on nucleus utils --- source/isaaclab/isaaclab/utils/nucleus.py | 67 +++++++++++++++++++ source/isaaclab/isaaclab/utils/prims.py | 1 + .../test/deps/isaacsim/check_camera.py | 6 +- .../check_floating_base_made_fixed.py | 2 +- .../deps/isaacsim/check_legged_robot_clone.py | 7 +- .../test/deps/isaacsim/check_ref_count.py | 7 +- 6 files changed, 72 insertions(+), 18 deletions(-) create mode 100644 source/isaaclab/isaaclab/utils/nucleus.py diff --git a/source/isaaclab/isaaclab/utils/nucleus.py b/source/isaaclab/isaaclab/utils/nucleus.py new file mode 100644 index 00000000000..998afa84c48 --- /dev/null +++ b/source/isaaclab/isaaclab/utils/nucleus.py @@ -0,0 +1,67 @@ +import carb +import omni.client +from omni.client import Result + +DEFAULT_ASSET_ROOT_PATH_SETTING = "/persistent/isaac/asset_root/default" +DEFAULT_ASSET_ROOT_TIMEOUT_SETTING = "/persistent/isaac/asset_root/timeout" + + +def check_server(server: str, path: str, timeout: float = 10.0) -> bool: + """Check a specific server for a path + + Args: + server (str): Name of Nucleus server + path (str): Path to search + timeout (float): Default value: 10 seconds + + Returns: + bool: True if folder is found + """ + carb.log_info("Checking path: {}{}".format(server, path)) + # Increase hang detection timeout + omni.client.set_hang_detection_time_ms(20000) + result, _ = omni.client.stat("{}{}".format(server, path)) + if result == Result.OK: + carb.log_info("Success: {}{}".format(server, path)) + return True + else: + carb.log_info("Failure: {}{} not accessible".format(server, path)) + return False + + +def get_assets_root_path(*, skip_check: bool = False) -> str: + """Tries to find the root path to the Isaac Sim assets on a Nucleus server + + Args: + skip_check (bool): If True, skip the checking step to verify that the resolved path exists. + + Raises: + RuntimeError: if the root path setting is not set. + RuntimeError: if the root path is not found. + + Returns: + url (str): URL of Nucleus server with root path to assets folder. + """ + + # get timeout + timeout = carb.settings.get_settings().get(DEFAULT_ASSET_ROOT_TIMEOUT_SETTING) + if not isinstance(timeout, (int, float)): + timeout = 10.0 + + # resolve path + carb.log_info(f"Check {DEFAULT_ASSET_ROOT_PATH_SETTING} setting") + default_asset_root = carb.settings.get_settings().get(DEFAULT_ASSET_ROOT_PATH_SETTING) + if not default_asset_root: + raise RuntimeError(f"The '{DEFAULT_ASSET_ROOT_PATH_SETTING}' setting is not set") + if skip_check: + return default_asset_root + + # check path + result = check_server(default_asset_root, "/Isaac", timeout) + if result: + result = check_server(default_asset_root, "/NVIDIA", timeout) + if result: + carb.log_info(f"Assets root found at {default_asset_root}") + return default_asset_root + + raise RuntimeError(f"Could not find assets root folder: {default_asset_root}") \ No newline at end of file diff --git a/source/isaaclab/isaaclab/utils/prims.py b/source/isaaclab/isaaclab/utils/prims.py index de6041cfb04..4f0e59b2c57 100644 --- a/source/isaaclab/isaaclab/utils/prims.py +++ b/source/isaaclab/isaaclab/utils/prims.py @@ -693,6 +693,7 @@ def create_prim( from isaacsim.core.api.simulation_context.simulation_context import SimulationContext if SimulationContext.instance() is None: + # FIXME: remove this, we should never even use backend utils especially not numpy ones import isaacsim.core.utils.numpy as backend_utils device = "cpu" diff --git a/source/isaaclab/test/deps/isaacsim/check_camera.py b/source/isaaclab/test/deps/isaacsim/check_camera.py index e5585121c76..120c4436c81 100644 --- a/source/isaaclab/test/deps/isaacsim/check_camera.py +++ b/source/isaaclab/test/deps/isaacsim/check_camera.py @@ -45,11 +45,6 @@ import os import random -try: - import isaacsim.storage.native as nucleus_utils -except ModuleNotFoundError: - import isaacsim.core.utils.nucleus as nucleus_utils - import omni.replicator.core as rep from isaacsim.core.api.world import World from isaacsim.core.prims import Articulation, RigidPrim, SingleGeometryPrim, SingleRigidPrim @@ -58,6 +53,7 @@ from PIL import Image, ImageChops from pxr import Gf, UsdGeom +import isaaclab.utils.nucleus as nucleus_utils import isaaclab.utils.prims as prim_utils # check nucleus connection diff --git a/source/isaaclab/test/deps/isaacsim/check_floating_base_made_fixed.py b/source/isaaclab/test/deps/isaacsim/check_floating_base_made_fixed.py index 487df23cc38..02073f424f6 100644 --- a/source/isaaclab/test/deps/isaacsim/check_floating_base_made_fixed.py +++ b/source/isaaclab/test/deps/isaacsim/check_floating_base_made_fixed.py @@ -32,7 +32,6 @@ import torch -import isaacsim.core.utils.nucleus as nucleus_utils import omni.kit.commands import omni.log import omni.physx @@ -42,6 +41,7 @@ from isaacsim.core.utils.viewports import set_camera_view from pxr import PhysxSchema, UsdPhysics +import isaaclab.utils.nucleus as nucleus_utils import isaaclab.utils.prims as prim_utils import isaaclab.utils.stage as stage_utils diff --git a/source/isaaclab/test/deps/isaacsim/check_legged_robot_clone.py b/source/isaaclab/test/deps/isaacsim/check_legged_robot_clone.py index a157a4ce4e3..44f5f84600e 100644 --- a/source/isaaclab/test/deps/isaacsim/check_legged_robot_clone.py +++ b/source/isaaclab/test/deps/isaacsim/check_legged_robot_clone.py @@ -44,18 +44,13 @@ import torch import omni.log - -try: - import isaacsim.storage.native as nucleus_utils -except ModuleNotFoundError: - import isaacsim.core.utils.nucleus as nucleus_utils - from isaacsim.core.api.world import World from isaacsim.core.cloner import GridCloner from isaacsim.core.prims import Articulation from isaacsim.core.utils.carb import set_carb_setting from isaacsim.core.utils.viewports import set_camera_view +import isaaclab.utils.nucleus as nucleus_utils import isaaclab.utils.prims as prim_utils # check nucleus connection diff --git a/source/isaaclab/test/deps/isaacsim/check_ref_count.py b/source/isaaclab/test/deps/isaacsim/check_ref_count.py index 9f4a9d7720f..1da33fa2dfa 100644 --- a/source/isaaclab/test/deps/isaacsim/check_ref_count.py +++ b/source/isaaclab/test/deps/isaacsim/check_ref_count.py @@ -38,16 +38,11 @@ import torch # noqa: F401 import omni.log - -try: - import isaacsim.storage.native as nucleus_utils -except ModuleNotFoundError: - import isaacsim.core.utils.nucleus as nucleus_utils - from isaacsim.core.api.simulation_context import SimulationContext from isaacsim.core.prims import Articulation from isaacsim.core.utils.carb import set_carb_setting +import isaaclab.utils.nucleus as nucleus_utils import isaaclab.utils.prims as prim_utils # check nucleus connection