From 52bbc99f10170c08d0478c1a28d247991dced40d Mon Sep 17 00:00:00 2001 From: Octi Zhang Date: Sun, 5 Apr 2026 23:42:52 -0700 Subject: [PATCH 01/24] warp native xform prim view --- .../benchmark_newton_xform_prim_view.py | 144 +++ .../benchmarks/benchmark_xform_prim_view.py | 151 ++- source/isaaclab/docs/CHANGELOG.rst | 14 + source/isaaclab/isaaclab/sim/__init__.pyi | 4 +- .../isaaclab/isaaclab/sim/views/__init__.pyi | 4 + .../sim/views/base_xform_prim_view.py | 109 ++ .../isaaclab/sim/views/xform_prim_view.py | 1077 +++-------------- .../sim/views/xform_prim_view_factory.py | 44 + .../test/sim/test_views_xform_prim.py | 160 +-- source/isaaclab_newton/config/extension.toml | 2 +- source/isaaclab_newton/docs/CHANGELOG.rst | 14 +- .../isaaclab_newton/sim/__init__.py | 10 + .../isaaclab_newton/sim/__init__.pyi | 10 + .../isaaclab_newton/sim/views/__init__.py | 10 + .../isaaclab_newton/sim/views/__init__.pyi | 10 + .../sim/views/xform_prim_view.py | 495 ++++++++ source/isaaclab_newton/test/sim/__init__.py | 4 + .../test/sim/test_views_xform_prim_newton.py | 432 +++++++ source/isaaclab_physx/config/extension.toml | 2 +- source/isaaclab_physx/docs/CHANGELOG.rst | 10 + .../isaaclab_physx/sim/__init__.py | 10 + .../isaaclab_physx/sim/__init__.pyi | 10 + .../isaaclab_physx/sim/views/__init__.py | 10 + .../isaaclab_physx/sim/views/__init__.pyi | 10 + .../sim/views/xform_prim_view.py | 380 ++++++ source/isaaclab_physx/test/sim/__init__.py | 4 + .../test/sim/test_views_xform_prim_fabric.py | 478 ++++++++ 27 files changed, 2569 insertions(+), 1039 deletions(-) create mode 100644 scripts/benchmarks/benchmark_newton_xform_prim_view.py create mode 100644 source/isaaclab/isaaclab/sim/views/base_xform_prim_view.py create mode 100644 source/isaaclab/isaaclab/sim/views/xform_prim_view_factory.py create mode 100644 source/isaaclab_newton/isaaclab_newton/sim/__init__.py create mode 100644 source/isaaclab_newton/isaaclab_newton/sim/__init__.pyi create mode 100644 source/isaaclab_newton/isaaclab_newton/sim/views/__init__.py create mode 100644 source/isaaclab_newton/isaaclab_newton/sim/views/__init__.pyi create mode 100644 source/isaaclab_newton/isaaclab_newton/sim/views/xform_prim_view.py create mode 100644 source/isaaclab_newton/test/sim/__init__.py create mode 100644 source/isaaclab_newton/test/sim/test_views_xform_prim_newton.py create mode 100644 source/isaaclab_physx/isaaclab_physx/sim/__init__.py create mode 100644 source/isaaclab_physx/isaaclab_physx/sim/__init__.pyi create mode 100644 source/isaaclab_physx/isaaclab_physx/sim/views/__init__.py create mode 100644 source/isaaclab_physx/isaaclab_physx/sim/views/__init__.pyi create mode 100644 source/isaaclab_physx/isaaclab_physx/sim/views/xform_prim_view.py create mode 100644 source/isaaclab_physx/test/sim/__init__.py create mode 100644 source/isaaclab_physx/test/sim/test_views_xform_prim_fabric.py diff --git a/scripts/benchmarks/benchmark_newton_xform_prim_view.py b/scripts/benchmarks/benchmark_newton_xform_prim_view.py new file mode 100644 index 000000000000..95b0a4773bcf --- /dev/null +++ b/scripts/benchmarks/benchmark_newton_xform_prim_view.py @@ -0,0 +1,144 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Benchmark script for Newton XformPrimView performance. + +Tests the performance of batched transform operations using Newton's GPU-backed +XformPrimView (sites / body_q) without requiring Isaac Sim Kit. + +Usage: + VIRTUAL_ENV=env_isaaclab ./isaaclab.sh -p scripts/benchmarks/benchmark_newton_xform_prim_view.py --num_envs 4096 + VIRTUAL_ENV=env_isaaclab ./isaaclab.sh -p scripts/benchmarks/benchmark_newton_xform_prim_view.py \ + --num_envs 4096 --num_iterations 200 +""" + +from __future__ import annotations + +import argparse +import time + +import torch +from isaaclab_newton.physics import MJWarpSolverCfg, NewtonCfg +from isaaclab_newton.sim.views import XformPrimView + +import isaaclab.sim as sim_utils +from isaaclab.assets import RigidObjectCfg +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg +from isaaclab.sim import SimulationCfg, build_simulation_context +from isaaclab.utils import configclass + +NEWTON_SIM_CFG = SimulationCfg( + physics=NewtonCfg(solver_cfg=MJWarpSolverCfg()), +) + + +@configclass +class BenchSceneCfg(InteractiveSceneCfg): + cube: RigidObjectCfg = RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/Cube", + spawn=sim_utils.CuboidCfg( + size=(0.2, 0.2, 0.2), + rigid_props=sim_utils.RigidBodyPropertiesCfg(), + mass_props=sim_utils.MassPropertiesCfg(mass=1.0), + collision_props=sim_utils.CollisionPropertiesCfg(), + ), + init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, 0.0, 1.0)), + ) + + +@torch.no_grad() +def benchmark(num_envs: int, num_iterations: int, device: str) -> dict[str, float]: + """Run the benchmark and return timing results in seconds.""" + NEWTON_SIM_CFG.device = device + results: dict[str, float] = {} + + with build_simulation_context(device=device, sim_cfg=NEWTON_SIM_CFG, add_ground_plane=True) as sim: + sim._app_control_on_stop_handle = None + InteractiveScene(BenchSceneCfg(num_envs=num_envs, env_spacing=2.0)) + sim.reset() + + view = XformPrimView("/World/envs/env_.*/Cube", device=device) + print(f" View count: {view.count}") + + # -- warmup (compile Warp kernel, first torch allocs) -- + for _ in range(5): + view.get_world_poses() + + # -- get_world_poses (full) -- + torch.cuda.synchronize() + t0 = time.perf_counter() + for _ in range(num_iterations): + pos, quat = view.get_world_poses() + torch.cuda.synchronize() + results["get_world_poses"] = (time.perf_counter() - t0) / num_iterations + + # -- get_world_poses (indexed, 50 %) -- + half = list(range(0, num_envs, 2)) + torch.cuda.synchronize() + t0 = time.perf_counter() + for _ in range(num_iterations): + pos, quat = view.get_world_poses(half) + torch.cuda.synchronize() + results["get_world_poses_indexed_50pct"] = (time.perf_counter() - t0) / num_iterations + + # -- set_world_poses (full) -- + new_pos = torch.rand((num_envs, 3), device=device) + new_quat = torch.tensor([[0.0, 0.0, 0.0, 1.0]] * num_envs, device=device) + torch.cuda.synchronize() + t0 = time.perf_counter() + for _ in range(num_iterations): + view.set_world_poses(new_pos, new_quat) + torch.cuda.synchronize() + results["set_world_poses"] = (time.perf_counter() - t0) / num_iterations + + # -- interleaved set -> get -- + torch.cuda.synchronize() + t0 = time.perf_counter() + for _ in range(num_iterations): + view.set_world_poses(new_pos, new_quat) + pos, quat = view.get_world_poses() + torch.cuda.synchronize() + results["interleaved_set_get"] = (time.perf_counter() - t0) / num_iterations + + return results + + +def print_results(results: dict[str, float], num_envs: int, num_iterations: int): + """Print benchmark results in a formatted table.""" + print("\n" + "=" * 70) + print(f"Newton XformPrimView Benchmark: {num_envs} envs, {num_iterations} iters") + print("=" * 70) + print(f"{'Operation':<40} {'Time (ms)':>12} {'us/env':>12}") + print("-" * 70) + for op, t in results.items(): + ms = t * 1000 + us_per_env = t * 1e6 / num_envs + print(f"{op:<40} {ms:>12.4f} {us_per_env:>12.4f}") + total = sum(results.values()) * 1000 + print("-" * 70) + print(f"{'Total':<40} {total:>12.4f}") + print("=" * 70) + print() + + +def main(): + parser = argparse.ArgumentParser(description="Benchmark Newton XformPrimView performance.") + parser.add_argument("--num_envs", type=int, default=4096) + parser.add_argument("--num_iterations", type=int, default=100) + parser.add_argument("--device", type=str, default="cuda:0") + args = parser.parse_args() + + print("=" * 70) + print("Newton XformPrimView Benchmark") + print("=" * 70) + print(f" Envs: {args.num_envs} Iterations: {args.num_iterations} Device: {args.device}") + print() + + results = benchmark(args.num_envs, args.num_iterations, args.device) + print_results(results, args.num_envs, args.num_iterations) + + +if __name__ == "__main__": + main() diff --git a/scripts/benchmarks/benchmark_xform_prim_view.py b/scripts/benchmarks/benchmark_xform_prim_view.py index e76796e20271..761a74e3e12a 100644 --- a/scripts/benchmarks/benchmark_xform_prim_view.py +++ b/scripts/benchmarks/benchmark_xform_prim_view.py @@ -67,15 +67,27 @@ import torch -from isaacsim.core.prims import XFormPrim as IsaacSimXformPrimView -from isaacsim.core.utils.extensions import enable_extension +try: + from isaacsim.core.prims import XFormPrim as IsaacSimXformPrimView + from isaacsim.core.utils.extensions import enable_extension -# compare against latest Isaac Sim implementation -enable_extension("isaacsim.core.experimental.prims") -from isaacsim.core.experimental.prims import XformPrim as IsaacSimExperimentalXformPrimView + enable_extension("isaacsim.core.experimental.prims") + from isaacsim.core.experimental.prims import XformPrim as IsaacSimExperimentalXformPrimView + + _HAS_ISAACSIM = True +except (ImportError, ModuleNotFoundError): + _HAS_ISAACSIM = False + +from isaaclab_newton.physics import MJWarpSolverCfg, NewtonCfg +from isaaclab_newton.sim.views import XformPrimView as IsaacLabNewtonXformPrimView +from isaaclab_physx.sim.views import XformPrimView as IsaacLabFabricXformPrimView import isaaclab.sim as sim_utils +from isaaclab.assets import RigidObjectCfg +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg +from isaaclab.sim import SimulationCfg, build_simulation_context from isaaclab.sim.views import XformPrimView as IsaacLabXformPrimView +from isaaclab.utils import configclass @torch.no_grad() @@ -133,8 +145,10 @@ def benchmark_xform_prim_view( # noqa: C901 # Create view start_time = time.perf_counter() - if api == "isaaclab-usd" or api == "isaaclab-fabric": + if api == "isaaclab-usd": xform_view = IsaacLabXformPrimView(pattern, device=args_cli.device, validate_xform_ops=False) + elif api == "isaaclab-fabric": + xform_view = IsaacLabFabricXformPrimView(pattern, device=args_cli.device, validate_xform_ops=False) elif api == "isaacsim-usd": xform_view = IsaacSimXformPrimView(pattern, reset_xform_properties=False, usd=True) elif api == "isaacsim-fabric": @@ -279,6 +293,109 @@ def benchmark_xform_prim_view( # noqa: C901 return timing_results, computed_results +@configclass +class _NewtonBenchSceneCfg(InteractiveSceneCfg): + cube: RigidObjectCfg = RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/Object", + spawn=sim_utils.CuboidCfg( + size=(0.2, 0.2, 0.2), + rigid_props=sim_utils.RigidBodyPropertiesCfg(), + mass_props=sim_utils.MassPropertiesCfg(mass=1.0), + collision_props=sim_utils.CollisionPropertiesCfg(), + ), + init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, 0.0, 1.0)), + ) + + +@torch.no_grad() +def benchmark_newton_xform_prim_view( + num_iterations: int, +) -> tuple[dict[str, float], dict[str, torch.Tensor]]: + """Benchmark Newton XformPrimView (GPU sites / body_q).""" + timing_results = {} + computed_results = {} + + print(" Setting up Newton scene") + newton_sim_cfg = SimulationCfg(device=args_cli.device, physics=NewtonCfg(solver_cfg=MJWarpSolverCfg())) + + with build_simulation_context(device=args_cli.device, sim_cfg=newton_sim_cfg, add_ground_plane=True) as sim: + sim._app_control_on_stop_handle = None + InteractiveScene(_NewtonBenchSceneCfg(num_envs=args_cli.num_envs, env_spacing=2.0)) + sim.reset() + + start_time = time.perf_counter() + xform_view = IsaacLabNewtonXformPrimView("/World/envs/env_.*/Object", device=args_cli.device) + timing_results["init"] = time.perf_counter() - start_time + + num_prims = xform_view.count + print(f" Newton XformView managing {num_prims} prims") + + # warmup + for _ in range(5): + xform_view.get_world_poses() + + # get_world_poses + torch.cuda.synchronize() + start_time = time.perf_counter() + for _ in range(num_iterations): + positions, orientations = xform_view.get_world_poses() + torch.cuda.synchronize() + timing_results["get_world_poses"] = (time.perf_counter() - start_time) / num_iterations + + computed_results["initial_world_positions"] = positions.clone() + computed_results["initial_world_orientations"] = orientations.clone() + + # set_world_poses + new_positions = positions.clone() + new_positions[:, 2] += 0.1 + torch.cuda.synchronize() + start_time = time.perf_counter() + for _ in range(num_iterations): + xform_view.set_world_poses(new_positions, orientations) + torch.cuda.synchronize() + timing_results["set_world_poses"] = (time.perf_counter() - start_time) / num_iterations + + positions_after, orientations_after = xform_view.get_world_poses() + computed_results["world_positions_after_set"] = positions_after.clone() + computed_results["world_orientations_after_set"] = orientations_after.clone() + + # get_local_poses (delegates to world for Newton) + torch.cuda.synchronize() + start_time = time.perf_counter() + for _ in range(num_iterations): + translations, orientations_local = xform_view.get_local_poses() + torch.cuda.synchronize() + timing_results["get_local_poses"] = (time.perf_counter() - start_time) / num_iterations + + computed_results["initial_local_translations"] = translations.clone() + computed_results["initial_local_orientations"] = orientations_local.clone() + + # set_local_poses + new_translations = translations.clone() + new_translations[:, 2] += 0.1 + torch.cuda.synchronize() + start_time = time.perf_counter() + for _ in range(num_iterations): + xform_view.set_local_poses(new_translations, orientations_local) + torch.cuda.synchronize() + timing_results["set_local_poses"] = (time.perf_counter() - start_time) / num_iterations + + translations_after, orientations_local_after = xform_view.get_local_poses() + computed_results["local_translations_after_set"] = translations_after.clone() + computed_results["local_orientations_after_set"] = orientations_local_after.clone() + + # interleaved set -> get + torch.cuda.synchronize() + start_time = time.perf_counter() + for _ in range(num_iterations): + xform_view.set_world_poses(new_positions, orientations) + positions, orientations = xform_view.get_world_poses() + torch.cuda.synchronize() + timing_results["interleaved_world_set_get"] = (time.perf_counter() - start_time) / num_iterations + + return timing_results, computed_results + + def compare_results( results_dict: dict[str, dict[str, torch.Tensor]], tolerance: float = 1e-4 ) -> dict[str, dict[str, dict[str, float]]]: @@ -566,10 +683,14 @@ def main(): apis_to_test = [ ("isaaclab-usd", "Isaac Lab XformPrimView (USD)"), ("isaaclab-fabric", "Isaac Lab XformPrimView (Fabric)"), - ("isaacsim-usd", "Isaac Sim XformPrimView (USD)"), - ("isaacsim-fabric", "Isaac Sim XformPrimView (Fabric)"), - ("isaacsim-exp", "Isaac Sim Experimental XformPrim"), ] + # Uncomment to include Isaac Sim APIs in the comparison (requires isaacsim package): + # if _HAS_ISAACSIM: + # apis_to_test += [ + # ("isaacsim-usd", "Isaac Sim XformPrimView (USD)"), + # ("isaacsim-fabric", "Isaac Sim XformPrimView (Fabric)"), + # ("isaacsim-exp", "Isaac Sim Experimental XformPrim"), + # ] # Benchmark each API for api_key, api_name in apis_to_test: @@ -579,7 +700,6 @@ def main(): profiler = cProfile.Profile() profiler.enable() - # Cast api_key to Literal type for type checker timing, computed = benchmark_xform_prim_view( api=api_key, # type: ignore[arg-type] num_iterations=args_cli.num_iterations, @@ -598,6 +718,17 @@ def main(): print(" Done!") print() + # Benchmark Newton (separate setup path) + if "cuda" in args_cli.device: + print("Benchmarking Isaac Lab XformPrimView (Newton)...") + timing, computed = benchmark_newton_xform_prim_view(num_iterations=args_cli.num_iterations) + all_timing_results["isaaclab-newton"] = timing + all_computed_results["isaaclab-newton"] = computed + print(" Done!") + print() + else: + print("Note: Skipping Newton benchmark (requires CUDA).\n") + # Print timing results print_results(all_timing_results, args_cli.num_envs, args_cli.num_iterations) diff --git a/source/isaaclab/docs/CHANGELOG.rst b/source/isaaclab/docs/CHANGELOG.rst index 59f13c8fa407..2be842d4964e 100644 --- a/source/isaaclab/docs/CHANGELOG.rst +++ b/source/isaaclab/docs/CHANGELOG.rst @@ -171,6 +171,20 @@ Added Added ^^^^^ + +* Added :class:`~isaaclab.sim.views.BaseXformPrimView` abstract base class that defines + the common interface for backend-specific ``XformPrimView`` implementations. +* Added :class:`~isaaclab.sim.views.XformPrimViewFactory` to instantiate the correct + backend-specific ``XformPrimView`` based on the active simulation backend. + +Changed +^^^^^^^ + +* Refactored :class:`~isaaclab.sim.views.XformPrimView` to delegate backend-specific + logic to :class:`~isaaclab_physx.sim.views.XformPrimView` and + :class:`~isaaclab_newton.sim.views.XformPrimView`. The public API is unchanged; + use :class:`~isaaclab.sim.views.XformPrimViewFactory` for backend-aware instantiation. + * Added release version to :class:`~isaaclab.test.benchmark.recorders.VersionInfoRecorder` output. diff --git a/source/isaaclab/isaaclab/sim/__init__.pyi b/source/isaaclab/isaaclab/sim/__init__.pyi index 32a5ea87e197..30028da60041 100644 --- a/source/isaaclab/isaaclab/sim/__init__.pyi +++ b/source/isaaclab/isaaclab/sim/__init__.pyi @@ -164,7 +164,9 @@ __all__ = [ "resolve_prim_pose", "resolve_prim_scale", "convert_world_pose_to_local", + "BaseXformPrimView", "XformPrimView", + "XformPrimViewFactory", ] from .simulation_cfg import RenderCfg, SimulationCfg @@ -333,4 +335,4 @@ from .utils import ( resolve_prim_scale, convert_world_pose_to_local, ) -from .views import XformPrimView +from .views import BaseXformPrimView, XformPrimView, XformPrimViewFactory diff --git a/source/isaaclab/isaaclab/sim/views/__init__.pyi b/source/isaaclab/isaaclab/sim/views/__init__.pyi index a666958e4387..aeec320b6350 100644 --- a/source/isaaclab/isaaclab/sim/views/__init__.pyi +++ b/source/isaaclab/isaaclab/sim/views/__init__.pyi @@ -4,7 +4,11 @@ # SPDX-License-Identifier: BSD-3-Clause __all__ = [ + "BaseXformPrimView", "XformPrimView", + "XformPrimViewFactory", ] +from .base_xform_prim_view import BaseXformPrimView from .xform_prim_view import XformPrimView +from .xform_prim_view_factory import XformPrimViewFactory diff --git a/source/isaaclab/isaaclab/sim/views/base_xform_prim_view.py b/source/isaaclab/isaaclab/sim/views/base_xform_prim_view.py new file mode 100644 index 000000000000..78fbdbad7369 --- /dev/null +++ b/source/isaaclab/isaaclab/sim/views/base_xform_prim_view.py @@ -0,0 +1,109 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Abstract base class for batched prim transform views.""" + +from __future__ import annotations + +import abc +from collections.abc import Sequence + +import warp as wp + + +class BaseXformPrimView(abc.ABC): + """Abstract interface for reading and writing world-space transforms of multiple prims. + + Backend-specific implementations (USD/Fabric, Newton GPU state, etc.) subclass + this to provide efficient batched pose queries. The factory + :class:`~isaaclab.sim.views.XformPrimViewFactory` selects the correct + implementation at runtime based on the active physics backend. + + All getters return ``wp.array``. Setters accept ``wp.array``. + """ + + @property + @abc.abstractmethod + def count(self) -> int: + """Number of prims in this view.""" + ... + + @abc.abstractmethod + def get_world_poses(self, indices: Sequence[int] | None = None) -> tuple[wp.array, wp.array]: + """Get world-space positions and orientations for prims in the view. + + Args: + indices: Subset of prims to query. ``None`` means all prims. + + Returns: + A tuple ``(positions (M, 3), orientations (M, 4))`` as ``wp.array``. + """ + ... + + @abc.abstractmethod + def set_world_poses( + self, + positions: wp.array | None = None, + orientations: wp.array | None = None, + indices: Sequence[int] | None = None, + ) -> None: + """Set world-space positions and/or orientations for prims in the view. + + Args: + positions: World-space positions ``(M, 3)``. ``None`` leaves positions unchanged. + orientations: World-space quaternions ``(M, 4)``. ``None`` leaves orientations unchanged. + indices: Subset of prims to update. ``None`` means all prims. + """ + ... + + @abc.abstractmethod + def get_local_poses(self, indices: Sequence[int] | None = None) -> tuple[wp.array, wp.array]: + """Get local-space positions and orientations for prims in the view. + + Args: + indices: Subset of prims to query. ``None`` means all prims. + + Returns: + A tuple ``(translations (M, 3), orientations (M, 4))`` as ``wp.array``. + """ + ... + + @abc.abstractmethod + def set_local_poses( + self, + translations: wp.array | None = None, + orientations: wp.array | None = None, + indices: Sequence[int] | None = None, + ) -> None: + """Set local-space translations and/or orientations for prims in the view. + + Args: + translations: Local-space translations ``(M, 3)``. ``None`` leaves translations unchanged. + orientations: Local-space quaternions ``(M, 4)``. ``None`` leaves orientations unchanged. + indices: Subset of prims to update. ``None`` means all prims. + """ + ... + + @abc.abstractmethod + def get_scales(self, indices: Sequence[int] | None = None) -> wp.array: + """Get scales for prims in the view. + + Args: + indices: Subset of prims to query. ``None`` means all prims. + + Returns: + A ``wp.array`` of shape ``(M, 3)``. + """ + ... + + @abc.abstractmethod + def set_scales(self, scales: wp.array, indices: Sequence[int] | None = None) -> None: + """Set scales for prims in the view. + + Args: + scales: Scales ``(M, 3)`` as ``wp.array``. + indices: Subset of prims to update. ``None`` means all prims. + """ + ... diff --git a/source/isaaclab/isaaclab/sim/views/xform_prim_view.py b/source/isaaclab/isaaclab/sim/views/xform_prim_view.py index 211994a7226b..f2eaf30f2fce 100644 --- a/source/isaaclab/isaaclab/sim/views/xform_prim_view.py +++ b/source/isaaclab/isaaclab/sim/views/xform_prim_view.py @@ -15,56 +15,27 @@ from pxr import Gf, Sdf, Usd, UsdGeom, Vt import isaaclab.sim as sim_utils -from isaaclab.app.settings_manager import SettingsManager -from isaaclab.utils.warp import fabric as fabric_utils + +from .base_xform_prim_view import BaseXformPrimView logger = logging.getLogger(__name__) -class XformPrimView: - """Optimized batched interface for reading and writing transforms of multiple USD prims. +class XformPrimView(BaseXformPrimView): + """Batched interface for reading and writing transforms of multiple USD prims. - This class provides efficient batch operations for getting and setting poses (position and orientation) - of multiple prims at once using torch tensors. It is designed for scenarios where you need to manipulate - many prims simultaneously, such as in multi-agent simulations or large-scale procedural generation. + Provides batch operations for getting and setting poses (position and orientation) + of multiple prims at once via USD's ``XformCache``. The class supports both world-space and local-space pose operations: - **World poses**: Positions and orientations in the global world frame - **Local poses**: Positions and orientations relative to each prim's parent - When Fabric is enabled, the class leverages NVIDIA's Fabric API for GPU-accelerated batch operations: - - - Uses `omni:fabric:worldMatrix` and `omni:fabric:localMatrix` attributes for all Boundable prims - - Performs batch matrix decomposition/composition using Warp kernels on GPU - - Achieves performance comparable to Isaac Sim's XFormPrim implementation - - Works for both physics-enabled and non-physics prims (cameras, meshes, etc.). - Note: renderers typically consume USD-authored camera transforms. - - .. warning:: - **Fabric requires CUDA**: Fabric is only supported with on CUDA devices. - Warp's CPU backend for fabric-array writes has known issues, so attempting to use - Fabric with CPU device (``device="cpu"``) will raise a ValueError at initialization. - - .. note:: - **Fabric Support:** - - When Fabric is enabled, this view ensures prims have the required Fabric hierarchy - attributes (``omni:fabric:localMatrix`` and ``omni:fabric:worldMatrix``). On first Fabric - read, USD-authored transforms initialize Fabric state. Fabric writes can optionally - be mirrored back to USD via :attr:`sync_usd_on_fabric_write`. + For GPU-accelerated Fabric operations, use the PhysX backend variant + obtained via :class:`~isaaclab.sim.views.XformPrimViewFactory`. - For more information, see the `Fabric Hierarchy documentation`_. - - .. _Fabric Hierarchy documentation: https://docs.omniverse.nvidia.com/kit/docs/usdrt/latest/docs/fabric_hierarchy.html - - .. note:: - **Performance Considerations:** - - * Tensor operations are performed on the specified device (CPU/CUDA) - * USD write operations use ``Sdf.ChangeBlock`` for batched updates - * Fabric operations use GPU-accelerated Warp kernels for maximum performance - * For maximum performance, minimize get/set operations within tight loops + All getters return ``wp.array``. Setters accept ``wp.array``. .. note:: **Transform Requirements:** @@ -85,47 +56,35 @@ def __init__( prim_path: str, device: str = "cpu", validate_xform_ops: bool = True, - sync_usd_on_fabric_write: bool = False, stage: Usd.Stage | None = None, + **kwargs, ): """Initialize the view with matching prims. - This method searches the USD stage for all prims matching the provided path pattern, - validates that they are Xformable with standard transform operations, and stores - references for efficient batch operations. - - We generally recommend to validate the xform operations, as it ensures that the prims are in a consistent state - and have the standard transform operations (translate, orient, scale in that order). - However, if you are sure that the prims are in a consistent state, you can set this to False to improve - performance. This can save around 45-50% of the time taken to initialize the view. - Args: prim_path: USD prim path pattern to match prims. Supports wildcards (``*``) and regex patterns (e.g., ``"/World/Env_.*/Robot"``). See :func:`isaaclab.sim.utils.find_matching_prims` for pattern syntax. - device: Device to place the tensors on. Can be ``"cpu"`` or CUDA devices like + device: Device to place arrays on. Can be ``"cpu"`` or CUDA devices like ``"cuda:0"``. Defaults to ``"cpu"``. validate_xform_ops: Whether to validate that the prims have standard xform operations. Defaults to True. - sync_usd_on_fabric_write: Whether to mirror Fabric transform writes back to USD. - When True, transform updates are synchronized to USD so that USD data readers (e.g., rendering - cameras) can observe these changes. Defaults to False for better performance. - stage: USD stage to search for prims. Defaults to None, in which case the current active stage - from the simulation context is used. + stage: USD stage to search for prims. Defaults to None, in which case the current + active stage from the simulation context is used. + **kwargs: Additional keyword arguments (ignored). Allows forward-compatible + construction when callers pass backend-specific options like + ``sync_usd_on_fabric_write``. Raises: ValueError: If any matched prim is not Xformable or doesn't have standardized transform operations (translate, orient, scale in that order). """ - # Store configuration self._prim_path = prim_path self._device = device - # Find and validate matching prims stage = sim_utils.get_current_stage() if stage is None else stage self._prims: list[Usd.Prim] = sim_utils.find_matching_prims(prim_path, stage=stage) - # Validate all prims have standard xform operations if validate_xform_ops: for prim in self._prims: sim_utils.standardize_xform_ops(prim) @@ -136,56 +95,11 @@ def __init__( " Use sim_utils.standardize_xform_ops() to prepare the prim." ) - # Determine if Fabric is supported on the device - settings = SettingsManager.instance() - self._use_fabric = bool(settings.get("/physics/fabricEnabled", False)) - - # Check for unsupported Fabric + CPU combination - if self._use_fabric and self._device == "cpu": - logger.warning( - "Fabric mode with Warp fabric-array operations is not supported on CPU devices. " - "While Fabric itself can run on both CPU and GPU, our batch Warp kernels for " - "fabric-array operations require CUDA and are not reliable on the CPU backend. " - "To ensure stability, Fabric is being disabled and execution will fall back " - "to standard USD operations on the CPU. This may impact performance." - ) - self._use_fabric = False - - # Check for unsupported Fabric + non-primary CUDA device combination. - # USDRT SelectPrims and Warp fabric arrays only support cuda:0 internally. - # When running on cuda:1 or higher, SelectPrims raises a C++ error regardless of - # the device argument, because USDRT uses the active CUDA context (which is cuda:1). - if self._use_fabric and self._device not in ("cuda", "cuda:0"): - logger.warning( - f"Fabric mode is not supported on device '{self._device}'. " - "USDRT SelectPrims and Warp fabric arrays only support cuda:0. " - "Falling back to standard USD operations. This may impact performance." - ) - self._use_fabric = False - - # Create indices buffer - # Since we iterate over the indices, we need to use range instead of torch tensor self._ALL_INDICES = list(range(len(self._prims))) - # Some prims (e.g., Cameras) require USD-authored transforms for rendering. - # When enabled, mirror Fabric pose writes to USD for those prims. - self._sync_usd_on_fabric_write = sync_usd_on_fabric_write - - # Fabric batch infrastructure (initialized lazily on first use) - self._fabric_initialized = False - self._fabric_usd_sync_done = False - self._fabric_selection = None - self._fabric_to_view: wp.array | None = None - self._view_to_fabric: wp.array | None = None - self._default_view_indices: wp.array | None = None - self._fabric_hierarchy = None - # Create a valid USD attribute name: namespace:name - # Use "isaaclab" namespace to identify our custom attributes - self._view_index_attr = f"isaaclab:view_index:{abs(hash(self))}" - - """ - Properties. - """ + # ------------------------------------------------------------------ + # Properties + # ------------------------------------------------------------------ @property def count(self) -> int: @@ -194,7 +108,7 @@ def count(self) -> int: @property def device(self) -> str: - """Device where tensors are allocated (cpu or cuda).""" + """Device where arrays are allocated (cpu or cuda).""" return self._device @property @@ -206,394 +120,93 @@ def prims(self) -> list[Usd.Prim]: def prim_paths(self) -> list[str]: """List of prim paths (as strings) for all prims being managed by this view. - This property converts each prim to its path string representation. The conversion is - performed lazily on first access and cached for subsequent accesses. - - Note: - For most use cases, prefer using :attr:`prims` directly as it provides direct access - to the USD prim objects without the conversion overhead. This property is mainly useful - for logging, debugging, or when string paths are explicitly required. + The conversion is performed lazily on first access and cached. """ - # we cache it the first time it is accessed. - # we don't compute it in constructor because it is expensive and we don't need it most of the time. - # users should usually deal with prims directly as they typically need to access the prims directly. if not hasattr(self, "_prim_paths"): self._prim_paths = [prim.GetPath().pathString for prim in self._prims] return self._prim_paths - """ - Operations - Setters. - """ + # ------------------------------------------------------------------ + # Setters + # ------------------------------------------------------------------ def set_world_poses( self, - positions: torch.Tensor | None = None, - orientations: torch.Tensor | None = None, + positions: wp.array | None = None, + orientations: wp.array | None = None, indices: Sequence[int] | None = None, ): """Set world-space poses for prims in the view. - This method sets the position and/or orientation of each prim in world space. - - - When Fabric is enabled, the function writes directly to Fabric's ``omni:fabric:worldMatrix`` - attribute using GPU-accelerated batch operations. - - When Fabric is disabled, the function converts to local space and writes to USD's ``xformOp:translate`` - and ``xformOp:orient`` attributes. - - Args: - positions: World-space positions as a tensor of shape (M, 3) where M is the number of prims - to set (either all prims if indices is None, or the number of indices provided). - Defaults to None, in which case positions are not modified. - orientations: World-space orientations as quaternions (w, x, y, z) with shape (M, 4). - Defaults to None, in which case orientations are not modified. - indices: Indices of prims to set poses for. Defaults to None, in which case poses are set - for all prims in the view. - - Raises: - ValueError: If positions shape is not (M, 3) or orientations shape is not (M, 4). - ValueError: If the number of poses doesn't match the number of indices provided. - """ - if self._use_fabric: - self._set_world_poses_fabric(positions, orientations, indices) - else: - self._set_world_poses_usd(positions, orientations, indices) - - def set_local_poses( - self, - translations: torch.Tensor | None = None, - orientations: torch.Tensor | None = None, - indices: Sequence[int] | None = None, - ): - """Set local-space poses for prims in the view. - - This method sets the position and/or orientation of each prim in local space (relative to - their parent prims). - - The function writes directly to USD's ``xformOp:translate`` and ``xformOp:orient`` attributes. - - Note: - Even in Fabric mode, local pose operations use USD. This behavior is based on Isaac Sim's design - where Fabric is only used for world pose operations. - - Rationale: - - Local pose writes need correct parent-child hierarchy relationships - - USD maintains these relationships correctly and efficiently - - Fabric is optimized for world pose operations, not local hierarchies - - Args: - translations: Local-space translations as a tensor of shape (M, 3) where M is the number of prims - to set (either all prims if indices is None, or the number of indices provided). - Defaults to None, in which case translations are not modified. - orientations: Local-space orientations as quaternions (w, x, y, z) with shape (M, 4). - Defaults to None, in which case orientations are not modified. - indices: Indices of prims to set poses for. Defaults to None, in which case poses are set - for all prims in the view. - - Raises: - ValueError: If translations shape is not (M, 3) or orientations shape is not (M, 4). - ValueError: If the number of poses doesn't match the number of indices provided. - """ - if self._use_fabric: - self._set_local_poses_fabric(translations, orientations, indices) - else: - self._set_local_poses_usd(translations, orientations, indices) - - def set_scales(self, scales: torch.Tensor, indices: Sequence[int] | None = None): - """Set scales for prims in the view. - - This method sets the scale of each prim in the view. - - - When Fabric is enabled, the function updates scales in Fabric matrices using GPU-accelerated batch operations. - - When Fabric is disabled, the function writes to USD's ``xformOp:scale`` attributes. - - Args: - scales: Scales as a tensor of shape (M, 3) where M is the number of prims - to set (either all prims if indices is None, or the number of indices provided). - indices: Indices of prims to set scales for. Defaults to None, in which case scales are set - for all prims in the view. - - Raises: - ValueError: If scales shape is not (M, 3). - """ - if self._use_fabric: - self._set_scales_fabric(scales, indices) - else: - self._set_scales_usd(scales, indices) - - def set_visibility(self, visibility: torch.Tensor, indices: Sequence[int] | None = None): - """Set visibility for prims in the view. - - This method sets the visibility of each prim in the view. - - Args: - visibility: Visibility as a boolean tensor of shape (M,) where M is the - number of prims to set (either all prims if indices is None, or the number of indices provided). - indices: Indices of prims to set visibility for. Defaults to None, in which case visibility is set - for all prims in the view. - - Raises: - ValueError: If visibility shape is not (M,). - """ - # Resolve indices - if indices is None or indices == slice(None): - indices_list = self._ALL_INDICES - else: - indices_list = indices.tolist() if isinstance(indices, torch.Tensor) else list(indices) - - # Validate inputs - if visibility.shape != (len(indices_list),): - raise ValueError(f"Expected visibility shape ({len(indices_list)},), got {visibility.shape}.") - - # Set visibility for each prim - with Sdf.ChangeBlock(): - for idx, prim_idx in enumerate(indices_list): - # Convert prim to imageable - imageable = UsdGeom.Imageable(self._prims[prim_idx]) - # Set visibility - if visibility[idx]: - imageable.MakeVisible() - else: - imageable.MakeInvisible() - - """ - Operations - Getters. - """ - - def get_world_poses(self, indices: Sequence[int] | None = None) -> tuple[torch.Tensor, torch.Tensor]: - """Get world-space poses for prims in the view. - - This method retrieves the position and orientation of each prim in world space by computing - the full transform hierarchy from the prim to the world root. - - - When Fabric is enabled, the function uses Fabric batch operations with Warp kernels. - - When Fabric is disabled, the function uses USD XformCache. - - Note: - Scale and skew are ignored. The returned poses contain only translation and rotation. - - Args: - indices: Indices of prims to get poses for. Defaults to None, in which case poses are retrieved - for all prims in the view. - - Returns: - A tuple of (positions, orientations) where: - - - positions: Torch tensor of shape (M, 3) containing world-space positions (x, y, z), - where M is the number of prims queried. - - orientations: Torch tensor of shape (M, 4) containing world-space quaternions (w, x, y, z) - """ - if self._use_fabric: - return self._get_world_poses_fabric(indices) - else: - return self._get_world_poses_usd(indices) - - def get_local_poses(self, indices: Sequence[int] | None = None) -> tuple[torch.Tensor, torch.Tensor]: - """Get local-space poses for prims in the view. - - This method retrieves the position and orientation of each prim in local space (relative to - their parent prims). It reads directly from USD's ``xformOp:translate`` and ``xformOp:orient`` attributes. - - Note: - Even in Fabric mode, local pose operations use USD. This behavior is based on Isaac Sim's design - where Fabric is only used for world pose operations. - - Rationale: - - Local pose reads need correct parent-child hierarchy relationships - - USD maintains these relationships correctly and efficiently - - Fabric is optimized for world pose operations, not local hierarchies - - Note: - Scale is ignored. The returned poses contain only translation and rotation. - - Args: - indices: Indices of prims to get poses for. Defaults to None, in which case poses are retrieved - for all prims in the view. - - Returns: - A tuple of (translations, orientations) where: - - - translations: Torch tensor of shape (M, 3) containing local-space translations (x, y, z), - where M is the number of prims queried. - - orientations: Torch tensor of shape (M, 4) containing local-space quaternions (w, x, y, z) - """ - if self._use_fabric: - return self._get_local_poses_fabric(indices) - else: - return self._get_local_poses_usd(indices) - - def get_scales(self, indices: Sequence[int] | None = None) -> torch.Tensor: - """Get scales for prims in the view. - - This method retrieves the scale of each prim in the view. - - - When Fabric is enabled, the function extracts scales from Fabric matrices using batch operations with - Warp kernels. - - When Fabric is disabled, the function reads from USD's ``xformOp:scale`` attributes. + Converts the desired world pose to local-space relative to each prim's + parent before writing to USD xform ops. Args: - indices: Indices of prims to get scales for. Defaults to None, in which case scales are retrieved - for all prims in the view. - - Returns: - A tensor of shape (M, 3) containing the scales of each prim, where M is the number of prims queried. + positions: World-space positions of shape ``(M, 3)``. + orientations: World-space quaternions ``(w, x, y, z)`` of shape ``(M, 4)``. + indices: Indices of prims to set poses for. Defaults to None (all prims). """ - if self._use_fabric: - return self._get_scales_fabric(indices) - else: - return self._get_scales_usd(indices) - - def get_visibility(self, indices: Sequence[int] | None = None) -> torch.Tensor: - """Get visibility for prims in the view. + indices_list = self._resolve_indices(indices) - This method retrieves the visibility of each prim in the view. + positions_array = Vt.Vec3dArray.FromNumpy(self._to_numpy(positions)) if positions is not None else None + orientations_array = Vt.QuatdArray.FromNumpy(self._to_numpy(orientations)) if orientations is not None else None - Args: - indices: Indices of prims to get visibility for. Defaults to None, in which case visibility is retrieved - for all prims in the view. - - Returns: - A tensor of shape (M,) containing the visibility of each prim, where M is the number of prims queried. - The tensor is of type bool. - """ - # Resolve indices - if indices is None or indices == slice(None): - indices_list = self._ALL_INDICES - else: - # Convert to list if it is a tensor array - indices_list = indices.tolist() if isinstance(indices, torch.Tensor) else list(indices) - - # Create buffers - visibility = torch.zeros(len(indices_list), dtype=torch.bool, device=self._device) - - for idx, prim_idx in enumerate(indices_list): - # Get prim - imageable = UsdGeom.Imageable(self._prims[prim_idx]) - # Get visibility - visibility[idx] = imageable.ComputeVisibility() != UsdGeom.Tokens.invisible - - return visibility - - """ - Internal Functions - USD. - """ - - def _set_world_poses_usd( - self, - positions: torch.Tensor | None = None, - orientations: torch.Tensor | None = None, - indices: Sequence[int] | None = None, - ): - """Set world poses to USD.""" - # Resolve indices - if indices is None or indices == slice(None): - indices_list = self._ALL_INDICES - else: - # Convert to list if it is a tensor array - indices_list = indices.tolist() if isinstance(indices, torch.Tensor) else list(indices) - - # Validate inputs - if positions is not None: - if positions.shape != (len(indices_list), 3): - raise ValueError( - f"Expected positions shape ({len(indices_list)}, 3), got {positions.shape}. " - "Number of positions must match the number of prims in the view." - ) - positions_array = Vt.Vec3dArray.FromNumpy(positions.cpu().numpy()) - else: - positions_array = None - if orientations is not None: - if orientations.shape != (len(indices_list), 4): - raise ValueError( - f"Expected orientations shape ({len(indices_list)}, 4), got {orientations.shape}. " - "Number of orientations must match the number of prims in the view." - ) - # Vt expects quaternions in xyzw order - orientations_array = Vt.QuatdArray.FromNumpy(orientations.cpu().numpy()) - else: - orientations_array = None - - # Create xform cache instance xform_cache = UsdGeom.XformCache(Usd.TimeCode.Default()) - # Set poses for each prim - # We use Sdf.ChangeBlock to minimize notification overhead. with Sdf.ChangeBlock(): for idx, prim_idx in enumerate(indices_list): - # Get prim prim = self._prims[prim_idx] - # Get parent prim for local space conversion parent_prim = prim.GetParent() - # Determine what to set world_pos = positions_array[idx] if positions_array is not None else None world_quat = orientations_array[idx] if orientations_array is not None else None - # Convert world pose to local if we have a valid parent - # Note: We don't use :func:`isaaclab.sim.utils.transforms.convert_world_pose_to_local` - # here since it isn't optimized for batch operations. + # Convert world pose to local pose relative to parent if parent_prim.IsValid() and parent_prim.GetPath() != Sdf.Path.absoluteRootPath: - # Get current world pose if we're only setting one component if positions_array is None or orientations_array is None: - # get prim xform prim_tf = xform_cache.GetLocalToWorldTransform(prim) - # sanitize quaternion - # this is needed, otherwise the quaternion might be non-normalized prim_tf.Orthonormalize() - # populate desired world transform if world_pos is not None: prim_tf.SetTranslateOnly(world_pos) if world_quat is not None: prim_tf.SetRotateOnly(world_quat) else: - # Both position and orientation are provided, create new transform prim_tf = Gf.Matrix4d() prim_tf.SetTranslateOnly(world_pos) prim_tf.SetRotateOnly(world_quat) - # Convert to local space parent_world_tf = xform_cache.GetLocalToWorldTransform(parent_prim) local_tf = prim_tf * parent_world_tf.GetInverse() local_pos = local_tf.ExtractTranslation() local_quat = local_tf.ExtractRotationQuat() else: - # No parent or parent is root, world == local + # Root-level prim: world == local local_pos = world_pos local_quat = world_quat - # Get or create the standard transform operations if local_pos is not None: prim.GetAttribute("xformOp:translate").Set(local_pos) if local_quat is not None: prim.GetAttribute("xformOp:orient").Set(local_quat) - def _set_local_poses_usd( + def set_local_poses( self, - translations: torch.Tensor | None = None, - orientations: torch.Tensor | None = None, + translations: wp.array | None = None, + orientations: wp.array | None = None, indices: Sequence[int] | None = None, ): - """Set local poses to USD.""" - # Resolve indices - if indices is None or indices == slice(None): - indices_list = self._ALL_INDICES - else: - indices_list = indices.tolist() if isinstance(indices, torch.Tensor) else list(indices) - - # Validate inputs - if translations is not None: - if translations.shape != (len(indices_list), 3): - raise ValueError(f"Expected translations shape ({len(indices_list)}, 3), got {translations.shape}.") - translations_array = Vt.Vec3dArray.FromNumpy(translations.cpu().numpy()) - else: - translations_array = None - if orientations is not None: - if orientations.shape != (len(indices_list), 4): - raise ValueError(f"Expected orientations shape ({len(indices_list)}, 4), got {orientations.shape}.") - orientations_array = Vt.QuatdArray.FromNumpy(orientations.cpu().numpy()) - else: - orientations_array = None - - # Set local poses + """Set local-space poses for prims in the view. + + Args: + translations: Local-space translations of shape ``(M, 3)``. + orientations: Local-space quaternions ``(w, x, y, z)`` of shape ``(M, 4)``. + indices: Indices of prims to set poses for. Defaults to None (all prims). + """ + indices_list = self._resolve_indices(indices) + + translations_array = Vt.Vec3dArray.FromNumpy(self._to_numpy(translations)) if translations is not None else None + orientations_array = Vt.QuatdArray.FromNumpy(self._to_numpy(orientations)) if orientations is not None else None + with Sdf.ChangeBlock(): for idx, prim_idx in enumerate(indices_list): prim = self._prims[prim_idx] @@ -602,72 +215,85 @@ def _set_local_poses_usd( if orientations_array is not None: prim.GetAttribute("xformOp:orient").Set(orientations_array[idx]) - def _set_scales_usd(self, scales: torch.Tensor, indices: Sequence[int] | None = None): - """Set scales to USD.""" - # Resolve indices - if indices is None or indices == slice(None): - indices_list = self._ALL_INDICES - else: - indices_list = indices.tolist() if isinstance(indices, torch.Tensor) else list(indices) + def set_scales(self, scales: wp.array, indices: Sequence[int] | None = None): + """Set scales for prims in the view. - # Validate inputs - if scales.shape != (len(indices_list), 3): - raise ValueError(f"Expected scales shape ({len(indices_list)}, 3), got {scales.shape}.") + Args: + scales: Scales of shape ``(M, 3)``. + indices: Indices of prims to set scales for. Defaults to None (all prims). + """ + indices_list = self._resolve_indices(indices) + scales_array = Vt.Vec3dArray.FromNumpy(self._to_numpy(scales)) - scales_array = Vt.Vec3dArray.FromNumpy(scales.cpu().numpy()) - # Set scales for each prim with Sdf.ChangeBlock(): for idx, prim_idx in enumerate(indices_list): prim = self._prims[prim_idx] prim.GetAttribute("xformOp:scale").Set(scales_array[idx]) - def _get_world_poses_usd(self, indices: Sequence[int] | None = None) -> tuple[torch.Tensor, torch.Tensor]: - """Get world poses from USD.""" - # Resolve indices - if indices is None or indices == slice(None): - indices_list = self._ALL_INDICES - else: - # Convert to list if it is a tensor array - indices_list = indices.tolist() if isinstance(indices, torch.Tensor) else list(indices) + def set_visibility(self, visibility: torch.Tensor, indices: Sequence[int] | None = None): + """Set visibility for prims in the view. + + Args: + visibility: Visibility as a boolean tensor of shape ``(M,)``. + indices: Indices of prims to set visibility for. Defaults to None (all prims). + """ + indices_list = self._resolve_indices(indices) + + if visibility.shape != (len(indices_list),): + raise ValueError(f"Expected visibility shape ({len(indices_list)},), got {visibility.shape}.") + + with Sdf.ChangeBlock(): + for idx, prim_idx in enumerate(indices_list): + imageable = UsdGeom.Imageable(self._prims[prim_idx]) + if visibility[idx]: + imageable.MakeVisible() + else: + imageable.MakeInvisible() + + # ------------------------------------------------------------------ + # Getters + # ------------------------------------------------------------------ + + def get_world_poses(self, indices: Sequence[int] | None = None) -> tuple[wp.array, wp.array]: + """Get world-space poses for prims in the view. + + Args: + indices: Indices of prims to get poses for. Defaults to None (all prims). + + Returns: + A tuple of ``(positions, orientations)`` as ``wp.array``. + """ + indices_list = self._resolve_indices(indices) - # Create buffers positions = Vt.Vec3dArray(len(indices_list)) orientations = Vt.QuatdArray(len(indices_list)) - # Create xform cache instance xform_cache = UsdGeom.XformCache(Usd.TimeCode.Default()) - # Note: We don't use :func:`isaaclab.sim.utils.transforms.resolve_prim_pose` - # here since it isn't optimized for batch operations. for idx, prim_idx in enumerate(indices_list): - # Get prim prim = self._prims[prim_idx] - # get prim xform prim_tf = xform_cache.GetLocalToWorldTransform(prim) - # sanitize quaternion - # this is needed, otherwise the quaternion might be non-normalized prim_tf.Orthonormalize() - # extract position and orientation positions[idx] = prim_tf.ExtractTranslation() orientations[idx] = prim_tf.ExtractRotationQuat() - # move to torch tensors - positions = torch.tensor(np.array(positions), dtype=torch.float32, device=self._device) - orientations = torch.tensor(np.array(orientations), dtype=torch.float32, device=self._device) - return positions, orientations # type: ignore + return ( + wp.array(np.array(positions, dtype=np.float32), dtype=wp.float32, device=self._device), + wp.array(np.array(orientations, dtype=np.float32), dtype=wp.float32, device=self._device), + ) - def _get_local_poses_usd(self, indices: Sequence[int] | None = None) -> tuple[torch.Tensor, torch.Tensor]: - """Get local poses from USD.""" - # Resolve indices - if indices is None or indices == slice(None): - indices_list = self._ALL_INDICES - else: - indices_list = indices.tolist() if isinstance(indices, torch.Tensor) else list(indices) + def get_local_poses(self, indices: Sequence[int] | None = None) -> tuple[wp.array, wp.array]: + """Get local-space poses for prims in the view. + + Args: + indices: Indices of prims to get poses for. Defaults to None (all prims). + + Returns: + A tuple of ``(translations, orientations)`` as ``wp.array``. + """ + indices_list = self._resolve_indices(indices) - # Create buffers translations = Vt.Vec3dArray(len(indices_list)) orientations = Vt.QuatdArray(len(indices_list)) - - # Create a fresh XformCache to avoid stale cached values xform_cache = UsdGeom.XformCache(Usd.TimeCode.Default()) for idx, prim_idx in enumerate(indices_list): @@ -677,464 +303,61 @@ def _get_local_poses_usd(self, indices: Sequence[int] | None = None) -> tuple[to translations[idx] = prim_tf.ExtractTranslation() orientations[idx] = prim_tf.ExtractRotationQuat() - translations = torch.tensor(np.array(translations), dtype=torch.float32, device=self._device) - orientations = torch.tensor(np.array(orientations), dtype=torch.float32, device=self._device) - return translations, orientations # type: ignore + return ( + wp.array(np.array(translations, dtype=np.float32), dtype=wp.float32, device=self._device), + wp.array(np.array(orientations, dtype=np.float32), dtype=wp.float32, device=self._device), + ) - def _get_scales_usd(self, indices: Sequence[int] | None = None) -> torch.Tensor: - """Get scales from USD.""" - # Resolve indices - if indices is None or indices == slice(None): - indices_list = self._ALL_INDICES - else: - indices_list = indices.tolist() if isinstance(indices, torch.Tensor) else list(indices) + def get_scales(self, indices: Sequence[int] | None = None) -> wp.array: + """Get scales for prims in the view. - # Create buffers - scales = Vt.Vec3dArray(len(indices_list)) + Args: + indices: Indices of prims to get scales for. Defaults to None (all prims). + Returns: + A ``wp.array`` of shape ``(M, 3)``. + """ + indices_list = self._resolve_indices(indices) + + scales = Vt.Vec3dArray(len(indices_list)) for idx, prim_idx in enumerate(indices_list): prim = self._prims[prim_idx] scales[idx] = prim.GetAttribute("xformOp:scale").Get() - # Convert to tensor - return torch.tensor(np.array(scales), dtype=torch.float32, device=self._device) - - """ - Internal Functions - Fabric. - """ - - def _set_world_poses_fabric( - self, - positions: torch.Tensor | None = None, - orientations: torch.Tensor | None = None, - indices: Sequence[int] | None = None, - ): - """Set world poses using Fabric GPU batch operations. - - Writes directly to Fabric's ``omni:fabric:worldMatrix`` attribute using Warp kernels. - Changes are propagated through Fabric's hierarchy system but remain GPU-resident. + return wp.array(np.array(scales, dtype=np.float32), dtype=wp.float32, device=self._device) - For workflows mixing Fabric world pose writes with USD local pose queries, note - that local poses read from USD's xformOp:* attributes, which may not immediately - reflect Fabric changes. For best performance and consistency, use Fabric methods - exclusively (get_world_poses/set_world_poses with Fabric enabled). - """ - # Lazy initialization - if not self._fabric_initialized: - self._initialize_fabric() - - # Resolve indices (treat slice(None) as None for consistency with USD path) - indices_wp = self._resolve_indices_wp(indices) - - count = indices_wp.shape[0] - - # Convert torch to warp (if provided), use dummy arrays for None to avoid Warp kernel issues - if positions is not None: - positions_wp = wp.from_torch(positions) - else: - positions_wp = wp.zeros((0, 3), dtype=wp.float32).to(self._device) - - if orientations is not None: - orientations_wp = wp.from_torch(orientations) - else: - orientations_wp = wp.zeros((0, 4), dtype=wp.float32).to(self._device) - - # Dummy array for scales (not modifying) - scales_wp = wp.zeros((0, 3), dtype=wp.float32).to(self._device) - - # Use cached fabricarray for world matrices - world_matrices = self._fabric_world_matrices - - # Batch compose matrices with a single kernel launch - # Note: world_matrices is a fabricarray on fabric_device, so we must launch on fabric_device - wp.launch( - kernel=fabric_utils.compose_fabric_transformation_matrix_from_warp_arrays, - dim=count, - inputs=[ - world_matrices, - positions_wp, - orientations_wp, - scales_wp, # dummy array instead of None - False, # broadcast_positions - False, # broadcast_orientations - False, # broadcast_scales - indices_wp, - self._view_to_fabric, - ], - device=self._fabric_device, - ) - - # Synchronize to ensure kernel completes - wp.synchronize() - - # Update world transforms within Fabric hierarchy - self._fabric_hierarchy.update_world_xforms() - # Fabric now has authoritative data; skip future USD syncs - self._fabric_usd_sync_done = True - # Mirror to USD for renderer-facing prims when enabled. - if self._sync_usd_on_fabric_write: - self._set_world_poses_usd(positions, orientations, indices) - - # Fabric writes are GPU-resident; local pose operations still use USD. - - def _set_local_poses_fabric( - self, - translations: torch.Tensor | None = None, - orientations: torch.Tensor | None = None, - indices: Sequence[int] | None = None, - ): - """Set local poses using USD (matches Isaac Sim's design). - - Note: Even in Fabric mode, local pose operations use USD. - This is Isaac Sim's design: the ``usd=False`` parameter only affects world poses. - - Rationale: - - Local pose writes need correct parent-child hierarchy relationships - - USD maintains these relationships correctly and efficiently - - Fabric is optimized for world pose operations, not local hierarchies - """ - self._set_local_poses_usd(translations, orientations, indices) - - def _set_scales_fabric(self, scales: torch.Tensor, indices: Sequence[int] | None = None): - """Set scales using Fabric GPU batch operations.""" - # Lazy initialization - if not self._fabric_initialized: - self._initialize_fabric() - - # Resolve indices (treat slice(None) as None for consistency with USD path) - indices_wp = self._resolve_indices_wp(indices) - - count = indices_wp.shape[0] - - # Convert torch to warp - scales_wp = wp.from_torch(scales) - - # Dummy arrays for positions and orientations (not modifying) - positions_wp = wp.zeros((0, 3), dtype=wp.float32).to(self._device) - orientations_wp = wp.zeros((0, 4), dtype=wp.float32).to(self._device) - - # Use cached fabricarray for world matrices - world_matrices = self._fabric_world_matrices - - # Batch compose matrices on GPU with a single kernel launch - # Note: world_matrices is a fabricarray on fabric_device, so we must launch on fabric_device - wp.launch( - kernel=fabric_utils.compose_fabric_transformation_matrix_from_warp_arrays, - dim=count, - inputs=[ - world_matrices, - positions_wp, # dummy array instead of None - orientations_wp, # dummy array instead of None - scales_wp, - False, # broadcast_positions - False, # broadcast_orientations - False, # broadcast_scales - indices_wp, - self._view_to_fabric, - ], - device=self._fabric_device, - ) + def get_visibility(self, indices: Sequence[int] | None = None) -> torch.Tensor: + """Get visibility for prims in the view. - # Synchronize to ensure kernel completes before syncing - wp.synchronize() - - # Update world transforms to propagate changes - self._fabric_hierarchy.update_world_xforms() - # Fabric now has authoritative data; skip future USD syncs - self._fabric_usd_sync_done = True - # Mirror to USD for renderer-facing prims when enabled. - if self._sync_usd_on_fabric_write: - self._set_scales_usd(scales, indices) - - def _get_world_poses_fabric(self, indices: Sequence[int] | None = None) -> tuple[torch.Tensor, torch.Tensor]: - """Get world poses from Fabric using GPU batch operations.""" - # Lazy initialization of Fabric infrastructure - if not self._fabric_initialized: - self._initialize_fabric() - # Sync once from USD to ensure reads see the latest authored transforms - if not self._fabric_usd_sync_done: - self._sync_fabric_from_usd_once() - - # Resolve indices (treat slice(None) as None for consistency with USD path) - indices_wp = self._resolve_indices_wp(indices) - - count = indices_wp.shape[0] - - # Use pre-allocated buffers for full reads, allocate only for partial reads - use_cached_buffers = indices is None or indices == slice(None) - if use_cached_buffers: - # Full read: Use cached buffers (zero allocation overhead!) - positions_wp = self._fabric_positions_buffer - orientations_wp = self._fabric_orientations_buffer - scales_wp = self._fabric_dummy_buffer - else: - # Partial read: Need to allocate buffers of appropriate size - positions_wp = wp.zeros((count, 3), dtype=wp.float32).to(self._device) - orientations_wp = wp.zeros((count, 4), dtype=wp.float32).to(self._device) - scales_wp = self._fabric_dummy_buffer # Always use dummy for scales - - # Use cached fabricarray for world matrices - # This eliminates the 0.06-0.30ms variability from creating fabricarray each call - world_matrices = self._fabric_world_matrices - - # Launch GPU kernel to decompose matrices in parallel - # Note: world_matrices is a fabricarray on fabric_device, so we must launch on fabric_device - wp.launch( - kernel=fabric_utils.decompose_fabric_transformation_matrix_to_warp_arrays, - dim=count, - inputs=[ - world_matrices, - positions_wp, - orientations_wp, - scales_wp, # dummy array instead of None - indices_wp, - self._view_to_fabric, - ], - device=self._fabric_device, - ) + Args: + indices: Indices of prims to get visibility for. Defaults to None (all prims). - # Return tensors: zero-copy for cached buffers, conversion for partial reads - if use_cached_buffers: - # Zero-copy! The Warp kernel wrote directly into the PyTorch tensors - # We just need to synchronize to ensure the kernel is done - wp.synchronize() - return self._fabric_positions_torch, self._fabric_orientations_torch - else: - # Partial read: Need to convert from Warp to torch - positions = wp.to_torch(positions_wp) - orientations = wp.to_torch(orientations_wp) - return positions, orientations - - def _get_local_poses_fabric(self, indices: Sequence[int] | None = None) -> tuple[torch.Tensor, torch.Tensor]: - """Get local poses using USD (matches Isaac Sim's design). - - Note: - Even in Fabric mode, local pose operations use USD's XformCache. - This is Isaac Sim's design: the ``usd=False`` parameter only affects world poses. - - Rationale: - - Local pose computation requires parent transforms which may not be in the view - - USD's XformCache provides efficient hierarchy-aware local transform queries - - Fabric is optimized for world pose operations, not local hierarchies + Returns: + A tensor of shape ``(M,)`` containing the visibility of each prim (bool). """ - return self._get_local_poses_usd(indices) - - def _get_scales_fabric(self, indices: Sequence[int] | None = None) -> torch.Tensor: - """Get scales from Fabric using GPU batch operations.""" - # Lazy initialization - if not self._fabric_initialized: - self._initialize_fabric() - # Sync once from USD to ensure reads see the latest authored transforms - if not self._fabric_usd_sync_done: - self._sync_fabric_from_usd_once() - - # Resolve indices (treat slice(None) as None for consistency with USD path) - indices_wp = self._resolve_indices_wp(indices) - - count = indices_wp.shape[0] - - # Use pre-allocated buffers for full reads, allocate only for partial reads - use_cached_buffers = indices is None or indices == slice(None) - if use_cached_buffers: - # Full read: Use cached buffers (zero allocation overhead!) - scales_wp = self._fabric_scales_buffer - else: - # Partial read: Need to allocate buffer of appropriate size - scales_wp = wp.zeros((count, 3), dtype=wp.float32).to(self._device) - - # Always use dummy buffers for positions and orientations (not needed for scales) - positions_wp = self._fabric_dummy_buffer - orientations_wp = self._fabric_dummy_buffer - - # Use cached fabricarray for world matrices - world_matrices = self._fabric_world_matrices - - # Launch GPU kernel to decompose matrices in parallel - # Note: world_matrices is a fabricarray on fabric_device, so we must launch on fabric_device - wp.launch( - kernel=fabric_utils.decompose_fabric_transformation_matrix_to_warp_arrays, - dim=count, - inputs=[ - world_matrices, - positions_wp, # dummy array instead of None - orientations_wp, # dummy array instead of None - scales_wp, - indices_wp, - self._view_to_fabric, - ], - device=self._fabric_device, - ) - - # Return tensor: zero-copy for cached buffers, conversion for partial reads - if use_cached_buffers: - # Zero-copy! The Warp kernel wrote directly into the PyTorch tensor - wp.synchronize() - return self._fabric_scales_torch - else: - # Partial read: Need to convert from Warp to torch - return wp.to_torch(scales_wp) + indices_list = self._resolve_indices(indices) - """ - Internal Functions - Initialization. - """ - - def _initialize_fabric(self) -> None: - """Initialize Fabric batch infrastructure for GPU-accelerated pose queries. - - This method ensures all prims have the required Fabric hierarchy attributes - (``omni:fabric:localMatrix`` and ``omni:fabric:worldMatrix``) and creates the necessary - infrastructure for batch GPU operations using Warp. + visibility = torch.zeros(len(indices_list), dtype=torch.bool, device=self._device) + for idx, prim_idx in enumerate(indices_list): + imageable = UsdGeom.Imageable(self._prims[prim_idx]) + visibility[idx] = imageable.ComputeVisibility() != UsdGeom.Tokens.invisible + return visibility - Based on the Fabric Hierarchy documentation, when Fabric Scene Delegate is enabled, - all boundable prims should have these attributes. This method ensures they exist - and are properly synchronized with USD. - """ - import usdrt - from usdrt import Rt - - # Get USDRT (Fabric) stage - stage_id = sim_utils.get_current_stage_id() - fabric_stage = usdrt.Usd.Stage.Attach(stage_id) - - # Step 1: Ensure all prims have Fabric hierarchy attributes - # According to the documentation, these attributes are created automatically - # when Fabric Scene Delegate is enabled, but we ensure they exist - for i in range(self.count): - rt_prim = fabric_stage.GetPrimAtPath(self.prim_paths[i]) - rt_xformable = Rt.Xformable(rt_prim) - - # Create Fabric hierarchy world matrix attribute if it doesn't exist - has_attr = ( - rt_xformable.HasFabricHierarchyWorldMatrixAttr() - if hasattr(rt_xformable, "HasFabricHierarchyWorldMatrixAttr") - else False - ) - if not has_attr: - rt_xformable.CreateFabricHierarchyWorldMatrixAttr() - - # Best-effort USD->Fabric sync; authoritative initialization happens on first read. - rt_xformable.SetWorldXformFromUsd() - - # Create view index attribute for batch operations - rt_prim.CreateAttribute(self._view_index_attr, usdrt.Sdf.ValueTypeNames.UInt, custom=True) - rt_prim.GetAttribute(self._view_index_attr).Set(i) - - # After syncing all prims, update the Fabric hierarchy to ensure world matrices are computed - self._fabric_hierarchy = usdrt.hierarchy.IFabricHierarchy().get_fabric_hierarchy( - fabric_stage.GetFabricId(), fabric_stage.GetStageIdAsStageId() - ) - self._fabric_hierarchy.update_world_xforms() - - # Step 2: Create index arrays for batch operations - self._default_view_indices = wp.zeros((self.count,), dtype=wp.uint32).to(self._device) - wp.launch( - kernel=fabric_utils.arange_k, - dim=self.count, - inputs=[self._default_view_indices], - device=self._device, - ) - wp.synchronize() # Ensure indices are ready - - # Step 3: Create Fabric selection with attribute filtering - # SelectPrims expects device format like "cuda:0" not "cuda" - # - # KNOWN ISSUE: SelectPrims may return prims in a different order than self._prims - # (which comes from USD's find_matching_prims). We create a bidirectional mapping - # (_view_to_fabric and _fabric_to_view) to handle this ordering difference. - # This works correctly for full-view operations but partial indexing still has issues. - # - # NOTE: SelectPrims only supports "cuda:0" regardless of which GPU the simulation - # is running on. In multi-GPU setups, we must use "cuda:0" for SelectPrims even if - # the simulation device is "cuda:1" or higher. - fabric_device = self._device - if self._device == "cuda": - logger.warning("Fabric device is not specified, defaulting to 'cuda:0'.") - fabric_device = "cuda:0" - elif self._device.startswith("cuda:"): - # SelectPrims only supports cuda:0, so we always use cuda:0 for SelectPrims - # even if the simulation is running on a different GPU - if self._device != "cuda:0": - logger.debug( - f"SelectPrims only supports cuda:0. Using cuda:0 for SelectPrims " - f"even though simulation device is {self._device}." - ) - fabric_device = "cuda:0" - - self._fabric_selection = fabric_stage.SelectPrims( - require_attrs=[ - (usdrt.Sdf.ValueTypeNames.UInt, self._view_index_attr, usdrt.Usd.Access.Read), - (usdrt.Sdf.ValueTypeNames.Matrix4d, "omni:fabric:worldMatrix", usdrt.Usd.Access.ReadWrite), - ], - device=fabric_device, - ) + # ------------------------------------------------------------------ + # Helpers + # ------------------------------------------------------------------ - # Step 4: Create bidirectional mapping between view and fabric indices - # Note: fabric_to_view is tied to fabric_device (cuda:0) because it's created from SelectPrims. - # view_to_fabric must also be on fabric_device since it's always used with fabricarrays in kernels. - self._view_to_fabric = wp.zeros((self.count,), dtype=wp.uint32).to(fabric_device) - self._fabric_to_view = wp.fabricarray(self._fabric_selection, self._view_index_attr) - - wp.launch( - kernel=fabric_utils.set_view_to_fabric_array, - dim=self._fabric_to_view.shape[0], - inputs=[self._fabric_to_view, self._view_to_fabric], - device=fabric_device, - ) - # Synchronize to ensure mapping is ready before any operations - wp.synchronize() - - # Pre-allocate reusable output buffers for read operations - self._fabric_positions_torch = torch.zeros((self.count, 3), dtype=torch.float32, device=self._device) - self._fabric_orientations_torch = torch.zeros((self.count, 4), dtype=torch.float32, device=self._device) - self._fabric_scales_torch = torch.zeros((self.count, 3), dtype=torch.float32, device=self._device) - - # Create Warp views of the PyTorch tensors - self._fabric_positions_buffer = wp.from_torch(self._fabric_positions_torch, dtype=wp.float32) - self._fabric_orientations_buffer = wp.from_torch(self._fabric_orientations_torch, dtype=wp.float32) - self._fabric_scales_buffer = wp.from_torch(self._fabric_scales_torch, dtype=wp.float32) - - # Dummy array for unused outputs (always empty) - self._fabric_dummy_buffer = wp.zeros((0, 3), dtype=wp.float32).to(self._device) - - # Cache fabricarray for world matrices to avoid recreation overhead - # Refs: https://docs.omniverse.nvidia.com/kit/docs/usdrt/latest/docs/usdrt_prim_selection.html - # https://docs.omniverse.nvidia.com/kit/docs/usdrt/latest/docs/scenegraph_use.html - self._fabric_world_matrices = wp.fabricarray(self._fabric_selection, "omni:fabric:worldMatrix") - - # Cache Fabric stage to avoid expensive get_current_stage() calls - self._fabric_stage = fabric_stage - - # Store fabric_device for use in kernel launches that involve fabricarrays - self._fabric_device = fabric_device - - self._fabric_initialized = True - # Force a one-time USD->Fabric sync on first read to pick up any USD edits - # made after the view was constructed. - self._fabric_usd_sync_done = False - - def _sync_fabric_from_usd_once(self) -> None: - """Sync Fabric world matrices from USD once, on the first read.""" - # Ensure Fabric is initialized - if not self._fabric_initialized: - self._initialize_fabric() - - # Read authoritative transforms from USD and write once into Fabric. - positions_usd, orientations_usd = self._get_world_poses_usd() - scales_usd = self._get_scales_usd() - - prev_sync = self._sync_usd_on_fabric_write - self._sync_usd_on_fabric_write = False - self._set_world_poses_fabric(positions_usd, orientations_usd) - self._set_scales_fabric(scales_usd) - self._sync_usd_on_fabric_write = prev_sync - - self._fabric_usd_sync_done = True - - def _resolve_indices_wp(self, indices: Sequence[int] | None) -> wp.array: - """Resolve view indices as a Warp array.""" + def _resolve_indices(self, indices: Sequence[int] | None) -> list[int]: + """Resolve indices to a list, defaulting to all prims.""" if indices is None or indices == slice(None): - if self._default_view_indices is None: - raise RuntimeError("Fabric indices are not initialized.") - return self._default_view_indices - indices_list = indices.tolist() if isinstance(indices, torch.Tensor) else list(indices) - return wp.array(indices_list, dtype=wp.uint32).to(self._device) + return self._ALL_INDICES + if isinstance(indices, torch.Tensor): + return indices.tolist() + return list(indices) + + @staticmethod + def _to_numpy(data: wp.array | torch.Tensor) -> np.ndarray: + """Convert a ``wp.array`` or ``torch.Tensor`` to a numpy array on CPU.""" + if isinstance(data, wp.array): + return data.numpy() + return data.cpu().numpy() diff --git a/source/isaaclab/isaaclab/sim/views/xform_prim_view_factory.py b/source/isaaclab/isaaclab/sim/views/xform_prim_view_factory.py new file mode 100644 index 000000000000..57b673d57952 --- /dev/null +++ b/source/isaaclab/isaaclab/sim/views/xform_prim_view_factory.py @@ -0,0 +1,44 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Factory for creating backend-specific XformPrimView instances.""" + +from __future__ import annotations + +from isaaclab.utils.backend_utils import FactoryBase + +from .base_xform_prim_view import BaseXformPrimView + + +class XformPrimViewFactory(FactoryBase, BaseXformPrimView): + """Factory that selects a :class:`BaseXformPrimView` implementation based on the active physics backend. + + - **PhysX / no backend**: returns the USD/Fabric-based + :class:`~isaaclab.sim.views.XformPrimView`. + - **Newton**: returns the GPU-state-backed + :class:`~isaaclab_newton.sim.views.XformPrimView`. + + Use this in place of constructing an ``XformPrimView`` directly when the + caller is physics-aware (e.g. ray-cast sensors) and wants the fastest + available implementation for the active backend. + """ + + _backend_class_names = {"physx": "XformPrimView", "newton": "XformPrimView"} + + @classmethod + def _get_backend(cls, *args, **kwargs) -> str: + from isaaclab.sim.simulation_context import SimulationContext # noqa: PLC0415 + + ctx = SimulationContext.instance() + if ctx is None: + return "physx" + manager_name = ctx.physics_manager.__name__.lower() + if "newton" in manager_name: + return "newton" + return "physx" + + def __new__(cls, *args, **kwargs) -> BaseXformPrimView: + """Create a new XformPrimView for the active physics backend.""" + return super().__new__(cls, *args, **kwargs) diff --git a/source/isaaclab/test/sim/test_views_xform_prim.py b/source/isaaclab/test/sim/test_views_xform_prim.py index 3de7a0b357a2..640631653304 100644 --- a/source/isaaclab/test/sim/test_views_xform_prim.py +++ b/source/isaaclab/test/sim/test_views_xform_prim.py @@ -14,6 +14,7 @@ import pytest # noqa: E402 import torch # noqa: E402 +import warp as wp # noqa: E402 try: from isaacsim.core.prims import XFormPrim as _IsaacSimXformPrimView @@ -254,7 +255,7 @@ def test_xform_prim_view_initialization_empty_pattern(device): @pytest.mark.parametrize("device", ["cpu", "cuda"]) -@pytest.mark.parametrize("backend", ["usd", "fabric"]) +@pytest.mark.parametrize("backend", ["usd"]) def test_get_world_poses(device, backend): """Test getting world poses from XformPrimView.""" _skip_if_backend_unavailable(backend, device) @@ -278,6 +279,7 @@ def test_get_world_poses(device, backend): # Get world poses positions, orientations = view.get_world_poses() + positions, orientations = wp.to_torch(positions), wp.to_torch(orientations) # Verify shapes assert positions.shape == (3, 3) @@ -294,7 +296,7 @@ def test_get_world_poses(device, backend): @pytest.mark.parametrize("device", ["cpu", "cuda"]) -@pytest.mark.parametrize("backend", ["usd", "fabric"]) +@pytest.mark.parametrize("backend", ["usd"]) def test_get_local_poses(device, backend): """Test getting local poses from XformPrimView.""" _skip_if_backend_unavailable(backend, device) @@ -321,6 +323,7 @@ def test_get_local_poses(device, backend): # Get local poses translations, orientations = view.get_local_poses() + translations, orientations = wp.to_torch(translations), wp.to_torch(orientations) # Verify shapes assert translations.shape == (3, 3) @@ -341,7 +344,7 @@ def test_get_local_poses(device, backend): @pytest.mark.parametrize("device", ["cpu", "cuda"]) -@pytest.mark.parametrize("backend", ["usd", "fabric"]) +@pytest.mark.parametrize("backend", ["usd"]) def test_get_scales(device, backend): """Test getting scales from XformPrimView.""" _skip_if_backend_unavailable(backend, device) @@ -362,6 +365,7 @@ def test_get_scales(device, backend): # Get scales scales = view.get_scales() + scales = wp.to_torch(scales) # Verify shape and values assert scales.shape == (3, 3) @@ -399,7 +403,7 @@ def test_get_visibility(device): @pytest.mark.parametrize("device", ["cpu", "cuda"]) -@pytest.mark.parametrize("backend", ["usd", "fabric"]) +@pytest.mark.parametrize("backend", ["usd"]) def test_set_world_poses(device, backend): """Test setting world poses in XformPrimView.""" _skip_if_backend_unavailable(backend, device) @@ -434,6 +438,7 @@ def test_set_world_poses(device, backend): # Get the poses back retrieved_positions, retrieved_orientations = view.get_world_poses() + retrieved_positions, retrieved_orientations = wp.to_torch(retrieved_positions), wp.to_torch(retrieved_orientations) # Verify they match torch.testing.assert_close(retrieved_positions, new_positions, atol=1e-5, rtol=0) @@ -445,7 +450,7 @@ def test_set_world_poses(device, backend): @pytest.mark.parametrize("device", ["cpu", "cuda"]) -@pytest.mark.parametrize("backend", ["usd", "fabric"]) +@pytest.mark.parametrize("backend", ["usd"]) def test_set_world_poses_only_positions(device, backend): """Test setting only positions, leaving orientations unchanged.""" _skip_if_backend_unavailable(backend, device) @@ -465,6 +470,7 @@ def test_set_world_poses_only_positions(device, backend): # Get initial orientations _, initial_orientations = view.get_world_poses() + initial_orientations = wp.to_torch(initial_orientations) # Set only positions new_positions = torch.tensor([[1.0, 0.0, 0.0], [0.0, 2.0, 0.0], [0.0, 0.0, 3.0]], device=device) @@ -472,6 +478,7 @@ def test_set_world_poses_only_positions(device, backend): # Get poses back retrieved_positions, retrieved_orientations = view.get_world_poses() + retrieved_positions, retrieved_orientations = wp.to_torch(retrieved_positions), wp.to_torch(retrieved_orientations) # Positions should be updated torch.testing.assert_close(retrieved_positions, new_positions, atol=1e-5, rtol=0) @@ -484,7 +491,7 @@ def test_set_world_poses_only_positions(device, backend): @pytest.mark.parametrize("device", ["cpu", "cuda"]) -@pytest.mark.parametrize("backend", ["usd", "fabric"]) +@pytest.mark.parametrize("backend", ["usd"]) def test_set_world_poses_only_orientations(device, backend): """Test setting only orientations, leaving positions unchanged.""" _skip_if_backend_unavailable(backend, device) @@ -501,6 +508,7 @@ def test_set_world_poses_only_orientations(device, backend): # Get initial positions initial_positions, _ = view.get_world_poses() + initial_positions = wp.to_torch(initial_positions) # Set only orientations new_orientations = torch.tensor( @@ -511,6 +519,7 @@ def test_set_world_poses_only_orientations(device, backend): # Get poses back retrieved_positions, retrieved_orientations = view.get_world_poses() + retrieved_positions, retrieved_orientations = wp.to_torch(retrieved_positions), wp.to_torch(retrieved_orientations) # Positions should be unchanged torch.testing.assert_close(retrieved_positions, initial_positions, atol=1e-5, rtol=0) @@ -523,7 +532,7 @@ def test_set_world_poses_only_orientations(device, backend): @pytest.mark.parametrize("device", ["cpu", "cuda"]) -@pytest.mark.parametrize("backend", ["usd", "fabric"]) +@pytest.mark.parametrize("backend", ["usd"]) def test_set_world_poses_with_hierarchy(device, backend): """Test setting world poses correctly handles parent transformations.""" _skip_if_backend_unavailable(backend, device) @@ -554,6 +563,7 @@ def test_set_world_poses_with_hierarchy(device, backend): # Get world poses back retrieved_positions, retrieved_orientations = view.get_world_poses() + retrieved_positions, retrieved_orientations = wp.to_torch(retrieved_positions), wp.to_torch(retrieved_orientations) # Should match desired world poses torch.testing.assert_close(retrieved_positions, desired_world_positions, atol=1e-4, rtol=0) @@ -564,7 +574,7 @@ def test_set_world_poses_with_hierarchy(device, backend): @pytest.mark.parametrize("device", ["cpu", "cuda"]) -@pytest.mark.parametrize("backend", ["usd", "fabric"]) +@pytest.mark.parametrize("backend", ["usd"]) def test_set_local_poses(device, backend): """Test setting local poses in XformPrimView.""" _skip_if_backend_unavailable(backend, device) @@ -599,6 +609,10 @@ def test_set_local_poses(device, backend): # Get local poses back retrieved_translations, retrieved_orientations = view.get_local_poses() + retrieved_translations, retrieved_orientations = ( + wp.to_torch(retrieved_translations), + wp.to_torch(retrieved_orientations), + ) # Verify they match torch.testing.assert_close(retrieved_translations, new_translations, atol=1e-5, rtol=0) @@ -609,7 +623,7 @@ def test_set_local_poses(device, backend): @pytest.mark.parametrize("device", ["cpu", "cuda"]) -@pytest.mark.parametrize("backend", ["usd", "fabric"]) +@pytest.mark.parametrize("backend", ["usd"]) def test_set_local_poses_only_translations(device, backend): """Test setting only local translations.""" _skip_if_backend_unavailable(backend, device) @@ -635,6 +649,7 @@ def test_set_local_poses_only_translations(device, backend): # Get initial orientations _, initial_orientations = view.get_local_poses() + initial_orientations = wp.to_torch(initial_orientations) # Set only translations new_translations = torch.tensor([[1.0, 0.0, 0.0], [0.0, 2.0, 0.0], [0.0, 0.0, 3.0]], device=device) @@ -642,6 +657,10 @@ def test_set_local_poses_only_translations(device, backend): # Get poses back retrieved_translations, retrieved_orientations = view.get_local_poses() + retrieved_translations, retrieved_orientations = ( + wp.to_torch(retrieved_translations), + wp.to_torch(retrieved_orientations), + ) # Translations should be updated torch.testing.assert_close(retrieved_translations, new_translations, atol=1e-5, rtol=0) @@ -654,7 +673,7 @@ def test_set_local_poses_only_translations(device, backend): @pytest.mark.parametrize("device", ["cpu", "cuda"]) -@pytest.mark.parametrize("backend", ["usd", "fabric"]) +@pytest.mark.parametrize("backend", ["usd"]) def test_set_scales(device, backend): """Test setting scales in XformPrimView.""" _skip_if_backend_unavailable(backend, device) @@ -679,6 +698,7 @@ def test_set_scales(device, backend): # Get scales back retrieved_scales = view.get_scales() + retrieved_scales = wp.to_torch(retrieved_scales) # Verify they match torch.testing.assert_close(retrieved_scales, new_scales, atol=1e-5, rtol=0) @@ -760,10 +780,13 @@ def test_index_types_get_methods(device, index_type, method): # Get all data as reference if method == "world_poses": all_data1, all_data2 = view.get_world_poses() + all_data1, all_data2 = wp.to_torch(all_data1), wp.to_torch(all_data2) elif method == "local_poses": all_data1, all_data2 = view.get_local_poses() + all_data1, all_data2 = wp.to_torch(all_data1), wp.to_torch(all_data2) elif method == "scales": all_data1 = view.get_scales() + all_data1 = wp.to_torch(all_data1) all_data2 = None else: # visibility all_data1 = view.get_visibility() @@ -776,10 +799,13 @@ def test_index_types_get_methods(device, index_type, method): # Get subset if method == "world_poses": subset_data1, subset_data2 = view.get_world_poses(indices=indices) # type: ignore[arg-type] + subset_data1, subset_data2 = wp.to_torch(subset_data1), wp.to_torch(subset_data2) elif method == "local_poses": subset_data1, subset_data2 = view.get_local_poses(indices=indices) # type: ignore[arg-type] + subset_data1, subset_data2 = wp.to_torch(subset_data1), wp.to_torch(subset_data2) elif method == "scales": subset_data1 = view.get_scales(indices=indices) # type: ignore[arg-type] + subset_data1 = wp.to_torch(subset_data1) subset_data2 = None else: # visibility subset_data1 = view.get_visibility(indices=indices) # type: ignore[arg-type] @@ -827,10 +853,13 @@ def test_index_types_set_methods(device, index_type, method): # Get initial data if method == "world_poses": initial_data1, initial_data2 = view.get_world_poses() + initial_data1, initial_data2 = wp.to_torch(initial_data1), wp.to_torch(initial_data2) elif method == "local_poses": initial_data1, initial_data2 = view.get_local_poses() + initial_data1, initial_data2 = wp.to_torch(initial_data1), wp.to_torch(initial_data2) elif method == "scales": initial_data1 = view.get_scales() + initial_data1 = wp.to_torch(initial_data1) initial_data2 = None else: # visibility initial_data1 = view.get_visibility() @@ -866,10 +895,13 @@ def test_index_types_set_methods(device, index_type, method): # Get all data after update if method == "world_poses": updated_data1, updated_data2 = view.get_world_poses() + updated_data1, updated_data2 = wp.to_torch(updated_data1), wp.to_torch(updated_data2) elif method == "local_poses": updated_data1, updated_data2 = view.get_local_poses() + updated_data1, updated_data2 = wp.to_torch(updated_data1), wp.to_torch(updated_data2) elif method == "scales": updated_data1 = view.get_scales() + updated_data1 = wp.to_torch(updated_data1) updated_data2 = None else: # visibility updated_data1 = view.get_visibility() @@ -899,7 +931,7 @@ def test_index_types_set_methods(device, index_type, method): @pytest.mark.parametrize("device", ["cpu", "cuda"]) -@pytest.mark.parametrize("backend", ["usd", "fabric"]) +@pytest.mark.parametrize("backend", ["usd"]) def test_indices_single_element(device, backend): """Test with a single index.""" _skip_if_backend_unavailable(backend, device) @@ -918,6 +950,7 @@ def test_indices_single_element(device, backend): # Test with single index indices = [3] positions, orientations = view.get_world_poses(indices=indices) + positions, orientations = wp.to_torch(positions), wp.to_torch(orientations) # Verify shapes assert positions.shape == (1, 3) @@ -928,12 +961,13 @@ def test_indices_single_element(device, backend): view.set_world_poses(positions=new_position, indices=indices) # Verify it was set - retrieved_positions, _ = view.get_world_poses(indices=indices) + retrieved_positions, _quat = view.get_world_poses(indices=indices) + retrieved_positions, _quat = wp.to_torch(retrieved_positions), wp.to_torch(_quat) torch.testing.assert_close(retrieved_positions, new_position, atol=1e-5, rtol=0) @pytest.mark.parametrize("device", ["cpu", "cuda"]) -@pytest.mark.parametrize("backend", ["usd", "fabric"]) +@pytest.mark.parametrize("backend", ["usd"]) def test_indices_out_of_order(device, backend): """Test with indices provided in non-sequential order.""" _skip_if_backend_unavailable(backend, device) @@ -959,7 +993,8 @@ def test_indices_out_of_order(device, backend): view.set_world_poses(positions=new_positions, indices=indices) # Get all poses - all_positions, _ = view.get_world_poses() + all_positions, _quat = view.get_world_poses() + all_positions, _quat = wp.to_torch(all_positions), wp.to_torch(_quat) # Verify each index got the correct value expected_x_values = [0.0, 0.0, 2.0, 0.0, 0.0, 5.0, 0.0, 7.0, 0.0, 9.0] @@ -968,7 +1003,7 @@ def test_indices_out_of_order(device, backend): @pytest.mark.parametrize("device", ["cpu", "cuda"]) -@pytest.mark.parametrize("backend", ["usd", "fabric"]) +@pytest.mark.parametrize("backend", ["usd"]) def test_indices_with_only_positions_or_orientations(device, backend): """Test indices work correctly when setting only positions or only orientations.""" _skip_if_backend_unavailable(backend, device) @@ -992,6 +1027,7 @@ def test_indices_with_only_positions_or_orientations(device, backend): # Get initial poses initial_positions, initial_orientations = view.get_world_poses() + initial_positions, initial_orientations = wp.to_torch(initial_positions), wp.to_torch(initial_orientations) # Set only positions for specific indices indices = [1, 3] @@ -1000,6 +1036,7 @@ def test_indices_with_only_positions_or_orientations(device, backend): # Get updated poses updated_positions, updated_orientations = view.get_world_poses() + updated_positions, updated_orientations = wp.to_torch(updated_positions), wp.to_torch(updated_orientations) # Verify positions updated for indices 1 and 3, others unchanged torch.testing.assert_close(updated_positions[1], new_positions[0], atol=1e-5, rtol=0) @@ -1019,6 +1056,7 @@ def test_indices_with_only_positions_or_orientations(device, backend): # Get final poses final_positions, final_orientations = view.get_world_poses() + final_positions, final_orientations = wp.to_torch(final_positions), wp.to_torch(final_orientations) # Verify positions unchanged from previous step torch.testing.assert_close(final_positions, updated_positions, atol=1e-5, rtol=0) @@ -1051,12 +1089,15 @@ def test_index_type_none_equivalent_to_all(device): # Get poses with indices=None pos_none, quat_none = view.get_world_poses(indices=None) + pos_none, quat_none = wp.to_torch(pos_none), wp.to_torch(quat_none) # Get poses with no argument (default) pos_default, quat_default = view.get_world_poses() + pos_default, quat_default = wp.to_torch(pos_default), wp.to_torch(quat_default) # Get poses with slice(None) pos_slice, quat_slice = view.get_world_poses(indices=slice(None)) # type: ignore[arg-type] + pos_slice, quat_slice = wp.to_torch(pos_slice), wp.to_torch(quat_slice) # All should be equivalent torch.testing.assert_close(pos_none, pos_default, atol=1e-10, rtol=0) @@ -1071,6 +1112,7 @@ def test_index_type_none_equivalent_to_all(device): # Set with indices=None view.set_world_poses(positions=new_positions, orientations=new_orientations, indices=None) pos_after_none, quat_after_none = view.get_world_poses() + pos_after_none, quat_after_none = wp.to_torch(pos_after_none), wp.to_torch(quat_after_none) # Reset view.set_world_poses(positions=torch.zeros(num_prims, 3, device=device), indices=None) @@ -1082,6 +1124,7 @@ def test_index_type_none_equivalent_to_all(device): indices=slice(None), # type: ignore[arg-type] ) pos_after_slice, quat_after_slice = view.get_world_poses() + pos_after_slice, quat_after_slice = wp.to_torch(pos_after_slice), wp.to_torch(quat_after_slice) # Should be equivalent torch.testing.assert_close(pos_after_none, pos_after_slice, atol=1e-5, rtol=0) @@ -1116,6 +1159,7 @@ def test_with_franka_robots(device): # Get initial world poses (should be at origin) initial_positions, initial_orientations = frankas_view.get_world_poses() + initial_positions, initial_orientations = wp.to_torch(initial_positions), wp.to_torch(initial_orientations) # Verify initial positions are at origin expected_initial_positions = torch.zeros(2, 3, device=device) @@ -1139,6 +1183,7 @@ def test_with_franka_robots(device): # Get poses back and verify retrieved_positions, retrieved_orientations = frankas_view.get_world_poses() + retrieved_positions, retrieved_orientations = wp.to_torch(retrieved_positions), wp.to_torch(retrieved_orientations) torch.testing.assert_close(retrieved_positions, new_positions, atol=1e-5, rtol=0) try: @@ -1177,6 +1222,7 @@ def test_with_nested_targets(device): # Get world poses of targets world_positions, _ = targets_view.get_world_poses() + world_positions = wp.to_torch(world_positions) # Expected world positions are frame_translation + target_translation expected_positions = torch.tensor([[0.0, 20.0, 10.0], [0.0, 40.0, 25.0], [0.0, 53.0, 15.0]], device=device) @@ -1280,6 +1326,7 @@ def test_compare_get_world_poses_with_isaacsim(): # Get world poses from both isaaclab_pos, isaaclab_quat = isaaclab_view.get_world_poses() # xyzw + isaaclab_pos, isaaclab_quat = wp.to_torch(isaaclab_pos), wp.to_torch(isaaclab_quat) isaacsim_pos, isaacsim_quat = isaacsim_view.get_world_poses() # wxyz # Convert Isaac Sim results to torch tensors if needed @@ -1327,6 +1374,7 @@ def test_compare_set_world_poses_with_isaacsim(): # Get poses back from both isaaclab_pos, isaaclab_quat = isaaclab_view.get_world_poses() # xyzw + isaaclab_pos, isaaclab_quat = wp.to_torch(isaaclab_pos), wp.to_torch(isaaclab_quat) isaacsim_pos, isaacsim_quat = isaacsim_view.get_world_poses() # wxyz # Convert Isaac Sim results to torch tensors if needed @@ -1371,6 +1419,7 @@ def test_compare_get_local_poses_with_isaacsim(): # Get local poses from both isaaclab_trans, isaaclab_quat = isaaclab_view.get_local_poses() + isaaclab_trans, isaaclab_quat = wp.to_torch(isaaclab_trans), wp.to_torch(isaaclab_quat) isaacsim_trans, isaacsim_quat = isaacsim_view.get_local_poses() # Convert Isaac Sim results to torch tensors if needed @@ -1419,6 +1468,7 @@ def test_compare_set_local_poses_with_isaacsim(): # Get local poses back from both isaaclab_trans, isaaclab_quat = isaaclab_view.get_local_poses() + isaaclab_trans, isaaclab_quat = wp.to_torch(isaaclab_trans), wp.to_torch(isaaclab_quat) isaacsim_trans, isaacsim_quat = isaacsim_view.get_local_poses() # Convert Isaac Sim results to torch tensors if needed @@ -1433,83 +1483,3 @@ def test_compare_set_local_poses_with_isaacsim(): torch.testing.assert_close(isaaclab_quat, isaacsim_quat, atol=1e-4, rtol=0) except AssertionError: torch.testing.assert_close(isaaclab_quat, -isaacsim_quat, atol=1e-4, rtol=0) - - -""" -Tests - Fabric Operations. -""" - - -@pytest.mark.parametrize("device", ["cpu", "cuda"]) -def test_fabric_initialization(device): - """Test XformPrimView initialization with Fabric enabled.""" - _skip_if_backend_unavailable("fabric", device) - - stage = sim_utils.get_current_stage() - - # Create camera prims (Boundable prims that support Fabric) - num_prims = 5 - for i in range(num_prims): - sim_utils.create_prim(f"/World/Cam_{i}", "Camera", translation=(i * 1.0, 0.0, 1.0), stage=stage) - - # Create view with Fabric enabled - view = _create_view("/World/Cam_.*", device=device, backend="fabric") - - # Verify properties - assert view.count == num_prims - assert view.device == device - assert len(view.prims) == num_prims - - -@pytest.mark.parametrize("device", ["cpu", "cuda"]) -def test_fabric_usd_consistency(device): - """Test that Fabric round-trip (write→read) is consistent, matching Isaac Sim's design. - - Note: This does NOT test Fabric vs USD reads on initialization, as Fabric is designed - for write-first workflows. Instead, it tests that: - 1. Fabric write→read round-trip works correctly - 2. This matches Isaac Sim's Fabric behavior - """ - _skip_if_backend_unavailable("fabric", device) - - stage = sim_utils.get_current_stage() - - # Create prims - num_prims = 5 - for i in range(num_prims): - sim_utils.create_prim( - f"/World/Cam_{i}", - "Camera", - translation=(i * 1.0, 2.0, 3.0), - orientation=(0.0, 0.0, 0.7071068, 0.7071068), - stage=stage, - ) - - # Create Fabric view - view_fabric = _create_view("/World/Cam_.*", device=device, backend="fabric") - - # Test Fabric write→read round-trip (Isaac Sim's intended workflow) - # Initialize Fabric state by WRITING first - init_positions = torch.zeros((num_prims, 3), dtype=torch.float32, device=device) - init_positions[:, 0] = torch.arange(num_prims, dtype=torch.float32, device=device) - init_positions[:, 1] = 2.0 - init_positions[:, 2] = 3.0 - init_orientations = torch.tensor([[0.0, 0.0, 0.7071068, 0.7071068]] * num_prims, dtype=torch.float32, device=device) - - view_fabric.set_world_poses(init_positions, init_orientations) - - # Read back from Fabric (should match what we wrote) - pos_fabric, quat_fabric = view_fabric.get_world_poses() - torch.testing.assert_close(pos_fabric, init_positions, atol=1e-4, rtol=0) - torch.testing.assert_close(quat_fabric, init_orientations, atol=1e-4, rtol=0) - - # Test another round-trip with different values - new_positions = torch.rand((num_prims, 3), dtype=torch.float32, device=device) * 10.0 - new_orientations = torch.tensor([[0.0, 0.0, 0.0, 1.0]] * num_prims, dtype=torch.float32, device=device) - - view_fabric.set_world_poses(new_positions, new_orientations) - - # Read back from Fabric (should match) - pos_fabric_after, quat_fabric_after = view_fabric.get_world_poses() - torch.testing.assert_close(pos_fabric_after, new_positions, atol=1e-4, rtol=0) - torch.testing.assert_close(quat_fabric_after, new_orientations, atol=1e-4, rtol=0) diff --git a/source/isaaclab_newton/config/extension.toml b/source/isaaclab_newton/config/extension.toml index fe59ab3dfbb2..d775364e1e0f 100644 --- a/source/isaaclab_newton/config/extension.toml +++ b/source/isaaclab_newton/config/extension.toml @@ -1,7 +1,7 @@ [package] # Note: Semantic Versioning is used: https://semver.org/ -version = "0.5.13" +version = "0.5.14" # Description title = "Newton simulation interfaces for IsaacLab core package" diff --git a/source/isaaclab_newton/docs/CHANGELOG.rst b/source/isaaclab_newton/docs/CHANGELOG.rst index 72715c38aaca..2ae5c9fe6fec 100644 --- a/source/isaaclab_newton/docs/CHANGELOG.rst +++ b/source/isaaclab_newton/docs/CHANGELOG.rst @@ -1,6 +1,16 @@ Changelog --------- +0.5.14 (2026-04-14) +~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added :class:`~isaaclab_newton.sim.views.XformPrimView` providing the Newton + backend implementation for xform prim views. + + 0.5.13 (2026-04-13) ~~~~~~~~~~~~~~~~~~~ @@ -52,10 +62,6 @@ Fixed so articulation write methods trigger ``eval_fk`` before the next ``collide()``. - -0.5.9 (2026-03-16) -~~~~~~~~~~~~~~~~~~ - Fixed ^^^^^ diff --git a/source/isaaclab_newton/isaaclab_newton/sim/__init__.py b/source/isaaclab_newton/isaaclab_newton/sim/__init__.py new file mode 100644 index 000000000000..b4646aabbd0a --- /dev/null +++ b/source/isaaclab_newton/isaaclab_newton/sim/__init__.py @@ -0,0 +1,10 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Newton simulation utilities.""" + +from isaaclab.utils.module import lazy_export + +lazy_export() diff --git a/source/isaaclab_newton/isaaclab_newton/sim/__init__.pyi b/source/isaaclab_newton/isaaclab_newton/sim/__init__.pyi new file mode 100644 index 000000000000..aac4c8327ccb --- /dev/null +++ b/source/isaaclab_newton/isaaclab_newton/sim/__init__.pyi @@ -0,0 +1,10 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +__all__ = [ + "views", +] + +from . import views diff --git a/source/isaaclab_newton/isaaclab_newton/sim/views/__init__.py b/source/isaaclab_newton/isaaclab_newton/sim/views/__init__.py new file mode 100644 index 000000000000..44e8303bcedf --- /dev/null +++ b/source/isaaclab_newton/isaaclab_newton/sim/views/__init__.py @@ -0,0 +1,10 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Newton simulation views.""" + +from isaaclab.utils.module import lazy_export + +lazy_export() diff --git a/source/isaaclab_newton/isaaclab_newton/sim/views/__init__.pyi b/source/isaaclab_newton/isaaclab_newton/sim/views/__init__.pyi new file mode 100644 index 000000000000..a666958e4387 --- /dev/null +++ b/source/isaaclab_newton/isaaclab_newton/sim/views/__init__.pyi @@ -0,0 +1,10 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +__all__ = [ + "XformPrimView", +] + +from .xform_prim_view import XformPrimView diff --git a/source/isaaclab_newton/isaaclab_newton/sim/views/xform_prim_view.py b/source/isaaclab_newton/isaaclab_newton/sim/views/xform_prim_view.py new file mode 100644 index 000000000000..7c92b7847903 --- /dev/null +++ b/source/isaaclab_newton/isaaclab_newton/sim/views/xform_prim_view.py @@ -0,0 +1,495 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Newton-backed XformPrimView — Warp-native, GPU-resident pose queries.""" + +from __future__ import annotations + +import logging +from collections.abc import Sequence + +import warp as wp + +from pxr import Gf, Usd, UsdGeom + +import isaaclab.sim as sim_utils +from isaaclab.sim.views.base_xform_prim_view import BaseXformPrimView + +from isaaclab_newton.physics.newton_manager import NewtonManager + +logger = logging.getLogger(__name__) + +WORLD_BODY_INDEX = -1 + + +# ------------------------------------------------------------------ +# Warp kernels +# ------------------------------------------------------------------ + + +@wp.kernel +def _compute_site_world_transforms( + body_q: wp.array(dtype=wp.transformf), + site_body: wp.array(dtype=wp.int32), + site_local: wp.array(dtype=wp.transformf), + out_pos: wp.array(dtype=wp.vec3f), + out_quat: wp.array(dtype=wp.vec4f), +): + """Compute world transforms for all sites: ``world = body_q[body] * local``. + + When ``site_body[i] == -1`` the site is attached to the world frame and + the world transform equals ``site_local[i]`` directly. + """ + i = wp.tid() + bid = site_body[i] + if bid == -1: + world = site_local[i] + else: + world = wp.transform_multiply(body_q[bid], site_local[i]) + out_pos[i] = wp.transform_get_translation(world) + q = wp.transform_get_rotation(world) + out_quat[i] = wp.vec4f(q[0], q[1], q[2], q[3]) + + +@wp.kernel +def _compute_site_world_transforms_indexed( + body_q: wp.array(dtype=wp.transformf), + site_body: wp.array(dtype=wp.int32), + site_local: wp.array(dtype=wp.transformf), + indices: wp.array(dtype=wp.int32), + out_pos: wp.array(dtype=wp.vec3f), + out_quat: wp.array(dtype=wp.vec4f), +): + """Compute world transforms for a subset of sites selected by ``indices``.""" + i = wp.tid() + si = indices[i] + bid = site_body[si] + if bid == -1: + world = site_local[si] + else: + world = wp.transform_multiply(body_q[bid], site_local[si]) + out_pos[i] = wp.transform_get_translation(world) + q = wp.transform_get_rotation(world) + out_quat[i] = wp.vec4f(q[0], q[1], q[2], q[3]) + + +@wp.kernel +def _write_body_q_from_site_poses( + body_q: wp.array(dtype=wp.transformf), + site_body: wp.array(dtype=wp.int32), + site_local: wp.array(dtype=wp.transformf), + world_pos: wp.array(dtype=wp.vec3f), + world_quat: wp.array(dtype=wp.vec4f), +): + """Compute ``body_q = world_site_pose * inverse(local)`` and write it. + + Launched over all sites; skips world-attached sites (``site_body == -1``) + since they have no body to write to. + """ + i = wp.tid() + bid = site_body[i] + if bid == -1: + return + + inv_local = wp.transform_inverse(site_local[i]) + w_pos = world_pos[i] + w_q = world_quat[i] + world_tf = wp.transform(w_pos, wp.quatf(w_q[0], w_q[1], w_q[2], w_q[3])) + body_q[bid] = wp.transform_multiply(world_tf, inv_local) + + +@wp.kernel +def _write_body_q_from_site_poses_indexed( + body_q: wp.array(dtype=wp.transformf), + site_body: wp.array(dtype=wp.int32), + site_local: wp.array(dtype=wp.transformf), + indices: wp.array(dtype=wp.int32), + world_pos: wp.array(dtype=wp.vec3f), + world_quat: wp.array(dtype=wp.vec4f), +): + """Indexed variant: writes body_q for sites selected by ``indices``. + + ``world_pos`` and ``world_quat`` are dense over the index list (length M), + while ``site_body`` and ``site_local`` are indexed via ``indices[i]``. + """ + i = wp.tid() + si = indices[i] + bid = site_body[si] + if bid == -1: + return + + inv_local = wp.transform_inverse(site_local[si]) + w_pos = world_pos[i] + w_q = world_quat[i] + world_tf = wp.transform(w_pos, wp.quatf(w_q[0], w_q[1], w_q[2], w_q[3])) + body_q[bid] = wp.transform_multiply(world_tf, inv_local) + + +@wp.kernel +def _gather_scales( + shape_scale: wp.array(dtype=wp.vec3f), + shape_body: wp.array(dtype=wp.int32), + site_body: wp.array(dtype=wp.int32), + num_shapes: wp.int32, + out_scales: wp.array(dtype=wp.vec3f), +): + """For each site, find the first shape on the same body and copy its scale.""" + i = wp.tid() + bid = site_body[i] + found = int(0) + for s in range(num_shapes): + if shape_body[s] == bid and found == 0: + out_scales[i] = shape_scale[s] + found = 1 + if found == 0: + out_scales[i] = wp.vec3f(1.0, 1.0, 1.0) + + +@wp.kernel +def _gather_scales_indexed( + shape_scale: wp.array(dtype=wp.vec3f), + shape_body: wp.array(dtype=wp.int32), + site_body: wp.array(dtype=wp.int32), + indices: wp.array(dtype=wp.int32), + num_shapes: wp.int32, + out_scales: wp.array(dtype=wp.vec3f), +): + """Indexed variant of _gather_scales.""" + i = wp.tid() + si = indices[i] + bid = site_body[si] + found = int(0) + for s in range(num_shapes): + if shape_body[s] == bid and found == 0: + out_scales[i] = shape_scale[s] + found = 1 + if found == 0: + out_scales[i] = wp.vec3f(1.0, 1.0, 1.0) + + +@wp.kernel +def _scatter_scales( + shape_scale: wp.array(dtype=wp.vec3f), + shape_body: wp.array(dtype=wp.int32), + site_body: wp.array(dtype=wp.int32), + num_shapes: wp.int32, + new_scales: wp.array(dtype=wp.vec3f), +): + """For each site, write its scale to all shapes on the same body.""" + i = wp.tid() + bid = site_body[i] + for s in range(num_shapes): + if shape_body[s] == bid: + shape_scale[s] = new_scales[i] + + +@wp.kernel +def _scatter_scales_indexed( + shape_scale: wp.array(dtype=wp.vec3f), + shape_body: wp.array(dtype=wp.int32), + site_body: wp.array(dtype=wp.int32), + indices: wp.array(dtype=wp.int32), + num_shapes: wp.int32, + new_scales: wp.array(dtype=wp.vec3f), +): + """Indexed variant of _scatter_scales.""" + i = wp.tid() + si = indices[i] + bid = site_body[si] + for s in range(num_shapes): + if shape_body[s] == bid: + shape_scale[s] = new_scales[i] + + +# ------------------------------------------------------------------ +# Helpers +# ------------------------------------------------------------------ + + +def _ensure_wp_vec3f(data: wp.array, device: str) -> wp.array: + """Pass-through for ``wp.array``; convert ``torch.Tensor`` via ``wp.from_torch``.""" + if isinstance(data, wp.array): + return data + import torch # noqa: PLC0415 + + if isinstance(data, torch.Tensor): + return wp.from_torch(data.contiguous(), dtype=wp.vec3f) + raise TypeError(f"Expected wp.array or torch.Tensor, got {type(data)}") + + +def _ensure_wp_vec4f(data: wp.array, device: str) -> wp.array: + """Pass-through for ``wp.array``; convert ``torch.Tensor`` via ``wp.from_torch``.""" + if isinstance(data, wp.array): + return data + import torch # noqa: PLC0415 + + if isinstance(data, torch.Tensor): + return wp.from_torch(data.contiguous(), dtype=wp.vec4f) + raise TypeError(f"Expected wp.array or torch.Tensor, got {type(data)}") + + +# ------------------------------------------------------------------ +# View class +# ------------------------------------------------------------------ + + +class XformPrimView(BaseXformPrimView): + """Batched prim view backed by Newton's native site concept on GPU. + + Each matched USD prim is resolved to a ``(body_index, local_transform)`` + pair. World poses are computed on GPU as + ``body_q[body_index] * local_transform`` via a Warp kernel, consistent + with Newton's site mechanism. + + Resolution order (per prim path): + + 1. **Shape label** -- look up in ``model.shape_label``. If found, use + ``shape_body`` and ``shape_transform`` for the body index and local + offset. + 2. **Body label** -- look up in ``model.body_label``. If found, the body + itself is the site; local offset is identity. + 3. **Ancestor walk** -- walk the USD parent hierarchy until a prim whose + path appears in ``model.body_label`` is found. The relative transform + from that ancestor body to the target prim is the local offset. If no + ancestor body exists, the site is attached to the world frame + (``body_index = -1``) and the local offset is the prim's world + transform. + + This supports arbitrary prims -- rigid bodies, collision shapes, cameras, + plain Xforms, and any other Xformable prim. + + All getters return ``wp.array``. Setters accept ``wp.array``. + """ + + def __init__(self, prim_path: str, device: str = "cpu", stage: Usd.Stage | None = None, **kwargs): + self._prim_path = prim_path + self._device = device + + stage = sim_utils.get_current_stage() if stage is None else stage + self._prims: list[Usd.Prim] = sim_utils.find_matching_prims(prim_path, stage=stage) + + model = NewtonManager.get_model() + body_labels = list(model.body_label) + body_label_set = set(body_labels) + shape_labels = list(model.shape_label) + shape_body_np = model.shape_body.numpy() + shape_xform_np = model.shape_transform.numpy() + + xform_cache = UsdGeom.XformCache(Usd.TimeCode.Default()) + + site_bodies: list[int] = [] + site_locals: list[list[float]] = [] + + identity_xform = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0] + + for prim in self._prims: + pp = prim.GetPath().pathString + + if pp in shape_labels: + si = shape_labels.index(pp) + site_bodies.append(int(shape_body_np[si])) + site_locals.append(shape_xform_np[si].tolist()) + elif pp in body_label_set: + site_bodies.append(body_labels.index(pp)) + site_locals.append(identity_xform) + else: + body_idx, local_xform = self._resolve_ancestor_body(prim, body_labels, body_label_set, xform_cache) + site_bodies.append(body_idx) + site_locals.append(local_xform) + + self._site_body = wp.array(site_bodies, dtype=wp.int32, device=device) + self._site_local = wp.array( + [wp.transform(*x) for x in site_locals], + dtype=wp.transformf, + device=device, + ) + + self._pos_buf = wp.zeros(self.count, dtype=wp.vec3f, device=device) + self._quat_buf = wp.zeros(self.count, dtype=wp.vec4f, device=device) + + @staticmethod + def _resolve_ancestor_body( + prim: Usd.Prim, + body_labels: list[str], + body_label_set: set[str], + xform_cache: UsdGeom.XformCache, + ) -> tuple[int, list[float]]: + """Walk USD ancestors to find the nearest Newton body and compute the relative local transform. + + Returns: + A tuple ``(body_index, local_xform_7)`` where *local_xform_7* is + ``[tx, ty, tz, qx, qy, qz, qw]``. If no body ancestor exists, + ``body_index`` is :data:`WORLD_BODY_INDEX` and the local transform + is the prim's world transform. + """ + prim_world_tf = xform_cache.GetLocalToWorldTransform(prim) + prim_world_tf.Orthonormalize() + + ancestor = prim.GetParent() + while ancestor and ancestor.IsValid() and ancestor.GetPath().pathString != "/": + ancestor_path = ancestor.GetPath().pathString + if ancestor_path in body_label_set: + body_idx = body_labels.index(ancestor_path) + ancestor_world_tf = xform_cache.GetLocalToWorldTransform(ancestor) + ancestor_world_tf.Orthonormalize() + local_tf = prim_world_tf * ancestor_world_tf.GetInverse() + return body_idx, _gf_matrix_to_xform7(local_tf) + ancestor = ancestor.GetParent() + + return WORLD_BODY_INDEX, _gf_matrix_to_xform7(prim_world_tf) + + @property + def count(self) -> int: + return len(self._prims) + + # ------------------------------------------------------------------ + # World poses + # ------------------------------------------------------------------ + + def get_world_poses(self, indices: Sequence[int] | None = None) -> tuple[wp.array, wp.array]: + state = NewtonManager.get_state_0() + + if indices is not None: + n = len(indices) + idx_wp = wp.array(list(indices), dtype=wp.int32, device=self._device) + pos_buf = wp.zeros(n, dtype=wp.vec3f, device=self._device) + quat_buf = wp.zeros(n, dtype=wp.vec4f, device=self._device) + wp.launch( + _compute_site_world_transforms_indexed, + dim=n, + inputs=[state.body_q, self._site_body, self._site_local, idx_wp], + outputs=[pos_buf, quat_buf], + device=self._device, + ) + return pos_buf, quat_buf + + wp.launch( + _compute_site_world_transforms, + dim=self.count, + inputs=[state.body_q, self._site_body, self._site_local], + outputs=[self._pos_buf, self._quat_buf], + device=self._device, + ) + return self._pos_buf, self._quat_buf + + def set_world_poses( + self, + positions: wp.array | None = None, + orientations: wp.array | None = None, + indices: Sequence[int] | None = None, + ) -> None: + """Write world poses into Newton ``state.body_q``. + + For sites with a non-identity local transform the Warp kernel computes + ``body_q = world_site_pose * inverse(local_transform)`` so that the + resulting world pose of the *site* matches the requested value. + World-attached sites (``body_index == -1``) are skipped. + """ + if positions is None and orientations is None: + return + + state = NewtonManager.get_state_0() + + if positions is None or orientations is None: + cur_pos, cur_quat = self.get_world_poses(indices) + if positions is None: + positions = cur_pos + if orientations is None: + orientations = cur_quat + + pos_wp = _ensure_wp_vec3f(positions, self._device) + quat_wp = _ensure_wp_vec4f(orientations, self._device) + + if indices is not None: + n = len(indices) + idx_wp = wp.array(list(indices), dtype=wp.int32, device=self._device) + wp.launch( + _write_body_q_from_site_poses_indexed, + dim=n, + inputs=[state.body_q, self._site_body, self._site_local, idx_wp, pos_wp, quat_wp], + device=self._device, + ) + else: + wp.launch( + _write_body_q_from_site_poses, + dim=self.count, + inputs=[state.body_q, self._site_body, self._site_local, pos_wp, quat_wp], + device=self._device, + ) + + # ------------------------------------------------------------------ + # Local poses -- delegate to world (Newton bodies live in world space) + # ------------------------------------------------------------------ + + def get_local_poses(self, indices: Sequence[int] | None = None) -> tuple[wp.array, wp.array]: + return self.get_world_poses(indices) + + def set_local_poses( + self, + translations: wp.array | None = None, + orientations: wp.array | None = None, + indices: Sequence[int] | None = None, + ) -> None: + self.set_world_poses(positions=translations, orientations=orientations, indices=indices) + + # ------------------------------------------------------------------ + # Scales + # ------------------------------------------------------------------ + + def get_scales(self, indices: Sequence[int] | None = None) -> wp.array: + model = NewtonManager.get_model() + num_shapes = model.shape_count + + if indices is not None: + n = len(indices) + idx_wp = wp.array(list(indices), dtype=wp.int32, device=self._device) + out = wp.zeros(n, dtype=wp.vec3f, device=self._device) + wp.launch( + _gather_scales_indexed, + dim=n, + inputs=[model.shape_scale, model.shape_body, self._site_body, idx_wp, num_shapes], + outputs=[out], + device=self._device, + ) + else: + out = wp.zeros(self.count, dtype=wp.vec3f, device=self._device) + wp.launch( + _gather_scales, + dim=self.count, + inputs=[model.shape_scale, model.shape_body, self._site_body, num_shapes], + outputs=[out], + device=self._device, + ) + return out + + def set_scales(self, scales: wp.array, indices: Sequence[int] | None = None) -> None: + model = NewtonManager.get_model() + num_shapes = model.shape_count + scales_wp = _ensure_wp_vec3f(scales, self._device) + + if indices is not None: + n = len(indices) + idx_wp = wp.array(list(indices), dtype=wp.int32, device=self._device) + wp.launch( + _scatter_scales_indexed, + dim=n, + inputs=[model.shape_scale, model.shape_body, self._site_body, idx_wp, num_shapes, scales_wp], + device=self._device, + ) + else: + wp.launch( + _scatter_scales, + dim=self.count, + inputs=[model.shape_scale, model.shape_body, self._site_body, num_shapes, scales_wp], + device=self._device, + ) + + +def _gf_matrix_to_xform7(mat: Gf.Matrix4d) -> list[float]: + """Convert a ``Gf.Matrix4d`` to ``[tx, ty, tz, qx, qy, qz, qw]``.""" + t = mat.ExtractTranslation() + q = mat.ExtractRotationQuat() + imag = q.GetImaginary() + return [float(t[0]), float(t[1]), float(t[2]), float(imag[0]), float(imag[1]), float(imag[2]), float(q.GetReal())] diff --git a/source/isaaclab_newton/test/sim/__init__.py b/source/isaaclab_newton/test/sim/__init__.py new file mode 100644 index 000000000000..460a30569089 --- /dev/null +++ b/source/isaaclab_newton/test/sim/__init__.py @@ -0,0 +1,4 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_newton/test/sim/test_views_xform_prim_newton.py b/source/isaaclab_newton/test/sim/test_views_xform_prim_newton.py new file mode 100644 index 000000000000..5c440e0cbbc7 --- /dev/null +++ b/source/isaaclab_newton/test/sim/test_views_xform_prim_newton.py @@ -0,0 +1,432 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Tests for Newton XformPrimView backed by sites (body_q + local transform). + +Covers the same test surface as the core USD and PhysX Fabric tests where +applicable, plus Newton-specific scenarios: ancestor-walk resolution for +non-physics prims, world-attached (body=-1) prims, and hierarchical prims +(Xform child under a rigid body). +""" + +import sys +from pathlib import Path + +sys.path.insert(0, str(Path(__file__).resolve().parents[1])) + +import pytest +import torch +import warp as wp +from isaaclab_newton.physics import MJWarpSolverCfg, NewtonCfg +from isaaclab_newton.physics.newton_manager import NewtonManager +from isaaclab_newton.sim.views import XformPrimView + +from pxr import Gf + +import isaaclab.sim as sim_utils +from isaaclab.assets import RigidObjectCfg +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg +from isaaclab.sim import SimulationCfg, build_simulation_context +from isaaclab.utils import configclass + +NEWTON_SIM_CFG = SimulationCfg( + physics=NewtonCfg( + solver_cfg=MJWarpSolverCfg(), + ), +) + + +def _assert_close(a, b, **kwargs): + """Compare two arrays (wp.array or torch.Tensor) via torch.testing.assert_close.""" + a_t = wp.to_torch(a) if isinstance(a, wp.array) else a + b_t = wp.to_torch(b) if isinstance(b, wp.array) else b + torch.testing.assert_close(a_t, b_t, **kwargs) + + +@configclass +class SimpleSceneCfg(InteractiveSceneCfg): + cube: RigidObjectCfg = RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/Cube", + spawn=sim_utils.CuboidCfg( + size=(0.2, 0.2, 0.2), + rigid_props=sim_utils.RigidBodyPropertiesCfg(), + mass_props=sim_utils.MassPropertiesCfg(mass=1.0), + collision_props=sim_utils.CollisionPropertiesCfg(), + ), + init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, 0.0, 1.0)), + ) + + +def _newton_sim_context(device="cuda:0", num_envs=4, **kwargs): + NEWTON_SIM_CFG.device = device + return build_simulation_context(device=device, sim_cfg=NEWTON_SIM_CFG, **kwargs) + + +def _build_scene(num_envs=4, device="cuda:0"): + """Build scene and return (sim, scene, view, ctx).""" + ctx = _newton_sim_context(device=device, num_envs=num_envs, add_ground_plane=True) + sim = ctx.__enter__() + sim._app_control_on_stop_handle = None + scene = InteractiveScene(SimpleSceneCfg(num_envs=num_envs, env_spacing=2.0)) + sim.reset() + view = XformPrimView("/World/envs/env_.*/Cube", device=device) + return sim, scene, view, ctx + + +def _build_scene_with_child_xform(num_envs=4, device="cuda:0"): + """Build scene with a child Xform prim under each Cube (camera mount scenario).""" + ctx = _newton_sim_context(device=device, num_envs=num_envs, add_ground_plane=True) + sim = ctx.__enter__() + sim._app_control_on_stop_handle = None + scene = InteractiveScene(SimpleSceneCfg(num_envs=num_envs, env_spacing=2.0)) + + stage = sim_utils.get_current_stage() + for i in range(num_envs): + xform_path = f"/World/envs/env_{i}/Cube/CameraMount" + xform_prim = stage.DefinePrim(xform_path, "Xform") + sim_utils.standardize_xform_ops(xform_prim) + xform_prim.GetAttribute("xformOp:translate").Set(Gf.Vec3d(0.1, 0.0, 0.05)) + xform_prim.GetAttribute("xformOp:orient").Set(Gf.Quatd(1.0, 0.0, 0.0, 0.0)) + + sim.reset() + view = XformPrimView("/World/envs/env_.*/Cube/CameraMount", device=device) + return sim, scene, view, ctx + + +def _build_scene_with_world_xform(num_envs=4, device="cuda:0"): + """Build scene with a plain Xform at the world root (no body ancestor).""" + ctx = _newton_sim_context(device=device, num_envs=num_envs, add_ground_plane=True) + sim = ctx.__enter__() + sim._app_control_on_stop_handle = None + scene = InteractiveScene(SimpleSceneCfg(num_envs=num_envs, env_spacing=2.0)) + + stage = sim_utils.get_current_stage() + xform_path = "/World/StaticMarker" + xform_prim = stage.DefinePrim(xform_path, "Xform") + sim_utils.standardize_xform_ops(xform_prim) + xform_prim.GetAttribute("xformOp:translate").Set(Gf.Vec3d(5.0, 3.0, 1.0)) + xform_prim.GetAttribute("xformOp:orient").Set(Gf.Quatd(1.0, 0.0, 0.0, 0.0)) + + sim.reset() + view = XformPrimView("/World/StaticMarker", device=device) + return sim, scene, view, ctx + + +# ====================================================================== +# Tests - Initialization +# ====================================================================== + + +@pytest.mark.skipif(not torch.cuda.is_available(), reason="CUDA not available") +def test_count(): + """Test that Newton XformPrimView reports correct prim count.""" + sim, scene, view, ctx = _build_scene(num_envs=4) + assert view.count == 4 + ctx.__exit__(None, None, None) + + +@pytest.mark.skipif(not torch.cuda.is_available(), reason="CUDA not available") +@pytest.mark.parametrize("num_envs", [1, 4, 8]) +def test_count_various_sizes(num_envs): + """Test count with different numbers of environments.""" + sim, scene, view, ctx = _build_scene(num_envs=num_envs) + assert view.count == num_envs + ctx.__exit__(None, None, None) + + +# ====================================================================== +# Tests - Getters (returns wp.array) +# ====================================================================== + + +@pytest.mark.skipif(not torch.cuda.is_available(), reason="CUDA not available") +def test_get_world_poses_returns_warp_arrays(): + """Test that get_world_poses returns wp.array, not torch.Tensor.""" + sim, scene, view, ctx = _build_scene(num_envs=4) + positions, orientations = view.get_world_poses() + + assert isinstance(positions, wp.array) + assert isinstance(orientations, wp.array) + assert positions.shape[0] == 4 + assert orientations.shape[0] == 4 + pos_t = wp.to_torch(positions) + quat_t = wp.to_torch(orientations) + assert pos_t.shape == (4, 3) + assert quat_t.shape == (4, 4) + assert pos_t.device.type == "cuda" + ctx.__exit__(None, None, None) + + +@pytest.mark.skipif(not torch.cuda.is_available(), reason="CUDA not available") +def test_get_world_poses_matches_body_q(): + """Test that get_world_poses returns values matching body_q.""" + sim, scene, view, ctx = _build_scene(num_envs=4) + positions, orientations = view.get_world_poses() + positions_t = wp.to_torch(positions) + + model = NewtonManager.get_model() + body_labels = list(model.body_label) + state = NewtonManager.get_state_0() + body_q = wp.to_torch(state.body_q) + + for i in range(4): + prim_path = f"/World/envs/env_{i}/Cube" + if prim_path in body_labels: + body_idx = body_labels.index(prim_path) + expected_pos = body_q[body_idx, :3] + torch.testing.assert_close(positions_t[i], expected_pos, atol=1e-5, rtol=0) + ctx.__exit__(None, None, None) + + +@pytest.mark.skipif(not torch.cuda.is_available(), reason="CUDA not available") +def test_get_world_poses_with_indices(): + """Test get_world_poses with a subset of indices.""" + sim, scene, view, ctx = _build_scene(num_envs=4) + pos_all_t = wp.to_torch(view.get_world_poses()[0]) + + indices = [0, 2] + positions_subset, orientations_subset = view.get_world_poses(indices) + + assert isinstance(positions_subset, wp.array) + pos_sub_t = wp.to_torch(positions_subset) + quat_sub_t = wp.to_torch(orientations_subset) + assert pos_sub_t.shape == (2, 3) + assert quat_sub_t.shape == (2, 4) + torch.testing.assert_close(pos_sub_t[0], pos_all_t[0], atol=1e-6, rtol=0) + torch.testing.assert_close(pos_sub_t[1], pos_all_t[2], atol=1e-6, rtol=0) + ctx.__exit__(None, None, None) + + +@pytest.mark.skipif(not torch.cuda.is_available(), reason="CUDA not available") +def test_get_world_poses_single_index(): + """Test get_world_poses with a single index.""" + sim, scene, view, ctx = _build_scene(num_envs=5) + pos_all_t = wp.to_torch(view.get_world_poses()[0]) + + positions_single, orientations_single = view.get_world_poses([3]) + pos_s_t = wp.to_torch(positions_single) + quat_s_t = wp.to_torch(orientations_single) + assert pos_s_t.shape == (1, 3) + assert quat_s_t.shape == (1, 4) + torch.testing.assert_close(pos_s_t[0], pos_all_t[3], atol=1e-6, rtol=0) + ctx.__exit__(None, None, None) + + +@pytest.mark.skipif(not torch.cuda.is_available(), reason="CUDA not available") +def test_get_world_poses_out_of_order_indices(): + """Test get_world_poses with out-of-order indices.""" + sim, scene, view, ctx = _build_scene(num_envs=5) + pos_all_t = wp.to_torch(view.get_world_poses()[0]) + + indices = [4, 1, 3] + positions_subset, _ = view.get_world_poses(indices) + pos_sub_t = wp.to_torch(positions_subset) + + assert pos_sub_t.shape == (3, 3) + torch.testing.assert_close(pos_sub_t[0], pos_all_t[4], atol=1e-6, rtol=0) + torch.testing.assert_close(pos_sub_t[1], pos_all_t[1], atol=1e-6, rtol=0) + torch.testing.assert_close(pos_sub_t[2], pos_all_t[3], atol=1e-6, rtol=0) + ctx.__exit__(None, None, None) + + +# ====================================================================== +# Tests - Setters (wp.array input) +# ====================================================================== + + +def _wp_vec3f(data, device="cuda:0"): + return wp.array([wp.vec3f(*row) for row in data], dtype=wp.vec3f, device=device) + + +def _wp_vec4f(data, device="cuda:0"): + return wp.array([wp.vec4f(*row) for row in data], dtype=wp.vec4f, device=device) + + +@pytest.mark.skipif(not torch.cuda.is_available(), reason="CUDA not available") +def test_set_world_poses(): + """Test setting world poses writes to body_q.""" + sim, scene, view, ctx = _build_scene(num_envs=4) + + new_pos = _wp_vec3f([[10.0, 20.0, 30.0], [40.0, 50.0, 60.0], [70.0, 80.0, 90.0], [100.0, 110.0, 120.0]]) + new_quat = _wp_vec4f( + [ + [0.0, 0.0, 0.0, 1.0], + [0.0, 0.0, 0.7071068, 0.7071068], + [0.7071068, 0.0, 0.0, 0.7071068], + [0.3826834, 0.0, 0.0, 0.9238795], + ] + ) + + view.set_world_poses(new_pos, new_quat) + ret_pos, ret_quat = view.get_world_poses() + + _assert_close(ret_pos, new_pos, atol=1e-5, rtol=0) + _assert_close(ret_quat, new_quat, atol=1e-5, rtol=0) + ctx.__exit__(None, None, None) + + +@pytest.mark.skipif(not torch.cuda.is_available(), reason="CUDA not available") +def test_set_world_poses_only_positions(): + """Test setting only positions, leaving orientations unchanged.""" + sim, scene, view, ctx = _build_scene(num_envs=3) + _, initial_quat = view.get_world_poses() + + new_pos = _wp_vec3f([[1.0, 0.0, 0.0], [0.0, 2.0, 0.0], [0.0, 0.0, 3.0]]) + view.set_world_poses(positions=new_pos, orientations=None) + + ret_pos, ret_quat = view.get_world_poses() + _assert_close(ret_pos, new_pos, atol=1e-5, rtol=0) + _assert_close(ret_quat, initial_quat, atol=1e-5, rtol=0) + ctx.__exit__(None, None, None) + + +@pytest.mark.skipif(not torch.cuda.is_available(), reason="CUDA not available") +def test_set_world_poses_only_orientations(): + """Test setting only orientations, leaving positions unchanged.""" + sim, scene, view, ctx = _build_scene(num_envs=3) + initial_pos, _ = view.get_world_poses() + + new_quat = _wp_vec4f( + [ + [0.0, 0.0, 0.7071068, 0.7071068], + [0.7071068, 0.0, 0.0, 0.7071068], + [0.3826834, 0.0, 0.0, 0.9238795], + ] + ) + view.set_world_poses(positions=None, orientations=new_quat) + + ret_pos, ret_quat = view.get_world_poses() + _assert_close(ret_pos, initial_pos, atol=1e-5, rtol=0) + _assert_close(ret_quat, new_quat, atol=1e-5, rtol=0) + ctx.__exit__(None, None, None) + + +@pytest.mark.skipif(not torch.cuda.is_available(), reason="CUDA not available") +def test_set_world_poses_with_indices(): + """Test setting world poses for a subset of indices.""" + sim, scene, view, ctx = _build_scene(num_envs=5) + init_pos_t = wp.to_torch(view.get_world_poses()[0]).clone() + + indices = [1, 3] + new_pos = _wp_vec3f([[10.0, 0.0, 0.0], [30.0, 0.0, 0.0]]) + view.set_world_poses(positions=new_pos, orientations=None, indices=indices) + + updated_pos_t = wp.to_torch(view.get_world_poses()[0]) + new_pos_t = wp.to_torch(new_pos) + + torch.testing.assert_close(updated_pos_t[1], new_pos_t[0], atol=1e-5, rtol=0) + torch.testing.assert_close(updated_pos_t[3], new_pos_t[1], atol=1e-5, rtol=0) + torch.testing.assert_close(updated_pos_t[0], init_pos_t[0], atol=1e-5, rtol=0) + torch.testing.assert_close(updated_pos_t[2], init_pos_t[2], atol=1e-5, rtol=0) + torch.testing.assert_close(updated_pos_t[4], init_pos_t[4], atol=1e-5, rtol=0) + ctx.__exit__(None, None, None) + + +# ====================================================================== +# Tests - Round-trip consistency +# ====================================================================== + + +@pytest.mark.skipif(not torch.cuda.is_available(), reason="CUDA not available") +def test_write_read_roundtrip(): + """Test that write -> read round-trip is consistent.""" + sim, scene, view, ctx = _build_scene(num_envs=4) + + new_pos = _wp_vec3f([[1.0, 2.0, 3.0], [4.0, 5.0, 6.0], [7.0, 8.0, 9.0], [10.0, 11.0, 12.0]]) + new_quat = _wp_vec4f([[0.0, 0.0, 0.0, 1.0]] * 4) + + view.set_world_poses(new_pos, new_quat) + pos, quat = view.get_world_poses() + + _assert_close(pos, new_pos, atol=1e-5, rtol=0) + _assert_close(quat, new_quat, atol=1e-5, rtol=0) + + new_pos2 = _wp_vec3f([[11.0, 12.0, 13.0], [14.0, 15.0, 16.0], [17.0, 18.0, 19.0], [20.0, 21.0, 22.0]]) + view.set_world_poses(new_pos2, new_quat) + pos2, quat2 = view.get_world_poses() + + _assert_close(pos2, new_pos2, atol=1e-5, rtol=0) + _assert_close(quat2, new_quat, atol=1e-5, rtol=0) + ctx.__exit__(None, None, None) + + +# ====================================================================== +# Tests - Ancestor-walk resolution (non-physics Xform under rigid body) +# ====================================================================== + + +@pytest.mark.skipif(not torch.cuda.is_available(), reason="CUDA not available") +def test_child_xform_initialization(): + """Test that a child Xform under a rigid body resolves via ancestor walk.""" + sim, scene, view, ctx = _build_scene_with_child_xform(num_envs=4) + assert view.count == 4 + ctx.__exit__(None, None, None) + + +@pytest.mark.skipif(not torch.cuda.is_available(), reason="CUDA not available") +def test_child_xform_world_pose_includes_offset(): + """Test that child Xform world pose includes the local offset from the body. + + The CameraMount is at (0.1, 0.0, 0.05) relative to the Cube body. + Its world pose should be body_world_pos + rotated_offset. + """ + sim, scene, view, ctx = _build_scene_with_child_xform(num_envs=4) + child_positions, child_orientations = view.get_world_poses() + child_pos_t = wp.to_torch(child_positions) + + body_view = XformPrimView("/World/envs/env_.*/Cube", device="cuda:0") + body_positions, body_orientations = body_view.get_world_poses() + body_pos_t = wp.to_torch(body_positions) + + offset = torch.tensor([0.1, 0.0, 0.05], device="cuda:0") + for i in range(4): + expected_pos = body_pos_t[i] + offset + torch.testing.assert_close(child_pos_t[i], expected_pos, atol=1e-4, rtol=0) + ctx.__exit__(None, None, None) + + +@pytest.mark.skipif(not torch.cuda.is_available(), reason="CUDA not available") +def test_child_xform_tracks_body_after_move(): + """Test that child Xform world pose updates when parent body moves.""" + sim, scene, view, ctx = _build_scene_with_child_xform(num_envs=2) + + body_view = XformPrimView("/World/envs/env_.*/Cube", device="cuda:0") + new_body_pos = _wp_vec3f([[5.0, 0.0, 0.0], [0.0, 5.0, 0.0]]) + new_body_orient = _wp_vec4f([[0.0, 0.0, 0.0, 1.0]] * 2) + body_view.set_world_poses(new_body_pos, new_body_orient) + + child_positions, _ = view.get_world_poses() + child_pos_t = wp.to_torch(child_positions) + new_body_pos_t = wp.to_torch(new_body_pos) + + offset = torch.tensor([0.1, 0.0, 0.05], device="cuda:0") + for i in range(2): + expected_pos = new_body_pos_t[i] + offset + torch.testing.assert_close(child_pos_t[i], expected_pos, atol=1e-4, rtol=0) + ctx.__exit__(None, None, None) + + +# ====================================================================== +# Tests - World-attached prims (body=-1) +# ====================================================================== + + +@pytest.mark.skipif(not torch.cuda.is_available(), reason="CUDA not available") +def test_world_xform_initialization(): + """Test that a world-rooted Xform resolves with body=-1.""" + sim, scene, view, ctx = _build_scene_with_world_xform(num_envs=2) + assert view.count == 1 + ctx.__exit__(None, None, None) + + +@pytest.mark.skipif(not torch.cuda.is_available(), reason="CUDA not available") +def test_world_xform_returns_correct_pose(): + """Test that a world-rooted Xform returns its USD world transform.""" + sim, scene, view, ctx = _build_scene_with_world_xform(num_envs=2) + positions, orientations = view.get_world_poses() + + expected_pos = torch.tensor([[5.0, 3.0, 1.0]], device="cuda:0") + _assert_close(positions, expected_pos, atol=1e-4, rtol=0) + ctx.__exit__(None, None, None) diff --git a/source/isaaclab_physx/config/extension.toml b/source/isaaclab_physx/config/extension.toml index e08d8e963594..041e62a8f5b5 100644 --- a/source/isaaclab_physx/config/extension.toml +++ b/source/isaaclab_physx/config/extension.toml @@ -1,7 +1,7 @@ [package] # Note: Semantic Versioning is used: https://semver.org/ -version = "0.5.16" +version = "0.5.17" # Description title = "PhysX simulation interfaces for IsaacLab core package" diff --git a/source/isaaclab_physx/docs/CHANGELOG.rst b/source/isaaclab_physx/docs/CHANGELOG.rst index 7cbb477aa057..0c7fa5f4a47c 100644 --- a/source/isaaclab_physx/docs/CHANGELOG.rst +++ b/source/isaaclab_physx/docs/CHANGELOG.rst @@ -1,6 +1,16 @@ Changelog --------- +0.5.17 (2026-04-14) +~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ + +* Added :class:`~isaaclab_physx.sim.views.XformPrimView` providing the PhysX/Fabric + backend implementation for xform prim views. + + 0.5.16 (2026-04-13) ~~~~~~~~~~~~~~~~~~~ diff --git a/source/isaaclab_physx/isaaclab_physx/sim/__init__.py b/source/isaaclab_physx/isaaclab_physx/sim/__init__.py new file mode 100644 index 000000000000..fd939d0bf4e5 --- /dev/null +++ b/source/isaaclab_physx/isaaclab_physx/sim/__init__.py @@ -0,0 +1,10 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""PhysX simulation utilities.""" + +from isaaclab.utils.module import lazy_export + +lazy_export() diff --git a/source/isaaclab_physx/isaaclab_physx/sim/__init__.pyi b/source/isaaclab_physx/isaaclab_physx/sim/__init__.pyi new file mode 100644 index 000000000000..aac4c8327ccb --- /dev/null +++ b/source/isaaclab_physx/isaaclab_physx/sim/__init__.pyi @@ -0,0 +1,10 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +__all__ = [ + "views", +] + +from . import views diff --git a/source/isaaclab_physx/isaaclab_physx/sim/views/__init__.py b/source/isaaclab_physx/isaaclab_physx/sim/views/__init__.py new file mode 100644 index 000000000000..85c69b44a24f --- /dev/null +++ b/source/isaaclab_physx/isaaclab_physx/sim/views/__init__.py @@ -0,0 +1,10 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""PhysX simulation views.""" + +from isaaclab.utils.module import lazy_export + +lazy_export() diff --git a/source/isaaclab_physx/isaaclab_physx/sim/views/__init__.pyi b/source/isaaclab_physx/isaaclab_physx/sim/views/__init__.pyi new file mode 100644 index 000000000000..a666958e4387 --- /dev/null +++ b/source/isaaclab_physx/isaaclab_physx/sim/views/__init__.pyi @@ -0,0 +1,10 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +__all__ = [ + "XformPrimView", +] + +from .xform_prim_view import XformPrimView diff --git a/source/isaaclab_physx/isaaclab_physx/sim/views/xform_prim_view.py b/source/isaaclab_physx/isaaclab_physx/sim/views/xform_prim_view.py new file mode 100644 index 000000000000..b044c7487ef5 --- /dev/null +++ b/source/isaaclab_physx/isaaclab_physx/sim/views/xform_prim_view.py @@ -0,0 +1,380 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""PhysX XformPrimView with Fabric GPU acceleration — Warp-native.""" + +from __future__ import annotations + +import logging +from collections.abc import Sequence + +import torch +import warp as wp + +from pxr import Usd + +import isaaclab.sim as sim_utils +from isaaclab.app.settings_manager import SettingsManager +from isaaclab.sim.views.xform_prim_view import XformPrimView as UsdXformPrimView +from isaaclab.utils.warp import fabric as fabric_utils + +logger = logging.getLogger(__name__) + + +class XformPrimView(UsdXformPrimView): + """XformPrimView with Fabric GPU acceleration for the PhysX backend. + + Inherits all USD-based operations from the core + :class:`~isaaclab.sim.views.XformPrimView` and adds GPU-accelerated Fabric + paths for ``get_world_poses``, ``set_world_poses``, ``get_scales``, and + ``set_scales``. + + When Fabric is enabled, this class leverages NVIDIA's Fabric API: + + - Uses ``omni:fabric:worldMatrix`` for all Boundable prims + - Performs batch matrix decomposition/composition using Warp kernels on GPU + - Works for both physics-enabled and non-physics prims (cameras, meshes, etc.) + + When Fabric is **not** enabled, all operations fall through to the USD base class. + + All getters return ``wp.array``. Setters accept ``wp.array``. + + .. warning:: + **Fabric requires CUDA**: Fabric is only supported on CUDA devices. + Warp's CPU backend for fabric-array writes has known issues, so attempting + to use Fabric with CPU device will automatically fall back to USD operations. + """ + + def __init__( + self, + prim_path: str, + device: str = "cpu", + validate_xform_ops: bool = True, + sync_usd_on_fabric_write: bool = False, + stage: Usd.Stage | None = None, + ): + """Initialize the view with matching prims and optional Fabric acceleration. + + Args: + prim_path: USD prim path pattern to match prims. + device: Device to place arrays on. Defaults to ``"cpu"``. + validate_xform_ops: Whether to validate standard xform operations. Defaults to True. + sync_usd_on_fabric_write: Whether to mirror Fabric transform writes back to USD. + Defaults to False for better performance. + stage: USD stage to search for prims. Defaults to None (current active stage). + """ + super().__init__(prim_path, device=device, validate_xform_ops=validate_xform_ops, stage=stage) + + self._sync_usd_on_fabric_write = sync_usd_on_fabric_write + + settings = SettingsManager.instance() + self._use_fabric = bool(settings.get("/physics/fabricEnabled", False)) + + if self._use_fabric and self._device == "cpu": + logger.warning( + "Fabric mode with Warp fabric-array operations is not supported on CPU devices. " + "Falling back to standard USD operations on the CPU. This may impact performance." + ) + self._use_fabric = False + + if self._use_fabric and self._device not in ("cuda", "cuda:0"): + logger.warning( + f"Fabric mode is not supported on device '{self._device}'. " + "USDRT SelectPrims and Warp fabric arrays only support cuda:0. " + "Falling back to standard USD operations. This may impact performance." + ) + self._use_fabric = False + + self._fabric_initialized = False + self._fabric_usd_sync_done = False + self._fabric_selection = None + self._fabric_to_view: wp.array | None = None + self._view_to_fabric: wp.array | None = None + self._default_view_indices: wp.array | None = None + self._fabric_hierarchy = None + self._view_index_attr = f"isaaclab:view_index:{abs(hash(self))}" + + # ------------------------------------------------------------------ + # Overrides — Fabric-accelerated or USD fallback + # ------------------------------------------------------------------ + + def set_world_poses( + self, + positions: wp.array | None = None, + orientations: wp.array | None = None, + indices: Sequence[int] | None = None, + ): + if not self._use_fabric: + super().set_world_poses(positions, orientations, indices) + return + + if not self._fabric_initialized: + self._initialize_fabric() + + indices_wp = self._resolve_indices_wp(indices) + count = indices_wp.shape[0] + + positions_wp = positions if positions is not None else wp.zeros((0, 3), dtype=wp.float32, device=self._device) + orientations_wp = ( + orientations if orientations is not None else wp.zeros((0, 4), dtype=wp.float32, device=self._device) + ) + scales_wp = wp.zeros((0, 3), dtype=wp.float32, device=self._device) + + wp.launch( + kernel=fabric_utils.compose_fabric_transformation_matrix_from_warp_arrays, + dim=count, + inputs=[ + self._fabric_world_matrices, + positions_wp, + orientations_wp, + scales_wp, + False, + False, + False, + indices_wp, + self._view_to_fabric, + ], + device=self._fabric_device, + ) + wp.synchronize() + + self._fabric_hierarchy.update_world_xforms() + self._fabric_usd_sync_done = True + if self._sync_usd_on_fabric_write: + super().set_world_poses(positions, orientations, indices) + + def set_local_poses( + self, + translations: wp.array | None = None, + orientations: wp.array | None = None, + indices: Sequence[int] | None = None, + ): + # Fabric only accelerates world poses; local poses always go through USD + super().set_local_poses(translations, orientations, indices) + + def set_scales(self, scales: wp.array, indices: Sequence[int] | None = None): + if not self._use_fabric: + super().set_scales(scales, indices) + return + + if not self._fabric_initialized: + self._initialize_fabric() + + indices_wp = self._resolve_indices_wp(indices) + count = indices_wp.shape[0] + + positions_wp = wp.zeros((0, 3), dtype=wp.float32, device=self._device) + orientations_wp = wp.zeros((0, 4), dtype=wp.float32, device=self._device) + + wp.launch( + kernel=fabric_utils.compose_fabric_transformation_matrix_from_warp_arrays, + dim=count, + inputs=[ + self._fabric_world_matrices, + positions_wp, + orientations_wp, + scales, + False, + False, + False, + indices_wp, + self._view_to_fabric, + ], + device=self._fabric_device, + ) + wp.synchronize() + + self._fabric_hierarchy.update_world_xforms() + self._fabric_usd_sync_done = True + if self._sync_usd_on_fabric_write: + super().set_scales(scales, indices) + + def get_world_poses(self, indices: Sequence[int] | None = None) -> tuple[wp.array, wp.array]: + if not self._use_fabric: + return super().get_world_poses(indices) + + if not self._fabric_initialized: + self._initialize_fabric() + if not self._fabric_usd_sync_done: + self._sync_fabric_from_usd_once() + + indices_wp = self._resolve_indices_wp(indices) + count = indices_wp.shape[0] + + use_cached = indices is None or indices == slice(None) + if use_cached: + positions_wp = self._fabric_positions_buf + orientations_wp = self._fabric_orientations_buf + else: + positions_wp = wp.zeros((count, 3), dtype=wp.float32, device=self._device) + orientations_wp = wp.zeros((count, 4), dtype=wp.float32, device=self._device) + + wp.launch( + kernel=fabric_utils.decompose_fabric_transformation_matrix_to_warp_arrays, + dim=count, + inputs=[ + self._fabric_world_matrices, + positions_wp, + orientations_wp, + self._fabric_dummy_buffer, + indices_wp, + self._view_to_fabric, + ], + device=self._fabric_device, + ) + + if use_cached: + wp.synchronize() + return positions_wp, orientations_wp + + def get_local_poses(self, indices: Sequence[int] | None = None) -> tuple[wp.array, wp.array]: + # Fabric only accelerates world poses; local poses always go through USD + return super().get_local_poses(indices) + + def get_scales(self, indices: Sequence[int] | None = None) -> wp.array: + if not self._use_fabric: + return super().get_scales(indices) + + if not self._fabric_initialized: + self._initialize_fabric() + if not self._fabric_usd_sync_done: + self._sync_fabric_from_usd_once() + + indices_wp = self._resolve_indices_wp(indices) + count = indices_wp.shape[0] + + use_cached = indices is None or indices == slice(None) + if use_cached: + scales_wp = self._fabric_scales_buf + else: + scales_wp = wp.zeros((count, 3), dtype=wp.float32, device=self._device) + + wp.launch( + kernel=fabric_utils.decompose_fabric_transformation_matrix_to_warp_arrays, + dim=count, + inputs=[ + self._fabric_world_matrices, + self._fabric_dummy_buffer, + self._fabric_dummy_buffer, + scales_wp, + indices_wp, + self._view_to_fabric, + ], + device=self._fabric_device, + ) + + if use_cached: + wp.synchronize() + return scales_wp + + # ------------------------------------------------------------------ + # Internal — Initialization + # ------------------------------------------------------------------ + + def _initialize_fabric(self) -> None: + """Initialize Fabric batch infrastructure for GPU-accelerated pose queries.""" + import usdrt # noqa: PLC0415 + from usdrt import Rt # noqa: PLC0415 + + stage_id = sim_utils.get_current_stage_id() + fabric_stage = usdrt.Usd.Stage.Attach(stage_id) + + for i in range(self.count): + rt_prim = fabric_stage.GetPrimAtPath(self.prim_paths[i]) + rt_xformable = Rt.Xformable(rt_prim) + + has_attr = ( + rt_xformable.HasFabricHierarchyWorldMatrixAttr() + if hasattr(rt_xformable, "HasFabricHierarchyWorldMatrixAttr") + else False + ) + if not has_attr: + rt_xformable.CreateFabricHierarchyWorldMatrixAttr() + + rt_xformable.SetWorldXformFromUsd() + + rt_prim.CreateAttribute(self._view_index_attr, usdrt.Sdf.ValueTypeNames.UInt, custom=True) + rt_prim.GetAttribute(self._view_index_attr).Set(i) + + self._fabric_hierarchy = usdrt.hierarchy.IFabricHierarchy().get_fabric_hierarchy( + fabric_stage.GetFabricId(), fabric_stage.GetStageIdAsStageId() + ) + self._fabric_hierarchy.update_world_xforms() + + self._default_view_indices = wp.zeros((self.count,), dtype=wp.uint32, device=self._device) + wp.launch( + kernel=fabric_utils.arange_k, dim=self.count, inputs=[self._default_view_indices], device=self._device + ) + wp.synchronize() + + fabric_device = self._device + if self._device == "cuda": + logger.warning("Fabric device is not specified, defaulting to 'cuda:0'.") + fabric_device = "cuda:0" + elif self._device.startswith("cuda:"): + if self._device != "cuda:0": + logger.debug( + f"SelectPrims only supports cuda:0. Using cuda:0 for SelectPrims " + f"even though simulation device is {self._device}." + ) + fabric_device = "cuda:0" + + self._fabric_selection = fabric_stage.SelectPrims( + require_attrs=[ + (usdrt.Sdf.ValueTypeNames.UInt, self._view_index_attr, usdrt.Usd.Access.Read), + (usdrt.Sdf.ValueTypeNames.Matrix4d, "omni:fabric:worldMatrix", usdrt.Usd.Access.ReadWrite), + ], + device=fabric_device, + ) + + self._view_to_fabric = wp.zeros((self.count,), dtype=wp.uint32, device=fabric_device) + self._fabric_to_view = wp.fabricarray(self._fabric_selection, self._view_index_attr) + + wp.launch( + kernel=fabric_utils.set_view_to_fabric_array, + dim=self._fabric_to_view.shape[0], + inputs=[self._fabric_to_view, self._view_to_fabric], + device=fabric_device, + ) + wp.synchronize() + + # Cached output buffers for the common no-indices path + self._fabric_positions_buf = wp.zeros((self.count, 3), dtype=wp.float32, device=self._device) + self._fabric_orientations_buf = wp.zeros((self.count, 4), dtype=wp.float32, device=self._device) + self._fabric_scales_buf = wp.zeros((self.count, 3), dtype=wp.float32, device=self._device) + self._fabric_dummy_buffer = wp.zeros((0, 3), dtype=wp.float32, device=self._device) + self._fabric_world_matrices = wp.fabricarray(self._fabric_selection, "omni:fabric:worldMatrix") + self._fabric_stage = fabric_stage + self._fabric_device = fabric_device + + self._fabric_initialized = True + self._fabric_usd_sync_done = False + + def _sync_fabric_from_usd_once(self) -> None: + """Sync Fabric world matrices from USD once, on the first read.""" + if not self._fabric_initialized: + self._initialize_fabric() + + # Read current USD poses/scales and push them into Fabric + positions_usd, orientations_usd = super().get_world_poses() + scales_usd = super().get_scales() + + prev_sync = self._sync_usd_on_fabric_write + self._sync_usd_on_fabric_write = False + self.set_world_poses(positions_usd, orientations_usd) + self.set_scales(scales_usd) + self._sync_usd_on_fabric_write = prev_sync + + self._fabric_usd_sync_done = True + + def _resolve_indices_wp(self, indices: Sequence[int] | None) -> wp.array: + """Resolve view indices as a Warp uint32 array.""" + if indices is None or indices == slice(None): + if self._default_view_indices is None: + raise RuntimeError("Fabric indices are not initialized.") + return self._default_view_indices + if isinstance(indices, torch.Tensor): + indices = indices.tolist() + return wp.array(list(indices), dtype=wp.uint32, device=self._device) diff --git a/source/isaaclab_physx/test/sim/__init__.py b/source/isaaclab_physx/test/sim/__init__.py new file mode 100644 index 000000000000..460a30569089 --- /dev/null +++ b/source/isaaclab_physx/test/sim/__init__.py @@ -0,0 +1,4 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause diff --git a/source/isaaclab_physx/test/sim/test_views_xform_prim_fabric.py b/source/isaaclab_physx/test/sim/test_views_xform_prim_fabric.py new file mode 100644 index 000000000000..4f757a69c2ff --- /dev/null +++ b/source/isaaclab_physx/test/sim/test_views_xform_prim_fabric.py @@ -0,0 +1,478 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Tests for PhysX XformPrimView with Fabric acceleration. + +Re-runs all the backend-parametrized tests from the core test suite with +``backend="fabric"`` and the PhysX :class:`XformPrimView`, plus the two +dedicated Fabric-only tests. +""" + +from isaaclab.app import AppLauncher + +simulation_app = AppLauncher(headless=True).app + +import pytest # noqa: E402 +import torch # noqa: E402 +import warp as wp # noqa: E402 +from isaaclab_physx.sim.views import XformPrimView # noqa: E402 + +import isaaclab.sim as sim_utils # noqa: E402 + +BACKEND = "fabric" + + +@pytest.fixture(autouse=True) +def test_setup_teardown(): + """Create a blank new stage for each test.""" + sim_utils.create_new_stage() + sim_utils.update_stage() + yield + sim_utils.clear_stage() + sim_utils.SimulationContext.clear_instance() + + +def _skip_if_unavailable(device: str): + if device.startswith("cuda") and not torch.cuda.is_available(): + pytest.skip("CUDA not available") + if device == "cpu": + pytest.skip("Warp fabricarray operations on CPU have known issues") + + +def _create_view(pattern: str, device: str) -> XformPrimView: + sim_utils.SimulationContext(sim_utils.SimulationCfg(dt=0.01, device=device, use_fabric=True)) + return XformPrimView(pattern, device=device) + + +""" +Tests - Getters. +""" + + +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +def test_get_world_poses(device): + _skip_if_unavailable(device) + stage = sim_utils.get_current_stage() + + expected_positions = [(1.0, 2.0, 3.0), (4.0, 5.0, 6.0), (7.0, 8.0, 9.0)] + expected_orientations = [(0.0, 0.0, 0.0, 1.0), (0.0, 0.0, 0.7071068, 0.7071068), (0.7071068, 0.0, 0.0, 0.7071068)] + + for i, (pos, quat) in enumerate(zip(expected_positions, expected_orientations)): + sim_utils.create_prim(f"/World/Object_{i}", "Camera", translation=pos, orientation=quat, stage=stage) + + view = _create_view("/World/Object_.*", device=device) + + expected_positions_tensor = torch.tensor(expected_positions, dtype=torch.float32, device=device) + expected_orientations_tensor = torch.tensor(expected_orientations, dtype=torch.float32, device=device) + + positions, orientations = view.get_world_poses() + positions, orientations = wp.to_torch(positions), wp.to_torch(orientations) + + assert positions.shape == (3, 3) + assert orientations.shape == (3, 4) + torch.testing.assert_close(positions, expected_positions_tensor, atol=1e-5, rtol=0) + + try: + torch.testing.assert_close(orientations, expected_orientations_tensor, atol=1e-5, rtol=0) + except AssertionError: + torch.testing.assert_close(orientations, -expected_orientations_tensor, atol=1e-5, rtol=0) + + +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +def test_get_local_poses(device): + _skip_if_unavailable(device) + stage = sim_utils.get_current_stage() + + sim_utils.create_prim("/World/Parent", "Xform", translation=(10.0, 0.0, 0.0), stage=stage) + + expected_local_positions = [(1.0, 0.0, 0.0), (0.0, 2.0, 0.0), (0.0, 0.0, 3.0)] + expected_local_orientations = [ + (0.0, 0.0, 0.0, 1.0), + (0.0, 0.0, 0.7071068, 0.7071068), + (0.7071068, 0.0, 0.0, 0.7071068), + ] + + for i, (pos, quat) in enumerate(zip(expected_local_positions, expected_local_orientations)): + sim_utils.create_prim(f"/World/Parent/Child_{i}", "Camera", translation=pos, orientation=quat, stage=stage) + + view = _create_view("/World/Parent/Child_.*", device=device) + translations, orientations = view.get_local_poses() + translations, orientations = wp.to_torch(translations), wp.to_torch(orientations) + + assert translations.shape == (3, 3) + assert orientations.shape == (3, 4) + + expected_translations_tensor = torch.tensor(expected_local_positions, dtype=torch.float32, device=device) + expected_orientations_tensor = torch.tensor(expected_local_orientations, dtype=torch.float32, device=device) + + torch.testing.assert_close(translations, expected_translations_tensor, atol=1e-5, rtol=0) + try: + torch.testing.assert_close(orientations, expected_orientations_tensor, atol=1e-5, rtol=0) + except AssertionError: + torch.testing.assert_close(orientations, -expected_orientations_tensor, atol=1e-5, rtol=0) + + +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +def test_get_scales(device): + _skip_if_unavailable(device) + stage = sim_utils.get_current_stage() + + expected_scales = [(1.0, 1.0, 1.0), (2.0, 2.0, 2.0), (1.0, 2.0, 3.0)] + for i, scale in enumerate(expected_scales): + sim_utils.create_prim(f"/World/Object_{i}", "Camera", scale=scale, stage=stage) + + view = _create_view("/World/Object_.*", device=device) + scales = wp.to_torch(view.get_scales()) + + assert scales.shape == (3, 3) + torch.testing.assert_close( + scales, torch.tensor(expected_scales, dtype=torch.float32, device=device), atol=1e-5, rtol=0 + ) + + +""" +Tests - Setters. +""" + + +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +def test_set_world_poses(device): + _skip_if_unavailable(device) + stage = sim_utils.get_current_stage() + + num_prims = 5 + for i in range(num_prims): + sim_utils.create_prim(f"/World/Object_{i}", "Camera", translation=(0.0, 0.0, 0.0), stage=stage) + + view = _create_view("/World/Object_.*", device=device) + + new_positions = torch.tensor( + [[1.0, 2.0, 3.0], [4.0, 5.0, 6.0], [7.0, 8.0, 9.0], [10.0, 11.0, 12.0], [13.0, 14.0, 15.0]], device=device + ) + new_orientations = torch.tensor( + [ + [0.0, 0.0, 0.0, 1.0], + [0.0, 0.0, 0.7071068, 0.7071068], + [0.7071068, 0.0, 0.0, 0.7071068], + [0.3826834, 0.0, 0.0, 0.9238795], + [0.0, 0.7071068, 0.0, 0.7071068], + ], + device=device, + ) + + view.set_world_poses(new_positions, new_orientations) + retrieved_positions, retrieved_orientations = view.get_world_poses() + retrieved_positions, retrieved_orientations = wp.to_torch(retrieved_positions), wp.to_torch(retrieved_orientations) + + torch.testing.assert_close(retrieved_positions, new_positions, atol=1e-5, rtol=0) + try: + torch.testing.assert_close(retrieved_orientations, new_orientations, atol=1e-5, rtol=0) + except AssertionError: + torch.testing.assert_close(retrieved_orientations, -new_orientations, atol=1e-5, rtol=0) + + +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +def test_set_world_poses_only_positions(device): + _skip_if_unavailable(device) + stage = sim_utils.get_current_stage() + + initial_quat = (0.0, 0.0, 0.7071068, 0.7071068) + for i in range(3): + sim_utils.create_prim( + f"/World/Object_{i}", "Camera", translation=(0.0, 0.0, 0.0), orientation=initial_quat, stage=stage + ) + + view = _create_view("/World/Object_.*", device=device) + _, initial_orientations = view.get_world_poses() + initial_orientations = wp.to_torch(initial_orientations) + + new_positions = torch.tensor([[1.0, 0.0, 0.0], [0.0, 2.0, 0.0], [0.0, 0.0, 3.0]], device=device) + view.set_world_poses(positions=new_positions, orientations=None) + + retrieved_positions, retrieved_orientations = view.get_world_poses() + retrieved_positions, retrieved_orientations = wp.to_torch(retrieved_positions), wp.to_torch(retrieved_orientations) + torch.testing.assert_close(retrieved_positions, new_positions, atol=1e-5, rtol=0) + try: + torch.testing.assert_close(retrieved_orientations, initial_orientations, atol=1e-5, rtol=0) + except AssertionError: + torch.testing.assert_close(retrieved_orientations, -initial_orientations, atol=1e-5, rtol=0) + + +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +def test_set_world_poses_only_orientations(device): + _skip_if_unavailable(device) + stage = sim_utils.get_current_stage() + + for i in range(3): + sim_utils.create_prim(f"/World/Object_{i}", "Camera", translation=(float(i), 0.0, 0.0), stage=stage) + + view = _create_view("/World/Object_.*", device=device) + initial_positions = wp.to_torch(view.get_world_poses()[0]) + + new_orientations = torch.tensor( + [[0.0, 0.0, 0.7071068, 0.7071068], [0.7071068, 0.0, 0.0, 0.7071068], [0.3826834, 0.0, 0.0, 0.9238795]], + device=device, + ) + view.set_world_poses(positions=None, orientations=new_orientations) + + retrieved_positions, retrieved_orientations = view.get_world_poses() + retrieved_positions, retrieved_orientations = wp.to_torch(retrieved_positions), wp.to_torch(retrieved_orientations) + torch.testing.assert_close(retrieved_positions, initial_positions, atol=1e-5, rtol=0) + try: + torch.testing.assert_close(retrieved_orientations, new_orientations, atol=1e-5, rtol=0) + except AssertionError: + torch.testing.assert_close(retrieved_orientations, -new_orientations, atol=1e-5, rtol=0) + + +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +def test_set_world_poses_with_hierarchy(device): + _skip_if_unavailable(device) + stage = sim_utils.get_current_stage() + + for i in range(3): + parent_pos = (i * 10.0, 0.0, 0.0) + parent_quat = (0.0, 0.0, 0.7071068, 0.7071068) + sim_utils.create_prim( + f"/World/Parent_{i}", "Xform", translation=parent_pos, orientation=parent_quat, stage=stage + ) + sim_utils.create_prim(f"/World/Parent_{i}/Child", "Camera", translation=(0.0, 0.0, 0.0), stage=stage) + + view = _create_view("/World/Parent_.*/Child", device=device) + + desired_world_positions = torch.tensor([[5.0, 5.0, 0.0], [15.0, 5.0, 0.0], [25.0, 5.0, 0.0]], device=device) + desired_world_orientations = torch.tensor([[0.0, 0.0, 0.0, 1.0]] * 3, device=device) + + view.set_world_poses(desired_world_positions, desired_world_orientations) + retrieved_positions, retrieved_orientations = view.get_world_poses() + retrieved_positions, retrieved_orientations = wp.to_torch(retrieved_positions), wp.to_torch(retrieved_orientations) + + torch.testing.assert_close(retrieved_positions, desired_world_positions, atol=1e-4, rtol=0) + try: + torch.testing.assert_close(retrieved_orientations, desired_world_orientations, atol=1e-4, rtol=0) + except AssertionError: + torch.testing.assert_close(retrieved_orientations, -desired_world_orientations, atol=1e-4, rtol=0) + + +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +def test_set_local_poses(device): + _skip_if_unavailable(device) + stage = sim_utils.get_current_stage() + + sim_utils.create_prim("/World/Parent", "Xform", translation=(5.0, 5.0, 5.0), stage=stage) + num_prims = 4 + for i in range(num_prims): + sim_utils.create_prim(f"/World/Parent/Child_{i}", "Camera", translation=(0.0, 0.0, 0.0), stage=stage) + + view = _create_view("/World/Parent/Child_.*", device=device) + + new_translations = torch.tensor([[1.0, 0.0, 0.0], [0.0, 2.0, 0.0], [0.0, 0.0, 3.0], [4.0, 4.0, 4.0]], device=device) + new_orientations = torch.tensor( + [ + [0.0, 0.0, 0.0, 1.0], + [0.0, 0.0, 0.7071068, 0.7071068], + [0.7071068, 0.0, 0.0, 0.7071068], + [0.3826834, 0.0, 0.0, 0.9238795], + ], + device=device, + ) + + view.set_local_poses(new_translations, new_orientations) + retrieved_translations, retrieved_orientations = view.get_local_poses() + retrieved_translations, retrieved_orientations = ( + wp.to_torch(retrieved_translations), + wp.to_torch(retrieved_orientations), + ) + + torch.testing.assert_close(retrieved_translations, new_translations, atol=1e-5, rtol=0) + try: + torch.testing.assert_close(retrieved_orientations, new_orientations, atol=1e-5, rtol=0) + except AssertionError: + torch.testing.assert_close(retrieved_orientations, -new_orientations, atol=1e-5, rtol=0) + + +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +def test_set_local_poses_only_translations(device): + _skip_if_unavailable(device) + stage = sim_utils.get_current_stage() + + sim_utils.create_prim("/World/Parent", "Xform", translation=(0.0, 0.0, 0.0), stage=stage) + initial_quat = (0.0, 0.0, 0.7071068, 0.7071068) + for i in range(3): + sim_utils.create_prim( + f"/World/Parent/Child_{i}", "Camera", translation=(0.0, 0.0, 0.0), orientation=initial_quat, stage=stage + ) + + view = _create_view("/World/Parent/Child_.*", device=device) + initial_orientations = wp.to_torch(view.get_local_poses()[1]) + + new_translations = torch.tensor([[1.0, 0.0, 0.0], [0.0, 2.0, 0.0], [0.0, 0.0, 3.0]], device=device) + view.set_local_poses(translations=new_translations, orientations=None) + + retrieved_translations, retrieved_orientations = view.get_local_poses() + retrieved_translations, retrieved_orientations = ( + wp.to_torch(retrieved_translations), + wp.to_torch(retrieved_orientations), + ) + torch.testing.assert_close(retrieved_translations, new_translations, atol=1e-5, rtol=0) + try: + torch.testing.assert_close(retrieved_orientations, initial_orientations, atol=1e-5, rtol=0) + except AssertionError: + torch.testing.assert_close(retrieved_orientations, -initial_orientations, atol=1e-5, rtol=0) + + +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +def test_set_scales(device): + _skip_if_unavailable(device) + stage = sim_utils.get_current_stage() + + num_prims = 5 + for i in range(num_prims): + sim_utils.create_prim(f"/World/Object_{i}", "Camera", scale=(1.0, 1.0, 1.0), stage=stage) + + view = _create_view("/World/Object_.*", device=device) + new_scales = torch.tensor( + [[2.0, 2.0, 2.0], [1.0, 2.0, 3.0], [0.5, 0.5, 0.5], [3.0, 1.0, 2.0], [1.5, 1.5, 1.5]], device=device + ) + view.set_scales(new_scales) + + retrieved_scales = wp.to_torch(view.get_scales()) + torch.testing.assert_close(retrieved_scales, new_scales, atol=1e-5, rtol=0) + + +""" +Tests - Indices. +""" + + +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +def test_indices_single_element(device): + _skip_if_unavailable(device) + stage = sim_utils.get_current_stage() + + num_prims = 5 + for i in range(num_prims): + sim_utils.create_prim(f"/World/Object_{i}", "Camera", translation=(float(i), 0.0, 0.0), stage=stage) + + view = _create_view("/World/Object_.*", device=device) + + indices = [3] + positions, orientations = view.get_world_poses(indices=indices) + positions, orientations = wp.to_torch(positions), wp.to_torch(orientations) + assert positions.shape == (1, 3) + assert orientations.shape == (1, 4) + + new_position = torch.tensor([[100.0, 200.0, 300.0]], device=device) + view.set_world_poses(positions=new_position, indices=indices) + + retrieved_positions = wp.to_torch(view.get_world_poses(indices=indices)[0]) + torch.testing.assert_close(retrieved_positions, new_position, atol=1e-5, rtol=0) + + +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +def test_indices_out_of_order(device): + _skip_if_unavailable(device) + stage = sim_utils.get_current_stage() + + num_prims = 10 + for i in range(num_prims): + sim_utils.create_prim(f"/World/Object_{i}", "Camera", translation=(0.0, 0.0, 0.0), stage=stage) + + view = _create_view("/World/Object_.*", device=device) + + indices = [7, 2, 9, 0, 5] + new_positions = torch.tensor( + [[7.0, 0.0, 0.0], [2.0, 0.0, 0.0], [9.0, 0.0, 0.0], [0.0, 0.0, 0.0], [5.0, 0.0, 0.0]], device=device + ) + view.set_world_poses(positions=new_positions, indices=indices) + + all_positions = wp.to_torch(view.get_world_poses()[0]) + + expected_x_values = [0.0, 0.0, 2.0, 0.0, 0.0, 5.0, 0.0, 7.0, 0.0, 9.0] + for i in range(num_prims): + assert abs(all_positions[i, 0].item() - expected_x_values[i]) < 1e-5 + + +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +def test_indices_with_only_positions_or_orientations(device): + _skip_if_unavailable(device) + stage = sim_utils.get_current_stage() + + num_prims = 5 + for i in range(num_prims): + sim_utils.create_prim( + f"/World/Object_{i}", "Camera", translation=(0.0, 0.0, 0.0), orientation=(0.0, 0.0, 0.0, 1.0), stage=stage + ) + + view = _create_view("/World/Object_.*", device=device) + initial_positions = wp.to_torch(view.get_world_poses()[0]).clone() + + indices = [1, 3] + new_positions = torch.tensor([[10.0, 0.0, 0.0], [30.0, 0.0, 0.0]], device=device) + view.set_world_poses(positions=new_positions, orientations=None, indices=indices) + + updated_positions = wp.to_torch(view.get_world_poses()[0]) + + torch.testing.assert_close(updated_positions[1], new_positions[0], atol=1e-5, rtol=0) + torch.testing.assert_close(updated_positions[3], new_positions[1], atol=1e-5, rtol=0) + torch.testing.assert_close(updated_positions[0], initial_positions[0], atol=1e-5, rtol=0) + + +""" +Tests - Fabric Operations. +""" + + +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +def test_fabric_initialization(device): + _skip_if_unavailable(device) + stage = sim_utils.get_current_stage() + + num_prims = 5 + for i in range(num_prims): + sim_utils.create_prim(f"/World/Cam_{i}", "Camera", translation=(i * 1.0, 0.0, 1.0), stage=stage) + + view = _create_view("/World/Cam_.*", device=device) + + assert view.count == num_prims + assert view.device == device + assert len(view.prims) == num_prims + + +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +def test_fabric_usd_consistency(device): + _skip_if_unavailable(device) + stage = sim_utils.get_current_stage() + + num_prims = 5 + for i in range(num_prims): + sim_utils.create_prim( + f"/World/Cam_{i}", + "Camera", + translation=(i * 1.0, 2.0, 3.0), + orientation=(0.0, 0.0, 0.7071068, 0.7071068), + stage=stage, + ) + + view = _create_view("/World/Cam_.*", device=device) + + init_positions = torch.zeros((num_prims, 3), dtype=torch.float32, device=device) + init_positions[:, 0] = torch.arange(num_prims, dtype=torch.float32, device=device) + init_positions[:, 1] = 2.0 + init_positions[:, 2] = 3.0 + init_orientations = torch.tensor([[0.0, 0.0, 0.7071068, 0.7071068]] * num_prims, dtype=torch.float32, device=device) + + view.set_world_poses(init_positions, init_orientations) + + pos, quat = view.get_world_poses() + torch.testing.assert_close(wp.to_torch(pos), init_positions, atol=1e-4, rtol=0) + torch.testing.assert_close(wp.to_torch(quat), init_orientations, atol=1e-4, rtol=0) + + new_positions = torch.rand((num_prims, 3), dtype=torch.float32, device=device) * 10.0 + new_orientations = torch.tensor([[0.0, 0.0, 0.0, 1.0]] * num_prims, dtype=torch.float32, device=device) + view.set_world_poses(new_positions, new_orientations) + + pos2, quat2 = view.get_world_poses() + torch.testing.assert_close(wp.to_torch(pos2), new_positions, atol=1e-4, rtol=0) + torch.testing.assert_close(wp.to_torch(quat2), new_orientations, atol=1e-4, rtol=0) From f0c3ca0a65fa94d1c13990f4be575a50e2adbfe4 Mon Sep 17 00:00:00 2001 From: Octi Zhang Date: Mon, 6 Apr 2026 01:30:36 -0700 Subject: [PATCH 02/24] update caller to use warp xformprimview --- .../isaaclab/isaaclab/sensors/camera/camera.py | 16 +++++++++------- .../sensors/ray_caster/multi_mesh_ray_caster.py | 4 ++-- .../sensors/ray_caster/ray_cast_utils.py | 10 ++++++---- .../isaaclab/sensors/ray_caster/ray_caster.py | 4 ++-- .../locomanipulation_sdg/scene_utils.py | 6 ++++-- .../physx_scene_data_provider.py | 8 ++++---- 6 files changed, 27 insertions(+), 21 deletions(-) diff --git a/source/isaaclab/isaaclab/sensors/camera/camera.py b/source/isaaclab/isaaclab/sensors/camera/camera.py index eb588489f729..65efe4a375cb 100644 --- a/source/isaaclab/isaaclab/sensors/camera/camera.py +++ b/source/isaaclab/isaaclab/sensors/camera/camera.py @@ -335,8 +335,10 @@ def set_world_poses( elif not isinstance(orientations, torch.Tensor): orientations = torch.tensor(orientations, device=self._device) orientations = convert_camera_frame_orientation_convention(orientations, origin=convention, target="opengl") - # set the pose - self._view.set_world_poses(positions, orientations, env_ids) + # convert torch tensors to warp arrays for the view + pos_wp = wp.from_torch(positions.contiguous()) if positions is not None else None + ori_wp = wp.from_torch(orientations.contiguous()) if orientations is not None else None + self._view.set_world_poses(pos_wp, ori_wp, env_ids) def set_world_poses_from_view( self, eyes: torch.Tensor, targets: torch.Tensor, env_ids: Sequence[int] | None = None @@ -359,7 +361,7 @@ def set_world_poses_from_view( up_axis = UsdGeom.GetStageUpAxis(self.stage) # set camera poses using the view orientations = quat_from_matrix(create_rotation_matrix_from_view(eyes, targets, up_axis, device=self._device)) - self._view.set_world_poses(eyes, orientations, env_ids) + self._view.set_world_poses(wp.from_torch(eyes.contiguous()), wp.from_torch(orientations.contiguous()), env_ids) """ Operations @@ -612,11 +614,11 @@ def _update_poses(self, env_ids: Sequence[int]): if len(self._sensor_prims) == 0: raise RuntimeError("Camera prim is None. Please call 'sim.play()' first.") - # get the poses from the view - poses, quat = self._view.get_world_poses(env_ids) - self._data.pos_w[env_ids] = poses + # get the poses from the view (returns wp.array, convert to torch) + pos_wp, quat_wp = self._view.get_world_poses(env_ids) + self._data.pos_w[env_ids] = wp.to_torch(pos_wp) self._data.quat_w_world[env_ids] = convert_camera_frame_orientation_convention( - quat, origin="opengl", target="world" + wp.to_torch(quat_wp), origin="opengl", target="world" ) # notify renderer of updated poses (guarded in case called before initialization completes) if self._render_data is not None: diff --git a/source/isaaclab/isaaclab/sensors/ray_caster/multi_mesh_ray_caster.py b/source/isaaclab/isaaclab/sensors/ray_caster/multi_mesh_ray_caster.py index 06ce2183e2ff..564b358c8609 100644 --- a/source/isaaclab/isaaclab/sensors/ray_caster/multi_mesh_ray_caster.py +++ b/source/isaaclab/isaaclab/sensors/ray_caster/multi_mesh_ray_caster.py @@ -17,7 +17,7 @@ import omni.physics.tensors.impl.api as physx import isaaclab.sim as sim_utils -from isaaclab.sim.views import XformPrimView +from isaaclab.sim.views import BaseXformPrimView from isaaclab.utils.math import matrix_from_quat, quat_mul from isaaclab.utils.mesh import PRIMITIVE_MESH_TYPES, create_trimesh_from_geom_mesh, create_trimesh_from_geom_shape from isaaclab.utils.warp import convert_to_warp_mesh, raycast_dynamic_meshes @@ -78,7 +78,7 @@ class MultiMeshRayCaster(RayCaster): mesh_offsets: dict[str, tuple[torch.Tensor, torch.Tensor]] = {} - mesh_views: ClassVar[dict[str, XformPrimView | physx.ArticulationView | physx.RigidBodyView]] = {} + mesh_views: ClassVar[dict[str, BaseXformPrimView | physx.ArticulationView | physx.RigidBodyView]] = {} """A dictionary to store mesh views for raycasting, shared across all instances. The keys correspond to the prim path for the mesh views, and values are the corresponding view objects. diff --git a/source/isaaclab/isaaclab/sensors/ray_caster/ray_cast_utils.py b/source/isaaclab/isaaclab/sensors/ray_caster/ray_cast_utils.py index ac503b28bf52..d3c5aacf58fa 100644 --- a/source/isaaclab/isaaclab/sensors/ray_caster/ray_cast_utils.py +++ b/source/isaaclab/isaaclab/sensors/ray_caster/ray_cast_utils.py @@ -12,11 +12,11 @@ import omni.physics.tensors.impl.api as physx -from isaaclab.sim.views import XformPrimView +from isaaclab.sim.views import BaseXformPrimView def obtain_world_pose_from_view( - physx_view: XformPrimView | physx.ArticulationView | physx.RigidBodyView, + physx_view: BaseXformPrimView | physx.ArticulationView | physx.RigidBodyView, env_ids: torch.Tensor, clone: bool = False, ) -> tuple[torch.Tensor, torch.Tensor]: @@ -34,8 +34,10 @@ def obtain_world_pose_from_view( Raises: NotImplementedError: If the prim view is not of the supported type. """ - if isinstance(physx_view, XformPrimView): - pos_w, quat_w = physx_view.get_world_poses(env_ids) + if isinstance(physx_view, BaseXformPrimView): + pos_wp, quat_wp = physx_view.get_world_poses(env_ids) + pos_w = wp.to_torch(pos_wp) + quat_w = wp.to_torch(quat_wp) elif isinstance(physx_view, physx.ArticulationView): pos_w, quat_w = wp.to_torch(physx_view.get_root_transforms())[env_ids].split([3, 4], dim=-1) elif isinstance(physx_view, physx.RigidBodyView): diff --git a/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster.py b/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster.py index 731d57f1638f..351154b78126 100644 --- a/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster.py +++ b/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster.py @@ -18,7 +18,7 @@ import isaaclab.sim as sim_utils import isaaclab.utils.math as math_utils from isaaclab.markers import VisualizationMarkers -from isaaclab.sim.views import XformPrimView +from isaaclab.sim.views import BaseXformPrimView, XformPrimView from isaaclab.terrains.trimesh.utils import make_plane from isaaclab.utils.math import quat_apply, quat_apply_yaw from isaaclab.utils.warp import convert_to_warp_mesh, raycast_mesh @@ -334,7 +334,7 @@ def _debug_vis_callback(self, event): def _obtain_trackable_prim_view( self, target_prim_path: str - ) -> tuple[XformPrimView | any, tuple[torch.Tensor, torch.Tensor]]: + ) -> tuple[BaseXformPrimView | any, tuple[torch.Tensor, torch.Tensor]]: """Obtain a prim view that can be used to track the pose of the parget prim. The target prim path is a regex expression that matches one or more mesh prims. While we can track its diff --git a/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/scene_utils.py b/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/scene_utils.py index 79cf307c9cb3..a3b922300468 100644 --- a/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/scene_utils.py +++ b/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/scene_utils.py @@ -113,7 +113,9 @@ def _get_xform_view(self) -> XformPrimView: def get_pose(self): """Get the 3D pose of the entity.""" xform_prim = self._get_xform_view() - position, orientation = xform_prim.get_world_poses() + pos_wp, ori_wp = xform_prim.get_world_poses() + position = wp.to_torch(pos_wp) + orientation = wp.to_torch(ori_wp) pose = torch.cat([position, orientation], dim=-1) return pose @@ -122,7 +124,7 @@ def set_pose(self, pose: torch.Tensor): xform_prim = self._get_xform_view() position = pose[..., :3] orientation = pose[..., 3:] - xform_prim.set_world_poses(position, orientation, None) + xform_prim.set_world_poses(wp.from_torch(position.contiguous()), wp.from_torch(orientation.contiguous()), None) class RelativePose(HasPose): diff --git a/source/isaaclab_physx/isaaclab_physx/scene_data_providers/physx_scene_data_provider.py b/source/isaaclab_physx/isaaclab_physx/scene_data_providers/physx_scene_data_provider.py index 19aa1eb34986..995fd0708ff0 100644 --- a/source/isaaclab_physx/isaaclab_physx/scene_data_providers/physx_scene_data_provider.py +++ b/source/isaaclab_physx/isaaclab_physx/scene_data_providers/physx_scene_data_provider.py @@ -569,10 +569,10 @@ def _apply_xform_poses(self, positions: Any, orientations: Any, covered: Any, xf path, device=self._device, stage=self._stage, validate_xform_ops=False ) - pos, quat = self._xform_views[path].get_world_poses() - if pos is not None and quat is not None: - positions[idx] = pos.to(device=self._device, dtype=torch.float32).squeeze() - orientations[idx] = quat.to(device=self._device, dtype=torch.float32).squeeze() + pos_wp, quat_wp = self._xform_views[path].get_world_poses() + if pos_wp is not None and quat_wp is not None: + positions[idx] = wp.to_torch(pos_wp).to(device=self._device, dtype=torch.float32).squeeze() + orientations[idx] = wp.to_torch(quat_wp).to(device=self._device, dtype=torch.float32).squeeze() covered[idx] = True xform_mask[idx] = True count += 1 From 67cb049a43c45c0812269cf2e760a47d63586bba Mon Sep 17 00:00:00 2001 From: Octi Zhang Date: Thu, 9 Apr 2026 16:30:06 -0700 Subject: [PATCH 03/24] improvements --- .../benchmark_newton_xform_prim_view.py | 144 -- .../benchmarks/benchmark_view_comparison.py | 629 ++++--- .../benchmarks/benchmark_xform_prim_view.py | 811 +++------ .../test/sim/test_views_xform_prim.py | 1493 ++--------------- .../isaaclab/test/sim/xform_contract_tests.py | 361 ++++ .../sim/views/xform_prim_view.py | 471 ++++-- .../test/sim/test_views_xform_prim_newton.py | 458 ++--- .../sim/views/xform_prim_view.py | 36 +- .../test/sim/test_views_xform_prim_fabric.py | 497 +----- 9 files changed, 1625 insertions(+), 3275 deletions(-) delete mode 100644 scripts/benchmarks/benchmark_newton_xform_prim_view.py create mode 100644 source/isaaclab/test/sim/xform_contract_tests.py diff --git a/scripts/benchmarks/benchmark_newton_xform_prim_view.py b/scripts/benchmarks/benchmark_newton_xform_prim_view.py deleted file mode 100644 index 95b0a4773bcf..000000000000 --- a/scripts/benchmarks/benchmark_newton_xform_prim_view.py +++ /dev/null @@ -1,144 +0,0 @@ -# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). -# All rights reserved. -# -# SPDX-License-Identifier: BSD-3-Clause - -"""Benchmark script for Newton XformPrimView performance. - -Tests the performance of batched transform operations using Newton's GPU-backed -XformPrimView (sites / body_q) without requiring Isaac Sim Kit. - -Usage: - VIRTUAL_ENV=env_isaaclab ./isaaclab.sh -p scripts/benchmarks/benchmark_newton_xform_prim_view.py --num_envs 4096 - VIRTUAL_ENV=env_isaaclab ./isaaclab.sh -p scripts/benchmarks/benchmark_newton_xform_prim_view.py \ - --num_envs 4096 --num_iterations 200 -""" - -from __future__ import annotations - -import argparse -import time - -import torch -from isaaclab_newton.physics import MJWarpSolverCfg, NewtonCfg -from isaaclab_newton.sim.views import XformPrimView - -import isaaclab.sim as sim_utils -from isaaclab.assets import RigidObjectCfg -from isaaclab.scene import InteractiveScene, InteractiveSceneCfg -from isaaclab.sim import SimulationCfg, build_simulation_context -from isaaclab.utils import configclass - -NEWTON_SIM_CFG = SimulationCfg( - physics=NewtonCfg(solver_cfg=MJWarpSolverCfg()), -) - - -@configclass -class BenchSceneCfg(InteractiveSceneCfg): - cube: RigidObjectCfg = RigidObjectCfg( - prim_path="{ENV_REGEX_NS}/Cube", - spawn=sim_utils.CuboidCfg( - size=(0.2, 0.2, 0.2), - rigid_props=sim_utils.RigidBodyPropertiesCfg(), - mass_props=sim_utils.MassPropertiesCfg(mass=1.0), - collision_props=sim_utils.CollisionPropertiesCfg(), - ), - init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, 0.0, 1.0)), - ) - - -@torch.no_grad() -def benchmark(num_envs: int, num_iterations: int, device: str) -> dict[str, float]: - """Run the benchmark and return timing results in seconds.""" - NEWTON_SIM_CFG.device = device - results: dict[str, float] = {} - - with build_simulation_context(device=device, sim_cfg=NEWTON_SIM_CFG, add_ground_plane=True) as sim: - sim._app_control_on_stop_handle = None - InteractiveScene(BenchSceneCfg(num_envs=num_envs, env_spacing=2.0)) - sim.reset() - - view = XformPrimView("/World/envs/env_.*/Cube", device=device) - print(f" View count: {view.count}") - - # -- warmup (compile Warp kernel, first torch allocs) -- - for _ in range(5): - view.get_world_poses() - - # -- get_world_poses (full) -- - torch.cuda.synchronize() - t0 = time.perf_counter() - for _ in range(num_iterations): - pos, quat = view.get_world_poses() - torch.cuda.synchronize() - results["get_world_poses"] = (time.perf_counter() - t0) / num_iterations - - # -- get_world_poses (indexed, 50 %) -- - half = list(range(0, num_envs, 2)) - torch.cuda.synchronize() - t0 = time.perf_counter() - for _ in range(num_iterations): - pos, quat = view.get_world_poses(half) - torch.cuda.synchronize() - results["get_world_poses_indexed_50pct"] = (time.perf_counter() - t0) / num_iterations - - # -- set_world_poses (full) -- - new_pos = torch.rand((num_envs, 3), device=device) - new_quat = torch.tensor([[0.0, 0.0, 0.0, 1.0]] * num_envs, device=device) - torch.cuda.synchronize() - t0 = time.perf_counter() - for _ in range(num_iterations): - view.set_world_poses(new_pos, new_quat) - torch.cuda.synchronize() - results["set_world_poses"] = (time.perf_counter() - t0) / num_iterations - - # -- interleaved set -> get -- - torch.cuda.synchronize() - t0 = time.perf_counter() - for _ in range(num_iterations): - view.set_world_poses(new_pos, new_quat) - pos, quat = view.get_world_poses() - torch.cuda.synchronize() - results["interleaved_set_get"] = (time.perf_counter() - t0) / num_iterations - - return results - - -def print_results(results: dict[str, float], num_envs: int, num_iterations: int): - """Print benchmark results in a formatted table.""" - print("\n" + "=" * 70) - print(f"Newton XformPrimView Benchmark: {num_envs} envs, {num_iterations} iters") - print("=" * 70) - print(f"{'Operation':<40} {'Time (ms)':>12} {'us/env':>12}") - print("-" * 70) - for op, t in results.items(): - ms = t * 1000 - us_per_env = t * 1e6 / num_envs - print(f"{op:<40} {ms:>12.4f} {us_per_env:>12.4f}") - total = sum(results.values()) * 1000 - print("-" * 70) - print(f"{'Total':<40} {total:>12.4f}") - print("=" * 70) - print() - - -def main(): - parser = argparse.ArgumentParser(description="Benchmark Newton XformPrimView performance.") - parser.add_argument("--num_envs", type=int, default=4096) - parser.add_argument("--num_iterations", type=int, default=100) - parser.add_argument("--device", type=str, default="cuda:0") - args = parser.parse_args() - - print("=" * 70) - print("Newton XformPrimView Benchmark") - print("=" * 70) - print(f" Envs: {args.num_envs} Iterations: {args.num_iterations} Device: {args.device}") - print() - - results = benchmark(args.num_envs, args.num_iterations, args.device) - print_results(results, args.num_envs, args.num_iterations) - - -if __name__ == "__main__": - main() diff --git a/scripts/benchmarks/benchmark_view_comparison.py b/scripts/benchmarks/benchmark_view_comparison.py index 8f2b60c49077..9b2d1f09d557 100644 --- a/scripts/benchmarks/benchmark_view_comparison.py +++ b/scripts/benchmarks/benchmark_view_comparison.py @@ -3,54 +3,50 @@ # # SPDX-License-Identifier: BSD-3-Clause -"""Benchmark script comparing XformPrimView vs PhysX RigidBodyView for transform operations. +"""Benchmark script comparing XformPrimView backends and PhysX RigidBodyView. -This script tests the performance of batched transform operations using: +Compares batched transform operation performance across: -- Isaac Lab's XformPrimView (USD-based) -- Isaac Lab's XformPrimView (Fabric-based) -- PhysX RigidBodyView (PhysX tensors-based, as used in RigidObject) - -Note: - XformPrimView operates on USD attributes directly (useful for non-physics prims), - or on Fabric attributes when Fabric is enabled. - while RigidBodyView requires rigid body physics components and operates on PhysX tensors. - This benchmark helps understand the performance trade-offs between the two approaches. +- **USD** (baseline): Isaac Lab's XformPrimView via USD XformCache +- **Fabric**: Isaac Lab's XformPrimView via Fabric GPU arrays +- **Newton**: Isaac Lab's Newton XformPrimView via Warp site kernels +- **PhysX**: PhysX RigidBodyView via PhysX tensor API (reference) Usage: - # Basic benchmark + # All backends ./isaaclab.sh -p scripts/benchmarks/benchmark_view_comparison.py --num_envs 1024 --device cuda:0 --headless - # With profiling enabled (for snakeviz visualization) - ./isaaclab.sh -p scripts/benchmarks/benchmark_view_comparison.py --num_envs 1024 --profile --headless + # Select specific backends + ./isaaclab.sh -p scripts/benchmarks/benchmark_view_comparison.py --backends usd fabric newton --headless - # Then visualize with snakeviz: - snakeviz profile_results/xform_view_benchmark.prof - snakeviz profile_results/physx_view_benchmark.prof + # With profiling + ./isaaclab.sh -p scripts/benchmarks/benchmark_view_comparison.py --num_envs 1024 --profile --headless """ from __future__ import annotations -"""Launch Isaac Sim Simulator first.""" - import argparse from isaaclab.app import AppLauncher -# parse the arguments -args_cli = argparse.Namespace() - -parser = argparse.ArgumentParser(description="Benchmark XformPrimView vs PhysX RigidBodyView performance.") +parser = argparse.ArgumentParser(description="Benchmark XformPrimView backends and PhysX RigidBodyView.") parser.add_argument("--num_envs", type=int, default=1000, help="Number of environments to simulate.") parser.add_argument("--num_iterations", type=int, default=50, help="Number of iterations for each test.") +parser.add_argument( + "--backends", + nargs="+", + default=["usd", "fabric", "newton", "physx"], + choices=["usd", "fabric", "newton", "physx"], + help="Backends to benchmark. Default: all four.", +) parser.add_argument( "--profile", action="store_true", help="Enable profiling with cProfile. Results saved as .prof files for snakeviz visualization.", ) parser.add_argument( - "--profile-dir", + "--profile_dir", type=str, default="./profile_results", help="Directory to save profile results. Default: ./profile_results", @@ -59,7 +55,6 @@ AppLauncher.add_app_launcher_args(parser) args_cli = parser.parse_args() -# launch omniverse app app_launcher = AppLauncher(args_cli) simulation_app = app_launcher.app @@ -69,40 +64,41 @@ import time import torch +import warp as wp + +from pxr import Gf import isaaclab.sim as sim_utils from isaaclab.sim.views import XformPrimView +try: + from isaaclab_newton.physics import MJWarpSolverCfg, NewtonCfg + from isaaclab_newton.sim.views import XformPrimView as NewtonXformPrimView + + HAS_NEWTON = True +except ImportError: + HAS_NEWTON = False + + +# ------------------------------------------------------------------ +# Benchmark functions +# ------------------------------------------------------------------ + @torch.no_grad() -def benchmark_view(view_type: str, num_iterations: int) -> tuple[dict[str, float], dict[str, torch.Tensor]]: - """Benchmark the specified view class. - - Args: - view_type: Type of view to benchmark ("xform", "xform_fabric", or "physx"). - num_iterations: Number of iterations to run. - - Returns: - A tuple of (timing_results, computed_results) where: - - timing_results: Dictionary containing timing results for various operations - - computed_results: Dictionary containing the computed values for validation - """ +def benchmark_usd_or_fabric(view_type: str, num_iterations: int) -> tuple[dict[str, float], dict[str, torch.Tensor]]: + """Benchmark USD or Fabric XformPrimView.""" timing_results = {} computed_results = {} - # Setup scene print(" Setting up scene") - # Clear stage sim_utils.create_new_stage() - # Create simulation context start_time = time.perf_counter() - sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device, use_fabric=(view_type == "xform_fabric")) + sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device, use_fabric=(view_type == "fabric")) sim = sim_utils.SimulationContext(sim_cfg) stage = sim_utils.get_current_stage() + print(f" SimulationContext: {time.perf_counter() - start_time:.4f}s") - print(f" Time taken to create simulation context: {time.perf_counter() - start_time:.4f} seconds") - - # create a rigid object object_cfg = sim_utils.ConeCfg( radius=0.15, height=0.5, @@ -111,110 +107,210 @@ def benchmark_view(view_type: str, num_iterations: int) -> tuple[dict[str, float collision_props=sim_utils.CollisionPropertiesCfg(), visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.0, 1.0, 0.0)), ) - # Create prims for i in range(args_cli.num_envs): sim_utils.create_prim(f"/World/Env_{i}", "Xform", stage=stage, translation=(i * 2.0, 0.0, 0.0)) object_cfg.func(f"/World/Env_{i}/Object", object_cfg, translation=(0.0, 0.0, 1.0)) + prim = stage.DefinePrim(f"/World/Env_{i}/Object/Sensor", "Xform") + sim_utils.standardize_xform_ops(prim) + prim.GetAttribute("xformOp:translate").Set(Gf.Vec3d(0.1, 0.0, 0.05)) + prim.GetAttribute("xformOp:orient").Set(Gf.Quatd(1.0, 0.0, 0.0, 0.0)) - # Play simulation sim.reset() - # Pattern to match all prims - pattern = "/World/Env_.*/Object" if view_type == "xform" else "/World/Env_*/Object" - print(f" Pattern: {pattern}") + pattern = "/World/Env_.*/Object/Sensor" - # Create view based on type start_time = time.perf_counter() - if view_type == "xform": - view = XformPrimView(pattern, device=args_cli.device, validate_xform_ops=False) - num_prims = view.count - view_name = "XformPrimView (USD)" - elif view_type == "xform_fabric": - if "cuda" not in args_cli.device: - raise ValueError("Fabric backend requires CUDA. Please use --device cuda:0 for this benchmark.") - view = XformPrimView(pattern, device=args_cli.device, validate_xform_ops=False) - num_prims = view.count - view_name = "XformPrimView (Fabric)" - else: # physx - physics_sim_view = sim.physics_manager.get_physics_sim_view() - view = physics_sim_view.create_rigid_body_view(pattern) - num_prims = view.count - view_name = "PhysX RigidBodyView" + if view_type == "fabric" and "cuda" not in args_cli.device: + raise ValueError("Fabric backend requires CUDA.") + view = XformPrimView(pattern, device=args_cli.device, validate_xform_ops=False) + num_prims = view.count timing_results["init"] = time.perf_counter() - start_time - # prepare indices for benchmarking - all_indices = torch.arange(num_prims, device=args_cli.device) - print(f" {view_name} managing {num_prims} prims") + print(f" XformPrimView ({view_type.upper()}) managing {num_prims} prims") + + positions, orientations = view.get_world_poses() + positions_t = wp.to_torch(positions) + orientations_t = wp.to_torch(orientations) + + _run_pose_benchmarks(view, num_prims, num_iterations, timing_results, computed_results, positions_t, orientations_t) - # Fabric is write-first: initialize it to match USD before benchmarking reads. - if view_type == "xform_fabric" and num_prims > 0: - init_positions = torch.zeros((num_prims, 3), dtype=torch.float32, device=args_cli.device) - init_positions[:, 0] = 2.0 * torch.arange(num_prims, device=args_cli.device, dtype=torch.float32) - init_positions[:, 2] = 1.0 - init_orientations = torch.tensor( - [[1.0, 0.0, 0.0, 0.0]] * num_prims, dtype=torch.float32, device=args_cli.device + sim.clear_instance() + return timing_results, computed_results + + +@torch.no_grad() +def benchmark_newton(num_iterations: int) -> tuple[dict[str, float], dict[str, torch.Tensor]]: + """Benchmark Newton XformPrimView.""" + from isaaclab.assets import RigidObjectCfg + from isaaclab.scene import InteractiveScene, InteractiveSceneCfg + from isaaclab.sim import SimulationCfg, build_simulation_context + from isaaclab.utils import configclass + + timing_results = {} + computed_results = {} + + @configclass + class _SceneCfg(InteractiveSceneCfg): + cube: RigidObjectCfg = RigidObjectCfg( + prim_path="{ENV_REGEX_NS}/Cube", + spawn=sim_utils.CuboidCfg( + size=(0.2, 0.2, 0.2), + rigid_props=sim_utils.RigidBodyPropertiesCfg(), + mass_props=sim_utils.MassPropertiesCfg(mass=1.0), + collision_props=sim_utils.CollisionPropertiesCfg(), + ), + init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, 0.0, 1.0)), ) - view.set_world_poses(init_positions, init_orientations) - # Benchmark get_world_poses + print(" Setting up Newton scene") + newton_cfg = SimulationCfg(physics=NewtonCfg(solver_cfg=MJWarpSolverCfg()), device=args_cli.device) + start_time = time.perf_counter() + ctx = build_simulation_context(device=args_cli.device, sim_cfg=newton_cfg, add_ground_plane=True) + sim = ctx.__enter__() + sim._app_control_on_stop_handle = None + InteractiveScene(_SceneCfg(num_envs=args_cli.num_envs, env_spacing=2.0)) + + stage = sim_utils.get_current_stage() + for i in range(args_cli.num_envs): + prim = stage.DefinePrim(f"/World/envs/env_{i}/Cube/Sensor", "Xform") + sim_utils.standardize_xform_ops(prim) + prim.GetAttribute("xformOp:translate").Set(Gf.Vec3d(0.1, 0.0, 0.05)) + prim.GetAttribute("xformOp:orient").Set(Gf.Quatd(1.0, 0.0, 0.0, 0.0)) + + sim.reset() + print(f" Newton scene setup: {time.perf_counter() - start_time:.4f}s") + + start_time = time.perf_counter() + view = NewtonXformPrimView("/World/envs/env_.*/Cube/Sensor", device=args_cli.device) + num_prims = view.count + timing_results["init"] = time.perf_counter() - start_time + + print(f" Newton XformPrimView managing {num_prims} prims") + + positions, orientations = view.get_world_poses() + positions_t = wp.to_torch(positions) + orientations_t = wp.to_torch(orientations) + + _run_pose_benchmarks(view, num_prims, num_iterations, timing_results, computed_results, positions_t, orientations_t) + + ctx.__exit__(None, None, None) + return timing_results, computed_results + + +@torch.no_grad() +def benchmark_physx(num_iterations: int) -> tuple[dict[str, float], dict[str, torch.Tensor]]: + """Benchmark PhysX RigidBodyView.""" + timing_results = {} + computed_results = {} + + print(" Setting up scene") + sim_utils.create_new_stage() + start_time = time.perf_counter() + sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device, use_fabric=False) + sim = sim_utils.SimulationContext(sim_cfg) + stage = sim_utils.get_current_stage() + print(f" SimulationContext: {time.perf_counter() - start_time:.4f}s") + + object_cfg = sim_utils.ConeCfg( + radius=0.15, + height=0.5, + rigid_props=sim_utils.RigidBodyPropertiesCfg(), + mass_props=sim_utils.MassPropertiesCfg(mass=1.0), + collision_props=sim_utils.CollisionPropertiesCfg(), + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(0.0, 1.0, 0.0)), + ) + for i in range(args_cli.num_envs): + sim_utils.create_prim(f"/World/Env_{i}", "Xform", stage=stage, translation=(i * 2.0, 0.0, 0.0)) + object_cfg.func(f"/World/Env_{i}/Object", object_cfg, translation=(0.0, 0.0, 1.0)) + + sim.reset() + + pattern = "/World/Env_*/Object" + start_time = time.perf_counter() + physics_sim_view = sim.physics_manager.get_physics_sim_view() + view = physics_sim_view.create_rigid_body_view(pattern) + num_prims = view.count + timing_results["init"] = time.perf_counter() - start_time + + print(f" PhysX RigidBodyView managing {num_prims} prims") + + all_indices = torch.arange(num_prims, device=args_cli.device) + + transforms = view.get_transforms() + positions_t = transforms[:, :3] + orientations_t = transforms[:, 3:7] + start_time = time.perf_counter() for _ in range(num_iterations): - if view_type in ("xform", "xform_fabric"): - positions, orientations = view.get_world_poses() - else: # physx - transforms = view.get_transforms() - positions = transforms[:, :3] - orientations = transforms[:, 3:7] + transforms = view.get_transforms() timing_results["get_world_poses"] = (time.perf_counter() - start_time) / num_iterations - # Store initial world poses - computed_results["initial_world_positions"] = positions.clone() - computed_results["initial_world_orientations"] = orientations.clone() + computed_results["initial_world_positions"] = positions_t.clone() + computed_results["initial_world_orientations"] = orientations_t.clone() - # Benchmark set_world_poses - new_positions = positions.clone() + new_positions = positions_t.clone() new_positions[:, 2] += 0.5 + new_transforms = torch.cat([new_positions, orientations_t], dim=-1) start_time = time.perf_counter() for _ in range(num_iterations): - if view_type in ("xform", "xform_fabric"): - view.set_world_poses(new_positions, orientations) - else: # physx - new_transforms = torch.cat([new_positions, orientations], dim=-1) - view.set_transforms(new_transforms, indices=all_indices) + view.set_transforms(new_transforms, indices=all_indices) timing_results["set_world_poses"] = (time.perf_counter() - start_time) / num_iterations - # Get world poses after setting to verify - if view_type in ("xform", "xform_fabric"): - positions_after_set, orientations_after_set = view.get_world_poses() - else: # physx - transforms_after = view.get_transforms() - positions_after_set = transforms_after[:, :3] - orientations_after_set = transforms_after[:, 3:7] - computed_results["world_positions_after_set"] = positions_after_set.clone() - computed_results["world_orientations_after_set"] = orientations_after_set.clone() - - # close simulation - sim.clear_instance() + transforms_after = view.get_transforms() + computed_results["world_positions_after_set"] = transforms_after[:, :3].clone() + computed_results["world_orientations_after_set"] = transforms_after[:, 3:7].clone() + sim.clear_instance() return timing_results, computed_results +def _run_pose_benchmarks( + view, + num_prims: int, + num_iterations: int, + timing_results: dict, + computed_results: dict, + positions_t: torch.Tensor, + orientations_t: torch.Tensor, +): + """Shared benchmark loop for get/set world poses on any XformPrimView.""" + device = args_cli.device + + start_time = time.perf_counter() + for _ in range(num_iterations): + view.get_world_poses() + timing_results["get_world_poses"] = (time.perf_counter() - start_time) / num_iterations + + computed_results["initial_world_positions"] = positions_t.clone() + computed_results["initial_world_orientations"] = orientations_t.clone() + + new_positions = positions_t.clone() + new_positions[:, 2] += 0.5 + new_pos_wp = wp.from_torch(new_positions) + new_quat_wp = wp.from_torch(orientations_t) + + start_time = time.perf_counter() + for _ in range(num_iterations): + view.set_world_poses(new_pos_wp, new_quat_wp) + timing_results["set_world_poses"] = (time.perf_counter() - start_time) / num_iterations + + ret_pos, ret_quat = view.get_world_poses() + computed_results["world_positions_after_set"] = wp.to_torch(ret_pos).clone() + computed_results["world_orientations_after_set"] = wp.to_torch(ret_quat).clone() + + +# ------------------------------------------------------------------ +# Reporting +# ------------------------------------------------------------------ + + def compare_results( results_dict: dict[str, dict[str, torch.Tensor]], tolerance: float = 1e-4 ) -> dict[str, dict[str, dict[str, float]]]: - """Compare computed results across implementations. - - Args: - results_dict: Dictionary mapping implementation names to their computed values. - tolerance: Tolerance for numerical comparison. - - Returns: - Nested dictionary: {comparison_pair: {metric: {stats}}} - """ + """Compare computed results across implementations.""" comparison_stats = {} impl_names = list(results_dict.keys()) - # Compare each pair of implementations for i, impl1 in enumerate(impl_names): for impl2 in impl_names[i + 1 :]: pair_key = f"{impl1}_vs_{impl2}" @@ -223,279 +319,182 @@ def compare_results( computed1 = results_dict[impl1] computed2 = results_dict[impl2] - for key in computed1.keys(): + for key in computed1: if key not in computed2: continue - - val1 = computed1[key] - val2 = computed2[key] - - # Skip zero tensors (not applicable tests) + val1, val2 = computed1[key], computed2[key] if torch.all(val1 == 0) or torch.all(val2 == 0): continue - # Compute differences diff = torch.abs(val1 - val2) - max_diff = torch.max(diff).item() - mean_diff = torch.mean(diff).item() - - # Check if within tolerance - all_close = torch.allclose(val1, val2, atol=tolerance, rtol=0) - comparison_stats[pair_key][key] = { - "max_diff": max_diff, - "mean_diff": mean_diff, - "all_close": all_close, + "max_diff": torch.max(diff).item(), + "mean_diff": torch.mean(diff).item(), + "all_close": torch.allclose(val1, val2, atol=tolerance, rtol=0), } return comparison_stats -def print_comparison_results(comparison_stats: dict[str, dict[str, dict[str, float]]], tolerance: float): - """Print comparison results. - - Args: - comparison_stats: Nested dictionary containing comparison statistics. - tolerance: Tolerance used for comparison. - """ +def print_comparison_results(comparison_stats: dict, tolerance: float): + """Print comparison results.""" for pair_key, pair_stats in comparison_stats.items(): - if not pair_stats: # Skip if no comparable results + if not pair_stats: continue - - # Format the pair key for display impl1, impl2 = pair_key.split("_vs_") - display_impl1 = impl1.replace("_", " ").title() - display_impl2 = impl2.replace("_", " ").title() - comparison_title = f"{display_impl1} vs {display_impl2}" - - # Check if all results match - all_match = all(stats["all_close"] for stats in pair_stats.values()) + title = f"{impl1.replace('_', ' ').title()} vs {impl2.replace('_', ' ').title()}" + all_match = all(s["all_close"] for s in pair_stats.values()) + print("\n" + "=" * 100) + print(f"RESULT COMPARISON: {title}") + print("=" * 100) if all_match: - # Compact output when everything matches - print("\n" + "=" * 100) - print(f"RESULT COMPARISON: {comparison_title}") - print("=" * 100) - print(f"✓ All computed values match within tolerance ({tolerance})") - print("=" * 100) + print(f" All computed values match within tolerance ({tolerance})") else: - # Detailed output when there are mismatches - print("\n" + "=" * 100) - print(f"RESULT COMPARISON: {comparison_title}") - print("=" * 100) print(f"{'Computed Value':<40} {'Max Diff':<15} {'Mean Diff':<15} {'Match':<10}") print("-" * 100) - for key, stats in pair_stats.items(): - # Format the key for display - display_key = key.replace("_", " ").title() - match_str = "✓ Yes" if stats["all_close"] else "✗ No" - - print(f"{display_key:<40} {stats['max_diff']:<15.6e} {stats['mean_diff']:<15.6e} {match_str:<10}") - - print("=" * 100) - print(f"\n✗ Some results differ beyond tolerance ({tolerance})") - print(f" This may indicate implementation differences between {display_impl1} and {display_impl2}") - + match_str = "Yes" if stats["all_close"] else "No" + print(f"{key:<40} {stats['max_diff']:<15.6e} {stats['mean_diff']:<15.6e} {match_str:<10}") print() def print_results(results_dict: dict[str, dict[str, float]], num_prims: int, num_iterations: int): - """Print benchmark results in a formatted table. - - Args: - results_dict: Dictionary mapping implementation names to their timing results. - num_prims: Number of prims tested. - num_iterations: Number of iterations run. - """ - print("\n" + "=" * 100) + """Print benchmark results in a formatted table.""" + print("\n" + "=" * 120) print(f"BENCHMARK RESULTS: {num_prims} prims, {num_iterations} iterations") - print("=" * 100) + print("=" * 120) impl_names = list(results_dict.keys()) - # Format names for display - display_names = [name.replace("_", " ").title() for name in impl_names] + display_names = {n: n.replace("_", " ").title() for n in impl_names} + col_width = 22 - # Calculate column width - col_width = 20 - - # Print header - header = f"{'Operation':<30}" - for display_name in display_names: - header += f" {display_name + ' (ms)':<{col_width}}" + header = f"{'Operation':<25}" + for name in impl_names: + header += f" {display_names[name] + ' (ms)':>{col_width}}" print(header) - print("-" * 100) + print("-" * 120) - # Print each operation - operations = [ - ("Initialization", "init"), - ("Get World Poses", "get_world_poses"), - ("Set World Poses", "set_world_poses"), - ] + operations = [("Initialization", "init"), ("Get World Poses", "get_world_poses"), ("Set World Poses", "set_world_poses")] for op_name, op_key in operations: - row = f"{op_name:<30}" - for impl_name in impl_names: - impl_time = results_dict[impl_name].get(op_key, 0) * 1000 # Convert to ms - row += f" {impl_time:>{col_width - 1}.4f}" + row = f"{op_name:<25}" + for name in impl_names: + val = results_dict[name].get(op_key, 0) * 1000 + row += f" {val:>{col_width}.4f}" print(row) - print("=" * 100) - - # Calculate and print total time (excluding N/A operations) - total_row = f"{'Total Time':<30}" - for impl_name in impl_names: - if impl_name == "physx_view": - # Exclude local pose operations for PhysX - total_time = ( - results_dict[impl_name].get("init", 0) * 1000 - + results_dict[impl_name].get("get_world_poses", 0) * 1000 - + results_dict[impl_name].get("set_world_poses", 0) * 1000 - ) - else: - total_time = sum(results_dict[impl_name].values()) * 1000 - total_row += f" {total_time:>{col_width - 1}.4f}" - print(f"\n{total_row}") - - # Calculate speedups relative to XformPrimView (USD baseline) - if "xform_view" in impl_names: - print("\n" + "=" * 100) - print("SPEEDUP vs XformPrimView (USD)") - print("=" * 100) - print(f"{'Operation':<30}", end="") - for impl_name, display_name in zip(impl_names, display_names): - if impl_name != "xform_view": - print(f" {display_name + ' Speedup':<{col_width}}", end="") - print() - print("-" * 100) - - xform_results = results_dict["xform_view"] + print("=" * 120) + + total_row = f"{'Total':<25}" + for name in impl_names: + total = sum(results_dict[name].values()) * 1000 + total_row += f" {total:>{col_width}.4f}" + print(total_row) + + baseline = "usd" + if baseline in results_dict and len(impl_names) > 1: + print("\n" + "=" * 120) + print(f"SPEEDUP vs {display_names[baseline]}") + print("=" * 120) + header = f"{'Operation':<25}" + for name in impl_names: + if name != baseline: + header += f" {display_names[name]:>{col_width}}" + print(header) + print("-" * 120) + + base = results_dict[baseline] for op_name, op_key in operations: - print(f"{op_name:<30}", end="") - xform_time = xform_results.get(op_key, 0) - for impl_name, display_name in zip(impl_names, display_names): - if impl_name != "xform_view": - impl_time = results_dict[impl_name].get(op_key, 0) - if xform_time > 0 and impl_time > 0: - speedup = impl_time / xform_time - print(f" {speedup:>{col_width - 1}.2f}x", end="") + row = f"{op_name:<25}" + base_t = base.get(op_key, 0) + for name in impl_names: + if name != baseline: + impl_t = results_dict[name].get(op_key, 0) + if base_t > 0 and impl_t > 0: + row += f" {impl_t / base_t:>{col_width}.2f}x" else: - print(f" {'N/A':>{col_width}}", end="") - print() - - # Overall speedup (only world pose operations) - print("=" * 100) - print(f"{'Overall Speedup (World Ops)':<30}", end="") - total_xform = ( - xform_results.get("init", 0) - + xform_results.get("get_world_poses", 0) - + xform_results.get("set_world_poses", 0) - ) - for impl_name, display_name in zip(impl_names, display_names): - if impl_name != "xform_view": - total_impl = ( - results_dict[impl_name].get("init", 0) - + results_dict[impl_name].get("get_world_poses", 0) - + results_dict[impl_name].get("set_world_poses", 0) - ) - if total_xform > 0 and total_impl > 0: - overall_speedup = total_impl / total_xform - print(f" {overall_speedup:>{col_width - 1}.2f}x", end="") - else: - print(f" {'N/A':>{col_width}}", end="") - print() + row += f" {'N/A':>{col_width}}" + print(row) + print("=" * 120) - print("\n" + "=" * 100) print("\nNotes:") print(" - Times are averaged over all iterations") - print(" - Speedup = (Implementation time) / (XformPrimView USD time)") - print(" - Speedup > 1.0 means USD XformPrimView is faster") - print(" - Speedup < 1.0 means the implementation is faster than USD") - print(" - PhysX View requires rigid body physics components") - print(" - XformPrimView works with any Xform prim (physics or non-physics)") - print(" - PhysX View does not support local pose operations directly") + print(" - Speedup < 1.0 means faster than USD baseline") + print(" - PhysX RigidBodyView requires rigid body physics; XformPrimView works with any Xformable prim") print() +# ------------------------------------------------------------------ +# Main +# ------------------------------------------------------------------ + + def main(): - """Main benchmark function.""" - print("=" * 100) - print("View Comparison Benchmark - XformPrimView vs PhysX RigidBodyView") - print("=" * 100) - print("Configuration:") - print(f" Number of environments: {args_cli.num_envs}") - print(f" Iterations per test: {args_cli.num_iterations}") - print(f" Device: {args_cli.device}") - print(f" Profiling: {'Enabled' if args_cli.profile else 'Disabled'}") - if args_cli.profile: - print(f" Profile directory: {args_cli.profile_dir}") + print("=" * 120) + print("XformPrimView Benchmark: USD vs Fabric vs Newton vs PhysX") + print("=" * 120) + print(f" Environments: {args_cli.num_envs}") + print(f" Iterations: {args_cli.num_iterations}") + print(f" Device: {args_cli.device}") + print(f" Backends: {', '.join(args_cli.backends)}") print() - # Create profile directory if profiling is enabled if args_cli.profile: import os os.makedirs(args_cli.profile_dir, exist_ok=True) - # Dictionary to store all results - all_timing_results = {} - all_computed_results = {} + all_timing = {} + all_computed = {} profile_files = {} - # Implementations to benchmark - implementations = [ - ("xform_view", "XformPrimView (USD)", "xform"), - ("xform_fabric_view", "XformPrimView (Fabric)", "xform_fabric"), - ("physx_view", "PhysX RigidBodyView", "physx"), - ] + dispatch = { + "usd": ("usd", "XformPrimView (USD)", lambda n: benchmark_usd_or_fabric("usd", n)), + "fabric": ("fabric", "XformPrimView (Fabric)", lambda n: benchmark_usd_or_fabric("fabric", n)), + "newton": ("newton", "XformPrimView (Newton)", lambda n: benchmark_newton(n)), + "physx": ("physx", "PhysX RigidBodyView", lambda n: benchmark_physx(n)), + } + + for backend in args_cli.backends: + if backend == "newton" and not HAS_NEWTON: + print(f"Skipping {backend}: isaaclab_newton not installed") + continue - # Benchmark each implementation - for impl_key, impl_name, view_type in implementations: - print(f"Benchmarking {impl_name}...") + key, display_name, bench_fn = dispatch[backend] + print(f"Benchmarking {display_name}...") if args_cli.profile: profiler = cProfile.Profile() profiler.enable() - timing, computed = benchmark_view(view_type=view_type, num_iterations=args_cli.num_iterations) + timing, computed = bench_fn(args_cli.num_iterations) if args_cli.profile: profiler.disable() - profile_file = f"{args_cli.profile_dir}/{impl_key}_benchmark.prof" - profiler.dump_stats(profile_file) - profile_files[impl_key] = profile_file - print(f" Profile saved to: {profile_file}") + pf = f"{args_cli.profile_dir}/{key}_benchmark.prof" + profiler.dump_stats(pf) + profile_files[key] = pf + print(f" Profile saved to: {pf}") - all_timing_results[impl_key] = timing - all_computed_results[impl_key] = computed + all_timing[key] = timing + all_computed[key] = computed + print(" Done!\n") - print(" Done!") - print() - - # Print timing results - print_results(all_timing_results, args_cli.num_envs, args_cli.num_iterations) + print_results(all_timing, args_cli.num_envs, args_cli.num_iterations) - # Compare computed results - print("\nComparing computed results across implementations...") - comparison_stats = compare_results(all_computed_results, tolerance=1e-4) + print("Comparing computed results...") + comparison_stats = compare_results(all_computed, tolerance=1e-4) print_comparison_results(comparison_stats, tolerance=1e-4) - # Print profiling instructions if enabled if args_cli.profile: print("\n" + "=" * 100) print("PROFILING RESULTS") print("=" * 100) - print("Profile files have been saved. To visualize with snakeviz, run:") - for impl_key, profile_file in profile_files.items(): - impl_display = impl_key.replace("_", " ").title() - print(f" # {impl_display}") - print(f" snakeviz {profile_file}") - print("\nAlternatively, use pstats to analyze in terminal:") - print(" python -m pstats ") - print("=" * 100) + for key, pf in profile_files.items(): + print(f" snakeviz {pf}") print() - # Clean up sim_utils.SimulationContext.clear_instance() diff --git a/scripts/benchmarks/benchmark_xform_prim_view.py b/scripts/benchmarks/benchmark_xform_prim_view.py index 761a74e3e12a..cd592b2b1168 100644 --- a/scripts/benchmarks/benchmark_xform_prim_view.py +++ b/scripts/benchmarks/benchmark_xform_prim_view.py @@ -3,59 +3,35 @@ # # SPDX-License-Identifier: BSD-3-Clause -"""Benchmark script comparing XformPrimView implementations across different APIs. +"""Benchmark script comparing XformPrimView implementations across backends. -This script tests the performance of batched transform operations using: -- Isaac Lab's XformPrimView implementation with USD backend -- Isaac Lab's XformPrimView implementation with Fabric backend -- Isaac Sim's XformPrimView implementation (legacy) -- Isaac Sim Experimental's XformPrim implementation (latest) +Compares batched transform operation performance across: +- Isaac Lab XformPrimView (USD backend) -- baseline +- Isaac Lab XformPrimView (Fabric backend) +- Isaac Lab XformPrimView (Newton backend) Usage: - # Basic benchmark (all APIs) ./isaaclab.sh -p scripts/benchmarks/benchmark_xform_prim_view.py --num_envs 1024 --device cuda:0 --headless - # With profiling enabled (for snakeviz visualization) + # With profiling ./isaaclab.sh -p scripts/benchmarks/benchmark_xform_prim_view.py --num_envs 1024 --profile --headless - - # Then visualize with snakeviz: - snakeviz profile_results/isaaclab_usd_benchmark.prof - snakeviz profile_results/isaaclab_fabric_benchmark.prof - snakeviz profile_results/isaacsim_benchmark.prof - snakeviz profile_results/isaacsim_exp_benchmark.prof """ from __future__ import annotations -"""Launch Isaac Sim Simulator first.""" - import argparse from isaaclab.app import AppLauncher -# parse the arguments -args_cli = argparse.Namespace() - -parser = argparse.ArgumentParser(description="This script can help you benchmark the performance of XformPrimView.") - +parser = argparse.ArgumentParser(description="Benchmark XformPrimView performance across backends.") parser.add_argument("--num_envs", type=int, default=100, help="Number of environments to simulate.") parser.add_argument("--num_iterations", type=int, default=50, help="Number of iterations for each test.") -parser.add_argument( - "--profile", - action="store_true", - help="Enable profiling with cProfile. Results saved as .prof files for snakeviz visualization.", -) -parser.add_argument( - "--profile-dir", - type=str, - default="./profile_results", - help="Directory to save profile results. Default: ./profile_results", -) +parser.add_argument("--profile", action="store_true", help="Enable cProfile profiling.") +parser.add_argument("--profile_dir", type=str, default="./profile_results", help="Directory for .prof files.") AppLauncher.add_app_launcher_args(parser) args_cli = parser.parse_args() -# launch omniverse app app_launcher = AppLauncher(args_cli) simulation_app = app_launcher.app @@ -66,235 +42,24 @@ from typing import Literal import torch +import warp as wp -try: - from isaacsim.core.prims import XFormPrim as IsaacSimXformPrimView - from isaacsim.core.utils.extensions import enable_extension - - enable_extension("isaacsim.core.experimental.prims") - from isaacsim.core.experimental.prims import XformPrim as IsaacSimExperimentalXformPrimView - - _HAS_ISAACSIM = True -except (ImportError, ModuleNotFoundError): - _HAS_ISAACSIM = False +from pxr import Gf from isaaclab_newton.physics import MJWarpSolverCfg, NewtonCfg -from isaaclab_newton.sim.views import XformPrimView as IsaacLabNewtonXformPrimView -from isaaclab_physx.sim.views import XformPrimView as IsaacLabFabricXformPrimView +from isaaclab_newton.sim.views import XformPrimView as NewtonXformPrimView +from isaaclab_physx.sim.views import XformPrimView as FabricXformPrimView import isaaclab.sim as sim_utils from isaaclab.assets import RigidObjectCfg from isaaclab.scene import InteractiveScene, InteractiveSceneCfg from isaaclab.sim import SimulationCfg, build_simulation_context -from isaaclab.sim.views import XformPrimView as IsaacLabXformPrimView +from isaaclab.sim.views import XformPrimView as UsdXformPrimView from isaaclab.utils import configclass -@torch.no_grad() -def benchmark_xform_prim_view( # noqa: C901 - api: Literal["isaaclab-usd", "isaaclab-fabric", "isaacsim-usd", "isaacsim-fabric", "isaacsim-exp"], - num_iterations: int, -) -> tuple[dict[str, float], dict[str, torch.Tensor]]: - """Benchmark the Xform view class from Isaac Lab, Isaac Sim, or Isaac Sim Experimental. - - Args: - api: Which API to benchmark: - - "isaaclab-usd": Isaac Lab XformPrimView with USD backend - - "isaaclab-fabric": Isaac Lab XformPrimView with Fabric backend - - "isaacsim-usd": Isaac Sim legacy XformPrimView with USD (usd=True) - - "isaacsim-fabric": Isaac Sim legacy XformPrimView with Fabric (usd=False) - - "isaacsim-exp": Isaac Sim Experimental XformPrim - num_iterations: Number of iterations to run. - - Returns: - A tuple of (timing_results, computed_results) where: - - timing_results: Dictionary containing timing results for various operations - - computed_results: Dictionary containing the computed values for validation - """ - timing_results = {} - computed_results = {} - - # Setup scene - print(" Setting up scene") - # Clear stage - sim_utils.create_new_stage() - # Create simulation context - start_time = time.perf_counter() - sim_cfg = sim_utils.SimulationCfg( - dt=0.01, - device=args_cli.device, - use_fabric=api in ("isaaclab-fabric", "isaacsim-fabric"), - ) - sim = sim_utils.SimulationContext(sim_cfg) - stage = sim_utils.get_current_stage() - - print(f" Time taken to create simulation context: {time.perf_counter() - start_time} seconds") - - # Create prims - prim_paths = [] - for i in range(args_cli.num_envs): - sim_utils.create_prim(f"/World/Env_{i}", "Xform", stage=stage, translation=(i * 2.0, 0.0, 1.0)) - sim_utils.create_prim(f"/World/Env_{i}/Object", "Xform", stage=stage, translation=(0.0, 0.0, 0.0)) - prim_paths.append(f"/World/Env_{i}/Object") - # Play simulation - sim.reset() - - # Pattern to match all prims - pattern = "/World/Env_.*/Object" - print(f" Pattern: {pattern}") - - # Create view - start_time = time.perf_counter() - if api == "isaaclab-usd": - xform_view = IsaacLabXformPrimView(pattern, device=args_cli.device, validate_xform_ops=False) - elif api == "isaaclab-fabric": - xform_view = IsaacLabFabricXformPrimView(pattern, device=args_cli.device, validate_xform_ops=False) - elif api == "isaacsim-usd": - xform_view = IsaacSimXformPrimView(pattern, reset_xform_properties=False, usd=True) - elif api == "isaacsim-fabric": - xform_view = IsaacSimXformPrimView(pattern, reset_xform_properties=False, usd=False) - elif api == "isaacsim-exp": - xform_view = IsaacSimExperimentalXformPrimView(pattern) - else: - raise ValueError(f"Invalid API: {api}") - timing_results["init"] = time.perf_counter() - start_time - - if api in ("isaaclab-usd", "isaaclab-fabric", "isaacsim-usd", "isaacsim-fabric"): - num_prims = xform_view.count - elif api == "isaacsim-exp": - num_prims = len(xform_view.prims) - print(f" XformView managing {num_prims} prims") - - # Benchmark get_world_poses - # Warmup call to initialize Fabric (if needed) - excluded from timing - positions, orientations = xform_view.get_world_poses() - - # Now time the actual iterations (steady-state performance) - start_time = time.perf_counter() - for _ in range(num_iterations): - positions, orientations = xform_view.get_world_poses() - - # Ensure tensors are torch tensors (do this AFTER timing) - if not isinstance(positions, torch.Tensor): - positions = torch.tensor(positions, dtype=torch.float32) - if not isinstance(orientations, torch.Tensor): - orientations = torch.tensor(orientations, dtype=torch.float32) - - timing_results["get_world_poses"] = (time.perf_counter() - start_time) / num_iterations - - # Store initial world poses - computed_results["initial_world_positions"] = positions.clone() - computed_results["initial_world_orientations"] = orientations.clone() - - # Benchmark set_world_poses - new_positions = positions.clone() - new_positions[:, 2] += 0.1 - start_time = time.perf_counter() - for _ in range(num_iterations): - if api in ("isaaclab-usd", "isaaclab-fabric", "isaacsim-usd", "isaacsim-fabric"): - xform_view.set_world_poses(new_positions, orientations) - elif api == "isaacsim-exp": - xform_view.set_world_poses(new_positions.cpu().numpy(), orientations.cpu().numpy()) - timing_results["set_world_poses"] = (time.perf_counter() - start_time) / num_iterations - - # Get world poses after setting to verify - positions_after_set, orientations_after_set = xform_view.get_world_poses() - if not isinstance(positions_after_set, torch.Tensor): - positions_after_set = torch.tensor(positions_after_set, dtype=torch.float32) - if not isinstance(orientations_after_set, torch.Tensor): - orientations_after_set = torch.tensor(orientations_after_set, dtype=torch.float32) - computed_results["world_positions_after_set"] = positions_after_set.clone() - computed_results["world_orientations_after_set"] = orientations_after_set.clone() - - # Benchmark get_local_poses - # Warmup call (though local poses use USD, so minimal overhead) - translations, orientations_local = xform_view.get_local_poses() - - # Now time the actual iterations - start_time = time.perf_counter() - for _ in range(num_iterations): - translations, orientations_local = xform_view.get_local_poses() - # Ensure tensors are torch tensors (do this AFTER timing) - if not isinstance(translations, torch.Tensor): - translations = torch.tensor(translations, dtype=torch.float32, device=args_cli.device) - if not isinstance(orientations_local, torch.Tensor): - orientations_local = torch.tensor(orientations_local, dtype=torch.float32, device=args_cli.device) - - timing_results["get_local_poses"] = (time.perf_counter() - start_time) / num_iterations - - # Store initial local poses - computed_results["initial_local_translations"] = translations.clone() - computed_results["initial_local_orientations"] = orientations_local.clone() - - # Benchmark set_local_poses - new_translations = translations.clone() - new_translations[:, 2] += 0.1 - start_time = time.perf_counter() - for _ in range(num_iterations): - if api in ("isaaclab-usd", "isaaclab-fabric", "isaacsim-usd", "isaacsim-fabric"): - xform_view.set_local_poses(new_translations, orientations_local) - elif api == "isaacsim-exp": - xform_view.set_local_poses(new_translations.cpu().numpy(), orientations_local.cpu().numpy()) - timing_results["set_local_poses"] = (time.perf_counter() - start_time) / num_iterations - - # Get local poses after setting to verify - translations_after_set, orientations_local_after_set = xform_view.get_local_poses() - if not isinstance(translations_after_set, torch.Tensor): - translations_after_set = torch.tensor(translations_after_set, dtype=torch.float32) - if not isinstance(orientations_local_after_set, torch.Tensor): - orientations_local_after_set = torch.tensor(orientations_local_after_set, dtype=torch.float32) - computed_results["local_translations_after_set"] = translations_after_set.clone() - computed_results["local_orientations_after_set"] = orientations_local_after_set.clone() - - # Benchmark combined get operation - # Warmup call (Fabric should already be initialized by now, but for consistency) - positions, orientations = xform_view.get_world_poses() - translations, local_orientations = xform_view.get_local_poses() - - # Now time the actual iterations - start_time = time.perf_counter() - for _ in range(num_iterations): - positions, orientations = xform_view.get_world_poses() - translations, local_orientations = xform_view.get_local_poses() - timing_results["get_both"] = (time.perf_counter() - start_time) / num_iterations - - # Benchmark interleaved set/get (realistic workflow pattern) - # Pre-convert tensors for experimental API to avoid conversion overhead in loop - if api == "isaacsim-exp": - new_positions_np = new_positions.cpu().numpy() - orientations_np = orientations - - # Warmup - if api in ("isaaclab-usd", "isaaclab-fabric", "isaacsim-usd", "isaacsim-fabric"): - xform_view.set_world_poses(new_positions, orientations) - positions, orientations = xform_view.get_world_poses() - elif api == "isaacsim-exp": - xform_view.set_world_poses(new_positions_np, orientations_np) - positions, orientations = xform_view.get_world_poses() - positions = torch.tensor(positions, dtype=torch.float32) - orientations = torch.tensor(orientations, dtype=torch.float32) - - # Now time the actual interleaved iterations - start_time = time.perf_counter() - for _ in range(num_iterations): - # Write then immediately read (common pattern: set pose, verify/query result) - if api in ("isaaclab-usd", "isaaclab-fabric", "isaacsim-usd", "isaacsim-fabric"): - xform_view.set_world_poses(new_positions, orientations) - positions, orientations = xform_view.get_world_poses() - elif api == "isaacsim-exp": - xform_view.set_world_poses(new_positions_np, orientations_np) - positions, orientations = xform_view.get_world_poses() - - timing_results["interleaved_world_set_get"] = (time.perf_counter() - start_time) / num_iterations - - # close simulation - sim.clear_instance() - - return timing_results, computed_results - - @configclass -class _NewtonBenchSceneCfg(InteractiveSceneCfg): +class _NewtonSceneCfg(InteractiveSceneCfg): cube: RigidObjectCfg = RigidObjectCfg( prim_path="{ENV_REGEX_NS}/Object", spawn=sim_utils.CuboidCfg( @@ -307,278 +72,202 @@ class _NewtonBenchSceneCfg(InteractiveSceneCfg): ) +# ------------------------------------------------------------------ +# Benchmark +# ------------------------------------------------------------------ + + @torch.no_grad() -def benchmark_newton_xform_prim_view( +def benchmark_xform_prim_view( # noqa: C901 + api: Literal["isaaclab-usd", "isaaclab-fabric", "isaaclab-newton-site"], num_iterations: int, ) -> tuple[dict[str, float], dict[str, torch.Tensor]]: - """Benchmark Newton XformPrimView (GPU sites / body_q).""" - timing_results = {} - computed_results = {} + """Benchmark get/set world/local poses for the given XformPrimView backend.""" + timing_results: dict[str, float] = {} + computed_results: dict[str, torch.Tensor] = {} + device = args_cli.device + num_envs = args_cli.num_envs + + # -- Scene setup (backend-specific) -------------------------------- - print(" Setting up Newton scene") - newton_sim_cfg = SimulationCfg(device=args_cli.device, physics=NewtonCfg(solver_cfg=MJWarpSolverCfg())) + print(" Setting up scene") + cleanup = None - with build_simulation_context(device=args_cli.device, sim_cfg=newton_sim_cfg, add_ground_plane=True) as sim: + if api == "isaaclab-newton-site": + newton_cfg = SimulationCfg(device=device, physics=NewtonCfg(solver_cfg=MJWarpSolverCfg())) + ctx = build_simulation_context(device=device, sim_cfg=newton_cfg, add_ground_plane=True) + sim = ctx.__enter__() sim._app_control_on_stop_handle = None - InteractiveScene(_NewtonBenchSceneCfg(num_envs=args_cli.num_envs, env_spacing=2.0)) + InteractiveScene(_NewtonSceneCfg(num_envs=num_envs, env_spacing=2.0)) + + stage = sim_utils.get_current_stage() + for i in range(num_envs): + prim = stage.DefinePrim(f"/World/envs/env_{i}/Object/Sensor", "Xform") + sim_utils.standardize_xform_ops(prim) + prim.GetAttribute("xformOp:translate").Set(Gf.Vec3d(0.1, 0.0, 0.05)) + prim.GetAttribute("xformOp:orient").Set(Gf.Quatd(1.0, 0.0, 0.0, 0.0)) + sim.reset() start_time = time.perf_counter() - xform_view = IsaacLabNewtonXformPrimView("/World/envs/env_.*/Object", device=args_cli.device) + xform_view = NewtonXformPrimView("/World/envs/env_.*/Object/Sensor", device=device) timing_results["init"] = time.perf_counter() - start_time + cleanup = lambda: ctx.__exit__(None, None, None) # noqa: E731 - num_prims = xform_view.count - print(f" Newton XformView managing {num_prims} prims") + else: + sim_utils.create_new_stage() + start_time = time.perf_counter() + use_fabric = api == "isaaclab-fabric" + sim = sim_utils.SimulationContext(sim_utils.SimulationCfg(dt=0.01, device=device, use_fabric=use_fabric)) + stage = sim_utils.get_current_stage() - # warmup - for _ in range(5): - xform_view.get_world_poses() + for i in range(num_envs): + sim_utils.create_prim(f"/World/Env_{i}", "Xform", stage=stage, translation=(i * 2.0, 0.0, 1.0)) + sim_utils.create_prim(f"/World/Env_{i}/Object", "Xform", stage=stage, translation=(0.0, 0.0, 0.0)) - # get_world_poses - torch.cuda.synchronize() + sim.reset() + + pattern = "/World/Env_.*/Object" + start_time = time.perf_counter() + ViewClass = FabricXformPrimView if use_fabric else UsdXformPrimView + xform_view = ViewClass(pattern, device=device, validate_xform_ops=False) + timing_results["init"] = time.perf_counter() - start_time + cleanup = lambda: sim.clear_instance() # noqa: E731 + + num_prims = xform_view.count + print(f" {api} managing {num_prims} prims") + + is_newton = api == "isaaclab-newton-site" + + def to_torch(a): + return wp.to_torch(a) if isinstance(a, wp.array) else a + + try: + # -- Warmup -------------------------------------------------------- + xform_view.get_world_poses() + + # -- get_world_poses ----------------------------------------------- + if is_newton: + torch.cuda.synchronize() start_time = time.perf_counter() for _ in range(num_iterations): positions, orientations = xform_view.get_world_poses() - torch.cuda.synchronize() + if is_newton: + torch.cuda.synchronize() timing_results["get_world_poses"] = (time.perf_counter() - start_time) / num_iterations - computed_results["initial_world_positions"] = positions.clone() - computed_results["initial_world_orientations"] = orientations.clone() + positions_t = to_torch(positions) + orientations_t = to_torch(orientations) + computed_results["initial_world_positions"] = positions_t.clone() + computed_results["initial_world_orientations"] = orientations_t.clone() - # set_world_poses - new_positions = positions.clone() - new_positions[:, 2] += 0.1 - torch.cuda.synchronize() + # -- set_world_poses ----------------------------------------------- + if is_newton: + new_positions = wp.clone(positions) + wp.to_torch(new_positions)[:, 2] += 0.1 + else: + new_positions = positions_t.clone() + new_positions[:, 2] += 0.1 + + if is_newton: + torch.cuda.synchronize() start_time = time.perf_counter() for _ in range(num_iterations): xform_view.set_world_poses(new_positions, orientations) - torch.cuda.synchronize() + if is_newton: + torch.cuda.synchronize() timing_results["set_world_poses"] = (time.perf_counter() - start_time) / num_iterations - positions_after, orientations_after = xform_view.get_world_poses() - computed_results["world_positions_after_set"] = positions_after.clone() - computed_results["world_orientations_after_set"] = orientations_after.clone() + pa, oa = xform_view.get_world_poses() + computed_results["world_positions_after_set"] = to_torch(pa).clone() + computed_results["world_orientations_after_set"] = to_torch(oa).clone() - # get_local_poses (delegates to world for Newton) - torch.cuda.synchronize() + # -- get_local_poses ----------------------------------------------- + if is_newton: + torch.cuda.synchronize() start_time = time.perf_counter() for _ in range(num_iterations): translations, orientations_local = xform_view.get_local_poses() - torch.cuda.synchronize() + if is_newton: + torch.cuda.synchronize() timing_results["get_local_poses"] = (time.perf_counter() - start_time) / num_iterations - computed_results["initial_local_translations"] = translations.clone() - computed_results["initial_local_orientations"] = orientations_local.clone() + translations_t = to_torch(translations) + orientations_local_t = to_torch(orientations_local) + computed_results["initial_local_translations"] = translations_t.clone() + computed_results["initial_local_orientations"] = orientations_local_t.clone() + + # -- set_local_poses ----------------------------------------------- + if is_newton: + new_translations = wp.clone(translations) + wp.to_torch(new_translations)[:, 2] += 0.1 + else: + new_translations = translations_t.clone() + new_translations[:, 2] += 0.1 - # set_local_poses - new_translations = translations.clone() - new_translations[:, 2] += 0.1 - torch.cuda.synchronize() + if is_newton: + torch.cuda.synchronize() start_time = time.perf_counter() for _ in range(num_iterations): xform_view.set_local_poses(new_translations, orientations_local) - torch.cuda.synchronize() + if is_newton: + torch.cuda.synchronize() timing_results["set_local_poses"] = (time.perf_counter() - start_time) / num_iterations - translations_after, orientations_local_after = xform_view.get_local_poses() - computed_results["local_translations_after_set"] = translations_after.clone() - computed_results["local_orientations_after_set"] = orientations_local_after.clone() + ta, ola = xform_view.get_local_poses() + computed_results["local_translations_after_set"] = to_torch(ta).clone() + computed_results["local_orientations_after_set"] = to_torch(ola).clone() - # interleaved set -> get - torch.cuda.synchronize() + # -- get_both (world + local) -------------------------------------- + if is_newton: + torch.cuda.synchronize() + start_time = time.perf_counter() + for _ in range(num_iterations): + xform_view.get_world_poses() + xform_view.get_local_poses() + if is_newton: + torch.cuda.synchronize() + timing_results["get_both"] = (time.perf_counter() - start_time) / num_iterations + + # -- interleaved set -> get ---------------------------------------- + if is_newton: + torch.cuda.synchronize() start_time = time.perf_counter() for _ in range(num_iterations): xform_view.set_world_poses(new_positions, orientations) - positions, orientations = xform_view.get_world_poses() - torch.cuda.synchronize() + xform_view.get_world_poses() + if is_newton: + torch.cuda.synchronize() timing_results["interleaved_world_set_get"] = (time.perf_counter() - start_time) / num_iterations + finally: + if cleanup: + cleanup() + return timing_results, computed_results -def compare_results( - results_dict: dict[str, dict[str, torch.Tensor]], tolerance: float = 1e-4 -) -> dict[str, dict[str, dict[str, float]]]: - """Compare computed results across multiple implementations. - - Only compares implementations using the same data path: - - USD implementations (isaaclab-usd, isaacsim-usd) are compared with each other - - Fabric implementations (isaaclab-fabric, isaacsim-fabric) are compared with each other - - This is because Fabric is designed for write-first workflows and may not match - USD reads on initialization. - - Args: - results_dict: Dictionary mapping API names to their computed values. - tolerance: Tolerance for numerical comparison. - - Returns: - Nested dictionary: {comparison_pair: {metric: {stats}}}, e.g., - {"isaaclab-usd_vs_isaacsim-usd": {"initial_world_positions": {"max_diff": 0.001, ...}}} - """ - comparison_stats = {} - - # Group APIs by their data path (USD vs Fabric) - usd_apis = [api for api in results_dict.keys() if "usd" in api and "fabric" not in api] - fabric_apis = [api for api in results_dict.keys() if "fabric" in api] - - # Compare within USD group - for i, api1 in enumerate(usd_apis): - for api2 in usd_apis[i + 1 :]: - pair_key = f"{api1}_vs_{api2}" - comparison_stats[pair_key] = {} - - computed1 = results_dict[api1] - computed2 = results_dict[api2] - - for key in computed1.keys(): - if key not in computed2: - print(f" Warning: Key '{key}' not found in {api2} results") - continue - - val1 = computed1[key] - val2 = computed2[key] - - # Compute differences - diff = torch.abs(val1 - val2) - max_diff = torch.max(diff).item() - mean_diff = torch.mean(diff).item() - - # Check if within tolerance - all_close = torch.allclose(val1, val2, atol=tolerance, rtol=0) - - comparison_stats[pair_key][key] = { - "max_diff": max_diff, - "mean_diff": mean_diff, - "all_close": all_close, - } - - # Compare within Fabric group - for i, api1 in enumerate(fabric_apis): - for api2 in fabric_apis[i + 1 :]: - pair_key = f"{api1}_vs_{api2}" - comparison_stats[pair_key] = {} - - computed1 = results_dict[api1] - computed2 = results_dict[api2] - - for key in computed1.keys(): - if key not in computed2: - print(f" Warning: Key '{key}' not found in {api2} results") - continue - - val1 = computed1[key] - val2 = computed2[key] - - # Compute differences - diff = torch.abs(val1 - val2) - max_diff = torch.max(diff).item() - mean_diff = torch.mean(diff).item() - - # Check if within tolerance - all_close = torch.allclose(val1, val2, atol=tolerance, rtol=0) - - comparison_stats[pair_key][key] = { - "max_diff": max_diff, - "mean_diff": mean_diff, - "all_close": all_close, - } - - return comparison_stats - - -def print_comparison_results(comparison_stats: dict[str, dict[str, dict[str, float]]], tolerance: float): - """Print comparison results across implementations. - - Args: - comparison_stats: Nested dictionary containing comparison statistics for each API pair. - tolerance: Tolerance used for comparison. - """ - if not comparison_stats: - print("\n" + "=" * 100) - print("RESULT COMPARISON") - print("=" * 100) - print("ℹ️ No comparisons performed.") - print(" USD and Fabric implementations are not compared because Fabric uses a") - print(" write-first workflow and may not match USD reads on initialization.") - print("=" * 100) - print() - return - - for pair_key, pair_stats in comparison_stats.items(): - # Format the pair key for display (e.g., "isaaclab_vs_isaacsim" -> "Isaac Lab vs Isaac Sim") - api1, api2 = pair_key.split("_vs_") - display_api1 = api1.replace("-", " ").title() - display_api2 = api2.replace("-", " ").title() - comparison_title = f"{display_api1} vs {display_api2}" - - # Check if all results match - all_match = all(stats["all_close"] for stats in pair_stats.values()) - - if all_match: - # Compact output when everything matches - print("\n" + "=" * 100) - print(f"RESULT COMPARISON: {comparison_title}") - print("=" * 100) - print(f"✓ All computed values match within tolerance ({tolerance})") - print("=" * 100) - else: - # Detailed output when there are mismatches - print("\n" + "=" * 100) - print(f"RESULT COMPARISON: {comparison_title}") - print("=" * 100) - print(f"{'Computed Value':<40} {'Max Diff':<15} {'Mean Diff':<15} {'Match':<10}") - print("-" * 100) - - for key, stats in pair_stats.items(): - # Format the key for display - display_key = key.replace("_", " ").title() - match_str = "✓ Yes" if stats["all_close"] else "✗ No" - - print(f"{display_key:<40} {stats['max_diff']:<15.6e} {stats['mean_diff']:<15.6e} {match_str:<10}") - - print("=" * 100) - print(f"\n✗ Some results differ beyond tolerance ({tolerance})") - - # Special note for Isaac Sim Fabric local pose bug - if "isaacsim-fabric" in pair_key and any("local_translations_after_set" in k for k in pair_stats.keys()): - if not pair_stats.get("local_translations_after_set", {}).get("all_close", True): - print("\n ⚠️ Known Issue: Isaac Sim Fabric has a bug where get_local_poses() returns stale") - print(" values after set_local_poses(). Isaac Lab Fabric correctly returns updated values.") - print(" This is a correctness issue in Isaac Sim's implementation, not Isaac Lab's.") - else: - print(f" This may indicate implementation differences between {display_api1} and {display_api2}") - - print() +# ------------------------------------------------------------------ +# Reporting +# ------------------------------------------------------------------ def print_results(results_dict: dict[str, dict[str, float]], num_prims: int, num_iterations: int): - """Print benchmark results in a formatted table. - - Args: - results_dict: Dictionary mapping API names to their timing results. - num_prims: Number of prims tested. - num_iterations: Number of iterations run. - """ - print("\n" + "=" * 100) + """Print benchmark results in a formatted table.""" + print("\n" + "=" * 120) print(f"BENCHMARK RESULTS: {num_prims} prims, {num_iterations} iterations") - print("=" * 100) + print("=" * 120) api_names = list(results_dict.keys()) - # Format API names for display - display_names = [name.replace("-", " ").replace("_", " ").title() for name in api_names] - - # Calculate column width based on number of APIs - col_width = 20 + display_names = [name.replace("-", " ").title() for name in api_names] + col_width = 22 - # Print header - header = f"{'Operation':<25}" - for display_name in display_names: - header += f" {display_name + ' (ms)':<{col_width}}" + header = f"{'Operation':<28}" + for dn in display_names: + header += f" {dn + ' (ms)':>{col_width}}" print(header) - print("-" * 100) + print("-" * 120) - # Print each operation operations = [ ("Initialization", "init"), ("Get World Poses", "get_world_poses"), @@ -586,173 +275,119 @@ def print_results(results_dict: dict[str, dict[str, float]], num_prims: int, num ("Get Local Poses", "get_local_poses"), ("Set Local Poses", "set_local_poses"), ("Get Both (World+Local)", "get_both"), - ("Interleaved World Set→Get", "interleaved_world_set_get"), + ("Interleaved World Set->Get", "interleaved_world_set_get"), ] for op_name, op_key in operations: - row = f"{op_name:<25}" - for api_name in api_names: - api_time = results_dict[api_name].get(op_key, 0) * 1000 # Convert to ms - row += f" {api_time:>{col_width - 1}.4f}" + row = f"{op_name:<28}" + for name in api_names: + val = results_dict[name].get(op_key, 0) * 1000 + row += f" {val:>{col_width}.4f}" print(row) - print("=" * 100) + print("=" * 120) - # Calculate and print total time - total_row = f"{'Total Time':<25}" - for api_name in api_names: - total_time = sum(results_dict[api_name].values()) * 1000 - total_row += f" {total_time:>{col_width - 1}.4f}" + total_row = f"{'Total':<28}" + for name in api_names: + total_row += f" {sum(results_dict[name].values()) * 1000:>{col_width}.4f}" print(f"\n{total_row}") - # Calculate speedups relative to Isaac Lab USD (baseline) - if "isaaclab-usd" in api_names: - print("\n" + "=" * 100) - print("SPEEDUP vs Isaac Lab USD (Baseline)") - print("=" * 100) - print(f"{'Operation':<25}", end="") - for api_name, display_name in zip(api_names, display_names): - if api_name != "isaaclab-usd": - print(f" {display_name:<{col_width}}", end="") - print() - print("-" * 100) - - isaaclab_usd_results = results_dict["isaaclab-usd"] + baseline = "isaaclab-usd" + if baseline in results_dict and len(api_names) > 1: + print("\n" + "=" * 120) + print(f"SPEEDUP vs {baseline.replace('-', ' ').title()}") + print("=" * 120) + header = f"{'Operation':<28}" + for name in api_names: + if name != baseline: + header += f" {name.replace('-', ' ').title():>{col_width}}" + print(header) + print("-" * 120) + + base = results_dict[baseline] for op_name, op_key in operations: - print(f"{op_name:<25}", end="") - isaaclab_usd_time = isaaclab_usd_results.get(op_key, 0) - for api_name, display_name in zip(api_names, display_names): - if api_name != "isaaclab-usd": - api_time = results_dict[api_name].get(op_key, 0) - if isaaclab_usd_time > 0 and api_time > 0: - speedup = isaaclab_usd_time / api_time - print(f" {speedup:>{col_width - 1}.2f}x", end="") + row = f"{op_name:<28}" + base_t = base.get(op_key, 0) + for name in api_names: + if name != baseline: + impl_t = results_dict[name].get(op_key, 0) + if base_t > 0 and impl_t > 0: + row += f" {base_t / impl_t:>{col_width}.2f}x" else: - print(f" {'N/A':>{col_width}}", end="") - print() - - # Overall speedup - print("=" * 100) - print(f"{'Overall Speedup':<25}", end="") - total_isaaclab_usd = sum(isaaclab_usd_results.values()) - for api_name, display_name in zip(api_names, display_names): - if api_name != "isaaclab-usd": - total_api = sum(results_dict[api_name].values()) - if total_isaaclab_usd > 0 and total_api > 0: - overall_speedup = total_isaaclab_usd / total_api - print(f" {overall_speedup:>{col_width - 1}.2f}x", end="") + row += f" {'N/A':>{col_width}}" + print(row) + + print("=" * 120) + print(f"{'Overall':>28}", end="") + total_base = sum(base.values()) + for name in api_names: + if name != baseline: + total_impl = sum(results_dict[name].values()) + if total_base > 0 and total_impl > 0: + print(f" {total_base / total_impl:>{col_width}.2f}x", end="") else: print(f" {'N/A':>{col_width}}", end="") print() - print("\n" + "=" * 100) + print("\n" + "=" * 120) print("\nNotes:") print(" - Times are averaged over all iterations") - print(" - Speedup = (Isaac Lab USD time) / (Other API time)") - print(" - Speedup > 1.0 means the other API is faster than Isaac Lab USD") - print(" - Speedup < 1.0 means the other API is slower than Isaac Lab USD") + print(" - Speedup > 1.0 means faster than USD baseline") print() def main(): - """Main benchmark function.""" - print("=" * 100) - print("XformPrimView Benchmark - Comparing Multiple APIs") - print("=" * 100) - print("Configuration:") - print(f" Number of environments: {args_cli.num_envs}") - print(f" Iterations per test: {args_cli.num_iterations}") - print(f" Device: {args_cli.device}") - print(f" Profiling: {'Enabled' if args_cli.profile else 'Disabled'}") - if args_cli.profile: - print(f" Profile directory: {args_cli.profile_dir}") + print("=" * 120) + print("XformPrimView Benchmark") + print("=" * 120) + print(f" Environments: {args_cli.num_envs}") + print(f" Iterations: {args_cli.num_iterations}") + print(f" Device: {args_cli.device}") print() - # Create profile directory if profiling is enabled if args_cli.profile: import os os.makedirs(args_cli.profile_dir, exist_ok=True) - # Dictionary to store all results - all_timing_results = {} - all_computed_results = {} + all_timing = {} + all_computed = {} profile_files = {} - # APIs to benchmark - apis_to_test = [ + apis = [ ("isaaclab-usd", "Isaac Lab XformPrimView (USD)"), ("isaaclab-fabric", "Isaac Lab XformPrimView (Fabric)"), + ("isaaclab-newton-site", "Isaac Lab XformPrimView (Newton Site)"), ] - # Uncomment to include Isaac Sim APIs in the comparison (requires isaacsim package): - # if _HAS_ISAACSIM: - # apis_to_test += [ - # ("isaacsim-usd", "Isaac Sim XformPrimView (USD)"), - # ("isaacsim-fabric", "Isaac Sim XformPrimView (Fabric)"), - # ("isaacsim-exp", "Isaac Sim Experimental XformPrim"), - # ] - - # Benchmark each API - for api_key, api_name in apis_to_test: + + for api_key, api_name in apis: print(f"Benchmarking {api_name}...") if args_cli.profile: profiler = cProfile.Profile() profiler.enable() - timing, computed = benchmark_xform_prim_view( - api=api_key, # type: ignore[arg-type] - num_iterations=args_cli.num_iterations, - ) + timing, computed = benchmark_xform_prim_view(api=api_key, num_iterations=args_cli.num_iterations) if args_cli.profile: profiler.disable() - profile_file = f"{args_cli.profile_dir}/{api_key.replace('-', '_')}_benchmark.prof" - profiler.dump_stats(profile_file) - profile_files[api_key] = profile_file - print(f" Profile saved to: {profile_file}") - - all_timing_results[api_key] = timing - all_computed_results[api_key] = computed - - print(" Done!") - print() - - # Benchmark Newton (separate setup path) - if "cuda" in args_cli.device: - print("Benchmarking Isaac Lab XformPrimView (Newton)...") - timing, computed = benchmark_newton_xform_prim_view(num_iterations=args_cli.num_iterations) - all_timing_results["isaaclab-newton"] = timing - all_computed_results["isaaclab-newton"] = computed - print(" Done!") - print() - else: - print("Note: Skipping Newton benchmark (requires CUDA).\n") + pf = f"{args_cli.profile_dir}/{api_key.replace('-', '_')}_benchmark.prof" + profiler.dump_stats(pf) + profile_files[api_key] = pf + print(f" Profile saved to: {pf}") - # Print timing results - print_results(all_timing_results, args_cli.num_envs, args_cli.num_iterations) + all_timing[api_key] = timing + all_computed[api_key] = computed + print(" Done!\n") - # Compare computed results - print("\nComparing computed results across APIs...") - comparison_stats = compare_results(all_computed_results, tolerance=1e-6) - print_comparison_results(comparison_stats, tolerance=1e-4) + print_results(all_timing, args_cli.num_envs, args_cli.num_iterations) - # Print profiling instructions if enabled if args_cli.profile: - print("\n" + "=" * 100) - print("PROFILING RESULTS") - print("=" * 100) - print("Profile files have been saved. To visualize with snakeviz, run:") - for api_key, profile_file in profile_files.items(): - api_display = api_key.replace("-", " ").title() - print(f" # {api_display}") - print(f" snakeviz {profile_file}") - print("\nAlternatively, use pstats to analyze in terminal:") - print(" python -m pstats ") - print("=" * 100) + print("\nProfile files:") + for key, pf in profile_files.items(): + print(f" snakeviz {pf}") print() - # Clean up sim_utils.SimulationContext.clear_instance() diff --git a/source/isaaclab/test/sim/test_views_xform_prim.py b/source/isaaclab/test/sim/test_views_xform_prim.py index 640631653304..00a3f832acd2 100644 --- a/source/isaaclab/test/sim/test_views_xform_prim.py +++ b/source/isaaclab/test/sim/test_views_xform_prim.py @@ -3,1483 +3,302 @@ # # SPDX-License-Identifier: BSD-3-Clause -"""Launch Isaac Sim Simulator first.""" +"""USD backend tests for XformPrimView. + +Imports the shared contract tests and provides the USD-specific +``view_factory`` fixture. Also includes USD-only tests for visibility, +prim ordering, xformOp standardization, and Isaac Sim comparison. +""" from isaaclab.app import AppLauncher -# launch omniverse app simulation_app = AppLauncher(headless=True).app -"""Rest everything follows.""" - import pytest # noqa: E402 import torch # noqa: E402 import warp as wp # noqa: E402 +from pxr import Gf, UsdGeom # noqa: E402 + try: from isaacsim.core.prims import XFormPrim as _IsaacSimXformPrimView except (ModuleNotFoundError, ImportError): _IsaacSimXformPrimView = None - import isaaclab.sim as sim_utils # noqa: E402 -from isaaclab.sim.views import XformPrimView as XformPrimView # noqa: E402 +from isaaclab.sim.views import XformPrimView # noqa: E402 from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR # noqa: E402 +from xform_contract_tests import * # noqa: F401, F403, E402 +from xform_contract_tests import CHILD_OFFSET, ViewBundle # noqa: E402 + +PARENT_POS = (0.0, 0.0, 1.0) + @pytest.fixture(autouse=True) def test_setup_teardown(): - """Create a blank new stage for each test.""" - # Setup: Create a new stage sim_utils.create_new_stage() sim_utils.update_stage() - - # Yield for the test yield - - # Teardown: Clear stage after each test sim_utils.clear_stage() sim_utils.SimulationContext.clear_instance() -""" -Helper functions. -""" - - -def _prepare_indices(index_type, target_indices, num_prims, device): - """Helper function to prepare indices based on type.""" - if index_type == "list": - return target_indices, target_indices - elif index_type == "torch_tensor": - return torch.tensor(target_indices, dtype=torch.int64, device=device), target_indices - elif index_type == "slice_none": - return slice(None), list(range(num_prims)) - else: - raise ValueError(f"Unknown index type: {index_type}") - +# ------------------------------------------------------------------ +# Contract fixture +# ------------------------------------------------------------------ -def _skip_if_backend_unavailable(backend: str, device: str): - """Skip tests when the requested backend is unavailable.""" - if device.startswith("cuda") and not torch.cuda.is_available(): - pytest.skip("CUDA not available") - if backend == "fabric" and device == "cpu": - pytest.skip("Warp fabricarray operations on CPU have known issues") - - -def _prim_type_for_backend(backend: str) -> str: - """Return a prim type that is compatible with the backend.""" - return "Camera" if backend == "fabric" else "Xform" - - -def _create_view(pattern: str, device: str, backend: str) -> XformPrimView: - """Create an XformPrimView for the requested backend.""" - if backend == "fabric": - sim_utils.SimulationContext(sim_utils.SimulationCfg(dt=0.01, device=device, use_fabric=True)) - return XformPrimView(pattern, device=device) - - -""" -Tests - Initialization. -""" - - -@pytest.mark.parametrize("device", ["cpu", "cuda"]) -def test_xform_prim_view_initialization_single_prim(device): - """Test XformPrimView initialization with a single prim.""" - # check if CUDA is available - if device == "cuda" and not torch.cuda.is_available(): - pytest.skip("CUDA not available") - # Create a single xform prim +def _get_parent_positions(num_envs, device="cpu"): + """Read parent Xform positions from USD.""" stage = sim_utils.get_current_stage() - sim_utils.create_prim("/World/Object", "Xform", translation=(1.0, 2.0, 3.0), stage=stage) + xform_cache = UsdGeom.XformCache() + positions = [] + for i in range(num_envs): + prim = stage.GetPrimAtPath(f"/World/Parent_{i}") + tf = xform_cache.GetLocalToWorldTransform(prim) + t = tf.ExtractTranslation() + positions.append([float(t[0]), float(t[1]), float(t[2])]) + return torch.tensor(positions, dtype=torch.float32, device=device) - # Create view - view = XformPrimView("/World/Object", device=device) - # Verify properties - assert view.count == 1 - assert view.prim_paths == ["/World/Object"] - assert view.device == device - assert len(view.prims) == 1 +def _set_parent_positions(positions, num_envs): + """Write parent Xform positions to USD.""" + from pxr import Sdf, Vt # noqa: PLC0415 - -@pytest.mark.parametrize("device", ["cpu", "cuda"]) -def test_xform_prim_view_initialization_multiple_prims(device): - """Test XformPrimView initialization with multiple prims using pattern matching.""" - # check if CUDA is available - if device == "cuda" and not torch.cuda.is_available(): - pytest.skip("CUDA not available") - - # Create multiple prims - num_prims = 10 stage = sim_utils.get_current_stage() - for i in range(num_prims): - sim_utils.create_prim(f"/World/Env_{i}/Object", "Xform", translation=(i * 2.0, 0.0, 1.0), stage=stage) - - # Create view with pattern - view = XformPrimView("/World/Env_.*/Object", device=device) - - # Verify properties - assert view.count == num_prims - assert view.device == device - assert len(view.prims) == num_prims - assert view.prim_paths == [f"/World/Env_{i}/Object" for i in range(num_prims)] - - -@pytest.mark.parametrize("device", ["cpu", "cuda"]) -def test_xform_prim_view_initialization_multiple_prims_order(device): - """Test XformPrimView initialization with multiple prims using pattern matching with multiple objects per prim. - - This test validates that XformPrimView respects USD stage traversal order, which is based on - creation order (depth-first search), NOT alphabetical/lexical sorting. This is an important - edge case that ensures deterministic prim ordering that matches USD's internal representation. - - The test creates prims in a deliberately non-alphabetical order (1, 0, A, a, 2) and verifies - that they are retrieved in creation order, not sorted order (0, 1, 2, A, a). - """ - # check if CUDA is available - if device == "cuda" and not torch.cuda.is_available(): - pytest.skip("CUDA not available") - - # Create multiple prims - num_prims = 10 - stage = sim_utils.get_current_stage() - - # NOTE: Prims are created in a specific order to test that XformPrimView respects - # USD stage traversal order (DFS based on creation order), NOT alphabetical/lexical order. - # This is an important edge case: children under the same parent are returned in the - # order they were created, not sorted by name. - - # First batch: Create Object_1, Object_0, Object_A for each environment - # (intentionally non-alphabetical: 1, 0, A instead of 0, 1, A) - for i in range(num_prims): - sim_utils.create_prim(f"/World/Env_{i}/Object_1", "Xform", translation=(i * 2.0, -2.0, 1.0), stage=stage) - sim_utils.create_prim(f"/World/Env_{i}/Object_0", "Xform", translation=(i * 2.0, 2.0, 1.0), stage=stage) - sim_utils.create_prim(f"/World/Env_{i}/Object_A", "Xform", translation=(i * 2.0, 0.0, -1.0), stage=stage) - - # Second batch: Create Object_a, Object_2 for each environment - # (created after the first batch to verify traversal is depth-first per environment) - for i in range(num_prims): - sim_utils.create_prim(f"/World/Env_{i}/Object_a", "Xform", translation=(i * 2.0, 2.0, -1.0), stage=stage) - sim_utils.create_prim(f"/World/Env_{i}/Object_2", "Xform", translation=(i * 2.0, 2.0, 1.0), stage=stage) - - # Create view with pattern - view = XformPrimView("/World/Env_.*/Object_.*", device=device) - - # Expected ordering: DFS traversal by environment, with children in creation order - # For each Env_i, we expect: Object_1, Object_0, Object_A, Object_a, Object_2 - # (matches creation order, NOT alphabetical: would be 0, 1, 2, A, a if sorted) - expected_prim_paths_ordering = [] - for i in range(num_prims): - expected_prim_paths_ordering.append(f"/World/Env_{i}/Object_1") - expected_prim_paths_ordering.append(f"/World/Env_{i}/Object_0") - expected_prim_paths_ordering.append(f"/World/Env_{i}/Object_A") - expected_prim_paths_ordering.append(f"/World/Env_{i}/Object_a") - expected_prim_paths_ordering.append(f"/World/Env_{i}/Object_2") - - # Verify properties - assert view.count == num_prims * 5 - assert view.device == device - assert len(view.prims) == num_prims * 5 - assert view.prim_paths == expected_prim_paths_ordering - - # Additional validation: Verify ordering is NOT alphabetical - # If it were alphabetical, Object_0 would come before Object_1 - alphabetical_order = [] - for i in range(num_prims): - alphabetical_order.append(f"/World/Env_{i}/Object_0") - alphabetical_order.append(f"/World/Env_{i}/Object_1") - alphabetical_order.append(f"/World/Env_{i}/Object_2") - alphabetical_order.append(f"/World/Env_{i}/Object_A") - alphabetical_order.append(f"/World/Env_{i}/Object_a") - - assert view.prim_paths != alphabetical_order, ( - "Prim paths should follow creation order, not alphabetical order. " - "This test validates that USD stage traversal respects creation order." - ) - - -@pytest.mark.parametrize("device", ["cpu", "cuda"]) -def test_xform_prim_view_standardizes_transform_op(device): - """Test that XformPrimView standardizes a prim with xformOp:transform to translate/orient/scale.""" - from pxr import Gf, UsdGeom - - if device == "cuda" and not torch.cuda.is_available(): - pytest.skip("CUDA not available") - - expected_pos = (3.0, -1.0, 0.5) - matrix = Gf.Matrix4d(1.0) - matrix.SetTranslateOnly(Gf.Vec3d(*expected_pos)) - - stage = sim_utils.get_current_stage() - prim = stage.DefinePrim("/World/TransformPrim", "Xform") - UsdGeom.Xformable(prim).AddTransformOp().Set(matrix) - - view = XformPrimView("/World/TransformPrim", device=device) - - assert view.count == 1 - assert sim_utils.validate_standard_xform_ops(view.prims[0]) - - xformable = UsdGeom.Xformable(view.prims[0]) - ordered_ops = xformable.GetOrderedXformOps() - op_names = [op.GetOpName() for op in ordered_ops] - assert op_names == ["xformOp:translate", "xformOp:orient", "xformOp:scale"] - - assert ordered_ops[0].Get() == Gf.Vec3d(*expected_pos) - assert ordered_ops[1].Get() == Gf.Quatd(1.0, 0.0, 0.0, 0.0) - assert ordered_ops[2].Get() == Gf.Vec3d(1.0, 1.0, 1.0) - - -@pytest.mark.parametrize("device", ["cpu", "cuda"]) -def test_xform_prim_view_initialization_empty_pattern(device): - """Test XformPrimView initialization with pattern that matches no prims.""" - # check if CUDA is available - if device == "cuda" and not torch.cuda.is_available(): - pytest.skip("CUDA not available") - - sim_utils.create_new_stage() - - # Create view with pattern that matches nothing - view = XformPrimView("/World/NonExistent_.*", device=device) - - # Should have zero count - assert view.count == 0 - assert len(view.prims) == 0 - - -""" -Tests - Getters. -""" - - -@pytest.mark.parametrize("device", ["cpu", "cuda"]) -@pytest.mark.parametrize("backend", ["usd"]) -def test_get_world_poses(device, backend): - """Test getting world poses from XformPrimView.""" - _skip_if_backend_unavailable(backend, device) - - stage = sim_utils.get_current_stage() - prim_type = _prim_type_for_backend(backend) - - # Create prims with known world poses - expected_positions = [(1.0, 2.0, 3.0), (4.0, 5.0, 6.0), (7.0, 8.0, 9.0)] - expected_orientations = [(0.0, 0.0, 0.0, 1.0), (0.0, 0.0, 0.7071068, 0.7071068), (0.7071068, 0.0, 0.0, 0.7071068)] - - for i, (pos, quat) in enumerate(zip(expected_positions, expected_orientations)): - sim_utils.create_prim(f"/World/Object_{i}", prim_type, translation=pos, orientation=quat, stage=stage) - - # Create view - view = _create_view("/World/Object_.*", device=device, backend=backend) - - # Convert expected values to tensors - expected_positions_tensor = torch.tensor(expected_positions, dtype=torch.float32, device=device) - expected_orientations_tensor = torch.tensor(expected_orientations, dtype=torch.float32, device=device) - - # Get world poses - positions, orientations = view.get_world_poses() - positions, orientations = wp.to_torch(positions), wp.to_torch(orientations) - - # Verify shapes - assert positions.shape == (3, 3) - assert orientations.shape == (3, 4) - - # Verify positions - torch.testing.assert_close(positions, expected_positions_tensor, atol=1e-5, rtol=0) - - # Verify orientations (allow for quaternion sign ambiguity) - try: - torch.testing.assert_close(orientations, expected_orientations_tensor, atol=1e-5, rtol=0) - except AssertionError: - torch.testing.assert_close(orientations, -expected_orientations_tensor, atol=1e-5, rtol=0) - - -@pytest.mark.parametrize("device", ["cpu", "cuda"]) -@pytest.mark.parametrize("backend", ["usd"]) -def test_get_local_poses(device, backend): - """Test getting local poses from XformPrimView.""" - _skip_if_backend_unavailable(backend, device) - - stage = sim_utils.get_current_stage() - prim_type = _prim_type_for_backend(backend) - - # Create parent and child prims - sim_utils.create_prim("/World/Parent", "Xform", translation=(10.0, 0.0, 0.0), stage=stage) - - # Children with different local poses - expected_local_positions = [(1.0, 0.0, 0.0), (0.0, 2.0, 0.0), (0.0, 0.0, 3.0)] - expected_local_orientations = [ - (0.0, 0.0, 0.0, 1.0), - (0.0, 0.0, 0.7071068, 0.7071068), - (0.7071068, 0.0, 0.0, 0.7071068), - ] - - for i, (pos, quat) in enumerate(zip(expected_local_positions, expected_local_orientations)): - sim_utils.create_prim(f"/World/Parent/Child_{i}", prim_type, translation=pos, orientation=quat, stage=stage) - - # Create view - view = _create_view("/World/Parent/Child_.*", device=device, backend=backend) - - # Get local poses - translations, orientations = view.get_local_poses() - translations, orientations = wp.to_torch(translations), wp.to_torch(orientations) - - # Verify shapes - assert translations.shape == (3, 3) - assert orientations.shape == (3, 4) - - # Convert expected values to tensors - expected_translations_tensor = torch.tensor(expected_local_positions, dtype=torch.float32, device=device) - expected_orientations_tensor = torch.tensor(expected_local_orientations, dtype=torch.float32, device=device) - - # Verify translations - torch.testing.assert_close(translations, expected_translations_tensor, atol=1e-5, rtol=0) - - # Verify orientations (allow for quaternion sign ambiguity) - try: - torch.testing.assert_close(orientations, expected_orientations_tensor, atol=1e-5, rtol=0) - except AssertionError: - torch.testing.assert_close(orientations, -expected_orientations_tensor, atol=1e-5, rtol=0) - - -@pytest.mark.parametrize("device", ["cpu", "cuda"]) -@pytest.mark.parametrize("backend", ["usd"]) -def test_get_scales(device, backend): - """Test getting scales from XformPrimView.""" - _skip_if_backend_unavailable(backend, device) - - stage = sim_utils.get_current_stage() - prim_type = _prim_type_for_backend(backend) - - # Create prims with different scales - expected_scales = [(1.0, 1.0, 1.0), (2.0, 2.0, 2.0), (1.0, 2.0, 3.0)] - - for i, scale in enumerate(expected_scales): - sim_utils.create_prim(f"/World/Object_{i}", prim_type, scale=scale, stage=stage) - - # Create view - view = _create_view("/World/Object_.*", device=device, backend=backend) - - expected_scales_tensor = torch.tensor(expected_scales, dtype=torch.float32, device=device) - - # Get scales - scales = view.get_scales() - scales = wp.to_torch(scales) - - # Verify shape and values - assert scales.shape == (3, 3) - torch.testing.assert_close(scales, expected_scales_tensor, atol=1e-5, rtol=0) - - -@pytest.mark.parametrize("device", ["cpu", "cuda"]) -def test_get_visibility(device): - """Test getting visibility when all prims are visible.""" - if device == "cuda" and not torch.cuda.is_available(): - pytest.skip("CUDA not available") - - stage = sim_utils.get_current_stage() - - # Create prims (default is visible) - num_prims = 5 - for i in range(num_prims): - sim_utils.create_prim(f"/World/Object_{i}", "Xform", translation=(float(i), 0.0, 0.0), stage=stage) - - # Create view - view = XformPrimView("/World/Object_.*", device=device) - - # Get visibility - visibility = view.get_visibility() - - # Verify shape and values - assert visibility.shape == (num_prims,) - assert visibility.dtype == torch.bool - assert torch.all(visibility), "All prims should be visible by default" - - -""" -Tests - Setters. -""" - - -@pytest.mark.parametrize("device", ["cpu", "cuda"]) -@pytest.mark.parametrize("backend", ["usd"]) -def test_set_world_poses(device, backend): - """Test setting world poses in XformPrimView.""" - _skip_if_backend_unavailable(backend, device) - - stage = sim_utils.get_current_stage() - prim_type = _prim_type_for_backend(backend) - - # Create prims - num_prims = 5 - for i in range(num_prims): - sim_utils.create_prim(f"/World/Object_{i}", prim_type, translation=(0.0, 0.0, 0.0), stage=stage) - - # Create view - view = _create_view("/World/Object_.*", device=device, backend=backend) - - # Set new world poses - new_positions = torch.tensor( - [[1.0, 2.0, 3.0], [4.0, 5.0, 6.0], [7.0, 8.0, 9.0], [10.0, 11.0, 12.0], [13.0, 14.0, 15.0]], device=device - ) - new_orientations = torch.tensor( - [ - [0.0, 0.0, 0.0, 1.0], - [0.0, 0.0, 0.7071068, 0.7071068], - [0.7071068, 0.0, 0.0, 0.7071068], - [0.3826834, 0.0, 0.0, 0.9238795], - [0.0, 0.7071068, 0.0, 0.7071068], - ], - device=device, - ) - - view.set_world_poses(new_positions, new_orientations) - - # Get the poses back - retrieved_positions, retrieved_orientations = view.get_world_poses() - retrieved_positions, retrieved_orientations = wp.to_torch(retrieved_positions), wp.to_torch(retrieved_orientations) - - # Verify they match - torch.testing.assert_close(retrieved_positions, new_positions, atol=1e-5, rtol=0) - # Check quaternions (allow sign flip) - try: - torch.testing.assert_close(retrieved_orientations, new_orientations, atol=1e-5, rtol=0) - except AssertionError: - torch.testing.assert_close(retrieved_orientations, -new_orientations, atol=1e-5, rtol=0) - - -@pytest.mark.parametrize("device", ["cpu", "cuda"]) -@pytest.mark.parametrize("backend", ["usd"]) -def test_set_world_poses_only_positions(device, backend): - """Test setting only positions, leaving orientations unchanged.""" - _skip_if_backend_unavailable(backend, device) - - stage = sim_utils.get_current_stage() - prim_type = _prim_type_for_backend(backend) - - # Create prims with specific orientations - initial_quat = (0.0, 0.0, 0.7071068, 0.7071068) # 90 deg around Z - for i in range(3): - sim_utils.create_prim( - f"/World/Object_{i}", prim_type, translation=(0.0, 0.0, 0.0), orientation=initial_quat, stage=stage - ) - - # Create view - view = _create_view("/World/Object_.*", device=device, backend=backend) - - # Get initial orientations - _, initial_orientations = view.get_world_poses() - initial_orientations = wp.to_torch(initial_orientations) - - # Set only positions - new_positions = torch.tensor([[1.0, 0.0, 0.0], [0.0, 2.0, 0.0], [0.0, 0.0, 3.0]], device=device) - view.set_world_poses(positions=new_positions, orientations=None) - - # Get poses back - retrieved_positions, retrieved_orientations = view.get_world_poses() - retrieved_positions, retrieved_orientations = wp.to_torch(retrieved_positions), wp.to_torch(retrieved_orientations) - - # Positions should be updated - torch.testing.assert_close(retrieved_positions, new_positions, atol=1e-5, rtol=0) - - # Orientations should be unchanged - try: - torch.testing.assert_close(retrieved_orientations, initial_orientations, atol=1e-5, rtol=0) - except AssertionError: - torch.testing.assert_close(retrieved_orientations, -initial_orientations, atol=1e-5, rtol=0) - - -@pytest.mark.parametrize("device", ["cpu", "cuda"]) -@pytest.mark.parametrize("backend", ["usd"]) -def test_set_world_poses_only_orientations(device, backend): - """Test setting only orientations, leaving positions unchanged.""" - _skip_if_backend_unavailable(backend, device) - - stage = sim_utils.get_current_stage() - prim_type = _prim_type_for_backend(backend) - - # Create prims with specific positions - for i in range(3): - sim_utils.create_prim(f"/World/Object_{i}", prim_type, translation=(float(i), 0.0, 0.0), stage=stage) - - # Create view - view = _create_view("/World/Object_.*", device=device, backend=backend) - - # Get initial positions - initial_positions, _ = view.get_world_poses() - initial_positions = wp.to_torch(initial_positions) - - # Set only orientations - new_orientations = torch.tensor( - [[0.0, 0.0, 0.7071068, 0.7071068], [0.7071068, 0.0, 0.0, 0.7071068], [0.3826834, 0.0, 0.0, 0.9238795]], - device=device, - ) - view.set_world_poses(positions=None, orientations=new_orientations) - - # Get poses back - retrieved_positions, retrieved_orientations = view.get_world_poses() - retrieved_positions, retrieved_orientations = wp.to_torch(retrieved_positions), wp.to_torch(retrieved_orientations) - - # Positions should be unchanged - torch.testing.assert_close(retrieved_positions, initial_positions, atol=1e-5, rtol=0) - - # Orientations should be updated - try: - torch.testing.assert_close(retrieved_orientations, new_orientations, atol=1e-5, rtol=0) - except AssertionError: - torch.testing.assert_close(retrieved_orientations, -new_orientations, atol=1e-5, rtol=0) - - -@pytest.mark.parametrize("device", ["cpu", "cuda"]) -@pytest.mark.parametrize("backend", ["usd"]) -def test_set_world_poses_with_hierarchy(device, backend): - """Test setting world poses correctly handles parent transformations.""" - _skip_if_backend_unavailable(backend, device) - - stage = sim_utils.get_current_stage() - child_prim_type = _prim_type_for_backend(backend) - - # Create parent prims - for i in range(3): - parent_pos = (i * 10.0, 0.0, 0.0) - parent_quat = (0.0, 0.0, 0.7071068, 0.7071068) # 90 deg around Z - sim_utils.create_prim( - f"/World/Parent_{i}", "Xform", translation=parent_pos, orientation=parent_quat, stage=stage - ) - # Create child prims - sim_utils.create_prim(f"/World/Parent_{i}/Child", child_prim_type, translation=(0.0, 0.0, 0.0), stage=stage) - - # Create view for children - view = _create_view("/World/Parent_.*/Child", device=device, backend=backend) - - # Set world poses for children - desired_world_positions = torch.tensor([[5.0, 5.0, 0.0], [15.0, 5.0, 0.0], [25.0, 5.0, 0.0]], device=device) - desired_world_orientations = torch.tensor( - [[0.0, 0.0, 0.0, 1.0], [0.0, 0.0, 0.0, 1.0], [0.0, 0.0, 0.0, 1.0]], device=device - ) - - view.set_world_poses(desired_world_positions, desired_world_orientations) - - # Get world poses back - retrieved_positions, retrieved_orientations = view.get_world_poses() - retrieved_positions, retrieved_orientations = wp.to_torch(retrieved_positions), wp.to_torch(retrieved_orientations) - - # Should match desired world poses - torch.testing.assert_close(retrieved_positions, desired_world_positions, atol=1e-4, rtol=0) - try: - torch.testing.assert_close(retrieved_orientations, desired_world_orientations, atol=1e-4, rtol=0) - except AssertionError: - torch.testing.assert_close(retrieved_orientations, -desired_world_orientations, atol=1e-4, rtol=0) - - -@pytest.mark.parametrize("device", ["cpu", "cuda"]) -@pytest.mark.parametrize("backend", ["usd"]) -def test_set_local_poses(device, backend): - """Test setting local poses in XformPrimView.""" - _skip_if_backend_unavailable(backend, device) - - stage = sim_utils.get_current_stage() - prim_type = _prim_type_for_backend(backend) - - # Create parent - sim_utils.create_prim("/World/Parent", "Xform", translation=(5.0, 5.0, 5.0), stage=stage) - - # Create children - num_prims = 4 - for i in range(num_prims): - sim_utils.create_prim(f"/World/Parent/Child_{i}", prim_type, translation=(0.0, 0.0, 0.0), stage=stage) - - # Create view - view = _create_view("/World/Parent/Child_.*", device=device, backend=backend) - - # Set new local poses - new_translations = torch.tensor([[1.0, 0.0, 0.0], [0.0, 2.0, 0.0], [0.0, 0.0, 3.0], [4.0, 4.0, 4.0]], device=device) - new_orientations = torch.tensor( - [ - [0.0, 0.0, 0.0, 1.0], - [0.0, 0.0, 0.7071068, 0.7071068], - [0.7071068, 0.0, 0.0, 0.7071068], - [0.3826834, 0.0, 0.0, 0.9238795], - ], - device=device, - ) + with Sdf.ChangeBlock(): + for i in range(num_envs): + prim = stage.GetPrimAtPath(f"/World/Parent_{i}") + pos = positions[i] + prim.GetAttribute("xformOp:translate").Set(Gf.Vec3d(float(pos[0]), float(pos[1]), float(pos[2]))) - view.set_local_poses(new_translations, new_orientations) - # Get local poses back - retrieved_translations, retrieved_orientations = view.get_local_poses() - retrieved_translations, retrieved_orientations = ( - wp.to_torch(retrieved_translations), - wp.to_torch(retrieved_orientations), - ) - - # Verify they match - torch.testing.assert_close(retrieved_translations, new_translations, atol=1e-5, rtol=0) - try: - torch.testing.assert_close(retrieved_orientations, new_orientations, atol=1e-5, rtol=0) - except AssertionError: - torch.testing.assert_close(retrieved_orientations, -new_orientations, atol=1e-5, rtol=0) - - -@pytest.mark.parametrize("device", ["cpu", "cuda"]) -@pytest.mark.parametrize("backend", ["usd"]) -def test_set_local_poses_only_translations(device, backend): - """Test setting only local translations.""" - _skip_if_backend_unavailable(backend, device) - - stage = sim_utils.get_current_stage() - prim_type = _prim_type_for_backend(backend) +@pytest.fixture +def view_factory(): + """USD factory: parent Xform at PARENT_POS + child Xform at CHILD_OFFSET.""" - # Create parent and children with specific orientations - sim_utils.create_prim("/World/Parent", "Xform", translation=(0.0, 0.0, 0.0), stage=stage) - initial_quat = (0.0, 0.0, 0.7071068, 0.7071068) + def factory(num_envs: int, device: str) -> ViewBundle: + stage = sim_utils.get_current_stage() + for i in range(num_envs): + sim_utils.create_prim( + f"/World/Parent_{i}", "Xform", translation=PARENT_POS, stage=stage + ) + sim_utils.create_prim( + f"/World/Parent_{i}/Child", "Xform", translation=CHILD_OFFSET, stage=stage + ) - for i in range(3): - sim_utils.create_prim( - f"/World/Parent/Child_{i}", - prim_type, - translation=(0.0, 0.0, 0.0), - orientation=initial_quat, - stage=stage, + view = XformPrimView("/World/Parent_.*/Child", device=device) + return ViewBundle( + view=view, + get_parent_pos=_get_parent_positions, + set_parent_pos=_set_parent_positions, + teardown=lambda: None, ) - # Create view - view = _create_view("/World/Parent/Child_.*", device=device, backend=backend) - - # Get initial orientations - _, initial_orientations = view.get_local_poses() - initial_orientations = wp.to_torch(initial_orientations) - - # Set only translations - new_translations = torch.tensor([[1.0, 0.0, 0.0], [0.0, 2.0, 0.0], [0.0, 0.0, 3.0]], device=device) - view.set_local_poses(translations=new_translations, orientations=None) + return factory - # Get poses back - retrieved_translations, retrieved_orientations = view.get_local_poses() - retrieved_translations, retrieved_orientations = ( - wp.to_torch(retrieved_translations), - wp.to_torch(retrieved_orientations), - ) - - # Translations should be updated - torch.testing.assert_close(retrieved_translations, new_translations, atol=1e-5, rtol=0) - # Orientations should be unchanged - try: - torch.testing.assert_close(retrieved_orientations, initial_orientations, atol=1e-5, rtol=0) - except AssertionError: - torch.testing.assert_close(retrieved_orientations, -initial_orientations, atol=1e-5, rtol=0) +# ================================================================== +# USD-only: Visibility +# ================================================================== @pytest.mark.parametrize("device", ["cpu", "cuda"]) -@pytest.mark.parametrize("backend", ["usd"]) -def test_set_scales(device, backend): - """Test setting scales in XformPrimView.""" - _skip_if_backend_unavailable(backend, device) - - stage = sim_utils.get_current_stage() - prim_type = _prim_type_for_backend(backend) - - # Create prims - num_prims = 5 - for i in range(num_prims): - sim_utils.create_prim(f"/World/Object_{i}", prim_type, scale=(1.0, 1.0, 1.0), stage=stage) - - # Create view - view = _create_view("/World/Object_.*", device=device, backend=backend) - - # Set new scales - new_scales = torch.tensor( - [[2.0, 2.0, 2.0], [1.0, 2.0, 3.0], [0.5, 0.5, 0.5], [3.0, 1.0, 2.0], [1.5, 1.5, 1.5]], device=device - ) - - view.set_scales(new_scales) - - # Get scales back - retrieved_scales = view.get_scales() - retrieved_scales = wp.to_torch(retrieved_scales) - - # Verify they match - torch.testing.assert_close(retrieved_scales, new_scales, atol=1e-5, rtol=0) - - -@pytest.mark.parametrize("device", ["cpu", "cuda"]) -def test_set_visibility(device): +def test_visibility_toggle(device): """Test toggling visibility multiple times.""" if device == "cuda" and not torch.cuda.is_available(): pytest.skip("CUDA not available") stage = sim_utils.get_current_stage() - - # Create prims num_prims = 3 for i in range(num_prims): sim_utils.create_prim(f"/World/Object_{i}", "Xform", stage=stage) - # Create view view = XformPrimView("/World/Object_.*", device=device) - # Initial state: all visible - visibility = view.get_visibility() - assert torch.all(visibility), "All should be visible initially" + assert torch.all(view.get_visibility()) - # Make all invisible view.set_visibility(torch.zeros(num_prims, dtype=torch.bool, device=device)) - visibility = view.get_visibility() - assert not torch.any(visibility), "All should be invisible" + assert not torch.any(view.get_visibility()) - # Make all visible again view.set_visibility(torch.ones(num_prims, dtype=torch.bool, device=device)) - visibility = view.get_visibility() - assert torch.all(visibility), "All should be visible again" + assert torch.all(view.get_visibility()) - # Toggle individual prims view.set_visibility(torch.tensor([False], dtype=torch.bool, device=device), indices=[1]) - visibility = view.get_visibility() - assert visibility[0] and not visibility[1] and visibility[2], "Only middle prim should be invisible" - - -""" -Tests - Index Handling. -""" + vis = view.get_visibility() + assert vis[0] and not vis[1] and vis[2] @pytest.mark.parametrize("device", ["cpu", "cuda"]) -@pytest.mark.parametrize("index_type", ["list", "torch_tensor", "slice_none"]) -@pytest.mark.parametrize("method", ["world_poses", "local_poses", "scales", "visibility"]) -def test_index_types_get_methods(device, index_type, method): - """Test that getter methods work with different index types.""" +def test_visibility_parent_inheritance(device): + """Making a parent invisible hides all children.""" if device == "cuda" and not torch.cuda.is_available(): pytest.skip("CUDA not available") stage = sim_utils.get_current_stage() + sim_utils.create_prim("/World/Parent", "Xform", stage=stage) + for i in range(4): + sim_utils.create_prim(f"/World/Parent/Child_{i}", "Xform", stage=stage) - # Create prims based on method type - num_prims = 10 - if method == "local_poses": - # Create parent and children for local poses - sim_utils.create_prim("/World/Parent", "Xform", translation=(10.0, 0.0, 0.0), stage=stage) - for i in range(num_prims): - sim_utils.create_prim( - f"/World/Parent/Child_{i}", "Xform", translation=(float(i), float(i) * 0.5, 0.0), stage=stage - ) - view = XformPrimView("/World/Parent/Child_.*", device=device) - elif method == "scales": - # Create prims with different scales - for i in range(num_prims): - scale = (1.0 + i * 0.5, 1.0 + i * 0.3, 1.0 + i * 0.2) - sim_utils.create_prim(f"/World/Object_{i}", "Xform", scale=scale, stage=stage) - view = XformPrimView("/World/Object_.*", device=device) - else: # world_poses - # Create prims with different positions - for i in range(num_prims): - sim_utils.create_prim(f"/World/Object_{i}", "Xform", translation=(float(i), 0.0, 0.0), stage=stage) - view = XformPrimView("/World/Object_.*", device=device) - - # Get all data as reference - if method == "world_poses": - all_data1, all_data2 = view.get_world_poses() - all_data1, all_data2 = wp.to_torch(all_data1), wp.to_torch(all_data2) - elif method == "local_poses": - all_data1, all_data2 = view.get_local_poses() - all_data1, all_data2 = wp.to_torch(all_data1), wp.to_torch(all_data2) - elif method == "scales": - all_data1 = view.get_scales() - all_data1 = wp.to_torch(all_data1) - all_data2 = None - else: # visibility - all_data1 = view.get_visibility() - all_data2 = None - - # Prepare indices - target_indices_base = [2, 5, 7] - indices, target_indices = _prepare_indices(index_type, target_indices_base, num_prims, device) - - # Get subset - if method == "world_poses": - subset_data1, subset_data2 = view.get_world_poses(indices=indices) # type: ignore[arg-type] - subset_data1, subset_data2 = wp.to_torch(subset_data1), wp.to_torch(subset_data2) - elif method == "local_poses": - subset_data1, subset_data2 = view.get_local_poses(indices=indices) # type: ignore[arg-type] - subset_data1, subset_data2 = wp.to_torch(subset_data1), wp.to_torch(subset_data2) - elif method == "scales": - subset_data1 = view.get_scales(indices=indices) # type: ignore[arg-type] - subset_data1 = wp.to_torch(subset_data1) - subset_data2 = None - else: # visibility - subset_data1 = view.get_visibility(indices=indices) # type: ignore[arg-type] - subset_data2 = None - - # Verify shapes - expected_count = len(target_indices) - if method == "visibility": - assert subset_data1.shape == (expected_count,) - else: - assert subset_data1.shape == (expected_count, 3) - if subset_data2 is not None: - assert subset_data2.shape == (expected_count, 4) - - # Verify values - target_indices_tensor = torch.tensor(target_indices, dtype=torch.int64, device=device) - torch.testing.assert_close(subset_data1, all_data1[target_indices_tensor], atol=1e-5, rtol=0) - if subset_data2 is not None and all_data2 is not None: - torch.testing.assert_close(subset_data2, all_data2[target_indices_tensor], atol=1e-5, rtol=0) - - -@pytest.mark.parametrize("device", ["cpu", "cuda"]) -@pytest.mark.parametrize("index_type", ["list", "torch_tensor", "slice_none"]) -@pytest.mark.parametrize("method", ["world_poses", "local_poses", "scales", "visibility"]) -def test_index_types_set_methods(device, index_type, method): - """Test that setter methods work with different index types.""" - if device == "cuda" and not torch.cuda.is_available(): - pytest.skip("CUDA not available") - - stage = sim_utils.get_current_stage() - - # Create prims based on method type - num_prims = 10 - if method == "local_poses": - # Create parent and children for local poses - sim_utils.create_prim("/World/Parent", "Xform", translation=(5.0, 5.0, 0.0), stage=stage) - for i in range(num_prims): - sim_utils.create_prim(f"/World/Parent/Child_{i}", "Xform", translation=(float(i), 0.0, 0.0), stage=stage) - view = XformPrimView("/World/Parent/Child_.*", device=device) - else: # world_poses or scales - for i in range(num_prims): - sim_utils.create_prim(f"/World/Object_{i}", "Xform", translation=(0.0, 0.0, 0.0), stage=stage) - view = XformPrimView("/World/Object_.*", device=device) - - # Get initial data - if method == "world_poses": - initial_data1, initial_data2 = view.get_world_poses() - initial_data1, initial_data2 = wp.to_torch(initial_data1), wp.to_torch(initial_data2) - elif method == "local_poses": - initial_data1, initial_data2 = view.get_local_poses() - initial_data1, initial_data2 = wp.to_torch(initial_data1), wp.to_torch(initial_data2) - elif method == "scales": - initial_data1 = view.get_scales() - initial_data1 = wp.to_torch(initial_data1) - initial_data2 = None - else: # visibility - initial_data1 = view.get_visibility() - initial_data2 = None - - # Prepare indices - target_indices_base = [2, 5, 7] - indices, target_indices = _prepare_indices(index_type, target_indices_base, num_prims, device) - - # Prepare new data - num_to_set = len(target_indices) - if method in ["world_poses", "local_poses"]: - new_data1 = torch.randn(num_to_set, 3, device=device) * 10.0 - new_data2 = torch.tensor([[0.0, 0.0, 0.0, 1.0]] * num_to_set, dtype=torch.float32, device=device) - elif method == "scales": - new_data1 = torch.rand(num_to_set, 3, device=device) * 2.0 + 0.5 - new_data2 = None - else: # visibility - # Set to False to test change (default is True) - new_data1 = torch.zeros(num_to_set, dtype=torch.bool, device=device) - new_data2 = None - - # Set data - if method == "world_poses": - view.set_world_poses(positions=new_data1, orientations=new_data2, indices=indices) # type: ignore[arg-type] - elif method == "local_poses": - view.set_local_poses(translations=new_data1, orientations=new_data2, indices=indices) # type: ignore[arg-type] - elif method == "scales": - view.set_scales(scales=new_data1, indices=indices) # type: ignore[arg-type] - else: # visibility - view.set_visibility(visibility=new_data1, indices=indices) # type: ignore[arg-type] - - # Get all data after update - if method == "world_poses": - updated_data1, updated_data2 = view.get_world_poses() - updated_data1, updated_data2 = wp.to_torch(updated_data1), wp.to_torch(updated_data2) - elif method == "local_poses": - updated_data1, updated_data2 = view.get_local_poses() - updated_data1, updated_data2 = wp.to_torch(updated_data1), wp.to_torch(updated_data2) - elif method == "scales": - updated_data1 = view.get_scales() - updated_data1 = wp.to_torch(updated_data1) - updated_data2 = None - else: # visibility - updated_data1 = view.get_visibility() - updated_data2 = None - - # Verify that specified indices were updated - for i, target_idx in enumerate(target_indices): - torch.testing.assert_close(updated_data1[target_idx], new_data1[i], atol=1e-5, rtol=0) - if new_data2 is not None and updated_data2 is not None: - try: - torch.testing.assert_close(updated_data2[target_idx], new_data2[i], atol=1e-5, rtol=0) - except AssertionError: - # Account for quaternion sign ambiguity - torch.testing.assert_close(updated_data2[target_idx], -new_data2[i], atol=1e-5, rtol=0) - - # Verify that other indices were NOT updated (only for non-slice(None) cases) - if index_type != "slice_none": - for i in range(num_prims): - if i not in target_indices: - torch.testing.assert_close(updated_data1[i], initial_data1[i], atol=1e-5, rtol=0) - if initial_data2 is not None and updated_data2 is not None: - try: - torch.testing.assert_close(updated_data2[i], initial_data2[i], atol=1e-5, rtol=0) - except AssertionError: - # Account for quaternion sign ambiguity - torch.testing.assert_close(updated_data2[i], -initial_data2[i], atol=1e-5, rtol=0) - - -@pytest.mark.parametrize("device", ["cpu", "cuda"]) -@pytest.mark.parametrize("backend", ["usd"]) -def test_indices_single_element(device, backend): - """Test with a single index.""" - _skip_if_backend_unavailable(backend, device) - - stage = sim_utils.get_current_stage() - prim_type = _prim_type_for_backend(backend) - - # Create prims - num_prims = 5 - for i in range(num_prims): - sim_utils.create_prim(f"/World/Object_{i}", prim_type, translation=(float(i), 0.0, 0.0), stage=stage) - - # Create view - view = _create_view("/World/Object_.*", device=device, backend=backend) - - # Test with single index - indices = [3] - positions, orientations = view.get_world_poses(indices=indices) - positions, orientations = wp.to_torch(positions), wp.to_torch(orientations) - - # Verify shapes - assert positions.shape == (1, 3) - assert orientations.shape == (1, 4) - - # Set pose for single index - new_position = torch.tensor([[100.0, 200.0, 300.0]], device=device) - view.set_world_poses(positions=new_position, indices=indices) - - # Verify it was set - retrieved_positions, _quat = view.get_world_poses(indices=indices) - retrieved_positions, _quat = wp.to_torch(retrieved_positions), wp.to_torch(_quat) - torch.testing.assert_close(retrieved_positions, new_position, atol=1e-5, rtol=0) - - -@pytest.mark.parametrize("device", ["cpu", "cuda"]) -@pytest.mark.parametrize("backend", ["usd"]) -def test_indices_out_of_order(device, backend): - """Test with indices provided in non-sequential order.""" - _skip_if_backend_unavailable(backend, device) - - stage = sim_utils.get_current_stage() - prim_type = _prim_type_for_backend(backend) - - # Create prims - num_prims = 10 - for i in range(num_prims): - sim_utils.create_prim(f"/World/Object_{i}", prim_type, translation=(0.0, 0.0, 0.0), stage=stage) - - # Create view - view = _create_view("/World/Object_.*", device=device, backend=backend) - - # Use out-of-order indices - indices = [7, 2, 9, 0, 5] - new_positions = torch.tensor( - [[7.0, 0.0, 0.0], [2.0, 0.0, 0.0], [9.0, 0.0, 0.0], [0.0, 0.0, 0.0], [5.0, 0.0, 0.0]], device=device - ) - - # Set poses with out-of-order indices - view.set_world_poses(positions=new_positions, indices=indices) - - # Get all poses - all_positions, _quat = view.get_world_poses() - all_positions, _quat = wp.to_torch(all_positions), wp.to_torch(_quat) - - # Verify each index got the correct value - expected_x_values = [0.0, 0.0, 2.0, 0.0, 0.0, 5.0, 0.0, 7.0, 0.0, 9.0] - for i in range(num_prims): - assert abs(all_positions[i, 0].item() - expected_x_values[i]) < 1e-5 - - -@pytest.mark.parametrize("device", ["cpu", "cuda"]) -@pytest.mark.parametrize("backend", ["usd"]) -def test_indices_with_only_positions_or_orientations(device, backend): - """Test indices work correctly when setting only positions or only orientations.""" - _skip_if_backend_unavailable(backend, device) - - stage = sim_utils.get_current_stage() - prim_type = _prim_type_for_backend(backend) - - # Create prims - num_prims = 5 - for i in range(num_prims): - sim_utils.create_prim( - f"/World/Object_{i}", - prim_type, - translation=(0.0, 0.0, 0.0), - orientation=(0.0, 0.0, 0.0, 1.0), - stage=stage, - ) - - # Create view - view = _create_view("/World/Object_.*", device=device, backend=backend) - - # Get initial poses - initial_positions, initial_orientations = view.get_world_poses() - initial_positions, initial_orientations = wp.to_torch(initial_positions), wp.to_torch(initial_orientations) - - # Set only positions for specific indices - indices = [1, 3] - new_positions = torch.tensor([[10.0, 0.0, 0.0], [30.0, 0.0, 0.0]], device=device) - view.set_world_poses(positions=new_positions, orientations=None, indices=indices) - - # Get updated poses - updated_positions, updated_orientations = view.get_world_poses() - updated_positions, updated_orientations = wp.to_torch(updated_positions), wp.to_torch(updated_orientations) - - # Verify positions updated for indices 1 and 3, others unchanged - torch.testing.assert_close(updated_positions[1], new_positions[0], atol=1e-5, rtol=0) - torch.testing.assert_close(updated_positions[3], new_positions[1], atol=1e-5, rtol=0) - torch.testing.assert_close(updated_positions[0], initial_positions[0], atol=1e-5, rtol=0) - - # Verify all orientations unchanged - try: - torch.testing.assert_close(updated_orientations, initial_orientations, atol=1e-5, rtol=0) - except AssertionError: - torch.testing.assert_close(updated_orientations, -initial_orientations, atol=1e-5, rtol=0) + parent_view = XformPrimView("/World/Parent", device=device) + children_view = XformPrimView("/World/Parent/Child_.*", device=device) - # Now set only orientations for different indices - indices2 = [0, 4] - new_orientations = torch.tensor([[0.0, 0.0, 0.7071068, 0.7071068], [0.7071068, 0.0, 0.0, 0.7071068]], device=device) - view.set_world_poses(positions=None, orientations=new_orientations, indices=indices2) + parent_view.set_visibility(torch.tensor([False], dtype=torch.bool, device=device)) + assert not torch.any(children_view.get_visibility()) - # Get final poses - final_positions, final_orientations = view.get_world_poses() - final_positions, final_orientations = wp.to_torch(final_positions), wp.to_torch(final_orientations) + parent_view.set_visibility(torch.tensor([True], dtype=torch.bool, device=device)) + assert torch.all(children_view.get_visibility()) - # Verify positions unchanged from previous step - torch.testing.assert_close(final_positions, updated_positions, atol=1e-5, rtol=0) - # Verify orientations updated for indices 0 and 4 - try: - torch.testing.assert_close(final_orientations[0], new_orientations[0], atol=1e-5, rtol=0) - torch.testing.assert_close(final_orientations[4], new_orientations[1], atol=1e-5, rtol=0) - except AssertionError: - # Account for quaternion sign ambiguity - torch.testing.assert_close(final_orientations[0], -new_orientations[0], atol=1e-5, rtol=0) - torch.testing.assert_close(final_orientations[4], -new_orientations[1], atol=1e-5, rtol=0) +# ================================================================== +# USD-only: Prim ordering +# ================================================================== @pytest.mark.parametrize("device", ["cpu", "cuda"]) -def test_index_type_none_equivalent_to_all(device): - """Test that indices=None is equivalent to getting/setting all prims.""" +def test_prim_ordering_follows_creation_order(device): + """Prims are returned in USD creation order (DFS), not alphabetical.""" if device == "cuda" and not torch.cuda.is_available(): pytest.skip("CUDA not available") stage = sim_utils.get_current_stage() + num_envs = 3 + for i in range(num_envs): + sim_utils.create_prim(f"/World/Env_{i}/Object_1", "Xform", stage=stage) + sim_utils.create_prim(f"/World/Env_{i}/Object_0", "Xform", stage=stage) + sim_utils.create_prim(f"/World/Env_{i}/Object_A", "Xform", stage=stage) - # Create prims - num_prims = 6 - for i in range(num_prims): - sim_utils.create_prim(f"/World/Object_{i}", "Xform", translation=(float(i), 0.0, 0.0), stage=stage) - - # Create view - view = XformPrimView("/World/Object_.*", device=device) + view = XformPrimView("/World/Env_.*/Object_.*", device=device) + expected = [] + for i in range(num_envs): + expected += [f"/World/Env_{i}/Object_1", f"/World/Env_{i}/Object_0", f"/World/Env_{i}/Object_A"] - # Get poses with indices=None - pos_none, quat_none = view.get_world_poses(indices=None) - pos_none, quat_none = wp.to_torch(pos_none), wp.to_torch(quat_none) - - # Get poses with no argument (default) - pos_default, quat_default = view.get_world_poses() - pos_default, quat_default = wp.to_torch(pos_default), wp.to_torch(quat_default) - - # Get poses with slice(None) - pos_slice, quat_slice = view.get_world_poses(indices=slice(None)) # type: ignore[arg-type] - pos_slice, quat_slice = wp.to_torch(pos_slice), wp.to_torch(quat_slice) - - # All should be equivalent - torch.testing.assert_close(pos_none, pos_default, atol=1e-10, rtol=0) - torch.testing.assert_close(quat_none, quat_default, atol=1e-10, rtol=0) - torch.testing.assert_close(pos_none, pos_slice, atol=1e-10, rtol=0) - torch.testing.assert_close(quat_none, quat_slice, atol=1e-10, rtol=0) - - # Test the same for set operations - new_positions = torch.randn(num_prims, 3, device=device) * 10.0 - new_orientations = torch.tensor([[0.0, 0.0, 0.0, 1.0]] * num_prims, dtype=torch.float32, device=device) - - # Set with indices=None - view.set_world_poses(positions=new_positions, orientations=new_orientations, indices=None) - pos_after_none, quat_after_none = view.get_world_poses() - pos_after_none, quat_after_none = wp.to_torch(pos_after_none), wp.to_torch(quat_after_none) - - # Reset - view.set_world_poses(positions=torch.zeros(num_prims, 3, device=device), indices=None) - - # Set with slice(None) - view.set_world_poses( - positions=new_positions, - orientations=new_orientations, - indices=slice(None), # type: ignore[arg-type] - ) - pos_after_slice, quat_after_slice = view.get_world_poses() - pos_after_slice, quat_after_slice = wp.to_torch(pos_after_slice), wp.to_torch(quat_after_slice) + assert view.prim_paths == expected - # Should be equivalent - torch.testing.assert_close(pos_after_none, pos_after_slice, atol=1e-5, rtol=0) - torch.testing.assert_close(quat_after_none, quat_after_slice, atol=1e-5, rtol=0) - -""" -Tests - Integration. -""" +# ================================================================== +# USD-only: xformOp standardization +# ================================================================== @pytest.mark.parametrize("device", ["cpu", "cuda"]) -def test_with_franka_robots(device): - """Test XformPrimView with real Franka robot USD assets.""" +def test_standardize_transform_op(device): + """XformPrimView standardizes a prim with xformOp:transform to translate/orient/scale.""" if device == "cuda" and not torch.cuda.is_available(): pytest.skip("CUDA not available") - stage = sim_utils.get_current_stage() - - # Load Franka robot assets - franka_usd_path = f"{ISAAC_NUCLEUS_DIR}/Robots/FrankaRobotics/FrankaPanda/franka.usd" - - # Add two Franka robots to the stage - sim_utils.create_prim("/World/Franka_1", "Xform", usd_path=franka_usd_path, stage=stage) - sim_utils.create_prim("/World/Franka_2", "Xform", usd_path=franka_usd_path, stage=stage) - - # Create view for both Frankas - frankas_view = XformPrimView("/World/Franka_.*", device=device) - - # Verify count - assert frankas_view.count == 2 - - # Get initial world poses (should be at origin) - initial_positions, initial_orientations = frankas_view.get_world_poses() - initial_positions, initial_orientations = wp.to_torch(initial_positions), wp.to_torch(initial_orientations) - - # Verify initial positions are at origin - expected_initial_positions = torch.zeros(2, 3, device=device) - torch.testing.assert_close(initial_positions, expected_initial_positions, atol=1e-5, rtol=0) + expected_pos = (3.0, -1.0, 0.5) + matrix = Gf.Matrix4d(1.0) + matrix.SetTranslateOnly(Gf.Vec3d(*expected_pos)) - # Verify initial orientations are identity - expected_initial_orientations = torch.tensor([[0.0, 0.0, 0.0, 1.0], [0.0, 0.0, 0.0, 1.0]], device=device) - try: - torch.testing.assert_close(initial_orientations, expected_initial_orientations, atol=1e-5, rtol=0) - except AssertionError: - torch.testing.assert_close(initial_orientations, -expected_initial_orientations, atol=1e-5, rtol=0) + stage = sim_utils.get_current_stage() + prim = stage.DefinePrim("/World/TransformPrim", "Xform") + UsdGeom.Xformable(prim).AddTransformOp().Set(matrix) - # Set new world poses - new_positions = torch.tensor([[10.0, 10.0, 0.0], [-40.0, -40.0, 0.0]], device=device) - # 90° rotation around Z axis for first, -90° for second - new_orientations = torch.tensor( - [[0.0, 0.0, 0.7071068, 0.7071068], [0.0, 0.0, -0.7071068, 0.7071068]], device=device - ) + view = XformPrimView("/World/TransformPrim", device=device) + assert sim_utils.validate_standard_xform_ops(view.prims[0]) - frankas_view.set_world_poses(positions=new_positions, orientations=new_orientations) + ordered_ops = UsdGeom.Xformable(view.prims[0]).GetOrderedXformOps() + op_names = [op.GetOpName() for op in ordered_ops] + assert op_names == ["xformOp:translate", "xformOp:orient", "xformOp:scale"] + assert ordered_ops[0].Get() == Gf.Vec3d(*expected_pos) - # Get poses back and verify - retrieved_positions, retrieved_orientations = frankas_view.get_world_poses() - retrieved_positions, retrieved_orientations = wp.to_torch(retrieved_positions), wp.to_torch(retrieved_orientations) - torch.testing.assert_close(retrieved_positions, new_positions, atol=1e-5, rtol=0) - try: - torch.testing.assert_close(retrieved_orientations, new_orientations, atol=1e-5, rtol=0) - except AssertionError: - torch.testing.assert_close(retrieved_orientations, -new_orientations, atol=1e-5, rtol=0) +# ================================================================== +# USD-only: Nested hierarchy (frame + target) +# ================================================================== @pytest.mark.parametrize("device", ["cpu", "cuda"]) -def test_with_nested_targets(device): - """Test with nested frame/target structure similar to Isaac Sim tests.""" +def test_nested_hierarchy_world_poses(device): + """World pose of nested child == sum of parent + child translations.""" if device == "cuda" and not torch.cuda.is_available(): pytest.skip("CUDA not available") stage = sim_utils.get_current_stage() + frame_positions = [(0.0, 0.0, 0.0), (0.0, 10.0, 5.0), (0.0, 3.0, 5.0)] + target_positions = [(0.0, 20.0, 10.0), (0.0, 30.0, 20.0), (0.0, 50.0, 10.0)] - # Create frames and targets - for i in range(1, 4): - sim_utils.create_prim(f"/World/Frame_{i}", "Xform", stage=stage) - sim_utils.create_prim(f"/World/Frame_{i}/Target", "Xform", stage=stage) + for i in range(3): + sim_utils.create_prim(f"/World/Frame_{i}", "Xform", translation=frame_positions[i], stage=stage) + sim_utils.create_prim(f"/World/Frame_{i}/Target", "Xform", translation=target_positions[i], stage=stage) - # Create views frames_view = XformPrimView("/World/Frame_.*", device=device) targets_view = XformPrimView("/World/Frame_.*/Target", device=device) - assert frames_view.count == 3 - assert targets_view.count == 3 - - # Set local poses for frames - frame_translations = torch.tensor([[0.0, 0.0, 0.0], [0.0, 10.0, 5.0], [0.0, 3.0, 5.0]], device=device) - frames_view.set_local_poses(translations=frame_translations) - - # Set local poses for targets - target_translations = torch.tensor([[0.0, 20.0, 10.0], [0.0, 30.0, 20.0], [0.0, 50.0, 10.0]], device=device) - targets_view.set_local_poses(translations=target_translations) - - # Get world poses of targets - world_positions, _ = targets_view.get_world_poses() - world_positions = wp.to_torch(world_positions) - - # Expected world positions are frame_translation + target_translation - expected_positions = torch.tensor([[0.0, 20.0, 10.0], [0.0, 40.0, 25.0], [0.0, 53.0, 15.0]], device=device) - - torch.testing.assert_close(world_positions, expected_positions, atol=1e-5, rtol=0) - - -@pytest.mark.parametrize("device", ["cpu", "cuda"]) -def test_visibility_with_hierarchy(device): - """Test visibility with parent-child hierarchy and inheritance.""" - if device == "cuda" and not torch.cuda.is_available(): - pytest.skip("CUDA not available") - - stage = sim_utils.get_current_stage() - - # Create parent and children - sim_utils.create_prim("/World/Parent", "Xform", stage=stage) - - num_children = 4 - for i in range(num_children): - sim_utils.create_prim(f"/World/Parent/Child_{i}", "Xform", stage=stage) - - # Create views for both parent and children - parent_view = XformPrimView("/World/Parent", device=device) - children_view = XformPrimView("/World/Parent/Child_.*", device=device) - - # Verify parent and all children are visible initially - parent_visibility = parent_view.get_visibility() - children_visibility = children_view.get_visibility() - assert parent_visibility[0], "Parent should be visible initially" - assert torch.all(children_visibility), "All children should be visible initially" - - # Make some children invisible directly - new_visibility = torch.tensor([True, False, True, False], dtype=torch.bool, device=device) - children_view.set_visibility(new_visibility) - - # Verify the visibility changes - retrieved_visibility = children_view.get_visibility() - torch.testing.assert_close(retrieved_visibility, new_visibility) - - # Make all children visible again - children_view.set_visibility(torch.ones(num_children, dtype=torch.bool, device=device)) - all_visible = children_view.get_visibility() - assert torch.all(all_visible), "All children should be visible again" - - # Now test parent visibility inheritance: - # Make parent invisible - parent_view.set_visibility(torch.tensor([False], dtype=torch.bool, device=device)) - - # Verify parent is invisible - parent_visibility = parent_view.get_visibility() - assert not parent_visibility[0], "Parent should be invisible" - - # Verify children are also invisible (due to parent being invisible) - children_visibility = children_view.get_visibility() - assert not torch.any(children_visibility), "All children should be invisible when parent is invisible" - - # Make parent visible again - parent_view.set_visibility(torch.tensor([True], dtype=torch.bool, device=device)) - - # Verify parent is visible - parent_visibility = parent_view.get_visibility() - assert parent_visibility[0], "Parent should be visible again" + frames_view.set_local_poses( + translations=torch.tensor(frame_positions, device=device) + ) + targets_view.set_local_poses( + translations=torch.tensor(target_positions, device=device) + ) - # Verify children are also visible again - children_visibility = children_view.get_visibility() - assert torch.all(children_visibility), "All children should be visible again when parent is visible" + world_pos = wp.to_torch(targets_view.get_world_poses()[0]) + expected = torch.tensor( + [[f[j] + t[j] for j in range(3)] for f, t in zip(frame_positions, target_positions)], + device=device, + ) + torch.testing.assert_close(world_pos, expected, atol=1e-5, rtol=0) -""" -Tests - Comparison with Isaac Sim Implementation. -""" +# ================================================================== +# USD-only: Comparison with Isaac Sim +# ================================================================== def test_compare_get_world_poses_with_isaacsim(): """Compare get_world_poses with Isaac Sim's implementation.""" - stage = sim_utils.get_current_stage() - - # Check if Isaac Sim is available if _IsaacSimXformPrimView is None: pytest.skip("Isaac Sim is not available") - # Create prims with various poses + stage = sim_utils.get_current_stage() num_prims = 10 for i in range(num_prims): pos = (i * 2.0, i * 0.5, i * 1.5) - # Vary orientations - if i % 3 == 0: - quat = (0.0, 0.0, 0.0, 1.0) # Identity - elif i % 3 == 1: - quat = (0.0, 0.0, 0.7071068, 0.7071068) # 90 deg around Z - else: - quat = (0.7071068, 0.0, 0.0, 0.7071068) # 90 deg around X + quat = (0.0, 0.0, 0.0, 1.0) if i % 2 == 0 else (0.0, 0.0, 0.7071068, 0.7071068) sim_utils.create_prim(f"/World/Env_{i}/Object", "Xform", translation=pos, orientation=quat, stage=stage) pattern = "/World/Env_.*/Object" - - # Create both views isaaclab_view = XformPrimView(pattern, device="cpu") isaacsim_view = _IsaacSimXformPrimView(pattern, reset_xform_properties=False) - # Get world poses from both - isaaclab_pos, isaaclab_quat = isaaclab_view.get_world_poses() # xyzw - isaaclab_pos, isaaclab_quat = wp.to_torch(isaaclab_pos), wp.to_torch(isaaclab_quat) - isaacsim_pos, isaacsim_quat = isaacsim_view.get_world_poses() # wxyz - - # Convert Isaac Sim results to torch tensors if needed + isaaclab_pos = wp.to_torch(isaaclab_view.get_world_poses()[0]) + isaacsim_pos, isaacsim_quat = isaacsim_view.get_world_poses() if not isinstance(isaacsim_pos, torch.Tensor): isaacsim_pos = torch.tensor(isaacsim_pos, dtype=torch.float32) - if not isinstance(isaacsim_quat, torch.Tensor): - isaacsim_quat = torch.tensor(isaacsim_quat, dtype=torch.float32).roll(-1, dims=1) - # Compare results torch.testing.assert_close(isaaclab_pos, isaacsim_pos, atol=1e-5, rtol=0) - # Compare quaternions (account for sign ambiguity) - try: - torch.testing.assert_close(isaaclab_quat, isaacsim_quat, atol=1e-5, rtol=0) - except AssertionError: - torch.testing.assert_close(isaaclab_quat, -isaacsim_quat, atol=1e-5, rtol=0) - - -def test_compare_set_world_poses_with_isaacsim(): - """Compare set_world_poses with Isaac Sim's implementation.""" - stage = sim_utils.get_current_stage() - - # Check if Isaac Sim is available - if _IsaacSimXformPrimView is None: - pytest.skip("Isaac Sim is not available") - - # Create prims - num_prims = 8 - for i in range(num_prims): - sim_utils.create_prim(f"/World/Env_{i}/Object", "Xform", translation=(0.0, 0.0, 0.0), stage=stage) - - pattern = "/World/Env_.*/Object" - - # Create both views - isaaclab_view = XformPrimView(pattern, device="cpu") - isaacsim_view = _IsaacSimXformPrimView(pattern, reset_xform_properties=False) - - # Generate new poses - new_positions = torch.randn(num_prims, 3) * 10.0 - new_orientations = torch.tensor([[0.0, 0.0, 0.0, 1.0]] * num_prims, dtype=torch.float32) - - # Set poses using both implementations - isaaclab_view.set_world_poses(new_positions.clone(), new_orientations.clone()) # xyzw - isaacsim_view.set_world_poses(new_positions.clone(), new_orientations.clone().roll(1, dims=1)) # wxyz - - # Get poses back from both - isaaclab_pos, isaaclab_quat = isaaclab_view.get_world_poses() # xyzw - isaaclab_pos, isaaclab_quat = wp.to_torch(isaaclab_pos), wp.to_torch(isaaclab_quat) - isaacsim_pos, isaacsim_quat = isaacsim_view.get_world_poses() # wxyz - - # Convert Isaac Sim results to torch tensors if needed - if not isinstance(isaacsim_pos, torch.Tensor): - isaacsim_pos = torch.tensor(isaacsim_pos, dtype=torch.float32) - if not isinstance(isaacsim_quat, torch.Tensor): - isaacsim_quat = torch.tensor(isaacsim_quat, dtype=torch.float32).roll(-1, dims=1) - - # Compare results - both implementations should produce the same world poses - torch.testing.assert_close(isaaclab_pos, isaacsim_pos, atol=1e-4, rtol=0) - try: - torch.testing.assert_close(isaaclab_quat, isaacsim_quat, atol=1e-4, rtol=0) - except AssertionError: - torch.testing.assert_close(isaaclab_quat, -isaacsim_quat, atol=1e-4, rtol=0) - - -def test_compare_get_local_poses_with_isaacsim(): - """Compare get_local_poses with Isaac Sim's implementation.""" - stage = sim_utils.get_current_stage() - - # Check if Isaac Sim is available - if _IsaacSimXformPrimView is None: - pytest.skip("Isaac Sim is not available") - - # Create hierarchical prims - num_prims = 5 - for i in range(num_prims): - # Create parent - sim_utils.create_prim(f"/World/Env_{i}", "Xform", translation=(i * 5.0, 0.0, 0.0), stage=stage) - # Create child with local pose - local_pos = (1.0, float(i), 0.0) - local_quat = (0.0, 0.0, 0.0, 1.0) if i % 2 == 0 else (0.0, 0.0, 0.7071068, 0.7071068) - sim_utils.create_prim( - f"/World/Env_{i}/Object", "Xform", translation=local_pos, orientation=local_quat, stage=stage - ) - - pattern = "/World/Env_.*/Object" - - # Create both views - isaaclab_view = XformPrimView(pattern, device="cpu") - isaacsim_view = _IsaacSimXformPrimView(pattern, reset_xform_properties=False) - # Get local poses from both - isaaclab_trans, isaaclab_quat = isaaclab_view.get_local_poses() - isaaclab_trans, isaaclab_quat = wp.to_torch(isaaclab_trans), wp.to_torch(isaaclab_quat) - isaacsim_trans, isaacsim_quat = isaacsim_view.get_local_poses() +# ================================================================== +# USD-only: Franka integration +# ================================================================== - # Convert Isaac Sim results to torch tensors if needed - if not isinstance(isaacsim_trans, torch.Tensor): - isaacsim_trans = torch.tensor(isaacsim_trans, dtype=torch.float32) - if not isinstance(isaacsim_quat, torch.Tensor): - isaacsim_quat = torch.tensor(isaacsim_quat, dtype=torch.float32).roll(-1, dims=1) - - # Compare results - torch.testing.assert_close(isaaclab_trans, isaacsim_trans, atol=1e-5, rtol=0) - try: - torch.testing.assert_close(isaaclab_quat, isaacsim_quat, atol=1e-5, rtol=0) - except AssertionError: - torch.testing.assert_close(isaaclab_quat, -isaacsim_quat, atol=1e-5, rtol=0) +@pytest.mark.parametrize("device", ["cpu", "cuda"]) +def test_with_franka_robots(device): + """Verify XformPrimView works with real Franka robot USD assets.""" + if device == "cuda" and not torch.cuda.is_available(): + pytest.skip("CUDA not available") -def test_compare_set_local_poses_with_isaacsim(): - """Compare set_local_poses with Isaac Sim's implementation.""" stage = sim_utils.get_current_stage() + franka_usd_path = f"{ISAAC_NUCLEUS_DIR}/Robots/FrankaRobotics/FrankaPanda/franka.usd" - # Check if Isaac Sim is available - if _IsaacSimXformPrimView is None: - pytest.skip("Isaac Sim is not available") - - # Create hierarchical prims - num_prims = 6 - for i in range(num_prims): - sim_utils.create_prim(f"/World/Env_{i}", "Xform", translation=(i * 3.0, 0.0, 0.0), stage=stage) - sim_utils.create_prim(f"/World/Env_{i}/Object", "Xform", translation=(0.0, 0.0, 0.0), stage=stage) + sim_utils.create_prim("/World/Franka_1", "Xform", usd_path=franka_usd_path, stage=stage) + sim_utils.create_prim("/World/Franka_2", "Xform", usd_path=franka_usd_path, stage=stage) - pattern = "/World/Env_.*/Object" + view = XformPrimView("/World/Franka_.*", device=device) + assert view.count == 2 - # Create both views - isaaclab_view = XformPrimView(pattern, device="cpu") - isaacsim_view = _IsaacSimXformPrimView(pattern, reset_xform_properties=False) + positions = wp.to_torch(view.get_world_poses()[0]) + torch.testing.assert_close(positions, torch.zeros(2, 3, device=device), atol=1e-5, rtol=0) - # Generate new local poses - new_translations = torch.randn(num_prims, 3) * 5.0 - new_orientations = torch.tensor( - [[0.0, 0.0, 0.0, 1.0], [0.0, 0.0, 0.7071068, 0.7071068]] * (num_prims // 2), dtype=torch.float32 + new_pos = torch.tensor([[10.0, 10.0, 0.0], [-40.0, -40.0, 0.0]], device=device) + new_quat = torch.tensor( + [[0.0, 0.0, 0.7071068, 0.7071068], [0.0, 0.0, -0.7071068, 0.7071068]], device=device ) + view.set_world_poses(positions=new_pos, orientations=new_quat) - # Set local poses using both implementations - isaaclab_view.set_local_poses(new_translations.clone(), new_orientations.clone()) - isaacsim_view.set_local_poses(new_translations.clone(), new_orientations.clone().roll(1, dims=1)) - - # Get local poses back from both - isaaclab_trans, isaaclab_quat = isaaclab_view.get_local_poses() - isaaclab_trans, isaaclab_quat = wp.to_torch(isaaclab_trans), wp.to_torch(isaaclab_quat) - isaacsim_trans, isaacsim_quat = isaacsim_view.get_local_poses() - - # Convert Isaac Sim results to torch tensors if needed - if not isinstance(isaacsim_trans, torch.Tensor): - isaacsim_trans = torch.tensor(isaacsim_trans, dtype=torch.float32) - if not isinstance(isaacsim_quat, torch.Tensor): - isaacsim_quat = torch.tensor(isaacsim_quat, dtype=torch.float32).roll(-1, dims=1) - - # Compare results - torch.testing.assert_close(isaaclab_trans, isaacsim_trans, atol=1e-4, rtol=0) - try: - torch.testing.assert_close(isaaclab_quat, isaacsim_quat, atol=1e-4, rtol=0) - except AssertionError: - torch.testing.assert_close(isaaclab_quat, -isaacsim_quat, atol=1e-4, rtol=0) + ret_pos = wp.to_torch(view.get_world_poses()[0]) + torch.testing.assert_close(ret_pos, new_pos, atol=1e-5, rtol=0) diff --git a/source/isaaclab/test/sim/xform_contract_tests.py b/source/isaaclab/test/sim/xform_contract_tests.py new file mode 100644 index 000000000000..199c88690197 --- /dev/null +++ b/source/isaaclab/test/sim/xform_contract_tests.py @@ -0,0 +1,361 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Shared XformPrimView contract tests. + +This module defines the invariants that **every** XformPrimView backend +(USD, Fabric, Newton) must satisfy. Backend test files import these tests +via ``from isaaclab.test.sim.xform_contract_tests import *`` and provide a +``view_factory`` pytest fixture that builds the backend-specific scene. + +The factory signature is:: + + def view_factory() -> Callable[[int, str], ViewBundle]: + ... + +Where ``ViewBundle`` is a :class:`NamedTuple`:: + + class ViewBundle(NamedTuple): + view: BaseXformPrimView + get_parent_pos: Callable[[int, str], torch.Tensor] + set_parent_pos: Callable[[torch.Tensor, int], None] + teardown: Callable[[], None] + +- ``view``: The XformPrimView under test. Must track child prims at + :data:`CHILD_OFFSET` under parent prims/bodies. +- ``get_parent_pos(n, device)``: Read the parent prim/body positions. +- ``set_parent_pos(positions, n)``: Write the parent prim/body positions. +- ``teardown()``: Cleanup (close context, clear stage, etc.). + +Tolerance policy: + - Indexed reads (exact copy): ``atol=0`` + - Composition / decomposition through float32 transforms: ``atol=ATOL`` + - Parent position identity checks (should be untouched): ``atol=0`` +""" + +from __future__ import annotations + +from typing import Callable, NamedTuple + +import pytest +import torch +import warp as wp + +CHILD_OFFSET = (0.1, 0.0, 0.05) +"""Local offset of the child prim from its parent, shared by all backend fixtures.""" + +ATOL = 1e-5 +"""Default absolute tolerance for float32 transform composition.""" + + +class ViewBundle(NamedTuple): + """Return type of the ``view_factory`` fixture.""" + + view: object + get_parent_pos: Callable + set_parent_pos: Callable + teardown: Callable + + +def _t(a): + """Convert wp.array to torch.Tensor (pass-through for Tensor).""" + return wp.to_torch(a) if isinstance(a, wp.array) else a + + +def _wp_vec3f(data, device="cpu"): + return wp.array([wp.vec3f(*row) for row in data], dtype=wp.vec3f, device=device) + + +def _wp_vec4f(data, device="cpu"): + return wp.array([wp.vec4f(*row) for row in data], dtype=wp.vec4f, device=device) + + +# ================================================================== +# Contract: Getters +# ================================================================== + + +@pytest.mark.parametrize("device", ["cpu", "cuda:0"]) +def test_world_pose_equals_parent_plus_offset(device, view_factory): + """world_pose == parent_pos + local offset (identity parent orientation).""" + bundle = view_factory(num_envs=4, device=device) + try: + child_pos = _t(bundle.view.get_world_poses()[0]) + parent_pos = bundle.get_parent_pos(4, device) + offset = torch.tensor(CHILD_OFFSET, device=device) + + torch.testing.assert_close(child_pos, parent_pos + offset.unsqueeze(0), atol=ATOL, rtol=0) + finally: + bundle.teardown() + + +@pytest.mark.parametrize("device", ["cpu", "cuda:0"]) +def test_local_pose_equals_structural_offset(device, view_factory): + """local_pose == the authored offset (0.1, 0, 0.05) for every prim.""" + bundle = view_factory(num_envs=4, device=device) + try: + local_pos, local_quat = bundle.view.get_local_poses() + expected_pos = torch.tensor(CHILD_OFFSET, device=device).expand(4, -1) + expected_quat = torch.tensor([0.0, 0.0, 0.0, 1.0], device=device).expand(4, -1) + + torch.testing.assert_close(_t(local_pos), expected_pos, atol=ATOL, rtol=0) + torch.testing.assert_close(_t(local_quat), expected_quat, atol=ATOL, rtol=0) + finally: + bundle.teardown() + + +@pytest.mark.parametrize("device", ["cpu", "cuda:0"]) +def test_local_differs_from_world(device, view_factory): + """local != world when parent is not at the origin. + + Asserts |world - local| > 0.5 to catch any implementation that returns + world as local. The parent is offset from the origin so the z-component + alone provides > 0.5 difference. + """ + bundle = view_factory(num_envs=2, device=device) + try: + world_pos = _t(bundle.view.get_world_poses()[0]) + local_pos = _t(bundle.view.get_local_poses()[0]) + + diff = (world_pos - local_pos).abs().max().item() + assert diff > 0.5, ( + f"Expected |world - local| > 0.5, got {diff:.4f}. " + f"world={world_pos.tolist()}, local={local_pos.tolist()}" + ) + finally: + bundle.teardown() + + +@pytest.mark.parametrize("device", ["cpu", "cuda:0"]) +def test_local_stable_after_parent_move(device, view_factory): + """Moving the parent changes world but NOT local.""" + bundle = view_factory(num_envs=2, device=device) + try: + local_before = _t(bundle.view.get_local_poses()[0]).clone() + bundle.set_parent_pos(torch.tensor([[99.0, 0.0, 0.0], [0.0, 99.0, 0.0]], device=device), 2) + local_after = _t(bundle.view.get_local_poses()[0]) + + torch.testing.assert_close(local_after, local_before, atol=ATOL, rtol=0) + finally: + bundle.teardown() + + +@pytest.mark.parametrize("device", ["cpu", "cuda:0"]) +def test_world_tracks_parent_move(device, view_factory): + """Moving the parent shifts world poses by the same amount.""" + bundle = view_factory(num_envs=2, device=device) + try: + new_parent_pos = torch.tensor([[5.0, 0.0, 0.0], [0.0, 5.0, 0.0]], device=device) + bundle.set_parent_pos(new_parent_pos, 2) + + child_pos = _t(bundle.view.get_world_poses()[0]) + offset = torch.tensor(CHILD_OFFSET, device=device) + + torch.testing.assert_close(child_pos, new_parent_pos + offset.unsqueeze(0), atol=ATOL, rtol=0) + finally: + bundle.teardown() + + +@pytest.mark.parametrize("device", ["cpu", "cuda:0"]) +def test_indexed_get_returns_correct_subset(device, view_factory): + """Indexed get (out-of-order) returns exact copies for both world and local.""" + bundle = view_factory(num_envs=5, device=device) + try: + all_world = _t(bundle.view.get_world_poses()[0]) + all_local = _t(bundle.view.get_local_poses()[0]) + + indices = [4, 1, 3] + sub_world = _t(bundle.view.get_world_poses(indices)[0]) + sub_local = _t(bundle.view.get_local_poses(indices)[0]) + + for out_i, view_i in enumerate(indices): + torch.testing.assert_close(sub_world[out_i], all_world[view_i], atol=0, rtol=0) + torch.testing.assert_close(sub_local[out_i], all_local[view_i], atol=0, rtol=0) + finally: + bundle.teardown() + + +# ================================================================== +# Contract: Setters +# ================================================================== + + +@pytest.mark.parametrize("device", ["cpu", "cuda:0"]) +def test_set_world_roundtrip(device, view_factory): + """set_world_poses -> get_world_poses returns the same values.""" + bundle = view_factory(num_envs=2, device=device) + try: + new_pos = _wp_vec3f([[10.0, 20.0, 30.0], [40.0, 50.0, 60.0]], device=device) + new_quat = _wp_vec4f([[0.0, 0.0, 0.7071068, 0.7071068], [0.0, 0.0, 0.0, 1.0]], device=device) + bundle.view.set_world_poses(new_pos, new_quat) + + ret_pos, ret_quat = bundle.view.get_world_poses() + torch.testing.assert_close(_t(ret_pos), _t(new_pos), atol=ATOL, rtol=0) + torch.testing.assert_close(_t(ret_quat), _t(new_quat), atol=ATOL, rtol=0) + finally: + bundle.teardown() + + +@pytest.mark.parametrize("device", ["cpu", "cuda:0"]) +def test_set_local_roundtrip(device, view_factory): + """set_local_poses -> get_local_poses returns the same values.""" + bundle = view_factory(num_envs=2, device=device) + try: + new_pos = _wp_vec3f([[0.5, 0.3, 0.1], [0.2, 0.7, 0.4]], device=device) + new_quat = _wp_vec4f([[0.0, 0.0, 0.0, 1.0]] * 2, device=device) + bundle.view.set_local_poses(new_pos, new_quat) + + ret_pos, ret_quat = bundle.view.get_local_poses() + torch.testing.assert_close(_t(ret_pos), _t(new_pos), atol=ATOL, rtol=0) + torch.testing.assert_close(_t(ret_quat), _t(new_quat), atol=ATOL, rtol=0) + finally: + bundle.teardown() + + +@pytest.mark.parametrize("device", ["cpu", "cuda:0"]) +def test_set_world_does_not_move_parent(device, view_factory): + """set_world_poses must not modify the parent prim/body position.""" + bundle = view_factory(num_envs=2, device=device) + try: + parent_before = bundle.get_parent_pos(2, device).clone() + bundle.view.set_world_poses( + _wp_vec3f([[99.0, 99.0, 99.0], [88.0, 88.0, 88.0]], device=device), + _wp_vec4f([[0.0, 0.0, 0.0, 1.0]] * 2, device=device), + ) + parent_after = bundle.get_parent_pos(2, device) + + torch.testing.assert_close(parent_after, parent_before, atol=0, rtol=0) + finally: + bundle.teardown() + + +@pytest.mark.parametrize("device", ["cpu", "cuda:0"]) +def test_set_local_does_not_move_parent(device, view_factory): + """set_local_poses must not modify the parent prim/body position.""" + bundle = view_factory(num_envs=2, device=device) + try: + parent_before = bundle.get_parent_pos(2, device).clone() + bundle.view.set_local_poses( + _wp_vec3f([[0.5, 0.5, 0.5], [1.0, 1.0, 1.0]], device=device), + _wp_vec4f([[0.0, 0.0, 0.0, 1.0]] * 2, device=device), + ) + parent_after = bundle.get_parent_pos(2, device) + + torch.testing.assert_close(parent_after, parent_before, atol=0, rtol=0) + finally: + bundle.teardown() + + +@pytest.mark.parametrize("device", ["cpu", "cuda:0"]) +def test_set_world_updates_local(device, view_factory): + """After set_world_poses, get_local_poses reflects the new offset. + + Uses non-axis-aligned offsets to catch coordinate swap bugs. + """ + bundle = view_factory(num_envs=2, device=device) + try: + parent_pos = bundle.get_parent_pos(2, device) + desired_offset = torch.tensor([[0.3, 0.7, 0.2], [0.8, 0.1, 0.6]], device=device) + new_world = parent_pos + desired_offset + + bundle.view.set_world_poses( + _wp_vec3f(new_world.tolist(), device=device), + _wp_vec4f([[0.0, 0.0, 0.0, 1.0]] * 2, device=device), + ) + + local_pos = _t(bundle.view.get_local_poses()[0]) + torch.testing.assert_close(local_pos, desired_offset, atol=ATOL, rtol=0) + finally: + bundle.teardown() + + +@pytest.mark.parametrize("device", ["cpu", "cuda:0"]) +def test_set_local_updates_world(device, view_factory): + """After set_local_poses, get_world_poses == parent + new_local. + + Uses non-axis-aligned offsets to catch coordinate swap bugs. + """ + bundle = view_factory(num_envs=2, device=device) + try: + parent_pos = bundle.get_parent_pos(2, device) + new_offset = torch.tensor([[0.4, 0.9, 0.15], [0.6, 0.2, 0.85]], device=device) + bundle.view.set_local_poses( + _wp_vec3f(new_offset.tolist(), device=device), + _wp_vec4f([[0.0, 0.0, 0.0, 1.0]] * 2, device=device), + ) + + world_pos = _t(bundle.view.get_world_poses()[0]) + torch.testing.assert_close(world_pos, parent_pos + new_offset, atol=ATOL, rtol=0) + finally: + bundle.teardown() + + +@pytest.mark.parametrize("device", ["cpu", "cuda:0"]) +def test_set_world_partial_position_only(device, view_factory): + """Setting only positions: new positions written, orientations preserved.""" + bundle = view_factory(num_envs=2, device=device) + try: + _, orig_quat = bundle.view.get_world_poses() + new_pos = _wp_vec3f([[1.0, 2.0, 3.0], [4.0, 5.0, 6.0]], device=device) + bundle.view.set_world_poses(positions=new_pos) + + ret_pos, ret_quat = bundle.view.get_world_poses() + torch.testing.assert_close(_t(ret_pos), _t(new_pos), atol=ATOL, rtol=0) + torch.testing.assert_close(_t(ret_quat), _t(orig_quat), atol=ATOL, rtol=0) + finally: + bundle.teardown() + + +@pytest.mark.parametrize("device", ["cpu", "cuda:0"]) +def test_set_world_partial_orientation_only(device, view_factory): + """Setting only orientations: new orientations written, positions preserved.""" + bundle = view_factory(num_envs=2, device=device) + try: + orig_pos, _ = bundle.view.get_world_poses() + new_quat = _wp_vec4f( + [[0.0, 0.0, 0.7071068, 0.7071068], [0.7071068, 0.0, 0.0, 0.7071068]], device=device + ) + bundle.view.set_world_poses(orientations=new_quat) + + ret_pos, ret_quat = bundle.view.get_world_poses() + torch.testing.assert_close(_t(ret_pos), _t(orig_pos), atol=ATOL, rtol=0) + torch.testing.assert_close(_t(ret_quat), _t(new_quat), atol=ATOL, rtol=0) + finally: + bundle.teardown() + + +@pytest.mark.parametrize("device", ["cpu", "cuda:0"]) +def test_set_local_partial_position_only(device, view_factory): + """Setting only local translations: new translations written, orientations preserved.""" + bundle = view_factory(num_envs=2, device=device) + try: + _, orig_quat = bundle.view.get_local_poses() + new_pos = _wp_vec3f([[0.2, 0.3, 0.4], [0.5, 0.6, 0.7]], device=device) + bundle.view.set_local_poses(translations=new_pos) + + ret_pos, ret_quat = bundle.view.get_local_poses() + torch.testing.assert_close(_t(ret_pos), _t(new_pos), atol=ATOL, rtol=0) + torch.testing.assert_close(_t(ret_quat), _t(orig_quat), atol=ATOL, rtol=0) + finally: + bundle.teardown() + + +@pytest.mark.parametrize("device", ["cpu", "cuda:0"]) +def test_set_world_indexed_only_affects_subset(device, view_factory): + """Indexed set_world_poses writes requested indices, leaves others untouched.""" + bundle = view_factory(num_envs=4, device=device) + try: + orig_pos = _t(bundle.view.get_world_poses()[0]).clone() + indices = [1, 3] + new_pos = _wp_vec3f([[10.0, 20.0, 30.0], [40.0, 50.0, 60.0]], device=device) + bundle.view.set_world_poses(positions=new_pos, indices=indices) + + updated = _t(bundle.view.get_world_poses()[0]) + torch.testing.assert_close(updated[0], orig_pos[0], atol=0, rtol=0) + torch.testing.assert_close(updated[2], orig_pos[2], atol=0, rtol=0) + torch.testing.assert_close(updated[1], _t(new_pos)[0], atol=ATOL, rtol=0) + torch.testing.assert_close(updated[3], _t(new_pos)[1], atol=ATOL, rtol=0) + finally: + bundle.teardown() diff --git a/source/isaaclab_newton/isaaclab_newton/sim/views/xform_prim_view.py b/source/isaaclab_newton/isaaclab_newton/sim/views/xform_prim_view.py index 7c92b7847903..6e3439b99a76 100644 --- a/source/isaaclab_newton/isaaclab_newton/sim/views/xform_prim_view.py +++ b/source/isaaclab_newton/isaaclab_newton/sim/views/xform_prim_view.py @@ -75,58 +75,6 @@ def _compute_site_world_transforms_indexed( out_quat[i] = wp.vec4f(q[0], q[1], q[2], q[3]) -@wp.kernel -def _write_body_q_from_site_poses( - body_q: wp.array(dtype=wp.transformf), - site_body: wp.array(dtype=wp.int32), - site_local: wp.array(dtype=wp.transformf), - world_pos: wp.array(dtype=wp.vec3f), - world_quat: wp.array(dtype=wp.vec4f), -): - """Compute ``body_q = world_site_pose * inverse(local)`` and write it. - - Launched over all sites; skips world-attached sites (``site_body == -1``) - since they have no body to write to. - """ - i = wp.tid() - bid = site_body[i] - if bid == -1: - return - - inv_local = wp.transform_inverse(site_local[i]) - w_pos = world_pos[i] - w_q = world_quat[i] - world_tf = wp.transform(w_pos, wp.quatf(w_q[0], w_q[1], w_q[2], w_q[3])) - body_q[bid] = wp.transform_multiply(world_tf, inv_local) - - -@wp.kernel -def _write_body_q_from_site_poses_indexed( - body_q: wp.array(dtype=wp.transformf), - site_body: wp.array(dtype=wp.int32), - site_local: wp.array(dtype=wp.transformf), - indices: wp.array(dtype=wp.int32), - world_pos: wp.array(dtype=wp.vec3f), - world_quat: wp.array(dtype=wp.vec4f), -): - """Indexed variant: writes body_q for sites selected by ``indices``. - - ``world_pos`` and ``world_quat`` are dense over the index list (length M), - while ``site_body`` and ``site_local`` are indexed via ``indices[i]``. - """ - i = wp.tid() - si = indices[i] - bid = site_body[si] - if bid == -1: - return - - inv_local = wp.transform_inverse(site_local[si]) - w_pos = world_pos[i] - w_q = world_quat[i] - world_tf = wp.transform(w_pos, wp.quatf(w_q[0], w_q[1], w_q[2], w_q[3])) - body_q[bid] = wp.transform_multiply(world_tf, inv_local) - - @wp.kernel def _gather_scales( shape_scale: wp.array(dtype=wp.vec3f), @@ -203,6 +151,199 @@ def _scatter_scales_indexed( shape_scale[s] = new_scales[i] +# ------------------------------------------------------------------ +# World-pose site_local write kernels +# ------------------------------------------------------------------ + + +@wp.kernel +def _write_site_local_from_world_poses( + body_q: wp.array(dtype=wp.transformf), + site_body: wp.array(dtype=wp.int32), + site_local: wp.array(dtype=wp.transformf), + world_pos: wp.array(dtype=wp.vec3f), + world_quat: wp.array(dtype=wp.vec4f), +): + """Update ``site_local`` so that ``body_q[bid] * site_local == desired_world``. + + Computes ``site_local[i] = inv(body_q[bid]) * desired_world``. + For world-attached sites (``site_body == -1``) writes the world transform + directly into ``site_local``. + """ + i = wp.tid() + w_pos = world_pos[i] + w_q = world_quat[i] + desired_world = wp.transform(w_pos, wp.quatf(w_q[0], w_q[1], w_q[2], w_q[3])) + + bid = site_body[i] + if bid == -1: + site_local[i] = desired_world + else: + site_local[i] = wp.transform_multiply(wp.transform_inverse(body_q[bid]), desired_world) + + +@wp.kernel +def _write_site_local_from_world_poses_indexed( + body_q: wp.array(dtype=wp.transformf), + site_body: wp.array(dtype=wp.int32), + site_local: wp.array(dtype=wp.transformf), + indices: wp.array(dtype=wp.int32), + world_pos: wp.array(dtype=wp.vec3f), + world_quat: wp.array(dtype=wp.vec4f), +): + """Indexed variant of :func:`_write_site_local_from_world_poses`.""" + i = wp.tid() + si = indices[i] + w_pos = world_pos[i] + w_q = world_quat[i] + desired_world = wp.transform(w_pos, wp.quatf(w_q[0], w_q[1], w_q[2], w_q[3])) + + bid = site_body[si] + if bid == -1: + site_local[si] = desired_world + else: + site_local[si] = wp.transform_multiply(wp.transform_inverse(body_q[bid]), desired_world) + + +# ------------------------------------------------------------------ +# Local-pose Warp kernels +# ------------------------------------------------------------------ + + +@wp.kernel +def _compute_site_local_transforms( + body_q: wp.array(dtype=wp.transformf), + site_body: wp.array(dtype=wp.int32), + site_local: wp.array(dtype=wp.transformf), + parent_site_body: wp.array(dtype=wp.int32), + parent_site_local: wp.array(dtype=wp.transformf), + out_pos: wp.array(dtype=wp.vec3f), + out_quat: wp.array(dtype=wp.vec4f), +): + """Compute parent-relative transforms: ``local = inv(parent_world) * prim_world``. + + When ``site_body[i] == -1`` the prim is attached to the world frame and + ``site_local[i]`` is its world transform. The same convention applies to + ``parent_site_body`` / ``parent_site_local``. + """ + i = wp.tid() + prim_bid = site_body[i] + if prim_bid == -1: + prim_world = site_local[i] + else: + prim_world = wp.transform_multiply(body_q[prim_bid], site_local[i]) + + parent_bid = parent_site_body[i] + if parent_bid == -1: + parent_world = parent_site_local[i] + else: + parent_world = wp.transform_multiply(body_q[parent_bid], parent_site_local[i]) + + local_tf = wp.transform_multiply(wp.transform_inverse(parent_world), prim_world) + out_pos[i] = wp.transform_get_translation(local_tf) + q = wp.transform_get_rotation(local_tf) + out_quat[i] = wp.vec4f(q[0], q[1], q[2], q[3]) + + +@wp.kernel +def _compute_site_local_transforms_indexed( + body_q: wp.array(dtype=wp.transformf), + site_body: wp.array(dtype=wp.int32), + site_local: wp.array(dtype=wp.transformf), + parent_site_body: wp.array(dtype=wp.int32), + parent_site_local: wp.array(dtype=wp.transformf), + indices: wp.array(dtype=wp.int32), + out_pos: wp.array(dtype=wp.vec3f), + out_quat: wp.array(dtype=wp.vec4f), +): + """Compute parent-relative transforms for a subset of sites selected by ``indices``.""" + i = wp.tid() + si = indices[i] + prim_bid = site_body[si] + if prim_bid == -1: + prim_world = site_local[si] + else: + prim_world = wp.transform_multiply(body_q[prim_bid], site_local[si]) + + parent_bid = parent_site_body[si] + if parent_bid == -1: + parent_world = parent_site_local[si] + else: + parent_world = wp.transform_multiply(body_q[parent_bid], parent_site_local[si]) + + local_tf = wp.transform_multiply(wp.transform_inverse(parent_world), prim_world) + out_pos[i] = wp.transform_get_translation(local_tf) + q = wp.transform_get_rotation(local_tf) + out_quat[i] = wp.vec4f(q[0], q[1], q[2], q[3]) + + +@wp.kernel +def _write_site_local_from_local_poses( + body_q: wp.array(dtype=wp.transformf), + site_body: wp.array(dtype=wp.int32), + site_local: wp.array(dtype=wp.transformf), + parent_site_body: wp.array(dtype=wp.int32), + parent_site_local: wp.array(dtype=wp.transformf), + local_pos: wp.array(dtype=wp.vec3f), + local_quat: wp.array(dtype=wp.vec4f), +): + """Update ``site_local`` so that ``inv(parent_world) * prim_world == desired_local``. + + Computes ``site_local[i] = inv(body_q[bid]) * parent_world * desired_local``. + For world-attached sites (``site_body == -1``) the site local IS the world + transform, so we write ``parent_world * desired_local`` directly. + """ + i = wp.tid() + parent_bid = parent_site_body[i] + if parent_bid == -1: + parent_world = parent_site_local[i] + else: + parent_world = wp.transform_multiply(body_q[parent_bid], parent_site_local[i]) + + l_pos = local_pos[i] + l_q = local_quat[i] + local_tf = wp.transform(l_pos, wp.quatf(l_q[0], l_q[1], l_q[2], l_q[3])) + desired_world = wp.transform_multiply(parent_world, local_tf) + + bid = site_body[i] + if bid == -1: + site_local[i] = desired_world + else: + site_local[i] = wp.transform_multiply(wp.transform_inverse(body_q[bid]), desired_world) + + +@wp.kernel +def _write_site_local_from_local_poses_indexed( + body_q: wp.array(dtype=wp.transformf), + site_body: wp.array(dtype=wp.int32), + site_local: wp.array(dtype=wp.transformf), + parent_site_body: wp.array(dtype=wp.int32), + parent_site_local: wp.array(dtype=wp.transformf), + indices: wp.array(dtype=wp.int32), + local_pos: wp.array(dtype=wp.vec3f), + local_quat: wp.array(dtype=wp.vec4f), +): + """Indexed variant of :func:`_write_site_local_from_local_poses`.""" + i = wp.tid() + si = indices[i] + parent_bid = parent_site_body[si] + if parent_bid == -1: + parent_world = parent_site_local[si] + else: + parent_world = wp.transform_multiply(body_q[parent_bid], parent_site_local[si]) + + l_pos = local_pos[i] + l_q = local_quat[i] + local_tf = wp.transform(l_pos, wp.quatf(l_q[0], l_q[1], l_q[2], l_q[3])) + desired_world = wp.transform_multiply(parent_world, local_tf) + + bid = site_body[si] + if bid == -1: + site_local[si] = desired_world + else: + site_local[si] = wp.transform_multiply(wp.transform_inverse(body_q[bid]), desired_world) + + # ------------------------------------------------------------------ # Helpers # ------------------------------------------------------------------ @@ -236,31 +377,31 @@ def _ensure_wp_vec4f(data: wp.array, device: str) -> wp.array: class XformPrimView(BaseXformPrimView): - """Batched prim view backed by Newton's native site concept on GPU. - - Each matched USD prim is resolved to a ``(body_index, local_transform)`` - pair. World poses are computed on GPU as - ``body_q[body_index] * local_transform`` via a Warp kernel, consistent - with Newton's site mechanism. - - Resolution order (per prim path): - - 1. **Shape label** -- look up in ``model.shape_label``. If found, use - ``shape_body`` and ``shape_transform`` for the body index and local - offset. - 2. **Body label** -- look up in ``model.body_label``. If found, the body - itself is the site; local offset is identity. - 3. **Ancestor walk** -- walk the USD parent hierarchy until a prim whose - path appears in ``model.body_label`` is found. The relative transform - from that ancestor body to the target prim is the local offset. If no - ancestor body exists, the site is attached to the world frame - (``body_index = -1``) and the local offset is the prim's world - transform. - - This supports arbitrary prims -- rigid bodies, collision shapes, cameras, - plain Xforms, and any other Xformable prim. + """Batched prim view for non-physics prims tracked as sites on Newton bodies. + + Each matched USD prim must be a **non-physics** prim (camera, sensor, + Xform marker, etc.) that sits as a child of a Newton rigid body in the + USD hierarchy. The prim path must **not** resolve directly to a physics + body or collision shape -- those are owned by Newton and should be + accessed through :class:`~isaaclab_newton.assets.Articulation` or + :class:`~isaaclab_newton.assets.RigidObject` instead. + + At init time each prim is resolved to a ``(body_index, site_local)`` + pair via ancestor walk: the nearest ancestor that appears in + ``model.body_label`` becomes the attachment body, and the relative USD + transform becomes the site offset. If no body ancestor exists the prim + is attached to the world frame (``body_index = -1``). + + World poses are computed on GPU as + ``body_q[body_index] * site_local`` via a Warp kernel. Both + ``set_world_poses`` and ``set_local_poses`` update ``site_local`` -- + neither touches ``body_q``. All getters return ``wp.array``. Setters accept ``wp.array``. + + Raises: + ValueError: If any matched prim resolves to a Newton physics body + or collision shape. """ def __init__(self, prim_path: str, device: str = "cpu", stage: Usd.Stage | None = None, **kwargs): @@ -273,31 +414,55 @@ def __init__(self, prim_path: str, device: str = "cpu", stage: Usd.Stage | None model = NewtonManager.get_model() body_labels = list(model.body_label) body_label_set = set(body_labels) - shape_labels = list(model.shape_label) - shape_body_np = model.shape_body.numpy() - shape_xform_np = model.shape_transform.numpy() + body_label_to_idx = {path: idx for idx, path in enumerate(body_labels)} + shape_label_set = set(model.shape_label) xform_cache = UsdGeom.XformCache(Usd.TimeCode.Default()) site_bodies: list[int] = [] site_locals: list[list[float]] = [] + parent_bodies: list[int] = [] + parent_locals: list[list[float]] = [] identity_xform = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0] + resolve_cache: dict[str, tuple[int, list[float]]] = {} for prim in self._prims: pp = prim.GetPath().pathString - - if pp in shape_labels: - si = shape_labels.index(pp) - site_bodies.append(int(shape_body_np[si])) - site_locals.append(shape_xform_np[si].tolist()) - elif pp in body_label_set: - site_bodies.append(body_labels.index(pp)) - site_locals.append(identity_xform) + if pp in body_label_set: + raise ValueError( + f"XformPrimView prim '{pp}' is a Newton physics body. " + "XformPrimView should only be used for non-physics prims (cameras, sensors, Xform markers). " + "Use Articulation or RigidObject APIs to control physics bodies." + ) + if pp in shape_label_set: + raise ValueError( + f"XformPrimView prim '{pp}' is a Newton collision shape. " + "XformPrimView should only be used for non-physics prims (cameras, sensors, Xform markers). " + "Use Articulation or RigidObject APIs to control collision shapes." + ) + + body_idx, local_xform = self._resolve_ancestor_body(prim, body_label_to_idx, xform_cache) + site_bodies.append(body_idx) + site_locals.append(local_xform) + + parent = prim.GetParent() + if not parent or not parent.IsValid() or parent.GetPath().pathString == "/": + parent_bodies.append(WORLD_BODY_INDEX) + parent_locals.append(identity_xform) else: - body_idx, local_xform = self._resolve_ancestor_body(prim, body_labels, body_label_set, xform_cache) - site_bodies.append(body_idx) - site_locals.append(local_xform) + parent_path = parent.GetPath().pathString + if parent_path in resolve_cache: + pb_idx, pb_local = resolve_cache[parent_path] + elif parent_path in body_label_to_idx: + pb_idx = body_label_to_idx[parent_path] + pb_local = identity_xform + resolve_cache[parent_path] = (pb_idx, pb_local) + else: + pb_idx, pb_local = self._resolve_ancestor_body(parent, body_label_to_idx, xform_cache) + resolve_cache[parent_path] = (pb_idx, pb_local) + parent_bodies.append(pb_idx) + parent_locals.append(pb_local) self._site_body = wp.array(site_bodies, dtype=wp.int32, device=device) self._site_local = wp.array( @@ -305,19 +470,31 @@ def __init__(self, prim_path: str, device: str = "cpu", stage: Usd.Stage | None dtype=wp.transformf, device=device, ) + self._parent_site_body = wp.array(parent_bodies, dtype=wp.int32, device=device) + self._parent_site_local = wp.array( + [wp.transform(*x) for x in parent_locals], + dtype=wp.transformf, + device=device, + ) self._pos_buf = wp.zeros(self.count, dtype=wp.vec3f, device=device) self._quat_buf = wp.zeros(self.count, dtype=wp.vec4f, device=device) + self._local_pos_buf = wp.zeros(self.count, dtype=wp.vec3f, device=device) + self._local_quat_buf = wp.zeros(self.count, dtype=wp.vec4f, device=device) @staticmethod def _resolve_ancestor_body( prim: Usd.Prim, - body_labels: list[str], - body_label_set: set[str], + body_label_to_idx: dict[str, int], xform_cache: UsdGeom.XformCache, ) -> tuple[int, list[float]]: """Walk USD ancestors to find the nearest Newton body and compute the relative local transform. + Args: + prim: The USD prim to resolve. + body_label_to_idx: Dict mapping body prim paths to their Newton body indices. + xform_cache: USD xform cache for efficient transform lookups. + Returns: A tuple ``(body_index, local_xform_7)`` where *local_xform_7* is ``[tx, ty, tz, qx, qy, qz, qw]``. If no body ancestor exists, @@ -330,8 +507,8 @@ def _resolve_ancestor_body( ancestor = prim.GetParent() while ancestor and ancestor.IsValid() and ancestor.GetPath().pathString != "/": ancestor_path = ancestor.GetPath().pathString - if ancestor_path in body_label_set: - body_idx = body_labels.index(ancestor_path) + body_idx = body_label_to_idx.get(ancestor_path) + if body_idx is not None: ancestor_world_tf = xform_cache.GetLocalToWorldTransform(ancestor) ancestor_world_tf.Orthonormalize() local_tf = prim_world_tf * ancestor_world_tf.GetInverse() @@ -380,12 +557,11 @@ def set_world_poses( orientations: wp.array | None = None, indices: Sequence[int] | None = None, ) -> None: - """Write world poses into Newton ``state.body_q``. + """Write world poses by updating the site's local offset. - For sites with a non-identity local transform the Warp kernel computes - ``body_q = world_site_pose * inverse(local_transform)`` so that the - resulting world pose of the *site* matches the requested value. - World-attached sites (``body_index == -1``) are skipped. + Computes the new ``site_local`` such that + ``body_q[body] * new_site_local == desired_world``. + Does not modify ``body_q``. """ if positions is None and orientations is None: return @@ -406,25 +582,62 @@ def set_world_poses( n = len(indices) idx_wp = wp.array(list(indices), dtype=wp.int32, device=self._device) wp.launch( - _write_body_q_from_site_poses_indexed, + _write_site_local_from_world_poses_indexed, dim=n, inputs=[state.body_q, self._site_body, self._site_local, idx_wp, pos_wp, quat_wp], device=self._device, ) else: wp.launch( - _write_body_q_from_site_poses, + _write_site_local_from_world_poses, dim=self.count, inputs=[state.body_q, self._site_body, self._site_local, pos_wp, quat_wp], device=self._device, ) # ------------------------------------------------------------------ - # Local poses -- delegate to world (Newton bodies live in world space) + # Local poses (parent-relative) # ------------------------------------------------------------------ def get_local_poses(self, indices: Sequence[int] | None = None) -> tuple[wp.array, wp.array]: - return self.get_world_poses(indices) + """Get parent-relative poses: ``local = inv(parent_world) * prim_world``.""" + state = NewtonManager.get_state_0() + + if indices is not None: + n = len(indices) + idx_wp = wp.array(list(indices), dtype=wp.int32, device=self._device) + pos_buf = wp.zeros(n, dtype=wp.vec3f, device=self._device) + quat_buf = wp.zeros(n, dtype=wp.vec4f, device=self._device) + wp.launch( + _compute_site_local_transforms_indexed, + dim=n, + inputs=[ + state.body_q, + self._site_body, + self._site_local, + self._parent_site_body, + self._parent_site_local, + idx_wp, + ], + outputs=[pos_buf, quat_buf], + device=self._device, + ) + return pos_buf, quat_buf + + wp.launch( + _compute_site_local_transforms, + dim=self.count, + inputs=[ + state.body_q, + self._site_body, + self._site_local, + self._parent_site_body, + self._parent_site_local, + ], + outputs=[self._local_pos_buf, self._local_quat_buf], + device=self._device, + ) + return self._local_pos_buf, self._local_quat_buf def set_local_poses( self, @@ -432,7 +645,59 @@ def set_local_poses( orientations: wp.array | None = None, indices: Sequence[int] | None = None, ) -> None: - self.set_world_poses(positions=translations, orientations=orientations, indices=indices) + """Write parent-relative poses by updating the site's local offset. + + Computes the new ``site_local`` such that + ``inv(parent_world) * (body_q[bid] * site_local) == desired_local``. + """ + if translations is None and orientations is None: + return + + state = NewtonManager.get_state_0() + + if translations is None or orientations is None: + cur_pos, cur_quat = self.get_local_poses(indices) + if translations is None: + translations = cur_pos + if orientations is None: + orientations = cur_quat + + pos_wp = _ensure_wp_vec3f(translations, self._device) + quat_wp = _ensure_wp_vec4f(orientations, self._device) + + if indices is not None: + n = len(indices) + idx_wp = wp.array(list(indices), dtype=wp.int32, device=self._device) + wp.launch( + _write_site_local_from_local_poses_indexed, + dim=n, + inputs=[ + state.body_q, + self._site_body, + self._site_local, + self._parent_site_body, + self._parent_site_local, + idx_wp, + pos_wp, + quat_wp, + ], + device=self._device, + ) + else: + wp.launch( + _write_site_local_from_local_poses, + dim=self.count, + inputs=[ + state.body_q, + self._site_body, + self._site_local, + self._parent_site_body, + self._parent_site_local, + pos_wp, + quat_wp, + ], + device=self._device, + ) # ------------------------------------------------------------------ # Scales diff --git a/source/isaaclab_newton/test/sim/test_views_xform_prim_newton.py b/source/isaaclab_newton/test/sim/test_views_xform_prim_newton.py index 5c440e0cbbc7..1b97414fb865 100644 --- a/source/isaaclab_newton/test/sim/test_views_xform_prim_newton.py +++ b/source/isaaclab_newton/test/sim/test_views_xform_prim_newton.py @@ -3,18 +3,18 @@ # # SPDX-License-Identifier: BSD-3-Clause -"""Tests for Newton XformPrimView backed by sites (body_q + local transform). +"""Newton backend tests for XformPrimView. -Covers the same test surface as the core USD and PhysX Fabric tests where -applicable, plus Newton-specific scenarios: ancestor-walk resolution for -non-physics prims, world-attached (body=-1) prims, and hierarchical prims -(Xform child under a rigid body). +Imports the shared contract tests and provides the Newton-specific +``view_factory`` fixture. Also includes Newton-only guard tests and +the world-attached prim edge case. """ import sys from pathlib import Path sys.path.insert(0, str(Path(__file__).resolve().parents[1])) +sys.path.insert(0, str(Path(__file__).resolve().parents[3] / "isaaclab" / "test" / "sim")) import pytest import torch @@ -29,24 +29,16 @@ from isaaclab.assets import RigidObjectCfg from isaaclab.scene import InteractiveScene, InteractiveSceneCfg from isaaclab.sim import SimulationCfg, build_simulation_context +from xform_contract_tests import * # noqa: F401, F403 — import all contract tests +from xform_contract_tests import CHILD_OFFSET, ViewBundle, _wp_vec3f, _wp_vec4f from isaaclab.utils import configclass -NEWTON_SIM_CFG = SimulationCfg( - physics=NewtonCfg( - solver_cfg=MJWarpSolverCfg(), - ), -) - - -def _assert_close(a, b, **kwargs): - """Compare two arrays (wp.array or torch.Tensor) via torch.testing.assert_close.""" - a_t = wp.to_torch(a) if isinstance(a, wp.array) else a - b_t = wp.to_torch(b) if isinstance(b, wp.array) else b - torch.testing.assert_close(a_t, b_t, **kwargs) +NEWTON_SIM_CFG = SimulationCfg(physics=NewtonCfg(solver_cfg=MJWarpSolverCfg())) +WORLD_MARKER_POS = (5.0, 3.0, 1.0) @configclass -class SimpleSceneCfg(InteractiveSceneCfg): +class _SceneCfg(InteractiveSceneCfg): cube: RigidObjectCfg = RigidObjectCfg( prim_path="{ENV_REGEX_NS}/Cube", spawn=sim_utils.CuboidCfg( @@ -59,374 +51,148 @@ class SimpleSceneCfg(InteractiveSceneCfg): ) -def _newton_sim_context(device="cuda:0", num_envs=4, **kwargs): +def _sim_context(device, num_envs=4): NEWTON_SIM_CFG.device = device - return build_simulation_context(device=device, sim_cfg=NEWTON_SIM_CFG, **kwargs) - - -def _build_scene(num_envs=4, device="cuda:0"): - """Build scene and return (sim, scene, view, ctx).""" - ctx = _newton_sim_context(device=device, num_envs=num_envs, add_ground_plane=True) - sim = ctx.__enter__() - sim._app_control_on_stop_handle = None - scene = InteractiveScene(SimpleSceneCfg(num_envs=num_envs, env_spacing=2.0)) - sim.reset() - view = XformPrimView("/World/envs/env_.*/Cube", device=device) - return sim, scene, view, ctx - - -def _build_scene_with_child_xform(num_envs=4, device="cuda:0"): - """Build scene with a child Xform prim under each Cube (camera mount scenario).""" - ctx = _newton_sim_context(device=device, num_envs=num_envs, add_ground_plane=True) - sim = ctx.__enter__() - sim._app_control_on_stop_handle = None - scene = InteractiveScene(SimpleSceneCfg(num_envs=num_envs, env_spacing=2.0)) - - stage = sim_utils.get_current_stage() - for i in range(num_envs): - xform_path = f"/World/envs/env_{i}/Cube/CameraMount" - xform_prim = stage.DefinePrim(xform_path, "Xform") - sim_utils.standardize_xform_ops(xform_prim) - xform_prim.GetAttribute("xformOp:translate").Set(Gf.Vec3d(0.1, 0.0, 0.05)) - xform_prim.GetAttribute("xformOp:orient").Set(Gf.Quatd(1.0, 0.0, 0.0, 0.0)) - - sim.reset() - view = XformPrimView("/World/envs/env_.*/Cube/CameraMount", device=device) - return sim, scene, view, ctx - - -def _build_scene_with_world_xform(num_envs=4, device="cuda:0"): - """Build scene with a plain Xform at the world root (no body ancestor).""" - ctx = _newton_sim_context(device=device, num_envs=num_envs, add_ground_plane=True) - sim = ctx.__enter__() - sim._app_control_on_stop_handle = None - scene = InteractiveScene(SimpleSceneCfg(num_envs=num_envs, env_spacing=2.0)) - - stage = sim_utils.get_current_stage() - xform_path = "/World/StaticMarker" - xform_prim = stage.DefinePrim(xform_path, "Xform") - sim_utils.standardize_xform_ops(xform_prim) - xform_prim.GetAttribute("xformOp:translate").Set(Gf.Vec3d(5.0, 3.0, 1.0)) - xform_prim.GetAttribute("xformOp:orient").Set(Gf.Quatd(1.0, 0.0, 0.0, 0.0)) - - sim.reset() - view = XformPrimView("/World/StaticMarker", device=device) - return sim, scene, view, ctx - - -# ====================================================================== -# Tests - Initialization -# ====================================================================== + return build_simulation_context(device=device, sim_cfg=NEWTON_SIM_CFG, add_ground_plane=True) -@pytest.mark.skipif(not torch.cuda.is_available(), reason="CUDA not available") -def test_count(): - """Test that Newton XformPrimView reports correct prim count.""" - sim, scene, view, ctx = _build_scene(num_envs=4) - assert view.count == 4 - ctx.__exit__(None, None, None) - - -@pytest.mark.skipif(not torch.cuda.is_available(), reason="CUDA not available") -@pytest.mark.parametrize("num_envs", [1, 4, 8]) -def test_count_various_sizes(num_envs): - """Test count with different numbers of environments.""" - sim, scene, view, ctx = _build_scene(num_envs=num_envs) - assert view.count == num_envs - ctx.__exit__(None, None, None) - - -# ====================================================================== -# Tests - Getters (returns wp.array) -# ====================================================================== - - -@pytest.mark.skipif(not torch.cuda.is_available(), reason="CUDA not available") -def test_get_world_poses_returns_warp_arrays(): - """Test that get_world_poses returns wp.array, not torch.Tensor.""" - sim, scene, view, ctx = _build_scene(num_envs=4) - positions, orientations = view.get_world_poses() - - assert isinstance(positions, wp.array) - assert isinstance(orientations, wp.array) - assert positions.shape[0] == 4 - assert orientations.shape[0] == 4 - pos_t = wp.to_torch(positions) - quat_t = wp.to_torch(orientations) - assert pos_t.shape == (4, 3) - assert quat_t.shape == (4, 4) - assert pos_t.device.type == "cuda" - ctx.__exit__(None, None, None) - - -@pytest.mark.skipif(not torch.cuda.is_available(), reason="CUDA not available") -def test_get_world_poses_matches_body_q(): - """Test that get_world_poses returns values matching body_q.""" - sim, scene, view, ctx = _build_scene(num_envs=4) - positions, orientations = view.get_world_poses() - positions_t = wp.to_torch(positions) - +def _get_body_positions(num_envs, device="cpu"): model = NewtonManager.get_model() body_labels = list(model.body_label) - state = NewtonManager.get_state_0() - body_q = wp.to_torch(state.body_q) - - for i in range(4): - prim_path = f"/World/envs/env_{i}/Cube" - if prim_path in body_labels: - body_idx = body_labels.index(prim_path) - expected_pos = body_q[body_idx, :3] - torch.testing.assert_close(positions_t[i], expected_pos, atol=1e-5, rtol=0) - ctx.__exit__(None, None, None) + body_q_t = wp.to_torch(NewtonManager.get_state_0().body_q) + return torch.stack([body_q_t[body_labels.index(f"/World/envs/env_{i}/Cube"), :3] for i in range(num_envs)]) -@pytest.mark.skipif(not torch.cuda.is_available(), reason="CUDA not available") -def test_get_world_poses_with_indices(): - """Test get_world_poses with a subset of indices.""" - sim, scene, view, ctx = _build_scene(num_envs=4) - pos_all_t = wp.to_torch(view.get_world_poses()[0]) - - indices = [0, 2] - positions_subset, orientations_subset = view.get_world_poses(indices) - - assert isinstance(positions_subset, wp.array) - pos_sub_t = wp.to_torch(positions_subset) - quat_sub_t = wp.to_torch(orientations_subset) - assert pos_sub_t.shape == (2, 3) - assert quat_sub_t.shape == (2, 4) - torch.testing.assert_close(pos_sub_t[0], pos_all_t[0], atol=1e-6, rtol=0) - torch.testing.assert_close(pos_sub_t[1], pos_all_t[2], atol=1e-6, rtol=0) - ctx.__exit__(None, None, None) - - -@pytest.mark.skipif(not torch.cuda.is_available(), reason="CUDA not available") -def test_get_world_poses_single_index(): - """Test get_world_poses with a single index.""" - sim, scene, view, ctx = _build_scene(num_envs=5) - pos_all_t = wp.to_torch(view.get_world_poses()[0]) - - positions_single, orientations_single = view.get_world_poses([3]) - pos_s_t = wp.to_torch(positions_single) - quat_s_t = wp.to_torch(orientations_single) - assert pos_s_t.shape == (1, 3) - assert quat_s_t.shape == (1, 4) - torch.testing.assert_close(pos_s_t[0], pos_all_t[3], atol=1e-6, rtol=0) - ctx.__exit__(None, None, None) - +def _set_body_positions(positions, num_envs): + model = NewtonManager.get_model() + body_labels = list(model.body_label) + body_q_t = wp.to_torch(NewtonManager.get_state_0().body_q) + for i in range(num_envs): + body_q_t[body_labels.index(f"/World/envs/env_{i}/Cube"), :3] = positions[i] -@pytest.mark.skipif(not torch.cuda.is_available(), reason="CUDA not available") -def test_get_world_poses_out_of_order_indices(): - """Test get_world_poses with out-of-order indices.""" - sim, scene, view, ctx = _build_scene(num_envs=5) - pos_all_t = wp.to_torch(view.get_world_poses()[0]) - indices = [4, 1, 3] - positions_subset, _ = view.get_world_poses(indices) - pos_sub_t = wp.to_torch(positions_subset) +# ------------------------------------------------------------------ +# Contract fixture +# ------------------------------------------------------------------ - assert pos_sub_t.shape == (3, 3) - torch.testing.assert_close(pos_sub_t[0], pos_all_t[4], atol=1e-6, rtol=0) - torch.testing.assert_close(pos_sub_t[1], pos_all_t[1], atol=1e-6, rtol=0) - torch.testing.assert_close(pos_sub_t[2], pos_all_t[3], atol=1e-6, rtol=0) - ctx.__exit__(None, None, None) +@pytest.fixture +def view_factory(): + """Newton factory: CameraMount child Xform at CHILD_OFFSET under each Cube body.""" -# ====================================================================== -# Tests - Setters (wp.array input) -# ====================================================================== + def factory(num_envs: int, device: str) -> ViewBundle: + ctx = _sim_context(device, num_envs=num_envs) + sim = ctx.__enter__() + sim._app_control_on_stop_handle = None + InteractiveScene(_SceneCfg(num_envs=num_envs, env_spacing=2.0)) + stage = sim_utils.get_current_stage() + for i in range(num_envs): + prim = stage.DefinePrim(f"/World/envs/env_{i}/Cube/CameraMount", "Xform") + sim_utils.standardize_xform_ops(prim) + prim.GetAttribute("xformOp:translate").Set(Gf.Vec3d(*CHILD_OFFSET)) + prim.GetAttribute("xformOp:orient").Set(Gf.Quatd(1.0, 0.0, 0.0, 0.0)) -def _wp_vec3f(data, device="cuda:0"): - return wp.array([wp.vec3f(*row) for row in data], dtype=wp.vec3f, device=device) + sim.reset() + view = XformPrimView("/World/envs/env_.*/Cube/CameraMount", device=device) + return ViewBundle( + view=view, + get_parent_pos=_get_body_positions, + set_parent_pos=_set_body_positions, + teardown=lambda: ctx.__exit__(None, None, None), + ) -def _wp_vec4f(data, device="cuda:0"): - return wp.array([wp.vec4f(*row) for row in data], dtype=wp.vec4f, device=device) + return factory -@pytest.mark.skipif(not torch.cuda.is_available(), reason="CUDA not available") -def test_set_world_poses(): - """Test setting world poses writes to body_q.""" - sim, scene, view, ctx = _build_scene(num_envs=4) +# ================================================================== +# Newton-only: guard tests +# ================================================================== - new_pos = _wp_vec3f([[10.0, 20.0, 30.0], [40.0, 50.0, 60.0], [70.0, 80.0, 90.0], [100.0, 110.0, 120.0]]) - new_quat = _wp_vec4f( - [ - [0.0, 0.0, 0.0, 1.0], - [0.0, 0.0, 0.7071068, 0.7071068], - [0.7071068, 0.0, 0.0, 0.7071068], - [0.3826834, 0.0, 0.0, 0.9238795], - ] - ) - view.set_world_poses(new_pos, new_quat) - ret_pos, ret_quat = view.get_world_poses() +@pytest.mark.parametrize("device", ["cpu", "cuda:0"]) +def test_reject_body_path(device): + """XformPrimView rejects prim paths that resolve to a Newton physics body.""" + ctx = _sim_context(device, num_envs=2) + sim = ctx.__enter__() + sim._app_control_on_stop_handle = None + InteractiveScene(_SceneCfg(num_envs=2, env_spacing=2.0)) + sim.reset() - _assert_close(ret_pos, new_pos, atol=1e-5, rtol=0) - _assert_close(ret_quat, new_quat, atol=1e-5, rtol=0) + with pytest.raises(ValueError, match="physics body"): + XformPrimView("/World/envs/env_.*/Cube", device=device) ctx.__exit__(None, None, None) -@pytest.mark.skipif(not torch.cuda.is_available(), reason="CUDA not available") -def test_set_world_poses_only_positions(): - """Test setting only positions, leaving orientations unchanged.""" - sim, scene, view, ctx = _build_scene(num_envs=3) - _, initial_quat = view.get_world_poses() +@pytest.mark.parametrize("device", ["cpu", "cuda:0"]) +def test_reject_shape_path(device): + """XformPrimView rejects prim paths that resolve to a Newton collision shape.""" + ctx = _sim_context(device, num_envs=2) + sim = ctx.__enter__() + sim._app_control_on_stop_handle = None + InteractiveScene(_SceneCfg(num_envs=2, env_spacing=2.0)) + sim.reset() - new_pos = _wp_vec3f([[1.0, 0.0, 0.0], [0.0, 2.0, 0.0], [0.0, 0.0, 3.0]]) - view.set_world_poses(positions=new_pos, orientations=None) + shape_labels = list(NewtonManager.get_model().shape_label) + if not shape_labels: + pytest.skip("No shapes in model") - ret_pos, ret_quat = view.get_world_poses() - _assert_close(ret_pos, new_pos, atol=1e-5, rtol=0) - _assert_close(ret_quat, initial_quat, atol=1e-5, rtol=0) + with pytest.raises(ValueError, match="collision shape"): + XformPrimView(shape_labels[0], device=device) ctx.__exit__(None, None, None) -@pytest.mark.skipif(not torch.cuda.is_available(), reason="CUDA not available") -def test_set_world_poses_only_orientations(): - """Test setting only orientations, leaving positions unchanged.""" - sim, scene, view, ctx = _build_scene(num_envs=3) - initial_pos, _ = view.get_world_poses() - - new_quat = _wp_vec4f( - [ - [0.0, 0.0, 0.7071068, 0.7071068], - [0.7071068, 0.0, 0.0, 0.7071068], - [0.3826834, 0.0, 0.0, 0.9238795], - ] - ) - view.set_world_poses(positions=None, orientations=new_quat) - - ret_pos, ret_quat = view.get_world_poses() - _assert_close(ret_pos, initial_pos, atol=1e-5, rtol=0) - _assert_close(ret_quat, new_quat, atol=1e-5, rtol=0) - ctx.__exit__(None, None, None) +# ================================================================== +# Newton edge case: world-attached prim (body=-1) +# ================================================================== -@pytest.mark.skipif(not torch.cuda.is_available(), reason="CUDA not available") -def test_set_world_poses_with_indices(): - """Test setting world poses for a subset of indices.""" - sim, scene, view, ctx = _build_scene(num_envs=5) - init_pos_t = wp.to_torch(view.get_world_poses()[0]).clone() +@pytest.mark.parametrize("device", ["cpu", "cuda:0"]) +def test_world_attached_returns_initial_pose(device): + """A world-rooted Xform returns its USD-authored position.""" + ctx = _sim_context(device, num_envs=2) + sim = ctx.__enter__() + sim._app_control_on_stop_handle = None + InteractiveScene(_SceneCfg(num_envs=2, env_spacing=2.0)) - indices = [1, 3] - new_pos = _wp_vec3f([[10.0, 0.0, 0.0], [30.0, 0.0, 0.0]]) - view.set_world_poses(positions=new_pos, orientations=None, indices=indices) + stage = sim_utils.get_current_stage() + prim = stage.DefinePrim("/World/StaticMarker", "Xform") + sim_utils.standardize_xform_ops(prim) + prim.GetAttribute("xformOp:translate").Set(Gf.Vec3d(*WORLD_MARKER_POS)) + prim.GetAttribute("xformOp:orient").Set(Gf.Quatd(1.0, 0.0, 0.0, 0.0)) - updated_pos_t = wp.to_torch(view.get_world_poses()[0]) - new_pos_t = wp.to_torch(new_pos) + sim.reset() + view = XformPrimView("/World/StaticMarker", device=device) - torch.testing.assert_close(updated_pos_t[1], new_pos_t[0], atol=1e-5, rtol=0) - torch.testing.assert_close(updated_pos_t[3], new_pos_t[1], atol=1e-5, rtol=0) - torch.testing.assert_close(updated_pos_t[0], init_pos_t[0], atol=1e-5, rtol=0) - torch.testing.assert_close(updated_pos_t[2], init_pos_t[2], atol=1e-5, rtol=0) - torch.testing.assert_close(updated_pos_t[4], init_pos_t[4], atol=1e-5, rtol=0) + pos = wp.to_torch(view.get_world_poses()[0]) + expected = torch.tensor([list(WORLD_MARKER_POS)], device=device) + torch.testing.assert_close(pos, expected, atol=1e-5, rtol=0) ctx.__exit__(None, None, None) -# ====================================================================== -# Tests - Round-trip consistency -# ====================================================================== - +@pytest.mark.parametrize("device", ["cpu", "cuda:0"]) +def test_world_attached_set_world_roundtrip(device): + """A world-attached prim can be repositioned via set_world_poses.""" + ctx = _sim_context(device, num_envs=2) + sim = ctx.__enter__() + sim._app_control_on_stop_handle = None + InteractiveScene(_SceneCfg(num_envs=2, env_spacing=2.0)) -@pytest.mark.skipif(not torch.cuda.is_available(), reason="CUDA not available") -def test_write_read_roundtrip(): - """Test that write -> read round-trip is consistent.""" - sim, scene, view, ctx = _build_scene(num_envs=4) + stage = sim_utils.get_current_stage() + prim = stage.DefinePrim("/World/StaticMarker", "Xform") + sim_utils.standardize_xform_ops(prim) + prim.GetAttribute("xformOp:translate").Set(Gf.Vec3d(*WORLD_MARKER_POS)) + prim.GetAttribute("xformOp:orient").Set(Gf.Quatd(1.0, 0.0, 0.0, 0.0)) - new_pos = _wp_vec3f([[1.0, 2.0, 3.0], [4.0, 5.0, 6.0], [7.0, 8.0, 9.0], [10.0, 11.0, 12.0]]) - new_quat = _wp_vec4f([[0.0, 0.0, 0.0, 1.0]] * 4) + sim.reset() + view = XformPrimView("/World/StaticMarker", device=device) + new_pos = _wp_vec3f([[10.0, 20.0, 30.0]], device=device) + new_quat = _wp_vec4f([[0.0, 0.0, 0.0, 1.0]], device=device) view.set_world_poses(new_pos, new_quat) - pos, quat = view.get_world_poses() - - _assert_close(pos, new_pos, atol=1e-5, rtol=0) - _assert_close(quat, new_quat, atol=1e-5, rtol=0) - - new_pos2 = _wp_vec3f([[11.0, 12.0, 13.0], [14.0, 15.0, 16.0], [17.0, 18.0, 19.0], [20.0, 21.0, 22.0]]) - view.set_world_poses(new_pos2, new_quat) - pos2, quat2 = view.get_world_poses() - _assert_close(pos2, new_pos2, atol=1e-5, rtol=0) - _assert_close(quat2, new_quat, atol=1e-5, rtol=0) - ctx.__exit__(None, None, None) - - -# ====================================================================== -# Tests - Ancestor-walk resolution (non-physics Xform under rigid body) -# ====================================================================== - - -@pytest.mark.skipif(not torch.cuda.is_available(), reason="CUDA not available") -def test_child_xform_initialization(): - """Test that a child Xform under a rigid body resolves via ancestor walk.""" - sim, scene, view, ctx = _build_scene_with_child_xform(num_envs=4) - assert view.count == 4 - ctx.__exit__(None, None, None) - - -@pytest.mark.skipif(not torch.cuda.is_available(), reason="CUDA not available") -def test_child_xform_world_pose_includes_offset(): - """Test that child Xform world pose includes the local offset from the body. - - The CameraMount is at (0.1, 0.0, 0.05) relative to the Cube body. - Its world pose should be body_world_pos + rotated_offset. - """ - sim, scene, view, ctx = _build_scene_with_child_xform(num_envs=4) - child_positions, child_orientations = view.get_world_poses() - child_pos_t = wp.to_torch(child_positions) - - body_view = XformPrimView("/World/envs/env_.*/Cube", device="cuda:0") - body_positions, body_orientations = body_view.get_world_poses() - body_pos_t = wp.to_torch(body_positions) - - offset = torch.tensor([0.1, 0.0, 0.05], device="cuda:0") - for i in range(4): - expected_pos = body_pos_t[i] + offset - torch.testing.assert_close(child_pos_t[i], expected_pos, atol=1e-4, rtol=0) - ctx.__exit__(None, None, None) - - -@pytest.mark.skipif(not torch.cuda.is_available(), reason="CUDA not available") -def test_child_xform_tracks_body_after_move(): - """Test that child Xform world pose updates when parent body moves.""" - sim, scene, view, ctx = _build_scene_with_child_xform(num_envs=2) - - body_view = XformPrimView("/World/envs/env_.*/Cube", device="cuda:0") - new_body_pos = _wp_vec3f([[5.0, 0.0, 0.0], [0.0, 5.0, 0.0]]) - new_body_orient = _wp_vec4f([[0.0, 0.0, 0.0, 1.0]] * 2) - body_view.set_world_poses(new_body_pos, new_body_orient) - - child_positions, _ = view.get_world_poses() - child_pos_t = wp.to_torch(child_positions) - new_body_pos_t = wp.to_torch(new_body_pos) - - offset = torch.tensor([0.1, 0.0, 0.05], device="cuda:0") - for i in range(2): - expected_pos = new_body_pos_t[i] + offset - torch.testing.assert_close(child_pos_t[i], expected_pos, atol=1e-4, rtol=0) - ctx.__exit__(None, None, None) - - -# ====================================================================== -# Tests - World-attached prims (body=-1) -# ====================================================================== - - -@pytest.mark.skipif(not torch.cuda.is_available(), reason="CUDA not available") -def test_world_xform_initialization(): - """Test that a world-rooted Xform resolves with body=-1.""" - sim, scene, view, ctx = _build_scene_with_world_xform(num_envs=2) - assert view.count == 1 - ctx.__exit__(None, None, None) - - -@pytest.mark.skipif(not torch.cuda.is_available(), reason="CUDA not available") -def test_world_xform_returns_correct_pose(): - """Test that a world-rooted Xform returns its USD world transform.""" - sim, scene, view, ctx = _build_scene_with_world_xform(num_envs=2) - positions, orientations = view.get_world_poses() - - expected_pos = torch.tensor([[5.0, 3.0, 1.0]], device="cuda:0") - _assert_close(positions, expected_pos, atol=1e-4, rtol=0) + ret_pos, ret_quat = view.get_world_poses() + torch.testing.assert_close(wp.to_torch(ret_pos), wp.to_torch(new_pos), atol=1e-5, rtol=0) + torch.testing.assert_close(wp.to_torch(ret_quat), wp.to_torch(new_quat), atol=1e-5, rtol=0) ctx.__exit__(None, None, None) diff --git a/source/isaaclab_physx/isaaclab_physx/sim/views/xform_prim_view.py b/source/isaaclab_physx/isaaclab_physx/sim/views/xform_prim_view.py index b044c7487ef5..100672f22fb6 100644 --- a/source/isaaclab_physx/isaaclab_physx/sim/views/xform_prim_view.py +++ b/source/isaaclab_physx/isaaclab_physx/sim/views/xform_prim_view.py @@ -23,6 +23,22 @@ logger = logging.getLogger(__name__) +def _to_float32_2d(a: wp.array | torch.Tensor) -> wp.array | torch.Tensor: + """Ensure array is compatible with Fabric kernels (2-D float32). + + For ``wp.array`` with vec dtypes (``vec3f``, ``vec4f``), uses + :meth:`wp.array.view` for zero-copy reinterpretation. + ``torch.Tensor`` and already-correct 2-D float32 arrays pass through. + """ + if not isinstance(a, wp.array): + return a + if a.shape[0] == 0: + return a + if a.ndim == 2 and a.dtype == wp.float32: + return a + return a.view(dtype=wp.float32) + + class XformPrimView(UsdXformPrimView): """XformPrimView with Fabric GPU acceleration for the PhysX backend. @@ -116,11 +132,10 @@ def set_world_poses( indices_wp = self._resolve_indices_wp(indices) count = indices_wp.shape[0] - positions_wp = positions if positions is not None else wp.zeros((0, 3), dtype=wp.float32, device=self._device) - orientations_wp = ( - orientations if orientations is not None else wp.zeros((0, 4), dtype=wp.float32, device=self._device) - ) - scales_wp = wp.zeros((0, 3), dtype=wp.float32, device=self._device) + dummy = wp.zeros((0, 3), dtype=wp.float32, device=self._device) + positions_wp = _to_float32_2d(positions) if positions is not None else dummy + orientations_wp = _to_float32_2d(orientations) if orientations is not None else wp.zeros((0, 4), dtype=wp.float32, device=self._device) + scales_wp = dummy wp.launch( kernel=fabric_utils.compose_fabric_transformation_matrix_from_warp_arrays, @@ -165,17 +180,18 @@ def set_scales(self, scales: wp.array, indices: Sequence[int] | None = None): indices_wp = self._resolve_indices_wp(indices) count = indices_wp.shape[0] - positions_wp = wp.zeros((0, 3), dtype=wp.float32, device=self._device) - orientations_wp = wp.zeros((0, 4), dtype=wp.float32, device=self._device) + dummy3 = wp.zeros((0, 3), dtype=wp.float32, device=self._device) + dummy4 = wp.zeros((0, 4), dtype=wp.float32, device=self._device) + scales_wp = _to_float32_2d(scales) wp.launch( kernel=fabric_utils.compose_fabric_transformation_matrix_from_warp_arrays, dim=count, inputs=[ self._fabric_world_matrices, - positions_wp, - orientations_wp, - scales, + dummy3, + dummy4, + scales_wp, False, False, False, diff --git a/source/isaaclab_physx/test/sim/test_views_xform_prim_fabric.py b/source/isaaclab_physx/test/sim/test_views_xform_prim_fabric.py index 4f757a69c2ff..a4b18fbe870c 100644 --- a/source/isaaclab_physx/test/sim/test_views_xform_prim_fabric.py +++ b/source/isaaclab_physx/test/sim/test_views_xform_prim_fabric.py @@ -3,13 +3,18 @@ # # SPDX-License-Identifier: BSD-3-Clause -"""Tests for PhysX XformPrimView with Fabric acceleration. +"""PhysX Fabric backend tests for XformPrimView. -Re-runs all the backend-parametrized tests from the core test suite with -``backend="fabric"`` and the PhysX :class:`XformPrimView`, plus the two -dedicated Fabric-only tests. +Imports the shared contract tests and provides the Fabric-specific +``view_factory`` fixture (SimulationContext with use_fabric=True, +Camera prim type for Fabric SelectPrims compatibility). """ +import sys +from pathlib import Path + +sys.path.insert(0, str(Path(__file__).resolve().parents[3] / "isaaclab" / "test" / "sim")) + from isaaclab.app import AppLauncher simulation_app = AppLauncher(headless=True).app @@ -17,16 +22,20 @@ import pytest # noqa: E402 import torch # noqa: E402 import warp as wp # noqa: E402 + +from pxr import Gf, UsdGeom # noqa: E402 + from isaaclab_physx.sim.views import XformPrimView # noqa: E402 import isaaclab.sim as sim_utils # noqa: E402 +from xform_contract_tests import * # noqa: F401, F403, E402 +from xform_contract_tests import CHILD_OFFSET, ViewBundle # noqa: E402 -BACKEND = "fabric" +PARENT_POS = (0.0, 0.0, 1.0) @pytest.fixture(autouse=True) def test_setup_teardown(): - """Create a blank new stage for each test.""" sim_utils.create_new_stage() sim_utils.update_stage() yield @@ -41,438 +50,62 @@ def _skip_if_unavailable(device: str): pytest.skip("Warp fabricarray operations on CPU have known issues") -def _create_view(pattern: str, device: str) -> XformPrimView: - sim_utils.SimulationContext(sim_utils.SimulationCfg(dt=0.01, device=device, use_fabric=True)) - return XformPrimView(pattern, device=device) - - -""" -Tests - Getters. -""" - - -@pytest.mark.parametrize("device", ["cpu", "cuda"]) -def test_get_world_poses(device): - _skip_if_unavailable(device) - stage = sim_utils.get_current_stage() - - expected_positions = [(1.0, 2.0, 3.0), (4.0, 5.0, 6.0), (7.0, 8.0, 9.0)] - expected_orientations = [(0.0, 0.0, 0.0, 1.0), (0.0, 0.0, 0.7071068, 0.7071068), (0.7071068, 0.0, 0.0, 0.7071068)] - - for i, (pos, quat) in enumerate(zip(expected_positions, expected_orientations)): - sim_utils.create_prim(f"/World/Object_{i}", "Camera", translation=pos, orientation=quat, stage=stage) - - view = _create_view("/World/Object_.*", device=device) - - expected_positions_tensor = torch.tensor(expected_positions, dtype=torch.float32, device=device) - expected_orientations_tensor = torch.tensor(expected_orientations, dtype=torch.float32, device=device) - - positions, orientations = view.get_world_poses() - positions, orientations = wp.to_torch(positions), wp.to_torch(orientations) - - assert positions.shape == (3, 3) - assert orientations.shape == (3, 4) - torch.testing.assert_close(positions, expected_positions_tensor, atol=1e-5, rtol=0) +# ------------------------------------------------------------------ +# Parent position helpers (via USD xformOps) +# ------------------------------------------------------------------ - try: - torch.testing.assert_close(orientations, expected_orientations_tensor, atol=1e-5, rtol=0) - except AssertionError: - torch.testing.assert_close(orientations, -expected_orientations_tensor, atol=1e-5, rtol=0) - -@pytest.mark.parametrize("device", ["cpu", "cuda"]) -def test_get_local_poses(device): - _skip_if_unavailable(device) +def _get_parent_positions(num_envs, device="cpu"): stage = sim_utils.get_current_stage() + xform_cache = UsdGeom.XformCache() + positions = [] + for i in range(num_envs): + prim = stage.GetPrimAtPath(f"/World/Parent_{i}") + tf = xform_cache.GetLocalToWorldTransform(prim) + t = tf.ExtractTranslation() + positions.append([float(t[0]), float(t[1]), float(t[2])]) + return torch.tensor(positions, dtype=torch.float32, device=device) - sim_utils.create_prim("/World/Parent", "Xform", translation=(10.0, 0.0, 0.0), stage=stage) - - expected_local_positions = [(1.0, 0.0, 0.0), (0.0, 2.0, 0.0), (0.0, 0.0, 3.0)] - expected_local_orientations = [ - (0.0, 0.0, 0.0, 1.0), - (0.0, 0.0, 0.7071068, 0.7071068), - (0.7071068, 0.0, 0.0, 0.7071068), - ] - - for i, (pos, quat) in enumerate(zip(expected_local_positions, expected_local_orientations)): - sim_utils.create_prim(f"/World/Parent/Child_{i}", "Camera", translation=pos, orientation=quat, stage=stage) - - view = _create_view("/World/Parent/Child_.*", device=device) - translations, orientations = view.get_local_poses() - translations, orientations = wp.to_torch(translations), wp.to_torch(orientations) - - assert translations.shape == (3, 3) - assert orientations.shape == (3, 4) - - expected_translations_tensor = torch.tensor(expected_local_positions, dtype=torch.float32, device=device) - expected_orientations_tensor = torch.tensor(expected_local_orientations, dtype=torch.float32, device=device) - - torch.testing.assert_close(translations, expected_translations_tensor, atol=1e-5, rtol=0) - try: - torch.testing.assert_close(orientations, expected_orientations_tensor, atol=1e-5, rtol=0) - except AssertionError: - torch.testing.assert_close(orientations, -expected_orientations_tensor, atol=1e-5, rtol=0) - - -@pytest.mark.parametrize("device", ["cpu", "cuda"]) -def test_get_scales(device): - _skip_if_unavailable(device) - stage = sim_utils.get_current_stage() - - expected_scales = [(1.0, 1.0, 1.0), (2.0, 2.0, 2.0), (1.0, 2.0, 3.0)] - for i, scale in enumerate(expected_scales): - sim_utils.create_prim(f"/World/Object_{i}", "Camera", scale=scale, stage=stage) - - view = _create_view("/World/Object_.*", device=device) - scales = wp.to_torch(view.get_scales()) - - assert scales.shape == (3, 3) - torch.testing.assert_close( - scales, torch.tensor(expected_scales, dtype=torch.float32, device=device), atol=1e-5, rtol=0 - ) - - -""" -Tests - Setters. -""" +def _set_parent_positions(positions, num_envs): + from pxr import Sdf # noqa: PLC0415 -@pytest.mark.parametrize("device", ["cpu", "cuda"]) -def test_set_world_poses(device): - _skip_if_unavailable(device) stage = sim_utils.get_current_stage() - - num_prims = 5 - for i in range(num_prims): - sim_utils.create_prim(f"/World/Object_{i}", "Camera", translation=(0.0, 0.0, 0.0), stage=stage) - - view = _create_view("/World/Object_.*", device=device) - - new_positions = torch.tensor( - [[1.0, 2.0, 3.0], [4.0, 5.0, 6.0], [7.0, 8.0, 9.0], [10.0, 11.0, 12.0], [13.0, 14.0, 15.0]], device=device - ) - new_orientations = torch.tensor( - [ - [0.0, 0.0, 0.0, 1.0], - [0.0, 0.0, 0.7071068, 0.7071068], - [0.7071068, 0.0, 0.0, 0.7071068], - [0.3826834, 0.0, 0.0, 0.9238795], - [0.0, 0.7071068, 0.0, 0.7071068], - ], - device=device, - ) - - view.set_world_poses(new_positions, new_orientations) - retrieved_positions, retrieved_orientations = view.get_world_poses() - retrieved_positions, retrieved_orientations = wp.to_torch(retrieved_positions), wp.to_torch(retrieved_orientations) - - torch.testing.assert_close(retrieved_positions, new_positions, atol=1e-5, rtol=0) - try: - torch.testing.assert_close(retrieved_orientations, new_orientations, atol=1e-5, rtol=0) - except AssertionError: - torch.testing.assert_close(retrieved_orientations, -new_orientations, atol=1e-5, rtol=0) - - -@pytest.mark.parametrize("device", ["cpu", "cuda"]) -def test_set_world_poses_only_positions(device): - _skip_if_unavailable(device) - stage = sim_utils.get_current_stage() - - initial_quat = (0.0, 0.0, 0.7071068, 0.7071068) - for i in range(3): - sim_utils.create_prim( - f"/World/Object_{i}", "Camera", translation=(0.0, 0.0, 0.0), orientation=initial_quat, stage=stage + with Sdf.ChangeBlock(): + for i in range(num_envs): + prim = stage.GetPrimAtPath(f"/World/Parent_{i}") + pos = positions[i] + prim.GetAttribute("xformOp:translate").Set(Gf.Vec3d(float(pos[0]), float(pos[1]), float(pos[2]))) + + +# ------------------------------------------------------------------ +# Contract fixture +# ------------------------------------------------------------------ + + +@pytest.fixture +def view_factory(): + """Fabric factory: Camera child at CHILD_OFFSET under parent Xforms, with Fabric enabled.""" + + def factory(num_envs: int, device: str) -> ViewBundle: + _skip_if_unavailable(device) + + stage = sim_utils.get_current_stage() + for i in range(num_envs): + sim_utils.create_prim( + f"/World/Parent_{i}", "Xform", translation=PARENT_POS, stage=stage + ) + sim_utils.create_prim( + f"/World/Parent_{i}/Child", "Camera", translation=CHILD_OFFSET, stage=stage + ) + + sim_utils.SimulationContext(sim_utils.SimulationCfg(dt=0.01, device=device, use_fabric=True)) + view = XformPrimView("/World/Parent_.*/Child", device=device) + return ViewBundle( + view=view, + get_parent_pos=_get_parent_positions, + set_parent_pos=_set_parent_positions, + teardown=lambda: None, ) - view = _create_view("/World/Object_.*", device=device) - _, initial_orientations = view.get_world_poses() - initial_orientations = wp.to_torch(initial_orientations) - - new_positions = torch.tensor([[1.0, 0.0, 0.0], [0.0, 2.0, 0.0], [0.0, 0.0, 3.0]], device=device) - view.set_world_poses(positions=new_positions, orientations=None) - - retrieved_positions, retrieved_orientations = view.get_world_poses() - retrieved_positions, retrieved_orientations = wp.to_torch(retrieved_positions), wp.to_torch(retrieved_orientations) - torch.testing.assert_close(retrieved_positions, new_positions, atol=1e-5, rtol=0) - try: - torch.testing.assert_close(retrieved_orientations, initial_orientations, atol=1e-5, rtol=0) - except AssertionError: - torch.testing.assert_close(retrieved_orientations, -initial_orientations, atol=1e-5, rtol=0) - - -@pytest.mark.parametrize("device", ["cpu", "cuda"]) -def test_set_world_poses_only_orientations(device): - _skip_if_unavailable(device) - stage = sim_utils.get_current_stage() - - for i in range(3): - sim_utils.create_prim(f"/World/Object_{i}", "Camera", translation=(float(i), 0.0, 0.0), stage=stage) - - view = _create_view("/World/Object_.*", device=device) - initial_positions = wp.to_torch(view.get_world_poses()[0]) - - new_orientations = torch.tensor( - [[0.0, 0.0, 0.7071068, 0.7071068], [0.7071068, 0.0, 0.0, 0.7071068], [0.3826834, 0.0, 0.0, 0.9238795]], - device=device, - ) - view.set_world_poses(positions=None, orientations=new_orientations) - - retrieved_positions, retrieved_orientations = view.get_world_poses() - retrieved_positions, retrieved_orientations = wp.to_torch(retrieved_positions), wp.to_torch(retrieved_orientations) - torch.testing.assert_close(retrieved_positions, initial_positions, atol=1e-5, rtol=0) - try: - torch.testing.assert_close(retrieved_orientations, new_orientations, atol=1e-5, rtol=0) - except AssertionError: - torch.testing.assert_close(retrieved_orientations, -new_orientations, atol=1e-5, rtol=0) - - -@pytest.mark.parametrize("device", ["cpu", "cuda"]) -def test_set_world_poses_with_hierarchy(device): - _skip_if_unavailable(device) - stage = sim_utils.get_current_stage() - - for i in range(3): - parent_pos = (i * 10.0, 0.0, 0.0) - parent_quat = (0.0, 0.0, 0.7071068, 0.7071068) - sim_utils.create_prim( - f"/World/Parent_{i}", "Xform", translation=parent_pos, orientation=parent_quat, stage=stage - ) - sim_utils.create_prim(f"/World/Parent_{i}/Child", "Camera", translation=(0.0, 0.0, 0.0), stage=stage) - - view = _create_view("/World/Parent_.*/Child", device=device) - - desired_world_positions = torch.tensor([[5.0, 5.0, 0.0], [15.0, 5.0, 0.0], [25.0, 5.0, 0.0]], device=device) - desired_world_orientations = torch.tensor([[0.0, 0.0, 0.0, 1.0]] * 3, device=device) - - view.set_world_poses(desired_world_positions, desired_world_orientations) - retrieved_positions, retrieved_orientations = view.get_world_poses() - retrieved_positions, retrieved_orientations = wp.to_torch(retrieved_positions), wp.to_torch(retrieved_orientations) - - torch.testing.assert_close(retrieved_positions, desired_world_positions, atol=1e-4, rtol=0) - try: - torch.testing.assert_close(retrieved_orientations, desired_world_orientations, atol=1e-4, rtol=0) - except AssertionError: - torch.testing.assert_close(retrieved_orientations, -desired_world_orientations, atol=1e-4, rtol=0) - - -@pytest.mark.parametrize("device", ["cpu", "cuda"]) -def test_set_local_poses(device): - _skip_if_unavailable(device) - stage = sim_utils.get_current_stage() - - sim_utils.create_prim("/World/Parent", "Xform", translation=(5.0, 5.0, 5.0), stage=stage) - num_prims = 4 - for i in range(num_prims): - sim_utils.create_prim(f"/World/Parent/Child_{i}", "Camera", translation=(0.0, 0.0, 0.0), stage=stage) - - view = _create_view("/World/Parent/Child_.*", device=device) - - new_translations = torch.tensor([[1.0, 0.0, 0.0], [0.0, 2.0, 0.0], [0.0, 0.0, 3.0], [4.0, 4.0, 4.0]], device=device) - new_orientations = torch.tensor( - [ - [0.0, 0.0, 0.0, 1.0], - [0.0, 0.0, 0.7071068, 0.7071068], - [0.7071068, 0.0, 0.0, 0.7071068], - [0.3826834, 0.0, 0.0, 0.9238795], - ], - device=device, - ) - - view.set_local_poses(new_translations, new_orientations) - retrieved_translations, retrieved_orientations = view.get_local_poses() - retrieved_translations, retrieved_orientations = ( - wp.to_torch(retrieved_translations), - wp.to_torch(retrieved_orientations), - ) - - torch.testing.assert_close(retrieved_translations, new_translations, atol=1e-5, rtol=0) - try: - torch.testing.assert_close(retrieved_orientations, new_orientations, atol=1e-5, rtol=0) - except AssertionError: - torch.testing.assert_close(retrieved_orientations, -new_orientations, atol=1e-5, rtol=0) - - -@pytest.mark.parametrize("device", ["cpu", "cuda"]) -def test_set_local_poses_only_translations(device): - _skip_if_unavailable(device) - stage = sim_utils.get_current_stage() - - sim_utils.create_prim("/World/Parent", "Xform", translation=(0.0, 0.0, 0.0), stage=stage) - initial_quat = (0.0, 0.0, 0.7071068, 0.7071068) - for i in range(3): - sim_utils.create_prim( - f"/World/Parent/Child_{i}", "Camera", translation=(0.0, 0.0, 0.0), orientation=initial_quat, stage=stage - ) - - view = _create_view("/World/Parent/Child_.*", device=device) - initial_orientations = wp.to_torch(view.get_local_poses()[1]) - - new_translations = torch.tensor([[1.0, 0.0, 0.0], [0.0, 2.0, 0.0], [0.0, 0.0, 3.0]], device=device) - view.set_local_poses(translations=new_translations, orientations=None) - - retrieved_translations, retrieved_orientations = view.get_local_poses() - retrieved_translations, retrieved_orientations = ( - wp.to_torch(retrieved_translations), - wp.to_torch(retrieved_orientations), - ) - torch.testing.assert_close(retrieved_translations, new_translations, atol=1e-5, rtol=0) - try: - torch.testing.assert_close(retrieved_orientations, initial_orientations, atol=1e-5, rtol=0) - except AssertionError: - torch.testing.assert_close(retrieved_orientations, -initial_orientations, atol=1e-5, rtol=0) - - -@pytest.mark.parametrize("device", ["cpu", "cuda"]) -def test_set_scales(device): - _skip_if_unavailable(device) - stage = sim_utils.get_current_stage() - - num_prims = 5 - for i in range(num_prims): - sim_utils.create_prim(f"/World/Object_{i}", "Camera", scale=(1.0, 1.0, 1.0), stage=stage) - - view = _create_view("/World/Object_.*", device=device) - new_scales = torch.tensor( - [[2.0, 2.0, 2.0], [1.0, 2.0, 3.0], [0.5, 0.5, 0.5], [3.0, 1.0, 2.0], [1.5, 1.5, 1.5]], device=device - ) - view.set_scales(new_scales) - - retrieved_scales = wp.to_torch(view.get_scales()) - torch.testing.assert_close(retrieved_scales, new_scales, atol=1e-5, rtol=0) - - -""" -Tests - Indices. -""" - - -@pytest.mark.parametrize("device", ["cpu", "cuda"]) -def test_indices_single_element(device): - _skip_if_unavailable(device) - stage = sim_utils.get_current_stage() - - num_prims = 5 - for i in range(num_prims): - sim_utils.create_prim(f"/World/Object_{i}", "Camera", translation=(float(i), 0.0, 0.0), stage=stage) - - view = _create_view("/World/Object_.*", device=device) - - indices = [3] - positions, orientations = view.get_world_poses(indices=indices) - positions, orientations = wp.to_torch(positions), wp.to_torch(orientations) - assert positions.shape == (1, 3) - assert orientations.shape == (1, 4) - - new_position = torch.tensor([[100.0, 200.0, 300.0]], device=device) - view.set_world_poses(positions=new_position, indices=indices) - - retrieved_positions = wp.to_torch(view.get_world_poses(indices=indices)[0]) - torch.testing.assert_close(retrieved_positions, new_position, atol=1e-5, rtol=0) - - -@pytest.mark.parametrize("device", ["cpu", "cuda"]) -def test_indices_out_of_order(device): - _skip_if_unavailable(device) - stage = sim_utils.get_current_stage() - - num_prims = 10 - for i in range(num_prims): - sim_utils.create_prim(f"/World/Object_{i}", "Camera", translation=(0.0, 0.0, 0.0), stage=stage) - - view = _create_view("/World/Object_.*", device=device) - - indices = [7, 2, 9, 0, 5] - new_positions = torch.tensor( - [[7.0, 0.0, 0.0], [2.0, 0.0, 0.0], [9.0, 0.0, 0.0], [0.0, 0.0, 0.0], [5.0, 0.0, 0.0]], device=device - ) - view.set_world_poses(positions=new_positions, indices=indices) - - all_positions = wp.to_torch(view.get_world_poses()[0]) - - expected_x_values = [0.0, 0.0, 2.0, 0.0, 0.0, 5.0, 0.0, 7.0, 0.0, 9.0] - for i in range(num_prims): - assert abs(all_positions[i, 0].item() - expected_x_values[i]) < 1e-5 - - -@pytest.mark.parametrize("device", ["cpu", "cuda"]) -def test_indices_with_only_positions_or_orientations(device): - _skip_if_unavailable(device) - stage = sim_utils.get_current_stage() - - num_prims = 5 - for i in range(num_prims): - sim_utils.create_prim( - f"/World/Object_{i}", "Camera", translation=(0.0, 0.0, 0.0), orientation=(0.0, 0.0, 0.0, 1.0), stage=stage - ) - - view = _create_view("/World/Object_.*", device=device) - initial_positions = wp.to_torch(view.get_world_poses()[0]).clone() - - indices = [1, 3] - new_positions = torch.tensor([[10.0, 0.0, 0.0], [30.0, 0.0, 0.0]], device=device) - view.set_world_poses(positions=new_positions, orientations=None, indices=indices) - - updated_positions = wp.to_torch(view.get_world_poses()[0]) - - torch.testing.assert_close(updated_positions[1], new_positions[0], atol=1e-5, rtol=0) - torch.testing.assert_close(updated_positions[3], new_positions[1], atol=1e-5, rtol=0) - torch.testing.assert_close(updated_positions[0], initial_positions[0], atol=1e-5, rtol=0) - - -""" -Tests - Fabric Operations. -""" - - -@pytest.mark.parametrize("device", ["cpu", "cuda"]) -def test_fabric_initialization(device): - _skip_if_unavailable(device) - stage = sim_utils.get_current_stage() - - num_prims = 5 - for i in range(num_prims): - sim_utils.create_prim(f"/World/Cam_{i}", "Camera", translation=(i * 1.0, 0.0, 1.0), stage=stage) - - view = _create_view("/World/Cam_.*", device=device) - - assert view.count == num_prims - assert view.device == device - assert len(view.prims) == num_prims - - -@pytest.mark.parametrize("device", ["cpu", "cuda"]) -def test_fabric_usd_consistency(device): - _skip_if_unavailable(device) - stage = sim_utils.get_current_stage() - - num_prims = 5 - for i in range(num_prims): - sim_utils.create_prim( - f"/World/Cam_{i}", - "Camera", - translation=(i * 1.0, 2.0, 3.0), - orientation=(0.0, 0.0, 0.7071068, 0.7071068), - stage=stage, - ) - - view = _create_view("/World/Cam_.*", device=device) - - init_positions = torch.zeros((num_prims, 3), dtype=torch.float32, device=device) - init_positions[:, 0] = torch.arange(num_prims, dtype=torch.float32, device=device) - init_positions[:, 1] = 2.0 - init_positions[:, 2] = 3.0 - init_orientations = torch.tensor([[0.0, 0.0, 0.7071068, 0.7071068]] * num_prims, dtype=torch.float32, device=device) - - view.set_world_poses(init_positions, init_orientations) - - pos, quat = view.get_world_poses() - torch.testing.assert_close(wp.to_torch(pos), init_positions, atol=1e-4, rtol=0) - torch.testing.assert_close(wp.to_torch(quat), init_orientations, atol=1e-4, rtol=0) - - new_positions = torch.rand((num_prims, 3), dtype=torch.float32, device=device) * 10.0 - new_orientations = torch.tensor([[0.0, 0.0, 0.0, 1.0]] * num_prims, dtype=torch.float32, device=device) - view.set_world_poses(new_positions, new_orientations) - - pos2, quat2 = view.get_world_poses() - torch.testing.assert_close(wp.to_torch(pos2), new_positions, atol=1e-4, rtol=0) - torch.testing.assert_close(wp.to_torch(quat2), new_orientations, atol=1e-4, rtol=0) + return factory From f3bbe5b87387eadfcdac44260abd51d2395acb58 Mon Sep 17 00:00:00 2001 From: Octi Zhang Date: Thu, 9 Apr 2026 16:54:01 -0700 Subject: [PATCH 04/24] address concerns --- .../benchmarks/benchmark_view_comparison.py | 4 +- .../benchmarks/benchmark_xform_prim_view.py | 8 +- source/isaaclab/docs/CHANGELOG.rst | 4 +- source/isaaclab/isaaclab/sim/__init__.pyi | 4 +- .../isaaclab/isaaclab/sim/views/__init__.pyi | 4 +- .../sim/views/base_xform_prim_view.py | 2 +- .../isaaclab/sim/views/usd_xform_prim_view.py | 363 +++++++++++++++++ .../isaaclab/sim/views/xform_prim_view.py | 373 ++---------------- .../sim/views/xform_prim_view_factory.py | 44 --- .../test/sim/test_views_xform_prim.py | 2 +- .../isaaclab_newton/sim/views/__init__.pyi | 4 +- ...view.py => newton_site_xform_prim_view.py} | 2 +- .../test/sim/test_views_xform_prim_newton.py | 2 +- .../isaaclab_physx/sim/views/__init__.pyi | 4 +- ...prim_view.py => fabric_xform_prim_view.py} | 209 +++++----- .../test/sim/test_views_xform_prim_fabric.py | 2 +- 16 files changed, 521 insertions(+), 510 deletions(-) create mode 100644 source/isaaclab/isaaclab/sim/views/usd_xform_prim_view.py delete mode 100644 source/isaaclab/isaaclab/sim/views/xform_prim_view_factory.py rename source/isaaclab_newton/isaaclab_newton/sim/views/{xform_prim_view.py => newton_site_xform_prim_view.py} (99%) rename source/isaaclab_physx/isaaclab_physx/sim/views/{xform_prim_view.py => fabric_xform_prim_view.py} (76%) diff --git a/scripts/benchmarks/benchmark_view_comparison.py b/scripts/benchmarks/benchmark_view_comparison.py index 9b2d1f09d557..2d0f7e1b65e1 100644 --- a/scripts/benchmarks/benchmark_view_comparison.py +++ b/scripts/benchmarks/benchmark_view_comparison.py @@ -73,7 +73,7 @@ try: from isaaclab_newton.physics import MJWarpSolverCfg, NewtonCfg - from isaaclab_newton.sim.views import XformPrimView as NewtonXformPrimView + from isaaclab_newton.sim.views import NewtonSiteXformPrimView HAS_NEWTON = True except ImportError: @@ -181,7 +181,7 @@ class _SceneCfg(InteractiveSceneCfg): print(f" Newton scene setup: {time.perf_counter() - start_time:.4f}s") start_time = time.perf_counter() - view = NewtonXformPrimView("/World/envs/env_.*/Cube/Sensor", device=args_cli.device) + view = NewtonSiteXformPrimView("/World/envs/env_.*/Cube/Sensor", device=args_cli.device) num_prims = view.count timing_results["init"] = time.perf_counter() - start_time diff --git a/scripts/benchmarks/benchmark_xform_prim_view.py b/scripts/benchmarks/benchmark_xform_prim_view.py index cd592b2b1168..289379f6f4df 100644 --- a/scripts/benchmarks/benchmark_xform_prim_view.py +++ b/scripts/benchmarks/benchmark_xform_prim_view.py @@ -47,14 +47,14 @@ from pxr import Gf from isaaclab_newton.physics import MJWarpSolverCfg, NewtonCfg -from isaaclab_newton.sim.views import XformPrimView as NewtonXformPrimView -from isaaclab_physx.sim.views import XformPrimView as FabricXformPrimView +from isaaclab_newton.sim.views import NewtonSiteXformPrimView +from isaaclab_physx.sim.views import FabricXformPrimView import isaaclab.sim as sim_utils from isaaclab.assets import RigidObjectCfg from isaaclab.scene import InteractiveScene, InteractiveSceneCfg from isaaclab.sim import SimulationCfg, build_simulation_context -from isaaclab.sim.views import XformPrimView as UsdXformPrimView +from isaaclab.sim.views import UsdXformPrimView from isaaclab.utils import configclass @@ -110,7 +110,7 @@ def benchmark_xform_prim_view( # noqa: C901 sim.reset() start_time = time.perf_counter() - xform_view = NewtonXformPrimView("/World/envs/env_.*/Object/Sensor", device=device) + xform_view = NewtonSiteXformPrimView("/World/envs/env_.*/Object/Sensor", device=device) timing_results["init"] = time.perf_counter() - start_time cleanup = lambda: ctx.__exit__(None, None, None) # noqa: E731 diff --git a/source/isaaclab/docs/CHANGELOG.rst b/source/isaaclab/docs/CHANGELOG.rst index 2be842d4964e..af85d3d766b5 100644 --- a/source/isaaclab/docs/CHANGELOG.rst +++ b/source/isaaclab/docs/CHANGELOG.rst @@ -174,7 +174,7 @@ Added * Added :class:`~isaaclab.sim.views.BaseXformPrimView` abstract base class that defines the common interface for backend-specific ``XformPrimView`` implementations. -* Added :class:`~isaaclab.sim.views.XformPrimViewFactory` to instantiate the correct +* Added :class:`~isaaclab.sim.views.XformPrimView` factory to instantiate the correct backend-specific ``XformPrimView`` based on the active simulation backend. Changed @@ -183,7 +183,7 @@ Changed * Refactored :class:`~isaaclab.sim.views.XformPrimView` to delegate backend-specific logic to :class:`~isaaclab_physx.sim.views.XformPrimView` and :class:`~isaaclab_newton.sim.views.XformPrimView`. The public API is unchanged; - use :class:`~isaaclab.sim.views.XformPrimViewFactory` for backend-aware instantiation. + use :class:`~isaaclab.sim.views.XformPrimView` for backend-aware instantiation. * Added release version to :class:`~isaaclab.test.benchmark.recorders.VersionInfoRecorder` output. diff --git a/source/isaaclab/isaaclab/sim/__init__.pyi b/source/isaaclab/isaaclab/sim/__init__.pyi index 30028da60041..ea20e7fcc6cf 100644 --- a/source/isaaclab/isaaclab/sim/__init__.pyi +++ b/source/isaaclab/isaaclab/sim/__init__.pyi @@ -165,8 +165,8 @@ __all__ = [ "resolve_prim_scale", "convert_world_pose_to_local", "BaseXformPrimView", + "UsdXformPrimView", "XformPrimView", - "XformPrimViewFactory", ] from .simulation_cfg import RenderCfg, SimulationCfg @@ -335,4 +335,4 @@ from .utils import ( resolve_prim_scale, convert_world_pose_to_local, ) -from .views import BaseXformPrimView, XformPrimView, XformPrimViewFactory +from .views import BaseXformPrimView, UsdXformPrimView, XformPrimView diff --git a/source/isaaclab/isaaclab/sim/views/__init__.pyi b/source/isaaclab/isaaclab/sim/views/__init__.pyi index aeec320b6350..444f9986b9a5 100644 --- a/source/isaaclab/isaaclab/sim/views/__init__.pyi +++ b/source/isaaclab/isaaclab/sim/views/__init__.pyi @@ -5,10 +5,10 @@ __all__ = [ "BaseXformPrimView", + "UsdXformPrimView", "XformPrimView", - "XformPrimViewFactory", ] from .base_xform_prim_view import BaseXformPrimView +from .usd_xform_prim_view import UsdXformPrimView from .xform_prim_view import XformPrimView -from .xform_prim_view_factory import XformPrimViewFactory diff --git a/source/isaaclab/isaaclab/sim/views/base_xform_prim_view.py b/source/isaaclab/isaaclab/sim/views/base_xform_prim_view.py index 78fbdbad7369..180ee90472db 100644 --- a/source/isaaclab/isaaclab/sim/views/base_xform_prim_view.py +++ b/source/isaaclab/isaaclab/sim/views/base_xform_prim_view.py @@ -18,7 +18,7 @@ class BaseXformPrimView(abc.ABC): Backend-specific implementations (USD/Fabric, Newton GPU state, etc.) subclass this to provide efficient batched pose queries. The factory - :class:`~isaaclab.sim.views.XformPrimViewFactory` selects the correct + :class:`~isaaclab.sim.views.XformPrimView` selects the correct implementation at runtime based on the active physics backend. All getters return ``wp.array``. Setters accept ``wp.array``. diff --git a/source/isaaclab/isaaclab/sim/views/usd_xform_prim_view.py b/source/isaaclab/isaaclab/sim/views/usd_xform_prim_view.py new file mode 100644 index 000000000000..8ba81e1e4cc6 --- /dev/null +++ b/source/isaaclab/isaaclab/sim/views/usd_xform_prim_view.py @@ -0,0 +1,363 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from __future__ import annotations + +import logging +from collections.abc import Sequence + +import numpy as np +import torch +import warp as wp + +from pxr import Gf, Sdf, Usd, UsdGeom, Vt + +import isaaclab.sim as sim_utils + +from .base_xform_prim_view import BaseXformPrimView + +logger = logging.getLogger(__name__) + + +class UsdXformPrimView(BaseXformPrimView): + """Batched interface for reading and writing transforms of multiple USD prims. + + Provides batch operations for getting and setting poses (position and orientation) + of multiple prims at once via USD's ``XformCache``. + + The class supports both world-space and local-space pose operations: + + - **World poses**: Positions and orientations in the global world frame + - **Local poses**: Positions and orientations relative to each prim's parent + + For GPU-accelerated Fabric operations, use the PhysX backend variant + obtained via :class:`~isaaclab.sim.views.XformPrimView`. + + All getters return ``wp.array``. Setters accept ``wp.array``. + + .. note:: + **Transform Requirements:** + + All prims in the view must be Xformable and have standardized transform operations: + ``[translate, orient, scale]``. Non-standard prims will raise a ValueError during + initialization if :attr:`validate_xform_ops` is True. Please use the function + :func:`isaaclab.sim.utils.standardize_xform_ops` to prepare prims before using this view. + + .. warning:: + This class operates at the USD default time code. Any animation or time-sampled data + will not be affected by write operations. For animated transforms, you need to handle + time-sampled keyframes separately. + """ + + def __init__( + self, + prim_path: str, + device: str = "cpu", + validate_xform_ops: bool = True, + stage: Usd.Stage | None = None, + **kwargs, + ): + """Initialize the view with matching prims. + + Args: + prim_path: USD prim path pattern to match prims. Supports wildcards (``*``) and + regex patterns (e.g., ``"/World/Env_.*/Robot"``). See + :func:`isaaclab.sim.utils.find_matching_prims` for pattern syntax. + device: Device to place arrays on. Can be ``"cpu"`` or CUDA devices like + ``"cuda:0"``. Defaults to ``"cpu"``. + validate_xform_ops: Whether to validate that the prims have standard xform operations. + Defaults to True. + stage: USD stage to search for prims. Defaults to None, in which case the current + active stage from the simulation context is used. + **kwargs: Additional keyword arguments (ignored). Allows forward-compatible + construction when callers pass backend-specific options like + ``sync_usd_on_fabric_write``. + + Raises: + ValueError: If any matched prim is not Xformable or doesn't have standardized + transform operations (translate, orient, scale in that order). + """ + self._prim_path = prim_path + self._device = device + + stage = sim_utils.get_current_stage() if stage is None else stage + self._prims: list[Usd.Prim] = sim_utils.find_matching_prims(prim_path, stage=stage) + + if validate_xform_ops: + for prim in self._prims: + sim_utils.standardize_xform_ops(prim) + if not sim_utils.validate_standard_xform_ops(prim): + raise ValueError( + f"Prim at path '{prim.GetPath().pathString}' is not a xformable prim with standard transform" + f" operations [translate, orient, scale]. Received type: '{prim.GetTypeName()}'." + " Use sim_utils.standardize_xform_ops() to prepare the prim." + ) + + self._ALL_INDICES = list(range(len(self._prims))) + + # ------------------------------------------------------------------ + # Properties + # ------------------------------------------------------------------ + + @property + def count(self) -> int: + """Number of prims in this view.""" + return len(self._prims) + + @property + def device(self) -> str: + """Device where arrays are allocated (cpu or cuda).""" + return self._device + + @property + def prims(self) -> list[Usd.Prim]: + """List of USD prims being managed by this view.""" + return self._prims + + @property + def prim_paths(self) -> list[str]: + """List of prim paths (as strings) for all prims being managed by this view. + + The conversion is performed lazily on first access and cached. + """ + if not hasattr(self, "_prim_paths"): + self._prim_paths = [prim.GetPath().pathString for prim in self._prims] + return self._prim_paths + + # ------------------------------------------------------------------ + # Setters + # ------------------------------------------------------------------ + + def set_world_poses( + self, + positions: wp.array | None = None, + orientations: wp.array | None = None, + indices: Sequence[int] | None = None, + ): + """Set world-space poses for prims in the view. + + Converts the desired world pose to local-space relative to each prim's + parent before writing to USD xform ops. + + Args: + positions: World-space positions of shape ``(M, 3)``. + orientations: World-space quaternions ``(w, x, y, z)`` of shape ``(M, 4)``. + indices: Indices of prims to set poses for. Defaults to None (all prims). + """ + indices_list = self._resolve_indices(indices) + + positions_array = Vt.Vec3dArray.FromNumpy(self._to_numpy(positions)) if positions is not None else None + orientations_array = Vt.QuatdArray.FromNumpy(self._to_numpy(orientations)) if orientations is not None else None + + xform_cache = UsdGeom.XformCache(Usd.TimeCode.Default()) + + with Sdf.ChangeBlock(): + for idx, prim_idx in enumerate(indices_list): + prim = self._prims[prim_idx] + parent_prim = prim.GetParent() + + world_pos = positions_array[idx] if positions_array is not None else None + world_quat = orientations_array[idx] if orientations_array is not None else None + + # Convert world pose to local pose relative to parent + if parent_prim.IsValid() and parent_prim.GetPath() != Sdf.Path.absoluteRootPath: + if positions_array is None or orientations_array is None: + prim_tf = xform_cache.GetLocalToWorldTransform(prim) + prim_tf.Orthonormalize() + if world_pos is not None: + prim_tf.SetTranslateOnly(world_pos) + if world_quat is not None: + prim_tf.SetRotateOnly(world_quat) + else: + prim_tf = Gf.Matrix4d() + prim_tf.SetTranslateOnly(world_pos) + prim_tf.SetRotateOnly(world_quat) + + parent_world_tf = xform_cache.GetLocalToWorldTransform(parent_prim) + local_tf = prim_tf * parent_world_tf.GetInverse() + local_pos = local_tf.ExtractTranslation() + local_quat = local_tf.ExtractRotationQuat() + else: + # Root-level prim: world == local + local_pos = world_pos + local_quat = world_quat + + if local_pos is not None: + prim.GetAttribute("xformOp:translate").Set(local_pos) + if local_quat is not None: + prim.GetAttribute("xformOp:orient").Set(local_quat) + + def set_local_poses( + self, + translations: wp.array | None = None, + orientations: wp.array | None = None, + indices: Sequence[int] | None = None, + ): + """Set local-space poses for prims in the view. + + Args: + translations: Local-space translations of shape ``(M, 3)``. + orientations: Local-space quaternions ``(w, x, y, z)`` of shape ``(M, 4)``. + indices: Indices of prims to set poses for. Defaults to None (all prims). + """ + indices_list = self._resolve_indices(indices) + + translations_array = Vt.Vec3dArray.FromNumpy(self._to_numpy(translations)) if translations is not None else None + orientations_array = Vt.QuatdArray.FromNumpy(self._to_numpy(orientations)) if orientations is not None else None + + with Sdf.ChangeBlock(): + for idx, prim_idx in enumerate(indices_list): + prim = self._prims[prim_idx] + if translations_array is not None: + prim.GetAttribute("xformOp:translate").Set(translations_array[idx]) + if orientations_array is not None: + prim.GetAttribute("xformOp:orient").Set(orientations_array[idx]) + + def set_scales(self, scales: wp.array, indices: Sequence[int] | None = None): + """Set scales for prims in the view. + + Args: + scales: Scales of shape ``(M, 3)``. + indices: Indices of prims to set scales for. Defaults to None (all prims). + """ + indices_list = self._resolve_indices(indices) + scales_array = Vt.Vec3dArray.FromNumpy(self._to_numpy(scales)) + + with Sdf.ChangeBlock(): + for idx, prim_idx in enumerate(indices_list): + prim = self._prims[prim_idx] + prim.GetAttribute("xformOp:scale").Set(scales_array[idx]) + + def set_visibility(self, visibility: torch.Tensor, indices: Sequence[int] | None = None): + """Set visibility for prims in the view. + + Args: + visibility: Visibility as a boolean tensor of shape ``(M,)``. + indices: Indices of prims to set visibility for. Defaults to None (all prims). + """ + indices_list = self._resolve_indices(indices) + + if visibility.shape != (len(indices_list),): + raise ValueError(f"Expected visibility shape ({len(indices_list)},), got {visibility.shape}.") + + with Sdf.ChangeBlock(): + for idx, prim_idx in enumerate(indices_list): + imageable = UsdGeom.Imageable(self._prims[prim_idx]) + if visibility[idx]: + imageable.MakeVisible() + else: + imageable.MakeInvisible() + + # ------------------------------------------------------------------ + # Getters + # ------------------------------------------------------------------ + + def get_world_poses(self, indices: Sequence[int] | None = None) -> tuple[wp.array, wp.array]: + """Get world-space poses for prims in the view. + + Args: + indices: Indices of prims to get poses for. Defaults to None (all prims). + + Returns: + A tuple of ``(positions, orientations)`` as ``wp.array``. + """ + indices_list = self._resolve_indices(indices) + + positions = Vt.Vec3dArray(len(indices_list)) + orientations = Vt.QuatdArray(len(indices_list)) + xform_cache = UsdGeom.XformCache(Usd.TimeCode.Default()) + + for idx, prim_idx in enumerate(indices_list): + prim = self._prims[prim_idx] + prim_tf = xform_cache.GetLocalToWorldTransform(prim) + prim_tf.Orthonormalize() + positions[idx] = prim_tf.ExtractTranslation() + orientations[idx] = prim_tf.ExtractRotationQuat() + + return ( + wp.array(np.array(positions, dtype=np.float32), dtype=wp.float32, device=self._device), + wp.array(np.array(orientations, dtype=np.float32), dtype=wp.float32, device=self._device), + ) + + def get_local_poses(self, indices: Sequence[int] | None = None) -> tuple[wp.array, wp.array]: + """Get local-space poses for prims in the view. + + Args: + indices: Indices of prims to get poses for. Defaults to None (all prims). + + Returns: + A tuple of ``(translations, orientations)`` as ``wp.array``. + """ + indices_list = self._resolve_indices(indices) + + translations = Vt.Vec3dArray(len(indices_list)) + orientations = Vt.QuatdArray(len(indices_list)) + xform_cache = UsdGeom.XformCache(Usd.TimeCode.Default()) + + for idx, prim_idx in enumerate(indices_list): + prim = self._prims[prim_idx] + prim_tf = xform_cache.GetLocalTransformation(prim)[0] + prim_tf.Orthonormalize() + translations[idx] = prim_tf.ExtractTranslation() + orientations[idx] = prim_tf.ExtractRotationQuat() + + return ( + wp.array(np.array(translations, dtype=np.float32), dtype=wp.float32, device=self._device), + wp.array(np.array(orientations, dtype=np.float32), dtype=wp.float32, device=self._device), + ) + + def get_scales(self, indices: Sequence[int] | None = None) -> wp.array: + """Get scales for prims in the view. + + Args: + indices: Indices of prims to get scales for. Defaults to None (all prims). + + Returns: + A ``wp.array`` of shape ``(M, 3)``. + """ + indices_list = self._resolve_indices(indices) + + scales = Vt.Vec3dArray(len(indices_list)) + for idx, prim_idx in enumerate(indices_list): + prim = self._prims[prim_idx] + scales[idx] = prim.GetAttribute("xformOp:scale").Get() + + return wp.array(np.array(scales, dtype=np.float32), dtype=wp.float32, device=self._device) + + def get_visibility(self, indices: Sequence[int] | None = None) -> torch.Tensor: + """Get visibility for prims in the view. + + Args: + indices: Indices of prims to get visibility for. Defaults to None (all prims). + + Returns: + A tensor of shape ``(M,)`` containing the visibility of each prim (bool). + """ + indices_list = self._resolve_indices(indices) + + visibility = torch.zeros(len(indices_list), dtype=torch.bool, device=self._device) + for idx, prim_idx in enumerate(indices_list): + imageable = UsdGeom.Imageable(self._prims[prim_idx]) + visibility[idx] = imageable.ComputeVisibility() != UsdGeom.Tokens.invisible + return visibility + + # ------------------------------------------------------------------ + # Helpers + # ------------------------------------------------------------------ + + def _resolve_indices(self, indices: Sequence[int] | None) -> list[int]: + """Resolve indices to a list, defaulting to all prims.""" + if indices is None or indices == slice(None): + return self._ALL_INDICES + if isinstance(indices, torch.Tensor): + return indices.tolist() + return list(indices) + + @staticmethod + def _to_numpy(data: wp.array | torch.Tensor) -> np.ndarray: + """Convert a ``wp.array`` or ``torch.Tensor`` to a numpy array on CPU.""" + if isinstance(data, wp.array): + return data.numpy() + return data.cpu().numpy() diff --git a/source/isaaclab/isaaclab/sim/views/xform_prim_view.py b/source/isaaclab/isaaclab/sim/views/xform_prim_view.py index f2eaf30f2fce..05e7a280171e 100644 --- a/source/isaaclab/isaaclab/sim/views/xform_prim_view.py +++ b/source/isaaclab/isaaclab/sim/views/xform_prim_view.py @@ -3,361 +3,46 @@ # # SPDX-License-Identifier: BSD-3-Clause -from __future__ import annotations - -import logging -from collections.abc import Sequence +"""Backend-dispatching XformPrimView. -import numpy as np -import torch -import warp as wp +``XformPrimView(path, device=...)`` automatically selects the right backend: +- PhysX: :class:`~isaaclab_physx.sim.views.FabricXformPrimView` +- Newton: :class:`~isaaclab_newton.sim.views.NewtonSiteXformPrimView` +""" -from pxr import Gf, Sdf, Usd, UsdGeom, Vt +from __future__ import annotations -import isaaclab.sim as sim_utils +from isaaclab.utils.backend_utils import FactoryBase from .base_xform_prim_view import BaseXformPrimView -logger = logging.getLogger(__name__) - - -class XformPrimView(BaseXformPrimView): - """Batched interface for reading and writing transforms of multiple USD prims. - - Provides batch operations for getting and setting poses (position and orientation) - of multiple prims at once via USD's ``XformCache``. - - The class supports both world-space and local-space pose operations: - - - **World poses**: Positions and orientations in the global world frame - - **Local poses**: Positions and orientations relative to each prim's parent - - For GPU-accelerated Fabric operations, use the PhysX backend variant - obtained via :class:`~isaaclab.sim.views.XformPrimViewFactory`. - All getters return ``wp.array``. Setters accept ``wp.array``. +class XformPrimView(FactoryBase, BaseXformPrimView): + """XformPrimView that dispatches to the active physics backend. - .. note:: - **Transform Requirements:** + Callers use ``XformPrimView(prim_path, device=device)`` and get the + correct implementation automatically: - All prims in the view must be Xformable and have standardized transform operations: - ``[translate, orient, scale]``. Non-standard prims will raise a ValueError during - initialization if :attr:`validate_xform_ops` is True. Please use the function - :func:`isaaclab.sim.utils.standardize_xform_ops` to prepare prims before using this view. - - .. warning:: - This class operates at the USD default time code. Any animation or time-sampled data - will not be affected by write operations. For animated transforms, you need to handle - time-sampled keyframes separately. + - **PhysX / no backend**: :class:`~isaaclab_physx.sim.views.FabricXformPrimView` + (Fabric GPU acceleration with USD fallback). + - **Newton**: :class:`~isaaclab_newton.sim.views.NewtonSiteXformPrimView` + (GPU-resident site-based transforms). """ - def __init__( - self, - prim_path: str, - device: str = "cpu", - validate_xform_ops: bool = True, - stage: Usd.Stage | None = None, - **kwargs, - ): - """Initialize the view with matching prims. - - Args: - prim_path: USD prim path pattern to match prims. Supports wildcards (``*``) and - regex patterns (e.g., ``"/World/Env_.*/Robot"``). See - :func:`isaaclab.sim.utils.find_matching_prims` for pattern syntax. - device: Device to place arrays on. Can be ``"cpu"`` or CUDA devices like - ``"cuda:0"``. Defaults to ``"cpu"``. - validate_xform_ops: Whether to validate that the prims have standard xform operations. - Defaults to True. - stage: USD stage to search for prims. Defaults to None, in which case the current - active stage from the simulation context is used. - **kwargs: Additional keyword arguments (ignored). Allows forward-compatible - construction when callers pass backend-specific options like - ``sync_usd_on_fabric_write``. - - Raises: - ValueError: If any matched prim is not Xformable or doesn't have standardized - transform operations (translate, orient, scale in that order). - """ - self._prim_path = prim_path - self._device = device - - stage = sim_utils.get_current_stage() if stage is None else stage - self._prims: list[Usd.Prim] = sim_utils.find_matching_prims(prim_path, stage=stage) - - if validate_xform_ops: - for prim in self._prims: - sim_utils.standardize_xform_ops(prim) - if not sim_utils.validate_standard_xform_ops(prim): - raise ValueError( - f"Prim at path '{prim.GetPath().pathString}' is not a xformable prim with standard transform" - f" operations [translate, orient, scale]. Received type: '{prim.GetTypeName()}'." - " Use sim_utils.standardize_xform_ops() to prepare the prim." - ) - - self._ALL_INDICES = list(range(len(self._prims))) - - # ------------------------------------------------------------------ - # Properties - # ------------------------------------------------------------------ - - @property - def count(self) -> int: - """Number of prims in this view.""" - return len(self._prims) - - @property - def device(self) -> str: - """Device where arrays are allocated (cpu or cuda).""" - return self._device - - @property - def prims(self) -> list[Usd.Prim]: - """List of USD prims being managed by this view.""" - return self._prims - - @property - def prim_paths(self) -> list[str]: - """List of prim paths (as strings) for all prims being managed by this view. - - The conversion is performed lazily on first access and cached. - """ - if not hasattr(self, "_prim_paths"): - self._prim_paths = [prim.GetPath().pathString for prim in self._prims] - return self._prim_paths - - # ------------------------------------------------------------------ - # Setters - # ------------------------------------------------------------------ - - def set_world_poses( - self, - positions: wp.array | None = None, - orientations: wp.array | None = None, - indices: Sequence[int] | None = None, - ): - """Set world-space poses for prims in the view. - - Converts the desired world pose to local-space relative to each prim's - parent before writing to USD xform ops. - - Args: - positions: World-space positions of shape ``(M, 3)``. - orientations: World-space quaternions ``(w, x, y, z)`` of shape ``(M, 4)``. - indices: Indices of prims to set poses for. Defaults to None (all prims). - """ - indices_list = self._resolve_indices(indices) - - positions_array = Vt.Vec3dArray.FromNumpy(self._to_numpy(positions)) if positions is not None else None - orientations_array = Vt.QuatdArray.FromNumpy(self._to_numpy(orientations)) if orientations is not None else None - - xform_cache = UsdGeom.XformCache(Usd.TimeCode.Default()) - - with Sdf.ChangeBlock(): - for idx, prim_idx in enumerate(indices_list): - prim = self._prims[prim_idx] - parent_prim = prim.GetParent() - - world_pos = positions_array[idx] if positions_array is not None else None - world_quat = orientations_array[idx] if orientations_array is not None else None - - # Convert world pose to local pose relative to parent - if parent_prim.IsValid() and parent_prim.GetPath() != Sdf.Path.absoluteRootPath: - if positions_array is None or orientations_array is None: - prim_tf = xform_cache.GetLocalToWorldTransform(prim) - prim_tf.Orthonormalize() - if world_pos is not None: - prim_tf.SetTranslateOnly(world_pos) - if world_quat is not None: - prim_tf.SetRotateOnly(world_quat) - else: - prim_tf = Gf.Matrix4d() - prim_tf.SetTranslateOnly(world_pos) - prim_tf.SetRotateOnly(world_quat) - - parent_world_tf = xform_cache.GetLocalToWorldTransform(parent_prim) - local_tf = prim_tf * parent_world_tf.GetInverse() - local_pos = local_tf.ExtractTranslation() - local_quat = local_tf.ExtractRotationQuat() - else: - # Root-level prim: world == local - local_pos = world_pos - local_quat = world_quat - - if local_pos is not None: - prim.GetAttribute("xformOp:translate").Set(local_pos) - if local_quat is not None: - prim.GetAttribute("xformOp:orient").Set(local_quat) - - def set_local_poses( - self, - translations: wp.array | None = None, - orientations: wp.array | None = None, - indices: Sequence[int] | None = None, - ): - """Set local-space poses for prims in the view. - - Args: - translations: Local-space translations of shape ``(M, 3)``. - orientations: Local-space quaternions ``(w, x, y, z)`` of shape ``(M, 4)``. - indices: Indices of prims to set poses for. Defaults to None (all prims). - """ - indices_list = self._resolve_indices(indices) - - translations_array = Vt.Vec3dArray.FromNumpy(self._to_numpy(translations)) if translations is not None else None - orientations_array = Vt.QuatdArray.FromNumpy(self._to_numpy(orientations)) if orientations is not None else None - - with Sdf.ChangeBlock(): - for idx, prim_idx in enumerate(indices_list): - prim = self._prims[prim_idx] - if translations_array is not None: - prim.GetAttribute("xformOp:translate").Set(translations_array[idx]) - if orientations_array is not None: - prim.GetAttribute("xformOp:orient").Set(orientations_array[idx]) - - def set_scales(self, scales: wp.array, indices: Sequence[int] | None = None): - """Set scales for prims in the view. - - Args: - scales: Scales of shape ``(M, 3)``. - indices: Indices of prims to set scales for. Defaults to None (all prims). - """ - indices_list = self._resolve_indices(indices) - scales_array = Vt.Vec3dArray.FromNumpy(self._to_numpy(scales)) - - with Sdf.ChangeBlock(): - for idx, prim_idx in enumerate(indices_list): - prim = self._prims[prim_idx] - prim.GetAttribute("xformOp:scale").Set(scales_array[idx]) - - def set_visibility(self, visibility: torch.Tensor, indices: Sequence[int] | None = None): - """Set visibility for prims in the view. - - Args: - visibility: Visibility as a boolean tensor of shape ``(M,)``. - indices: Indices of prims to set visibility for. Defaults to None (all prims). - """ - indices_list = self._resolve_indices(indices) - - if visibility.shape != (len(indices_list),): - raise ValueError(f"Expected visibility shape ({len(indices_list)},), got {visibility.shape}.") - - with Sdf.ChangeBlock(): - for idx, prim_idx in enumerate(indices_list): - imageable = UsdGeom.Imageable(self._prims[prim_idx]) - if visibility[idx]: - imageable.MakeVisible() - else: - imageable.MakeInvisible() - - # ------------------------------------------------------------------ - # Getters - # ------------------------------------------------------------------ - - def get_world_poses(self, indices: Sequence[int] | None = None) -> tuple[wp.array, wp.array]: - """Get world-space poses for prims in the view. - - Args: - indices: Indices of prims to get poses for. Defaults to None (all prims). - - Returns: - A tuple of ``(positions, orientations)`` as ``wp.array``. - """ - indices_list = self._resolve_indices(indices) - - positions = Vt.Vec3dArray(len(indices_list)) - orientations = Vt.QuatdArray(len(indices_list)) - xform_cache = UsdGeom.XformCache(Usd.TimeCode.Default()) - - for idx, prim_idx in enumerate(indices_list): - prim = self._prims[prim_idx] - prim_tf = xform_cache.GetLocalToWorldTransform(prim) - prim_tf.Orthonormalize() - positions[idx] = prim_tf.ExtractTranslation() - orientations[idx] = prim_tf.ExtractRotationQuat() - - return ( - wp.array(np.array(positions, dtype=np.float32), dtype=wp.float32, device=self._device), - wp.array(np.array(orientations, dtype=np.float32), dtype=wp.float32, device=self._device), - ) - - def get_local_poses(self, indices: Sequence[int] | None = None) -> tuple[wp.array, wp.array]: - """Get local-space poses for prims in the view. - - Args: - indices: Indices of prims to get poses for. Defaults to None (all prims). - - Returns: - A tuple of ``(translations, orientations)`` as ``wp.array``. - """ - indices_list = self._resolve_indices(indices) - - translations = Vt.Vec3dArray(len(indices_list)) - orientations = Vt.QuatdArray(len(indices_list)) - xform_cache = UsdGeom.XformCache(Usd.TimeCode.Default()) - - for idx, prim_idx in enumerate(indices_list): - prim = self._prims[prim_idx] - prim_tf = xform_cache.GetLocalTransformation(prim)[0] - prim_tf.Orthonormalize() - translations[idx] = prim_tf.ExtractTranslation() - orientations[idx] = prim_tf.ExtractRotationQuat() - - return ( - wp.array(np.array(translations, dtype=np.float32), dtype=wp.float32, device=self._device), - wp.array(np.array(orientations, dtype=np.float32), dtype=wp.float32, device=self._device), - ) - - def get_scales(self, indices: Sequence[int] | None = None) -> wp.array: - """Get scales for prims in the view. - - Args: - indices: Indices of prims to get scales for. Defaults to None (all prims). - - Returns: - A ``wp.array`` of shape ``(M, 3)``. - """ - indices_list = self._resolve_indices(indices) - - scales = Vt.Vec3dArray(len(indices_list)) - for idx, prim_idx in enumerate(indices_list): - prim = self._prims[prim_idx] - scales[idx] = prim.GetAttribute("xformOp:scale").Get() - - return wp.array(np.array(scales, dtype=np.float32), dtype=wp.float32, device=self._device) - - def get_visibility(self, indices: Sequence[int] | None = None) -> torch.Tensor: - """Get visibility for prims in the view. - - Args: - indices: Indices of prims to get visibility for. Defaults to None (all prims). - - Returns: - A tensor of shape ``(M,)`` containing the visibility of each prim (bool). - """ - indices_list = self._resolve_indices(indices) - - visibility = torch.zeros(len(indices_list), dtype=torch.bool, device=self._device) - for idx, prim_idx in enumerate(indices_list): - imageable = UsdGeom.Imageable(self._prims[prim_idx]) - visibility[idx] = imageable.ComputeVisibility() != UsdGeom.Tokens.invisible - return visibility + _backend_class_names = {"physx": "FabricXformPrimView", "newton": "NewtonSiteXformPrimView"} - # ------------------------------------------------------------------ - # Helpers - # ------------------------------------------------------------------ + @classmethod + def _get_backend(cls, *args, **kwargs) -> str: + from isaaclab.sim.simulation_context import SimulationContext # noqa: PLC0415 - def _resolve_indices(self, indices: Sequence[int] | None) -> list[int]: - """Resolve indices to a list, defaulting to all prims.""" - if indices is None or indices == slice(None): - return self._ALL_INDICES - if isinstance(indices, torch.Tensor): - return indices.tolist() - return list(indices) + ctx = SimulationContext.instance() + if ctx is None: + return "physx" + manager_name = ctx.physics_manager.__name__.lower() + if "newton" in manager_name: + return "newton" + return "physx" - @staticmethod - def _to_numpy(data: wp.array | torch.Tensor) -> np.ndarray: - """Convert a ``wp.array`` or ``torch.Tensor`` to a numpy array on CPU.""" - if isinstance(data, wp.array): - return data.numpy() - return data.cpu().numpy() + def __new__(cls, *args, **kwargs) -> BaseXformPrimView: + """Create a new XformPrimView for the active physics backend.""" + return super().__new__(cls, *args, **kwargs) diff --git a/source/isaaclab/isaaclab/sim/views/xform_prim_view_factory.py b/source/isaaclab/isaaclab/sim/views/xform_prim_view_factory.py deleted file mode 100644 index 57b673d57952..000000000000 --- a/source/isaaclab/isaaclab/sim/views/xform_prim_view_factory.py +++ /dev/null @@ -1,44 +0,0 @@ -# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). -# All rights reserved. -# -# SPDX-License-Identifier: BSD-3-Clause - -"""Factory for creating backend-specific XformPrimView instances.""" - -from __future__ import annotations - -from isaaclab.utils.backend_utils import FactoryBase - -from .base_xform_prim_view import BaseXformPrimView - - -class XformPrimViewFactory(FactoryBase, BaseXformPrimView): - """Factory that selects a :class:`BaseXformPrimView` implementation based on the active physics backend. - - - **PhysX / no backend**: returns the USD/Fabric-based - :class:`~isaaclab.sim.views.XformPrimView`. - - **Newton**: returns the GPU-state-backed - :class:`~isaaclab_newton.sim.views.XformPrimView`. - - Use this in place of constructing an ``XformPrimView`` directly when the - caller is physics-aware (e.g. ray-cast sensors) and wants the fastest - available implementation for the active backend. - """ - - _backend_class_names = {"physx": "XformPrimView", "newton": "XformPrimView"} - - @classmethod - def _get_backend(cls, *args, **kwargs) -> str: - from isaaclab.sim.simulation_context import SimulationContext # noqa: PLC0415 - - ctx = SimulationContext.instance() - if ctx is None: - return "physx" - manager_name = ctx.physics_manager.__name__.lower() - if "newton" in manager_name: - return "newton" - return "physx" - - def __new__(cls, *args, **kwargs) -> BaseXformPrimView: - """Create a new XformPrimView for the active physics backend.""" - return super().__new__(cls, *args, **kwargs) diff --git a/source/isaaclab/test/sim/test_views_xform_prim.py b/source/isaaclab/test/sim/test_views_xform_prim.py index 00a3f832acd2..30f90af14634 100644 --- a/source/isaaclab/test/sim/test_views_xform_prim.py +++ b/source/isaaclab/test/sim/test_views_xform_prim.py @@ -26,7 +26,7 @@ _IsaacSimXformPrimView = None import isaaclab.sim as sim_utils # noqa: E402 -from isaaclab.sim.views import XformPrimView # noqa: E402 +from isaaclab.sim.views import UsdXformPrimView as XformPrimView # noqa: E402 from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR # noqa: E402 from xform_contract_tests import * # noqa: F401, F403, E402 diff --git a/source/isaaclab_newton/isaaclab_newton/sim/views/__init__.pyi b/source/isaaclab_newton/isaaclab_newton/sim/views/__init__.pyi index a666958e4387..f8a1a09c23f7 100644 --- a/source/isaaclab_newton/isaaclab_newton/sim/views/__init__.pyi +++ b/source/isaaclab_newton/isaaclab_newton/sim/views/__init__.pyi @@ -4,7 +4,7 @@ # SPDX-License-Identifier: BSD-3-Clause __all__ = [ - "XformPrimView", + "NewtonSiteXformPrimView", ] -from .xform_prim_view import XformPrimView +from .newton_site_xform_prim_view import NewtonSiteXformPrimView diff --git a/source/isaaclab_newton/isaaclab_newton/sim/views/xform_prim_view.py b/source/isaaclab_newton/isaaclab_newton/sim/views/newton_site_xform_prim_view.py similarity index 99% rename from source/isaaclab_newton/isaaclab_newton/sim/views/xform_prim_view.py rename to source/isaaclab_newton/isaaclab_newton/sim/views/newton_site_xform_prim_view.py index 6e3439b99a76..a1a41794c7f0 100644 --- a/source/isaaclab_newton/isaaclab_newton/sim/views/xform_prim_view.py +++ b/source/isaaclab_newton/isaaclab_newton/sim/views/newton_site_xform_prim_view.py @@ -376,7 +376,7 @@ def _ensure_wp_vec4f(data: wp.array, device: str) -> wp.array: # ------------------------------------------------------------------ -class XformPrimView(BaseXformPrimView): +class NewtonSiteXformPrimView(BaseXformPrimView): """Batched prim view for non-physics prims tracked as sites on Newton bodies. Each matched USD prim must be a **non-physics** prim (camera, sensor, diff --git a/source/isaaclab_newton/test/sim/test_views_xform_prim_newton.py b/source/isaaclab_newton/test/sim/test_views_xform_prim_newton.py index 1b97414fb865..26b5aac84ebd 100644 --- a/source/isaaclab_newton/test/sim/test_views_xform_prim_newton.py +++ b/source/isaaclab_newton/test/sim/test_views_xform_prim_newton.py @@ -21,7 +21,7 @@ import warp as wp from isaaclab_newton.physics import MJWarpSolverCfg, NewtonCfg from isaaclab_newton.physics.newton_manager import NewtonManager -from isaaclab_newton.sim.views import XformPrimView +from isaaclab_newton.sim.views import NewtonSiteXformPrimView as XformPrimView from pxr import Gf diff --git a/source/isaaclab_physx/isaaclab_physx/sim/views/__init__.pyi b/source/isaaclab_physx/isaaclab_physx/sim/views/__init__.pyi index a666958e4387..6deedc4b8e4d 100644 --- a/source/isaaclab_physx/isaaclab_physx/sim/views/__init__.pyi +++ b/source/isaaclab_physx/isaaclab_physx/sim/views/__init__.pyi @@ -4,7 +4,7 @@ # SPDX-License-Identifier: BSD-3-Clause __all__ = [ - "XformPrimView", + "FabricXformPrimView", ] -from .xform_prim_view import XformPrimView +from .fabric_xform_prim_view import FabricXformPrimView diff --git a/source/isaaclab_physx/isaaclab_physx/sim/views/xform_prim_view.py b/source/isaaclab_physx/isaaclab_physx/sim/views/fabric_xform_prim_view.py similarity index 76% rename from source/isaaclab_physx/isaaclab_physx/sim/views/xform_prim_view.py rename to source/isaaclab_physx/isaaclab_physx/sim/views/fabric_xform_prim_view.py index 100672f22fb6..1a4101f6f5c5 100644 --- a/source/isaaclab_physx/isaaclab_physx/sim/views/xform_prim_view.py +++ b/source/isaaclab_physx/isaaclab_physx/sim/views/fabric_xform_prim_view.py @@ -3,12 +3,11 @@ # # SPDX-License-Identifier: BSD-3-Clause -"""PhysX XformPrimView with Fabric GPU acceleration — Warp-native.""" +"""PhysX XformPrimView with Fabric GPU acceleration.""" from __future__ import annotations import logging -from collections.abc import Sequence import torch import warp as wp @@ -17,7 +16,8 @@ import isaaclab.sim as sim_utils from isaaclab.app.settings_manager import SettingsManager -from isaaclab.sim.views.xform_prim_view import XformPrimView as UsdXformPrimView +from isaaclab.sim.views.base_xform_prim_view import BaseXformPrimView +from isaaclab.sim.views.usd_xform_prim_view import UsdXformPrimView from isaaclab.utils.warp import fabric as fabric_utils logger = logging.getLogger(__name__) @@ -39,28 +39,18 @@ def _to_float32_2d(a: wp.array | torch.Tensor) -> wp.array | torch.Tensor: return a.view(dtype=wp.float32) -class XformPrimView(UsdXformPrimView): +class FabricXformPrimView(BaseXformPrimView): """XformPrimView with Fabric GPU acceleration for the PhysX backend. - Inherits all USD-based operations from the core - :class:`~isaaclab.sim.views.XformPrimView` and adds GPU-accelerated Fabric - paths for ``get_world_poses``, ``set_world_poses``, ``get_scales``, and - ``set_scales``. + Uses composition: holds a :class:`UsdXformPrimView` internally for USD + fallback and non-accelerated operations (local poses, visibility, scales + when Fabric is disabled). - When Fabric is enabled, this class leverages NVIDIA's Fabric API: - - - Uses ``omni:fabric:worldMatrix`` for all Boundable prims - - Performs batch matrix decomposition/composition using Warp kernels on GPU - - Works for both physics-enabled and non-physics prims (cameras, meshes, etc.) - - When Fabric is **not** enabled, all operations fall through to the USD base class. + When Fabric is enabled, world-pose and scale operations use GPU-accelerated + Warp kernels operating on ``omni:fabric:worldMatrix``. All other operations + delegate to the internal USD view. All getters return ``wp.array``. Setters accept ``wp.array``. - - .. warning:: - **Fabric requires CUDA**: Fabric is only supported on CUDA devices. - Warp's CPU backend for fabric-array writes has known issues, so attempting - to use Fabric with CPU device will automatically fall back to USD operations. """ def __init__( @@ -71,18 +61,8 @@ def __init__( sync_usd_on_fabric_write: bool = False, stage: Usd.Stage | None = None, ): - """Initialize the view with matching prims and optional Fabric acceleration. - - Args: - prim_path: USD prim path pattern to match prims. - device: Device to place arrays on. Defaults to ``"cpu"``. - validate_xform_ops: Whether to validate standard xform operations. Defaults to True. - sync_usd_on_fabric_write: Whether to mirror Fabric transform writes back to USD. - Defaults to False for better performance. - stage: USD stage to search for prims. Defaults to None (current active stage). - """ - super().__init__(prim_path, device=device, validate_xform_ops=validate_xform_ops, stage=stage) - + self._usd_view = UsdXformPrimView(prim_path, device=device, validate_xform_ops=validate_xform_ops, stage=stage) + self._device = device self._sync_usd_on_fabric_write = sync_usd_on_fabric_write settings = SettingsManager.instance() @@ -113,65 +93,42 @@ def __init__( self._view_index_attr = f"isaaclab:view_index:{abs(hash(self))}" # ------------------------------------------------------------------ - # Overrides — Fabric-accelerated or USD fallback + # Delegated properties # ------------------------------------------------------------------ - def set_world_poses( - self, - positions: wp.array | None = None, - orientations: wp.array | None = None, - indices: Sequence[int] | None = None, - ): - if not self._use_fabric: - super().set_world_poses(positions, orientations, indices) - return + @property + def count(self) -> int: + return self._usd_view.count - if not self._fabric_initialized: - self._initialize_fabric() + @property + def device(self) -> str: + return self._device - indices_wp = self._resolve_indices_wp(indices) - count = indices_wp.shape[0] + @property + def prims(self) -> list: + return self._usd_view.prims - dummy = wp.zeros((0, 3), dtype=wp.float32, device=self._device) - positions_wp = _to_float32_2d(positions) if positions is not None else dummy - orientations_wp = _to_float32_2d(orientations) if orientations is not None else wp.zeros((0, 4), dtype=wp.float32, device=self._device) - scales_wp = dummy + @property + def prim_paths(self) -> list[str]: + return self._usd_view.prim_paths - wp.launch( - kernel=fabric_utils.compose_fabric_transformation_matrix_from_warp_arrays, - dim=count, - inputs=[ - self._fabric_world_matrices, - positions_wp, - orientations_wp, - scales_wp, - False, - False, - False, - indices_wp, - self._view_to_fabric, - ], - device=self._fabric_device, - ) - wp.synchronize() + # ------------------------------------------------------------------ + # Delegated operations (USD-only) + # ------------------------------------------------------------------ - self._fabric_hierarchy.update_world_xforms() - self._fabric_usd_sync_done = True - if self._sync_usd_on_fabric_write: - super().set_world_poses(positions, orientations, indices) + def get_visibility(self, indices=None): + return self._usd_view.get_visibility(indices) - def set_local_poses( - self, - translations: wp.array | None = None, - orientations: wp.array | None = None, - indices: Sequence[int] | None = None, - ): - # Fabric only accelerates world poses; local poses always go through USD - super().set_local_poses(translations, orientations, indices) + def set_visibility(self, visibility, indices=None): + self._usd_view.set_visibility(visibility, indices) + + # ------------------------------------------------------------------ + # World poses — Fabric-accelerated or USD fallback + # ------------------------------------------------------------------ - def set_scales(self, scales: wp.array, indices: Sequence[int] | None = None): + def set_world_poses(self, positions=None, orientations=None, indices=None): if not self._use_fabric: - super().set_scales(scales, indices) + self._usd_view.set_world_poses(positions, orientations, indices) return if not self._fabric_initialized: @@ -180,18 +137,22 @@ def set_scales(self, scales: wp.array, indices: Sequence[int] | None = None): indices_wp = self._resolve_indices_wp(indices) count = indices_wp.shape[0] - dummy3 = wp.zeros((0, 3), dtype=wp.float32, device=self._device) - dummy4 = wp.zeros((0, 4), dtype=wp.float32, device=self._device) - scales_wp = _to_float32_2d(scales) + dummy = wp.zeros((0, 3), dtype=wp.float32, device=self._device) + positions_wp = _to_float32_2d(positions) if positions is not None else dummy + orientations_wp = ( + _to_float32_2d(orientations) + if orientations is not None + else wp.zeros((0, 4), dtype=wp.float32, device=self._device) + ) wp.launch( kernel=fabric_utils.compose_fabric_transformation_matrix_from_warp_arrays, dim=count, inputs=[ self._fabric_world_matrices, - dummy3, - dummy4, - scales_wp, + positions_wp, + orientations_wp, + dummy, False, False, False, @@ -205,11 +166,11 @@ def set_scales(self, scales: wp.array, indices: Sequence[int] | None = None): self._fabric_hierarchy.update_world_xforms() self._fabric_usd_sync_done = True if self._sync_usd_on_fabric_write: - super().set_scales(scales, indices) + self._usd_view.set_world_poses(positions, orientations, indices) - def get_world_poses(self, indices: Sequence[int] | None = None) -> tuple[wp.array, wp.array]: + def get_world_poses(self, indices=None): if not self._use_fabric: - return super().get_world_poses(indices) + return self._usd_view.get_world_poses(indices) if not self._fabric_initialized: self._initialize_fabric() @@ -245,13 +206,61 @@ def get_world_poses(self, indices: Sequence[int] | None = None) -> tuple[wp.arra wp.synchronize() return positions_wp, orientations_wp - def get_local_poses(self, indices: Sequence[int] | None = None) -> tuple[wp.array, wp.array]: - # Fabric only accelerates world poses; local poses always go through USD - return super().get_local_poses(indices) + # ------------------------------------------------------------------ + # Local poses — USD fallback (Fabric only accelerates world poses) + # ------------------------------------------------------------------ + + def set_local_poses(self, translations=None, orientations=None, indices=None): + self._usd_view.set_local_poses(translations, orientations, indices) + + def get_local_poses(self, indices=None): + return self._usd_view.get_local_poses(indices) + + # ------------------------------------------------------------------ + # Scales — Fabric-accelerated or USD fallback + # ------------------------------------------------------------------ + + def set_scales(self, scales, indices=None): + if not self._use_fabric: + self._usd_view.set_scales(scales, indices) + return + + if not self._fabric_initialized: + self._initialize_fabric() + + indices_wp = self._resolve_indices_wp(indices) + count = indices_wp.shape[0] + + dummy3 = wp.zeros((0, 3), dtype=wp.float32, device=self._device) + dummy4 = wp.zeros((0, 4), dtype=wp.float32, device=self._device) + scales_wp = _to_float32_2d(scales) + + wp.launch( + kernel=fabric_utils.compose_fabric_transformation_matrix_from_warp_arrays, + dim=count, + inputs=[ + self._fabric_world_matrices, + dummy3, + dummy4, + scales_wp, + False, + False, + False, + indices_wp, + self._view_to_fabric, + ], + device=self._fabric_device, + ) + wp.synchronize() + + self._fabric_hierarchy.update_world_xforms() + self._fabric_usd_sync_done = True + if self._sync_usd_on_fabric_write: + self._usd_view.set_scales(scales, indices) - def get_scales(self, indices: Sequence[int] | None = None) -> wp.array: + def get_scales(self, indices=None): if not self._use_fabric: - return super().get_scales(indices) + return self._usd_view.get_scales(indices) if not self._fabric_initialized: self._initialize_fabric() @@ -286,7 +295,7 @@ def get_scales(self, indices: Sequence[int] | None = None) -> wp.array: return scales_wp # ------------------------------------------------------------------ - # Internal — Initialization + # Internal — Fabric initialization # ------------------------------------------------------------------ def _initialize_fabric(self) -> None: @@ -356,7 +365,6 @@ def _initialize_fabric(self) -> None: ) wp.synchronize() - # Cached output buffers for the common no-indices path self._fabric_positions_buf = wp.zeros((self.count, 3), dtype=wp.float32, device=self._device) self._fabric_orientations_buf = wp.zeros((self.count, 4), dtype=wp.float32, device=self._device) self._fabric_scales_buf = wp.zeros((self.count, 3), dtype=wp.float32, device=self._device) @@ -373,9 +381,8 @@ def _sync_fabric_from_usd_once(self) -> None: if not self._fabric_initialized: self._initialize_fabric() - # Read current USD poses/scales and push them into Fabric - positions_usd, orientations_usd = super().get_world_poses() - scales_usd = super().get_scales() + positions_usd, orientations_usd = self._usd_view.get_world_poses() + scales_usd = self._usd_view.get_scales() prev_sync = self._sync_usd_on_fabric_write self._sync_usd_on_fabric_write = False @@ -385,7 +392,7 @@ def _sync_fabric_from_usd_once(self) -> None: self._fabric_usd_sync_done = True - def _resolve_indices_wp(self, indices: Sequence[int] | None) -> wp.array: + def _resolve_indices_wp(self, indices) -> wp.array: """Resolve view indices as a Warp uint32 array.""" if indices is None or indices == slice(None): if self._default_view_indices is None: diff --git a/source/isaaclab_physx/test/sim/test_views_xform_prim_fabric.py b/source/isaaclab_physx/test/sim/test_views_xform_prim_fabric.py index a4b18fbe870c..1ad25191f573 100644 --- a/source/isaaclab_physx/test/sim/test_views_xform_prim_fabric.py +++ b/source/isaaclab_physx/test/sim/test_views_xform_prim_fabric.py @@ -25,7 +25,7 @@ from pxr import Gf, UsdGeom # noqa: E402 -from isaaclab_physx.sim.views import XformPrimView # noqa: E402 +from isaaclab_physx.sim.views import FabricXformPrimView as XformPrimView # noqa: E402 import isaaclab.sim as sim_utils # noqa: E402 from xform_contract_tests import * # noqa: F401, F403, E402 From 22b38d669e8478f25d17a8a3b0596ce980e38fcf Mon Sep 17 00:00:00 2001 From: Octi Zhang Date: Thu, 9 Apr 2026 16:54:34 -0700 Subject: [PATCH 05/24] fix linter --- .../benchmarks/benchmark_view_comparison.py | 8 ++++-- .../benchmarks/benchmark_xform_prim_view.py | 5 ++-- .../test/sim/test_views_xform_prim.py | 28 ++++++------------- .../isaaclab/test/sim/xform_contract_tests.py | 13 ++++----- .../test/sim/test_views_xform_prim_newton.py | 4 +-- .../test/sim/test_views_xform_prim_fabric.py | 16 ++++------- 6 files changed, 28 insertions(+), 46 deletions(-) diff --git a/scripts/benchmarks/benchmark_view_comparison.py b/scripts/benchmarks/benchmark_view_comparison.py index 2d0f7e1b65e1..a326493c1639 100644 --- a/scripts/benchmarks/benchmark_view_comparison.py +++ b/scripts/benchmarks/benchmark_view_comparison.py @@ -274,8 +274,6 @@ def _run_pose_benchmarks( orientations_t: torch.Tensor, ): """Shared benchmark loop for get/set world poses on any XformPrimView.""" - device = args_cli.device - start_time = time.perf_counter() for _ in range(num_iterations): view.get_world_poses() @@ -375,7 +373,11 @@ def print_results(results_dict: dict[str, dict[str, float]], num_prims: int, num print(header) print("-" * 120) - operations = [("Initialization", "init"), ("Get World Poses", "get_world_poses"), ("Set World Poses", "set_world_poses")] + operations = [ + ("Initialization", "init"), + ("Get World Poses", "get_world_poses"), + ("Set World Poses", "set_world_poses"), + ] for op_name, op_key in operations: row = f"{op_name:<25}" diff --git a/scripts/benchmarks/benchmark_xform_prim_view.py b/scripts/benchmarks/benchmark_xform_prim_view.py index 289379f6f4df..b4e630bec98f 100644 --- a/scripts/benchmarks/benchmark_xform_prim_view.py +++ b/scripts/benchmarks/benchmark_xform_prim_view.py @@ -43,13 +43,12 @@ import torch import warp as wp - -from pxr import Gf - from isaaclab_newton.physics import MJWarpSolverCfg, NewtonCfg from isaaclab_newton.sim.views import NewtonSiteXformPrimView from isaaclab_physx.sim.views import FabricXformPrimView +from pxr import Gf + import isaaclab.sim as sim_utils from isaaclab.assets import RigidObjectCfg from isaaclab.scene import InteractiveScene, InteractiveSceneCfg diff --git a/source/isaaclab/test/sim/test_views_xform_prim.py b/source/isaaclab/test/sim/test_views_xform_prim.py index 30f90af14634..3f604341f8d0 100644 --- a/source/isaaclab/test/sim/test_views_xform_prim.py +++ b/source/isaaclab/test/sim/test_views_xform_prim.py @@ -25,13 +25,13 @@ except (ModuleNotFoundError, ImportError): _IsaacSimXformPrimView = None +from xform_contract_tests import * # noqa: F401, F403, E402 +from xform_contract_tests import CHILD_OFFSET, ViewBundle # noqa: E402 + import isaaclab.sim as sim_utils # noqa: E402 from isaaclab.sim.views import UsdXformPrimView as XformPrimView # noqa: E402 from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR # noqa: E402 -from xform_contract_tests import * # noqa: F401, F403, E402 -from xform_contract_tests import CHILD_OFFSET, ViewBundle # noqa: E402 - PARENT_POS = (0.0, 0.0, 1.0) @@ -64,7 +64,7 @@ def _get_parent_positions(num_envs, device="cpu"): def _set_parent_positions(positions, num_envs): """Write parent Xform positions to USD.""" - from pxr import Sdf, Vt # noqa: PLC0415 + from pxr import Sdf # noqa: PLC0415 stage = sim_utils.get_current_stage() with Sdf.ChangeBlock(): @@ -81,12 +81,8 @@ def view_factory(): def factory(num_envs: int, device: str) -> ViewBundle: stage = sim_utils.get_current_stage() for i in range(num_envs): - sim_utils.create_prim( - f"/World/Parent_{i}", "Xform", translation=PARENT_POS, stage=stage - ) - sim_utils.create_prim( - f"/World/Parent_{i}/Child", "Xform", translation=CHILD_OFFSET, stage=stage - ) + sim_utils.create_prim(f"/World/Parent_{i}", "Xform", translation=PARENT_POS, stage=stage) + sim_utils.create_prim(f"/World/Parent_{i}/Child", "Xform", translation=CHILD_OFFSET, stage=stage) view = XformPrimView("/World/Parent_.*/Child", device=device) return ViewBundle( @@ -227,12 +223,8 @@ def test_nested_hierarchy_world_poses(device): frames_view = XformPrimView("/World/Frame_.*", device=device) targets_view = XformPrimView("/World/Frame_.*/Target", device=device) - frames_view.set_local_poses( - translations=torch.tensor(frame_positions, device=device) - ) - targets_view.set_local_poses( - translations=torch.tensor(target_positions, device=device) - ) + frames_view.set_local_poses(translations=torch.tensor(frame_positions, device=device)) + targets_view.set_local_poses(translations=torch.tensor(target_positions, device=device)) world_pos = wp.to_torch(targets_view.get_world_poses()[0]) expected = torch.tensor( @@ -295,9 +287,7 @@ def test_with_franka_robots(device): torch.testing.assert_close(positions, torch.zeros(2, 3, device=device), atol=1e-5, rtol=0) new_pos = torch.tensor([[10.0, 10.0, 0.0], [-40.0, -40.0, 0.0]], device=device) - new_quat = torch.tensor( - [[0.0, 0.0, 0.7071068, 0.7071068], [0.0, 0.0, -0.7071068, 0.7071068]], device=device - ) + new_quat = torch.tensor([[0.0, 0.0, 0.7071068, 0.7071068], [0.0, 0.0, -0.7071068, 0.7071068]], device=device) view.set_world_poses(positions=new_pos, orientations=new_quat) ret_pos = wp.to_torch(view.get_world_poses()[0]) diff --git a/source/isaaclab/test/sim/xform_contract_tests.py b/source/isaaclab/test/sim/xform_contract_tests.py index 199c88690197..0f1023e357ed 100644 --- a/source/isaaclab/test/sim/xform_contract_tests.py +++ b/source/isaaclab/test/sim/xform_contract_tests.py @@ -12,8 +12,7 @@ The factory signature is:: - def view_factory() -> Callable[[int, str], ViewBundle]: - ... + def view_factory() -> Callable[[int, str], ViewBundle]: ... Where ``ViewBundle`` is a :class:`NamedTuple`:: @@ -37,7 +36,8 @@ class ViewBundle(NamedTuple): from __future__ import annotations -from typing import Callable, NamedTuple +from collections.abc import Callable +from typing import NamedTuple import pytest import torch @@ -121,8 +121,7 @@ def test_local_differs_from_world(device, view_factory): diff = (world_pos - local_pos).abs().max().item() assert diff > 0.5, ( - f"Expected |world - local| > 0.5, got {diff:.4f}. " - f"world={world_pos.tolist()}, local={local_pos.tolist()}" + f"Expected |world - local| > 0.5, got {diff:.4f}. world={world_pos.tolist()}, local={local_pos.tolist()}" ) finally: bundle.teardown() @@ -314,9 +313,7 @@ def test_set_world_partial_orientation_only(device, view_factory): bundle = view_factory(num_envs=2, device=device) try: orig_pos, _ = bundle.view.get_world_poses() - new_quat = _wp_vec4f( - [[0.0, 0.0, 0.7071068, 0.7071068], [0.7071068, 0.0, 0.0, 0.7071068]], device=device - ) + new_quat = _wp_vec4f([[0.0, 0.0, 0.7071068, 0.7071068], [0.7071068, 0.0, 0.0, 0.7071068]], device=device) bundle.view.set_world_poses(orientations=new_quat) ret_pos, ret_quat = bundle.view.get_world_poses() diff --git a/source/isaaclab_newton/test/sim/test_views_xform_prim_newton.py b/source/isaaclab_newton/test/sim/test_views_xform_prim_newton.py index 26b5aac84ebd..cbb50ba1dc3d 100644 --- a/source/isaaclab_newton/test/sim/test_views_xform_prim_newton.py +++ b/source/isaaclab_newton/test/sim/test_views_xform_prim_newton.py @@ -22,6 +22,8 @@ from isaaclab_newton.physics import MJWarpSolverCfg, NewtonCfg from isaaclab_newton.physics.newton_manager import NewtonManager from isaaclab_newton.sim.views import NewtonSiteXformPrimView as XformPrimView +from xform_contract_tests import * # noqa: F401, F403 — import all contract tests +from xform_contract_tests import CHILD_OFFSET, ViewBundle, _wp_vec3f, _wp_vec4f from pxr import Gf @@ -29,8 +31,6 @@ from isaaclab.assets import RigidObjectCfg from isaaclab.scene import InteractiveScene, InteractiveSceneCfg from isaaclab.sim import SimulationCfg, build_simulation_context -from xform_contract_tests import * # noqa: F401, F403 — import all contract tests -from xform_contract_tests import CHILD_OFFSET, ViewBundle, _wp_vec3f, _wp_vec4f from isaaclab.utils import configclass NEWTON_SIM_CFG = SimulationCfg(physics=NewtonCfg(solver_cfg=MJWarpSolverCfg())) diff --git a/source/isaaclab_physx/test/sim/test_views_xform_prim_fabric.py b/source/isaaclab_physx/test/sim/test_views_xform_prim_fabric.py index 1ad25191f573..0e4275c133a2 100644 --- a/source/isaaclab_physx/test/sim/test_views_xform_prim_fabric.py +++ b/source/isaaclab_physx/test/sim/test_views_xform_prim_fabric.py @@ -21,15 +21,13 @@ import pytest # noqa: E402 import torch # noqa: E402 -import warp as wp # noqa: E402 +from isaaclab_physx.sim.views import FabricXformPrimView as XformPrimView # noqa: E402 +from xform_contract_tests import * # noqa: F401, F403, E402 +from xform_contract_tests import CHILD_OFFSET, ViewBundle # noqa: E402 from pxr import Gf, UsdGeom # noqa: E402 -from isaaclab_physx.sim.views import FabricXformPrimView as XformPrimView # noqa: E402 - import isaaclab.sim as sim_utils # noqa: E402 -from xform_contract_tests import * # noqa: F401, F403, E402 -from xform_contract_tests import CHILD_OFFSET, ViewBundle # noqa: E402 PARENT_POS = (0.0, 0.0, 1.0) @@ -92,12 +90,8 @@ def factory(num_envs: int, device: str) -> ViewBundle: stage = sim_utils.get_current_stage() for i in range(num_envs): - sim_utils.create_prim( - f"/World/Parent_{i}", "Xform", translation=PARENT_POS, stage=stage - ) - sim_utils.create_prim( - f"/World/Parent_{i}/Child", "Camera", translation=CHILD_OFFSET, stage=stage - ) + sim_utils.create_prim(f"/World/Parent_{i}", "Xform", translation=PARENT_POS, stage=stage) + sim_utils.create_prim(f"/World/Parent_{i}/Child", "Camera", translation=CHILD_OFFSET, stage=stage) sim_utils.SimulationContext(sim_utils.SimulationCfg(dt=0.01, device=device, use_fabric=True)) view = XformPrimView("/World/Parent_.*/Child", device=device) From 4fd67c822f038c9abf49f12f921e00361374109b Mon Sep 17 00:00:00 2001 From: Octi Zhang Date: Thu, 9 Apr 2026 17:14:06 -0700 Subject: [PATCH 06/24] fixes --- .../benchmarks/benchmark_view_comparison.py | 146 ++++++------------ 1 file changed, 45 insertions(+), 101 deletions(-) diff --git a/scripts/benchmarks/benchmark_view_comparison.py b/scripts/benchmarks/benchmark_view_comparison.py index a326493c1639..346fd4c86af2 100644 --- a/scripts/benchmarks/benchmark_view_comparison.py +++ b/scripts/benchmarks/benchmark_view_comparison.py @@ -86,10 +86,9 @@ @torch.no_grad() -def benchmark_usd_or_fabric(view_type: str, num_iterations: int) -> tuple[dict[str, float], dict[str, torch.Tensor]]: +def benchmark_usd_or_fabric(view_type: str, num_iterations: int) -> dict[str, float]: """Benchmark USD or Fabric XformPrimView.""" timing_results = {} - computed_results = {} print(" Setting up scene") sim_utils.create_new_stage() @@ -129,17 +128,15 @@ def benchmark_usd_or_fabric(view_type: str, num_iterations: int) -> tuple[dict[s print(f" XformPrimView ({view_type.upper()}) managing {num_prims} prims") positions, orientations = view.get_world_poses() - positions_t = wp.to_torch(positions) - orientations_t = wp.to_torch(orientations) - _run_pose_benchmarks(view, num_prims, num_iterations, timing_results, computed_results, positions_t, orientations_t) + _run_pose_benchmarks(view, num_prims, num_iterations, timing_results, positions, orientations) sim.clear_instance() - return timing_results, computed_results + return timing_results @torch.no_grad() -def benchmark_newton(num_iterations: int) -> tuple[dict[str, float], dict[str, torch.Tensor]]: +def benchmark_newton(num_iterations: int) -> dict[str, float]: """Benchmark Newton XformPrimView.""" from isaaclab.assets import RigidObjectCfg from isaaclab.scene import InteractiveScene, InteractiveSceneCfg @@ -147,7 +144,6 @@ def benchmark_newton(num_iterations: int) -> tuple[dict[str, float], dict[str, t from isaaclab.utils import configclass timing_results = {} - computed_results = {} @configclass class _SceneCfg(InteractiveSceneCfg): @@ -188,20 +184,17 @@ class _SceneCfg(InteractiveSceneCfg): print(f" Newton XformPrimView managing {num_prims} prims") positions, orientations = view.get_world_poses() - positions_t = wp.to_torch(positions) - orientations_t = wp.to_torch(orientations) - _run_pose_benchmarks(view, num_prims, num_iterations, timing_results, computed_results, positions_t, orientations_t) + _run_pose_benchmarks(view, num_prims, num_iterations, timing_results, positions, orientations) ctx.__exit__(None, None, None) - return timing_results, computed_results + return timing_results @torch.no_grad() -def benchmark_physx(num_iterations: int) -> tuple[dict[str, float], dict[str, torch.Tensor]]: +def benchmark_physx(num_iterations: int) -> dict[str, float]: """Benchmark PhysX RigidBodyView.""" timing_results = {} - computed_results = {} print(" Setting up scene") sim_utils.create_new_stage() @@ -234,34 +227,40 @@ def benchmark_physx(num_iterations: int) -> tuple[dict[str, float], dict[str, to print(f" PhysX RigidBodyView managing {num_prims} prims") - all_indices = torch.arange(num_prims, device=args_cli.device) + all_indices = wp.from_torch(torch.arange(num_prims, dtype=torch.int32, device=args_cli.device)) transforms = view.get_transforms() - positions_t = transforms[:, :3] - orientations_t = transforms[:, 3:7] + transforms_t = wp.to_torch(transforms) if isinstance(transforms, wp.array) else transforms + positions_t = transforms_t[:, :3] + orientations_t = transforms_t[:, 3:7] start_time = time.perf_counter() for _ in range(num_iterations): transforms = view.get_transforms() timing_results["get_world_poses"] = (time.perf_counter() - start_time) / num_iterations - computed_results["initial_world_positions"] = positions_t.clone() - computed_results["initial_world_orientations"] = orientations_t.clone() - new_positions = positions_t.clone() new_positions[:, 2] += 0.5 - new_transforms = torch.cat([new_positions, orientations_t], dim=-1) + expected_positions = new_positions.clone() + new_transforms = wp.from_torch(torch.cat([new_positions, orientations_t], dim=-1).contiguous()) start_time = time.perf_counter() for _ in range(num_iterations): view.set_transforms(new_transforms, indices=all_indices) timing_results["set_world_poses"] = (time.perf_counter() - start_time) / num_iterations transforms_after = view.get_transforms() - computed_results["world_positions_after_set"] = transforms_after[:, :3].clone() - computed_results["world_orientations_after_set"] = transforms_after[:, 3:7].clone() + ta = wp.to_torch(transforms_after) if isinstance(transforms_after, wp.array) else transforms_after + pos_ok = torch.allclose(ta[:, :3], expected_positions, atol=1e-4, rtol=0) + quat_ok = torch.allclose(ta[:, 3:7], orientations_t, atol=1e-4, rtol=0) + if pos_ok and quat_ok: + print(" Round-trip verification: PASS") + else: + pos_diff = (ta[:, :3] - expected_positions).abs().max().item() + quat_diff = (ta[:, 3:7] - orientations_t).abs().max().item() + print(f" Round-trip verification: FAIL (pos max_diff={pos_diff:.6e}, quat max_diff={quat_diff:.6e})") sim.clear_instance() - return timing_results, computed_results + return timing_results def _run_pose_benchmarks( @@ -269,9 +268,8 @@ def _run_pose_benchmarks( num_prims: int, num_iterations: int, timing_results: dict, - computed_results: dict, - positions_t: torch.Tensor, - orientations_t: torch.Tensor, + positions: wp.array, + orientations: wp.array, ): """Shared benchmark loop for get/set world poses on any XformPrimView.""" start_time = time.perf_counter() @@ -279,22 +277,29 @@ def _run_pose_benchmarks( view.get_world_poses() timing_results["get_world_poses"] = (time.perf_counter() - start_time) / num_iterations - computed_results["initial_world_positions"] = positions_t.clone() - computed_results["initial_world_orientations"] = orientations_t.clone() - - new_positions = positions_t.clone() - new_positions[:, 2] += 0.5 - new_pos_wp = wp.from_torch(new_positions) - new_quat_wp = wp.from_torch(orientations_t) + new_positions = wp.clone(positions) + new_positions_t = wp.to_torch(new_positions) + new_positions_t[:, 2] += 0.5 + expected_positions = new_positions_t.clone() start_time = time.perf_counter() for _ in range(num_iterations): - view.set_world_poses(new_pos_wp, new_quat_wp) + view.set_world_poses(new_positions, orientations) timing_results["set_world_poses"] = (time.perf_counter() - start_time) / num_iterations ret_pos, ret_quat = view.get_world_poses() - computed_results["world_positions_after_set"] = wp.to_torch(ret_pos).clone() - computed_results["world_orientations_after_set"] = wp.to_torch(ret_quat).clone() + ret_pos_t = wp.to_torch(ret_pos) + ret_quat_t = wp.to_torch(ret_quat) + ori_t = wp.to_torch(orientations) + + pos_ok = torch.allclose(ret_pos_t, expected_positions, atol=1e-4, rtol=0) + quat_ok = torch.allclose(ret_quat_t, ori_t, atol=1e-4, rtol=0) + if pos_ok and quat_ok: + print(" Round-trip verification: PASS") + else: + pos_diff = (ret_pos_t - expected_positions).abs().max().item() + quat_diff = (ret_quat_t - ori_t).abs().max().item() + print(f" Round-trip verification: FAIL (pos max_diff={pos_diff:.6e}, quat max_diff={quat_diff:.6e})") # ------------------------------------------------------------------ @@ -302,61 +307,6 @@ def _run_pose_benchmarks( # ------------------------------------------------------------------ -def compare_results( - results_dict: dict[str, dict[str, torch.Tensor]], tolerance: float = 1e-4 -) -> dict[str, dict[str, dict[str, float]]]: - """Compare computed results across implementations.""" - comparison_stats = {} - impl_names = list(results_dict.keys()) - - for i, impl1 in enumerate(impl_names): - for impl2 in impl_names[i + 1 :]: - pair_key = f"{impl1}_vs_{impl2}" - comparison_stats[pair_key] = {} - - computed1 = results_dict[impl1] - computed2 = results_dict[impl2] - - for key in computed1: - if key not in computed2: - continue - val1, val2 = computed1[key], computed2[key] - if torch.all(val1 == 0) or torch.all(val2 == 0): - continue - - diff = torch.abs(val1 - val2) - comparison_stats[pair_key][key] = { - "max_diff": torch.max(diff).item(), - "mean_diff": torch.mean(diff).item(), - "all_close": torch.allclose(val1, val2, atol=tolerance, rtol=0), - } - - return comparison_stats - - -def print_comparison_results(comparison_stats: dict, tolerance: float): - """Print comparison results.""" - for pair_key, pair_stats in comparison_stats.items(): - if not pair_stats: - continue - impl1, impl2 = pair_key.split("_vs_") - title = f"{impl1.replace('_', ' ').title()} vs {impl2.replace('_', ' ').title()}" - all_match = all(s["all_close"] for s in pair_stats.values()) - - print("\n" + "=" * 100) - print(f"RESULT COMPARISON: {title}") - print("=" * 100) - if all_match: - print(f" All computed values match within tolerance ({tolerance})") - else: - print(f"{'Computed Value':<40} {'Max Diff':<15} {'Mean Diff':<15} {'Match':<10}") - print("-" * 100) - for key, stats in pair_stats.items(): - match_str = "Yes" if stats["all_close"] else "No" - print(f"{key:<40} {stats['max_diff']:<15.6e} {stats['mean_diff']:<15.6e} {match_str:<10}") - print() - - def print_results(results_dict: dict[str, dict[str, float]], num_prims: int, num_iterations: int): """Print benchmark results in a formatted table.""" print("\n" + "=" * 120) @@ -414,7 +364,7 @@ def print_results(results_dict: dict[str, dict[str, float]], num_prims: int, num if name != baseline: impl_t = results_dict[name].get(op_key, 0) if base_t > 0 and impl_t > 0: - row += f" {impl_t / base_t:>{col_width}.2f}x" + row += f" {base_t / impl_t:>{col_width}.2f}x" else: row += f" {'N/A':>{col_width}}" print(row) @@ -422,7 +372,7 @@ def print_results(results_dict: dict[str, dict[str, float]], num_prims: int, num print("\nNotes:") print(" - Times are averaged over all iterations") - print(" - Speedup < 1.0 means faster than USD baseline") + print(" - Speedup > 1.0 means faster than USD baseline") print(" - PhysX RigidBodyView requires rigid body physics; XformPrimView works with any Xformable prim") print() @@ -448,7 +398,6 @@ def main(): os.makedirs(args_cli.profile_dir, exist_ok=True) all_timing = {} - all_computed = {} profile_files = {} dispatch = { @@ -470,7 +419,7 @@ def main(): profiler = cProfile.Profile() profiler.enable() - timing, computed = bench_fn(args_cli.num_iterations) + timing = bench_fn(args_cli.num_iterations) if args_cli.profile: profiler.disable() @@ -480,15 +429,10 @@ def main(): print(f" Profile saved to: {pf}") all_timing[key] = timing - all_computed[key] = computed print(" Done!\n") print_results(all_timing, args_cli.num_envs, args_cli.num_iterations) - print("Comparing computed results...") - comparison_stats = compare_results(all_computed, tolerance=1e-4) - print_comparison_results(comparison_stats, tolerance=1e-4) - if args_cli.profile: print("\n" + "=" * 100) print("PROFILING RESULTS") From 8bd1f3fe3f99169c905ff9c2cbecbbb5e7ff080e Mon Sep 17 00:00:00 2001 From: Octi Zhang Date: Thu, 9 Apr 2026 17:43:19 -0700 Subject: [PATCH 07/24] fix the input --- source/isaaclab/docs/CHANGELOG.rst | 7 ++- .../sim/views/base_xform_prim_view.py | 13 ++-- .../isaaclab/sim/views/usd_xform_prim_view.py | 25 ++++---- .../test/sim/test_views_xform_prim.py | 2 +- .../isaaclab/test/sim/xform_contract_tests.py | 7 ++- .../sim/views/newton_site_xform_prim_view.py | 61 ++++++------------- .../sim/views/fabric_xform_prim_view.py | 8 +-- 7 files changed, 47 insertions(+), 76 deletions(-) diff --git a/source/isaaclab/docs/CHANGELOG.rst b/source/isaaclab/docs/CHANGELOG.rst index af85d3d766b5..d9eb2915800e 100644 --- a/source/isaaclab/docs/CHANGELOG.rst +++ b/source/isaaclab/docs/CHANGELOG.rst @@ -181,9 +181,10 @@ Changed ^^^^^^^ * Refactored :class:`~isaaclab.sim.views.XformPrimView` to delegate backend-specific - logic to :class:`~isaaclab_physx.sim.views.XformPrimView` and - :class:`~isaaclab_newton.sim.views.XformPrimView`. The public API is unchanged; - use :class:`~isaaclab.sim.views.XformPrimView` for backend-aware instantiation. + logic to :class:`~isaaclab_physx.sim.views.FabricXformPrimView` and + :class:`~isaaclab_newton.sim.views.NewtonSiteXformPrimView`. The public API is + unchanged; use :class:`~isaaclab.sim.views.XformPrimView` for backend-aware + instantiation. * Added release version to :class:`~isaaclab.test.benchmark.recorders.VersionInfoRecorder` output. diff --git a/source/isaaclab/isaaclab/sim/views/base_xform_prim_view.py b/source/isaaclab/isaaclab/sim/views/base_xform_prim_view.py index 180ee90472db..c54f2a576170 100644 --- a/source/isaaclab/isaaclab/sim/views/base_xform_prim_view.py +++ b/source/isaaclab/isaaclab/sim/views/base_xform_prim_view.py @@ -8,7 +8,6 @@ from __future__ import annotations import abc -from collections.abc import Sequence import warp as wp @@ -31,7 +30,7 @@ def count(self) -> int: ... @abc.abstractmethod - def get_world_poses(self, indices: Sequence[int] | None = None) -> tuple[wp.array, wp.array]: + def get_world_poses(self, indices: wp.array | None = None) -> tuple[wp.array, wp.array]: """Get world-space positions and orientations for prims in the view. Args: @@ -47,7 +46,7 @@ def set_world_poses( self, positions: wp.array | None = None, orientations: wp.array | None = None, - indices: Sequence[int] | None = None, + indices: wp.array | None = None, ) -> None: """Set world-space positions and/or orientations for prims in the view. @@ -59,7 +58,7 @@ def set_world_poses( ... @abc.abstractmethod - def get_local_poses(self, indices: Sequence[int] | None = None) -> tuple[wp.array, wp.array]: + def get_local_poses(self, indices: wp.array | None = None) -> tuple[wp.array, wp.array]: """Get local-space positions and orientations for prims in the view. Args: @@ -75,7 +74,7 @@ def set_local_poses( self, translations: wp.array | None = None, orientations: wp.array | None = None, - indices: Sequence[int] | None = None, + indices: wp.array | None = None, ) -> None: """Set local-space translations and/or orientations for prims in the view. @@ -87,7 +86,7 @@ def set_local_poses( ... @abc.abstractmethod - def get_scales(self, indices: Sequence[int] | None = None) -> wp.array: + def get_scales(self, indices: wp.array | None = None) -> wp.array: """Get scales for prims in the view. Args: @@ -99,7 +98,7 @@ def get_scales(self, indices: Sequence[int] | None = None) -> wp.array: ... @abc.abstractmethod - def set_scales(self, scales: wp.array, indices: Sequence[int] | None = None) -> None: + def set_scales(self, scales: wp.array, indices: wp.array | None = None) -> None: """Set scales for prims in the view. Args: diff --git a/source/isaaclab/isaaclab/sim/views/usd_xform_prim_view.py b/source/isaaclab/isaaclab/sim/views/usd_xform_prim_view.py index 8ba81e1e4cc6..db784b27f3ab 100644 --- a/source/isaaclab/isaaclab/sim/views/usd_xform_prim_view.py +++ b/source/isaaclab/isaaclab/sim/views/usd_xform_prim_view.py @@ -6,7 +6,6 @@ from __future__ import annotations import logging -from collections.abc import Sequence import numpy as np import torch @@ -134,7 +133,7 @@ def set_world_poses( self, positions: wp.array | None = None, orientations: wp.array | None = None, - indices: Sequence[int] | None = None, + indices: wp.array | None = None, ): """Set world-space poses for prims in the view. @@ -193,7 +192,7 @@ def set_local_poses( self, translations: wp.array | None = None, orientations: wp.array | None = None, - indices: Sequence[int] | None = None, + indices: wp.array | None = None, ): """Set local-space poses for prims in the view. @@ -215,7 +214,7 @@ def set_local_poses( if orientations_array is not None: prim.GetAttribute("xformOp:orient").Set(orientations_array[idx]) - def set_scales(self, scales: wp.array, indices: Sequence[int] | None = None): + def set_scales(self, scales: wp.array, indices: wp.array | None = None): """Set scales for prims in the view. Args: @@ -230,7 +229,7 @@ def set_scales(self, scales: wp.array, indices: Sequence[int] | None = None): prim = self._prims[prim_idx] prim.GetAttribute("xformOp:scale").Set(scales_array[idx]) - def set_visibility(self, visibility: torch.Tensor, indices: Sequence[int] | None = None): + def set_visibility(self, visibility: torch.Tensor, indices: wp.array | None = None): """Set visibility for prims in the view. Args: @@ -254,7 +253,7 @@ def set_visibility(self, visibility: torch.Tensor, indices: Sequence[int] | None # Getters # ------------------------------------------------------------------ - def get_world_poses(self, indices: Sequence[int] | None = None) -> tuple[wp.array, wp.array]: + def get_world_poses(self, indices: wp.array | None = None) -> tuple[wp.array, wp.array]: """Get world-space poses for prims in the view. Args: @@ -281,7 +280,7 @@ def get_world_poses(self, indices: Sequence[int] | None = None) -> tuple[wp.arra wp.array(np.array(orientations, dtype=np.float32), dtype=wp.float32, device=self._device), ) - def get_local_poses(self, indices: Sequence[int] | None = None) -> tuple[wp.array, wp.array]: + def get_local_poses(self, indices: wp.array | None = None) -> tuple[wp.array, wp.array]: """Get local-space poses for prims in the view. Args: @@ -308,7 +307,7 @@ def get_local_poses(self, indices: Sequence[int] | None = None) -> tuple[wp.arra wp.array(np.array(orientations, dtype=np.float32), dtype=wp.float32, device=self._device), ) - def get_scales(self, indices: Sequence[int] | None = None) -> wp.array: + def get_scales(self, indices: wp.array | None = None) -> wp.array: """Get scales for prims in the view. Args: @@ -326,7 +325,7 @@ def get_scales(self, indices: Sequence[int] | None = None) -> wp.array: return wp.array(np.array(scales, dtype=np.float32), dtype=wp.float32, device=self._device) - def get_visibility(self, indices: Sequence[int] | None = None) -> torch.Tensor: + def get_visibility(self, indices: wp.array | None = None) -> torch.Tensor: """Get visibility for prims in the view. Args: @@ -347,13 +346,11 @@ def get_visibility(self, indices: Sequence[int] | None = None) -> torch.Tensor: # Helpers # ------------------------------------------------------------------ - def _resolve_indices(self, indices: Sequence[int] | None) -> list[int]: - """Resolve indices to a list, defaulting to all prims.""" + def _resolve_indices(self, indices: wp.array | None): + """Resolve warp indices to an iterable of ints for per-prim USD operations.""" if indices is None or indices == slice(None): return self._ALL_INDICES - if isinstance(indices, torch.Tensor): - return indices.tolist() - return list(indices) + return indices.numpy() @staticmethod def _to_numpy(data: wp.array | torch.Tensor) -> np.ndarray: diff --git a/source/isaaclab/test/sim/test_views_xform_prim.py b/source/isaaclab/test/sim/test_views_xform_prim.py index 3f604341f8d0..ba229f4541be 100644 --- a/source/isaaclab/test/sim/test_views_xform_prim.py +++ b/source/isaaclab/test/sim/test_views_xform_prim.py @@ -121,7 +121,7 @@ def test_visibility_toggle(device): view.set_visibility(torch.ones(num_prims, dtype=torch.bool, device=device)) assert torch.all(view.get_visibility()) - view.set_visibility(torch.tensor([False], dtype=torch.bool, device=device), indices=[1]) + view.set_visibility(torch.tensor([False], dtype=torch.bool, device=device), indices=wp.array([1], dtype=wp.int32, device=device)) vis = view.get_visibility() assert vis[0] and not vis[1] and vis[2] diff --git a/source/isaaclab/test/sim/xform_contract_tests.py b/source/isaaclab/test/sim/xform_contract_tests.py index 0f1023e357ed..9e231582a3c4 100644 --- a/source/isaaclab/test/sim/xform_contract_tests.py +++ b/source/isaaclab/test/sim/xform_contract_tests.py @@ -165,11 +165,12 @@ def test_indexed_get_returns_correct_subset(device, view_factory): all_world = _t(bundle.view.get_world_poses()[0]) all_local = _t(bundle.view.get_local_poses()[0]) - indices = [4, 1, 3] + indices_list = [4, 1, 3] + indices = wp.array(indices_list, dtype=wp.int32, device=device) sub_world = _t(bundle.view.get_world_poses(indices)[0]) sub_local = _t(bundle.view.get_local_poses(indices)[0]) - for out_i, view_i in enumerate(indices): + for out_i, view_i in enumerate(indices_list): torch.testing.assert_close(sub_world[out_i], all_world[view_i], atol=0, rtol=0) torch.testing.assert_close(sub_local[out_i], all_local[view_i], atol=0, rtol=0) finally: @@ -345,7 +346,7 @@ def test_set_world_indexed_only_affects_subset(device, view_factory): bundle = view_factory(num_envs=4, device=device) try: orig_pos = _t(bundle.view.get_world_poses()[0]).clone() - indices = [1, 3] + indices = wp.array([1, 3], dtype=wp.int32, device=device) new_pos = _wp_vec3f([[10.0, 20.0, 30.0], [40.0, 50.0, 60.0]], device=device) bundle.view.set_world_poses(positions=new_pos, indices=indices) diff --git a/source/isaaclab_newton/isaaclab_newton/sim/views/newton_site_xform_prim_view.py b/source/isaaclab_newton/isaaclab_newton/sim/views/newton_site_xform_prim_view.py index a1a41794c7f0..39bc42a64064 100644 --- a/source/isaaclab_newton/isaaclab_newton/sim/views/newton_site_xform_prim_view.py +++ b/source/isaaclab_newton/isaaclab_newton/sim/views/newton_site_xform_prim_view.py @@ -8,7 +8,6 @@ from __future__ import annotations import logging -from collections.abc import Sequence import warp as wp @@ -344,32 +343,6 @@ def _write_site_local_from_local_poses_indexed( site_local[si] = wp.transform_multiply(wp.transform_inverse(body_q[bid]), desired_world) -# ------------------------------------------------------------------ -# Helpers -# ------------------------------------------------------------------ - - -def _ensure_wp_vec3f(data: wp.array, device: str) -> wp.array: - """Pass-through for ``wp.array``; convert ``torch.Tensor`` via ``wp.from_torch``.""" - if isinstance(data, wp.array): - return data - import torch # noqa: PLC0415 - - if isinstance(data, torch.Tensor): - return wp.from_torch(data.contiguous(), dtype=wp.vec3f) - raise TypeError(f"Expected wp.array or torch.Tensor, got {type(data)}") - - -def _ensure_wp_vec4f(data: wp.array, device: str) -> wp.array: - """Pass-through for ``wp.array``; convert ``torch.Tensor`` via ``wp.from_torch``.""" - if isinstance(data, wp.array): - return data - import torch # noqa: PLC0415 - - if isinstance(data, torch.Tensor): - return wp.from_torch(data.contiguous(), dtype=wp.vec4f) - raise TypeError(f"Expected wp.array or torch.Tensor, got {type(data)}") - # ------------------------------------------------------------------ # View class @@ -525,12 +498,12 @@ def count(self) -> int: # World poses # ------------------------------------------------------------------ - def get_world_poses(self, indices: Sequence[int] | None = None) -> tuple[wp.array, wp.array]: + def get_world_poses(self, indices: wp.array | None = None) -> tuple[wp.array, wp.array]: state = NewtonManager.get_state_0() if indices is not None: n = len(indices) - idx_wp = wp.array(list(indices), dtype=wp.int32, device=self._device) + idx_wp = indices pos_buf = wp.zeros(n, dtype=wp.vec3f, device=self._device) quat_buf = wp.zeros(n, dtype=wp.vec4f, device=self._device) wp.launch( @@ -555,7 +528,7 @@ def set_world_poses( self, positions: wp.array | None = None, orientations: wp.array | None = None, - indices: Sequence[int] | None = None, + indices: wp.array | None = None, ) -> None: """Write world poses by updating the site's local offset. @@ -575,12 +548,12 @@ def set_world_poses( if orientations is None: orientations = cur_quat - pos_wp = _ensure_wp_vec3f(positions, self._device) - quat_wp = _ensure_wp_vec4f(orientations, self._device) + pos_wp = positions + quat_wp = orientations if indices is not None: n = len(indices) - idx_wp = wp.array(list(indices), dtype=wp.int32, device=self._device) + idx_wp = indices wp.launch( _write_site_local_from_world_poses_indexed, dim=n, @@ -599,13 +572,13 @@ def set_world_poses( # Local poses (parent-relative) # ------------------------------------------------------------------ - def get_local_poses(self, indices: Sequence[int] | None = None) -> tuple[wp.array, wp.array]: + def get_local_poses(self, indices: wp.array | None = None) -> tuple[wp.array, wp.array]: """Get parent-relative poses: ``local = inv(parent_world) * prim_world``.""" state = NewtonManager.get_state_0() if indices is not None: n = len(indices) - idx_wp = wp.array(list(indices), dtype=wp.int32, device=self._device) + idx_wp = indices pos_buf = wp.zeros(n, dtype=wp.vec3f, device=self._device) quat_buf = wp.zeros(n, dtype=wp.vec4f, device=self._device) wp.launch( @@ -643,7 +616,7 @@ def set_local_poses( self, translations: wp.array | None = None, orientations: wp.array | None = None, - indices: Sequence[int] | None = None, + indices: wp.array | None = None, ) -> None: """Write parent-relative poses by updating the site's local offset. @@ -662,12 +635,12 @@ def set_local_poses( if orientations is None: orientations = cur_quat - pos_wp = _ensure_wp_vec3f(translations, self._device) - quat_wp = _ensure_wp_vec4f(orientations, self._device) + pos_wp = translations + quat_wp = orientations if indices is not None: n = len(indices) - idx_wp = wp.array(list(indices), dtype=wp.int32, device=self._device) + idx_wp = indices wp.launch( _write_site_local_from_local_poses_indexed, dim=n, @@ -703,13 +676,13 @@ def set_local_poses( # Scales # ------------------------------------------------------------------ - def get_scales(self, indices: Sequence[int] | None = None) -> wp.array: + def get_scales(self, indices: wp.array | None = None) -> wp.array: model = NewtonManager.get_model() num_shapes = model.shape_count if indices is not None: n = len(indices) - idx_wp = wp.array(list(indices), dtype=wp.int32, device=self._device) + idx_wp = indices out = wp.zeros(n, dtype=wp.vec3f, device=self._device) wp.launch( _gather_scales_indexed, @@ -729,14 +702,14 @@ def get_scales(self, indices: Sequence[int] | None = None) -> wp.array: ) return out - def set_scales(self, scales: wp.array, indices: Sequence[int] | None = None) -> None: + def set_scales(self, scales: wp.array, indices: wp.array | None = None) -> None: model = NewtonManager.get_model() num_shapes = model.shape_count - scales_wp = _ensure_wp_vec3f(scales, self._device) + scales_wp = scales if indices is not None: n = len(indices) - idx_wp = wp.array(list(indices), dtype=wp.int32, device=self._device) + idx_wp = indices wp.launch( _scatter_scales_indexed, dim=n, diff --git a/source/isaaclab_physx/isaaclab_physx/sim/views/fabric_xform_prim_view.py b/source/isaaclab_physx/isaaclab_physx/sim/views/fabric_xform_prim_view.py index 1a4101f6f5c5..63d3ff968073 100644 --- a/source/isaaclab_physx/isaaclab_physx/sim/views/fabric_xform_prim_view.py +++ b/source/isaaclab_physx/isaaclab_physx/sim/views/fabric_xform_prim_view.py @@ -392,12 +392,12 @@ def _sync_fabric_from_usd_once(self) -> None: self._fabric_usd_sync_done = True - def _resolve_indices_wp(self, indices) -> wp.array: + def _resolve_indices_wp(self, indices: wp.array | None) -> wp.array: """Resolve view indices as a Warp uint32 array.""" if indices is None or indices == slice(None): if self._default_view_indices is None: raise RuntimeError("Fabric indices are not initialized.") return self._default_view_indices - if isinstance(indices, torch.Tensor): - indices = indices.tolist() - return wp.array(list(indices), dtype=wp.uint32, device=self._device) + if indices.dtype != wp.uint32: + return wp.array(indices.numpy().astype("uint32"), dtype=wp.uint32, device=self._device) + return indices From d0ab973c20cd5f92da8a283ddcb7bb1e5906db1b Mon Sep 17 00:00:00 2001 From: Octi Zhang Date: Thu, 9 Apr 2026 17:51:20 -0700 Subject: [PATCH 08/24] fix linter --- source/isaaclab/test/sim/test_views_xform_prim.py | 4 +++- .../isaaclab_newton/sim/views/newton_site_xform_prim_view.py | 1 - 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/source/isaaclab/test/sim/test_views_xform_prim.py b/source/isaaclab/test/sim/test_views_xform_prim.py index ba229f4541be..75e5d7535c2e 100644 --- a/source/isaaclab/test/sim/test_views_xform_prim.py +++ b/source/isaaclab/test/sim/test_views_xform_prim.py @@ -121,7 +121,9 @@ def test_visibility_toggle(device): view.set_visibility(torch.ones(num_prims, dtype=torch.bool, device=device)) assert torch.all(view.get_visibility()) - view.set_visibility(torch.tensor([False], dtype=torch.bool, device=device), indices=wp.array([1], dtype=wp.int32, device=device)) + view.set_visibility( + torch.tensor([False], dtype=torch.bool, device=device), indices=wp.array([1], dtype=wp.int32, device=device) + ) vis = view.get_visibility() assert vis[0] and not vis[1] and vis[2] diff --git a/source/isaaclab_newton/isaaclab_newton/sim/views/newton_site_xform_prim_view.py b/source/isaaclab_newton/isaaclab_newton/sim/views/newton_site_xform_prim_view.py index 39bc42a64064..b295fe019edc 100644 --- a/source/isaaclab_newton/isaaclab_newton/sim/views/newton_site_xform_prim_view.py +++ b/source/isaaclab_newton/isaaclab_newton/sim/views/newton_site_xform_prim_view.py @@ -343,7 +343,6 @@ def _write_site_local_from_local_poses_indexed( site_local[si] = wp.transform_multiply(wp.transform_inverse(body_q[bid]), desired_world) - # ------------------------------------------------------------------ # View class # ------------------------------------------------------------------ From 0b660d010a27b52170a137e3a535e3a5b26c4c40 Mon Sep 17 00:00:00 2001 From: Octi Zhang Date: Fri, 10 Apr 2026 00:18:30 -0700 Subject: [PATCH 09/24] fix raycaster warp conversion --- .../isaaclab/sensors/ray_caster/ray_cast_utils.py | 14 ++++---------- 1 file changed, 4 insertions(+), 10 deletions(-) diff --git a/source/isaaclab/isaaclab/sensors/ray_caster/ray_cast_utils.py b/source/isaaclab/isaaclab/sensors/ray_caster/ray_cast_utils.py index d3c5aacf58fa..d7fb69704eaa 100644 --- a/source/isaaclab/isaaclab/sensors/ray_caster/ray_cast_utils.py +++ b/source/isaaclab/isaaclab/sensors/ray_caster/ray_cast_utils.py @@ -34,16 +34,10 @@ def obtain_world_pose_from_view( Raises: NotImplementedError: If the prim view is not of the supported type. """ - if isinstance(physx_view, BaseXformPrimView): - pos_wp, quat_wp = physx_view.get_world_poses(env_ids) - pos_w = wp.to_torch(pos_wp) - quat_w = wp.to_torch(quat_wp) - elif isinstance(physx_view, physx.ArticulationView): - pos_w, quat_w = wp.to_torch(physx_view.get_root_transforms())[env_ids].split([3, 4], dim=-1) - elif isinstance(physx_view, physx.RigidBodyView): - pos_w, quat_w = wp.to_torch(physx_view.get_transforms())[env_ids].split([3, 4], dim=-1) - else: - raise NotImplementedError(f"Cannot get world poses for prim view of type '{type(physx_view)}'.") + indices = wp.from_torch(env_ids.to(dtype=torch.int32), dtype=wp.int32) if env_ids is not None else None + pos_wp, quat_wp = physx_view.get_world_poses(indices) + pos_w = wp.to_torch(pos_wp) + quat_w = wp.to_torch(quat_wp) if clone: return pos_w.clone(), quat_w.clone() From 7f99a3df8d7941782d6f424b6877df8c37e343c9 Mon Sep 17 00:00:00 2001 From: Octi Zhang Date: Fri, 10 Apr 2026 02:26:53 -0700 Subject: [PATCH 10/24] fix xformprimview callers --- .../04_sensors/add_sensors_on_robot.rst | 8 +- scripts/demos/sensors/multi_mesh_raycaster.py | 6 +- scripts/demos/sensors/raycaster_sensor.py | 3 +- .../03_envs/create_quadruped_base_env.py | 3 +- .../04_sensors/add_sensors_on_robot.py | 3 +- .../tutorials/04_sensors/run_ray_caster.py | 2 +- source/isaaclab/config/extension.toml | 2 +- source/isaaclab/docs/CHANGELOG.rst | 24 +++ .../isaaclab/scene/interactive_scene_cfg.py | 3 +- source/isaaclab/isaaclab/sensors/__init__.py | 2 +- .../ray_caster/multi_mesh_ray_caster.py | 4 +- .../sensors/ray_caster/ray_cast_utils.py | 7 +- .../isaaclab/sensors/ray_caster/ray_caster.py | 154 ++++++++---------- .../sensors/ray_caster/ray_caster_cfg.py | 37 +++-- source/isaaclab/isaaclab/sim/__init__.pyi | 4 + .../isaaclab/sim/spawners/__init__.pyi | 4 +- .../sim/spawners/sensors/__init__.pyi | 6 +- .../isaaclab/sim/spawners/sensors/sensors.py | 50 ++++++ .../sim/spawners/sensors/sensors_cfg.py | 13 ++ ...eck_manager_based_env_anymal_locomotion.py | 3 +- .../test/markers/check_markers_visibility.py | 3 +- .../test/scene/check_interactive_scene.py | 3 +- .../sensors/check_multi_mesh_ray_caster.py | 4 +- .../isaaclab/test/sensors/check_ray_caster.py | 2 +- .../sim/views/newton_site_xform_prim_view.py | 15 ++ source/isaaclab_tasks/config/extension.toml | 2 +- source/isaaclab_tasks/docs/CHANGELOG.rst | 12 ++ .../direct/anymal_c/anymal_c_env_cfg.py | 3 +- .../velocity/config/a1/rough_env_cfg.py | 2 +- .../velocity/config/cassie/rough_env_cfg.py | 2 +- .../velocity/config/digit/rough_env_cfg.py | 2 +- .../velocity/config/g1/rough_env_cfg.py | 2 +- .../velocity/config/go1/rough_env_cfg.py | 2 +- .../velocity/config/go2/rough_env_cfg.py | 2 +- .../velocity/config/h1/rough_env_cfg.py | 2 +- .../locomotion/velocity/velocity_env_cfg.py | 3 +- 36 files changed, 259 insertions(+), 140 deletions(-) diff --git a/docs/source/tutorials/04_sensors/add_sensors_on_robot.rst b/docs/source/tutorials/04_sensors/add_sensors_on_robot.rst index 3d9f40667b62..85383a876ed4 100644 --- a/docs/source/tutorials/04_sensors/add_sensors_on_robot.rst +++ b/docs/source/tutorials/04_sensors/add_sensors_on_robot.rst @@ -93,11 +93,11 @@ Height scanner The height-scanner is implemented as a virtual sensor using the NVIDIA Warp ray-casting kernels. Through the :class:`sensors.RayCasterCfg`, we can specify the pattern of rays to cast and the -meshes against which to cast the rays. Since they are virtual sensors, there is no corresponding -prim created in the scene for them. Instead they are attached to a prim in the scene, which is -used to specify the location of the sensor. +meshes against which to cast the rays. By default, :attr:`~sensors.RayCasterCfg.spawn` creates +a plain USD Xform at :attr:`~sensors.RayCasterCfg.prim_path` to serve as the sensor's +attachment frame, similar to how :class:`sensors.CameraCfg` spawns a Camera prim. -For this tutorial, the ray-cast based height scanner is attached to the base frame of the robot. +For this tutorial, the ray-cast based height scanner is attached under the base frame of the robot. The pattern of rays is specified using the :attr:`~sensors.RayCasterCfg.pattern` attribute. For a uniform grid pattern, we specify the pattern using :class:`~sensors.patterns.GridPatternCfg`. Since we only care about the height information, we do not need to consider the roll and pitch diff --git a/scripts/demos/sensors/multi_mesh_raycaster.py b/scripts/demos/sensors/multi_mesh_raycaster.py index 1a586a8aa2d1..e03ff282f2e5 100644 --- a/scripts/demos/sensors/multi_mesh_raycaster.py +++ b/scripts/demos/sensors/multi_mesh_raycaster.py @@ -80,7 +80,7 @@ if args_cli.asset_type == "allegro_hand": asset_cfg = ALLEGRO_HAND_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") ray_caster_cfg = MultiMeshRayCasterCfg( - prim_path="{ENV_REGEX_NS}/Robot", + prim_path="{ENV_REGEX_NS}/Robot/raycaster", update_period=1 / 60, offset=MultiMeshRayCasterCfg.OffsetCfg(pos=(0, -0.1, 0.3)), mesh_prim_paths=[ @@ -101,7 +101,7 @@ elif args_cli.asset_type == "anymal_d": asset_cfg = ANYMAL_D_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") ray_caster_cfg = MultiMeshRayCasterCfg( - prim_path="{ENV_REGEX_NS}/Robot/base", + prim_path="{ENV_REGEX_NS}/Robot/base/raycaster", update_period=1 / 60, offset=MultiMeshRayCasterCfg.OffsetCfg(pos=(0, -0.1, 0.3)), mesh_prim_paths=[ @@ -160,7 +160,7 @@ init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, 0.0, 2.0)), ) ray_caster_cfg = MultiMeshRayCasterCfg( - prim_path="{ENV_REGEX_NS}/Object", + prim_path="{ENV_REGEX_NS}/Object/raycaster", update_period=1 / 60, offset=MultiMeshRayCasterCfg.OffsetCfg(pos=(0, 0.0, 0.6)), mesh_prim_paths=[ diff --git a/scripts/demos/sensors/raycaster_sensor.py b/scripts/demos/sensors/raycaster_sensor.py index 43c6eb6911e0..a9cfed141f19 100644 --- a/scripts/demos/sensors/raycaster_sensor.py +++ b/scripts/demos/sensors/raycaster_sensor.py @@ -62,7 +62,8 @@ class RaycasterSensorSceneCfg(InteractiveSceneCfg): robot = ANYMAL_C_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") ray_caster = RayCasterCfg( - prim_path="{ENV_REGEX_NS}/Robot/base/lidar_cage", + prim_path="{ENV_REGEX_NS}/Robot/base/raycaster", + spawn=sim_utils.RayCasterXformCfg(), update_period=1 / 60, offset=RayCasterCfg.OffsetCfg(pos=(0, 0, 0.5)), mesh_prim_paths=["/World/Ground"], diff --git a/scripts/tutorials/03_envs/create_quadruped_base_env.py b/scripts/tutorials/03_envs/create_quadruped_base_env.py index 78f5b75ec5f8..fb51e3991be5 100644 --- a/scripts/tutorials/03_envs/create_quadruped_base_env.py +++ b/scripts/tutorials/03_envs/create_quadruped_base_env.py @@ -103,7 +103,8 @@ class MySceneCfg(InteractiveSceneCfg): # sensors height_scanner = RayCasterCfg( - prim_path="{ENV_REGEX_NS}/Robot/base", + prim_path="{ENV_REGEX_NS}/Robot/base/raycaster", + spawn=sim_utils.RayCasterXformCfg(), offset=RayCasterCfg.OffsetCfg(pos=(0.0, 0.0, 20.0)), ray_alignment="yaw", pattern_cfg=patterns.GridPatternCfg(resolution=0.1, size=[1.6, 1.0]), diff --git a/scripts/tutorials/04_sensors/add_sensors_on_robot.py b/scripts/tutorials/04_sensors/add_sensors_on_robot.py index 31f9a2bcefcb..86c602cd55a1 100644 --- a/scripts/tutorials/04_sensors/add_sensors_on_robot.py +++ b/scripts/tutorials/04_sensors/add_sensors_on_robot.py @@ -82,7 +82,8 @@ class SensorsSceneCfg(InteractiveSceneCfg): offset=CameraCfg.OffsetCfg(pos=(0.510, 0.0, 0.015), rot=(0.5, -0.5, 0.5, -0.5), convention="ros"), ) height_scanner = RayCasterCfg( - prim_path="{ENV_REGEX_NS}/Robot/base", + prim_path="{ENV_REGEX_NS}/Robot/base/raycaster", + spawn=sim_utils.RayCasterXformCfg(), update_period=0.02, offset=RayCasterCfg.OffsetCfg(pos=(0.0, 0.0, 20.0)), ray_alignment="yaw", diff --git a/scripts/tutorials/04_sensors/run_ray_caster.py b/scripts/tutorials/04_sensors/run_ray_caster.py index 3e46ef1a08fd..a1238a3ef6ea 100644 --- a/scripts/tutorials/04_sensors/run_ray_caster.py +++ b/scripts/tutorials/04_sensors/run_ray_caster.py @@ -45,7 +45,7 @@ def define_sensor() -> RayCaster: """Defines the ray-caster sensor to add to the scene.""" # Create a ray-caster sensor ray_caster_cfg = RayCasterCfg( - prim_path="/World/Origin.*/ball", + prim_path="/World/Origin.*/ball/raycaster", mesh_prim_paths=["/World/ground"], pattern_cfg=patterns.GridPatternCfg(resolution=0.1, size=(2.0, 2.0)), ray_alignment="yaw", diff --git a/source/isaaclab/config/extension.toml b/source/isaaclab/config/extension.toml index 4a6a460ae0c0..bce11c36676d 100644 --- a/source/isaaclab/config/extension.toml +++ b/source/isaaclab/config/extension.toml @@ -1,7 +1,7 @@ [package] # Note: Semantic Versioning is used: https://semver.org/ -version = "4.6.1" +version = "4.6.2" # Description title = "Isaac Lab framework for Robot Learning" diff --git a/source/isaaclab/docs/CHANGELOG.rst b/source/isaaclab/docs/CHANGELOG.rst index d9eb2915800e..f381bbc9f492 100644 --- a/source/isaaclab/docs/CHANGELOG.rst +++ b/source/isaaclab/docs/CHANGELOG.rst @@ -1,6 +1,30 @@ Changelog --------- +4.6.2 (2026-04-14) +~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Changed :class:`~isaaclab.sensors.RayCaster` to spawn its own non-physics Xform prim via + the new :attr:`~isaaclab.sensors.RayCasterCfg.spawn` attribute. ``prim_path`` should now + point to a child under the parent link (e.g. ``{ENV_REGEX_NS}/Robot/base/raycaster``). + +Deprecated +^^^^^^^^^^ + +* Deprecated passing a ``prim_path`` with ``ArticulationRootAPI`` or ``RigidBodyAPI`` to + :class:`~isaaclab.sensors.RayCasterCfg`. The path is automatically extended with + ``/raycaster``; users should migrate to the child-path convention. + +Removed +^^^^^^^ + +* Removed :attr:`~isaaclab.sensors.RayCasterCfg.attach_yaw_only` (deprecated since 2.1.1). + Use ``ray_alignment="yaw"`` or ``ray_alignment="base"`` instead. + + 4.6.1 (2026-04-14) ~~~~~~~~~~~~~~~~~~ diff --git a/source/isaaclab/isaaclab/scene/interactive_scene_cfg.py b/source/isaaclab/isaaclab/scene/interactive_scene_cfg.py index f4328324152c..291585de34f8 100644 --- a/source/isaaclab/isaaclab/scene/interactive_scene_cfg.py +++ b/source/isaaclab/isaaclab/scene/interactive_scene_cfg.py @@ -50,7 +50,8 @@ class MySceneCfg(InteractiveSceneCfg): # sensor - ray caster attached to the base of robot 1 that scans the ground height_scanner = RayCasterCfg( - prim_path="{ENV_REGEX_NS}/Robot_1/base", + prim_path="{ENV_REGEX_NS}/Robot_1/base/raycaster", + spawn=sim_utils.RayCasterXformCfg(), offset=RayCasterCfg.OffsetCfg(pos=(0.0, 0.0, 20.0)), ray_alignment="yaw", pattern_cfg=GridPatternCfg(resolution=0.1, size=[1.6, 1.0]), diff --git a/source/isaaclab/isaaclab/sensors/__init__.py b/source/isaaclab/isaaclab/sensors/__init__.py index 1128b11c1202..faefd42c743b 100644 --- a/source/isaaclab/isaaclab/sensors/__init__.py +++ b/source/isaaclab/isaaclab/sensors/__init__.py @@ -26,7 +26,7 @@ +---------------------+---------------------------+---------------------------------------------------------------+ | Contact Sensor | /World/robot/feet_* | Leaf is available and checks if the schema exists | +---------------------+---------------------------+---------------------------------------------------------------+ -| Ray Caster | /World/robot/base | Leaf exists and is a physics body (Articulation / Rigid Body) | +| Ray Caster | /World/robot/base/raycast | ``spawn`` creates an Xform leaf; else the leaf must exist | +---------------------+---------------------------+---------------------------------------------------------------+ | Frame Transformer | /World/robot/base | Leaf exists and is a physics body (Articulation / Rigid Body) | +---------------------+---------------------------+---------------------------------------------------------------+ diff --git a/source/isaaclab/isaaclab/sensors/ray_caster/multi_mesh_ray_caster.py b/source/isaaclab/isaaclab/sensors/ray_caster/multi_mesh_ray_caster.py index 564b358c8609..9d708a7be1c1 100644 --- a/source/isaaclab/isaaclab/sensors/ray_caster/multi_mesh_ray_caster.py +++ b/source/isaaclab/isaaclab/sensors/ray_caster/multi_mesh_ray_caster.py @@ -14,8 +14,6 @@ import trimesh import warp as wp -import omni.physics.tensors.impl.api as physx - import isaaclab.sim as sim_utils from isaaclab.sim.views import BaseXformPrimView from isaaclab.utils.math import matrix_from_quat, quat_mul @@ -78,7 +76,7 @@ class MultiMeshRayCaster(RayCaster): mesh_offsets: dict[str, tuple[torch.Tensor, torch.Tensor]] = {} - mesh_views: ClassVar[dict[str, BaseXformPrimView | physx.ArticulationView | physx.RigidBodyView]] = {} + mesh_views: ClassVar[dict[str, BaseXformPrimView]] = {} """A dictionary to store mesh views for raycasting, shared across all instances. The keys correspond to the prim path for the mesh views, and values are the corresponding view objects. diff --git a/source/isaaclab/isaaclab/sensors/ray_caster/ray_cast_utils.py b/source/isaaclab/isaaclab/sensors/ray_caster/ray_cast_utils.py index d7fb69704eaa..2824420c7059 100644 --- a/source/isaaclab/isaaclab/sensors/ray_caster/ray_cast_utils.py +++ b/source/isaaclab/isaaclab/sensors/ray_caster/ray_cast_utils.py @@ -10,13 +10,11 @@ import torch import warp as wp -import omni.physics.tensors.impl.api as physx - from isaaclab.sim.views import BaseXformPrimView def obtain_world_pose_from_view( - physx_view: BaseXformPrimView | physx.ArticulationView | physx.RigidBodyView, + physx_view: BaseXformPrimView, env_ids: torch.Tensor, clone: bool = False, ) -> tuple[torch.Tensor, torch.Tensor]: @@ -30,9 +28,6 @@ def obtain_world_pose_from_view( Returns: A tuple containing the world positions and orientations of the prims. Orientation is in (x, y, z, w) format. - - Raises: - NotImplementedError: If the prim view is not of the supported type. """ indices = wp.from_torch(env_ids.to(dtype=torch.int32), dtype=wp.int32) if env_ids is not None else None pos_wp, quat_wp = physx_view.get_world_poses(indices) diff --git a/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster.py b/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster.py index 351154b78126..73335523bf01 100644 --- a/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster.py +++ b/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster.py @@ -6,6 +6,7 @@ from __future__ import annotations import logging +import warnings from collections.abc import Sequence from typing import TYPE_CHECKING, ClassVar @@ -68,8 +69,26 @@ def __init__(self, cfg: RayCasterCfg): cfg: The configuration parameters. """ RayCaster._instance_count += 1 + self._resolve_physics_prim_path(cfg) # Initialize base class super().__init__(cfg) + # Spawn the sensor Xform prim (mirrors how Camera spawns a Camera prim). + if self.cfg.spawn is not None: + spawn_target = ( + self.cfg.spawn.spawn_path + if getattr(self.cfg.spawn, "spawn_path", None) is not None + else self.cfg.prim_path + ) + self.cfg.spawn.func(spawn_target, self.cfg.spawn) + # Verify the prim exists (use spawn_path when set because env paths are + # not yet populated — clone_environments runs after this). + check_path = ( + self.cfg.spawn.spawn_path + if self.cfg.spawn is not None and getattr(self.cfg.spawn, "spawn_path", None) is not None + else self.cfg.prim_path + ) + if len(sim_utils.find_matching_prims(check_path)) == 0: + raise RuntimeError(f"Could not find prim with path {check_path!r}.") # Create empty variables for storing output data self._data = RayCasterData() @@ -132,16 +151,6 @@ def reset(self, env_ids: Sequence[int] | None = None, env_mask: wp.array | None def _initialize_impl(self): super()._initialize_impl() - # obtain global simulation view - - self._physics_sim_view = sim_utils.SimulationContext.instance().physics_manager.get_physics_sim_view() - prim = sim_utils.find_first_matching_prim(self.cfg.prim_path) - if prim is None: - available_prims = ",".join([str(p.GetPath()) for p in sim_utils.get_current_stage().Traverse()]) - raise RuntimeError( - f"Failed to find a prim at path expression: {self.cfg.prim_path}. Available prims: {available_prims}" - ) - self._view, self._offset = self._obtain_trackable_prim_view(self.cfg.prim_path) # load the meshes by parsing the stage @@ -241,21 +250,6 @@ def _update_ray_infos(self, env_ids: Sequence[int]): self._data.pos_w[env_ids] = pos_w self._data.quat_w[env_ids] = quat_w - # check if user provided attach_yaw_only flag - if self.cfg.attach_yaw_only is not None: - msg = ( - "Raycaster attribute 'attach_yaw_only' property will be deprecated in a future release." - " Please use the parameter 'ray_alignment' instead." - ) - # set ray alignment to yaw - if self.cfg.attach_yaw_only: - self.cfg.ray_alignment = "yaw" - msg += " Setting ray_alignment to 'yaw'." - else: - self.cfg.ray_alignment = "base" - msg += " Setting ray_alignment to 'base'." - # log the warning - logger.warning(msg) # ray cast based on the sensor poses if self.cfg.ray_alignment == "world": # apply horizontal drift to ray starting position in ray caster frame @@ -332,77 +326,73 @@ def _debug_vis_callback(self, event): Internal Helpers. """ + @staticmethod + def _resolve_physics_prim_path(cfg: RayCasterCfg) -> None: + """Auto-detect legacy ``prim_path`` pointing at a physics prim and redirect to a child Xform. + + Before this refactor, ``prim_path`` was expected to point at an existing link prim (e.g. + ``{ENV_REGEX_NS}/Robot/base``) that typically carried an ``ArticulationRootAPI`` or + ``RigidBodyAPI``. The new convention requires ``prim_path`` to be a **non-physics** child + (e.g. ``{ENV_REGEX_NS}/Robot/base/raycaster``) where a plain Xform is spawned. + + This helper preserves backward compatibility: when ``spawn`` is set and the resolved + ``prim_path`` points at a prim that already has a physics API, the path is automatically + extended with ``/raycaster`` and a deprecation warning is emitted. + """ + if cfg.spawn is None: + return + + # Determine which path to inspect. spawn_path is set by InteractiveScene when + # cloning; fall back to prim_path for standalone usage. + resolve_path = cfg.spawn.spawn_path if getattr(cfg.spawn, "spawn_path", None) is not None else cfg.prim_path + + prim = sim_utils.find_first_matching_prim(resolve_path) + if prim is None or not prim.IsValid(): + return + + is_physics_prim = prim.HasAPI(UsdPhysics.ArticulationRootAPI) or prim.HasAPI(UsdPhysics.RigidBodyAPI) + if not is_physics_prim: + return + + child_name = "raycaster" + warnings.warn( + f"RayCasterCfg.prim_path {cfg.prim_path!r} resolves to a prim with a physics API" + f" (ArticulationRootAPI or RigidBodyAPI). This usage is deprecated. Please set" + f" prim_path to a non-physics child such as '{cfg.prim_path}/{child_name}'." + " The path has been automatically adjusted for this session.", + DeprecationWarning, + stacklevel=4, + ) + + cfg.prim_path = f"{cfg.prim_path}/{child_name}" + # Also update spawn_path so the Xform is created at the correct location. + if getattr(cfg.spawn, "spawn_path", None) is not None: + cfg.spawn.spawn_path = f"{cfg.spawn.spawn_path}/{child_name}" + def _obtain_trackable_prim_view( self, target_prim_path: str - ) -> tuple[BaseXformPrimView | any, tuple[torch.Tensor, torch.Tensor]]: - """Obtain a prim view that can be used to track the pose of the parget prim. + ) -> tuple[BaseXformPrimView, tuple[torch.Tensor, torch.Tensor]]: + """Obtain a prim view that can be used to track the pose of the target prim. - The target prim path is a regex expression that matches one or more mesh prims. While we can track its - pose directly using XFormPrim, this is not efficient and can be slow. Instead, we create a prim view - using the physics simulation view, which provides a more efficient way to track the pose of the mesh prims. + Creates an :class:`XformPrimView` for the target prim path, which dispatches to the + correct backend implementation automatically (Fabric for PhysX, site-based for Newton). - The function additionally resolves the relative pose between the mesh and its corresponding physics prim. - This is especially useful if the mesh is not directly parented to the physics prim. + The function additionally resolves the relative pose between the mesh and the view prim. Args: target_prim_path: The target prim path to obtain the prim view for. Returns: - A tuple containing: - - - An XFormPrim or a physics prim view (ArticulationView or RigidBodyView). - - A tuple containing the positions and orientations of the mesh prims in the physics prim frame. - + A tuple containing the :class:`XformPrimView` and the relative offsets + (positions, orientations) of each matched prim. """ + prim_view = XformPrimView(target_prim_path, device=self._device, stage=self.stage) - mesh_prim = sim_utils.find_first_matching_prim(target_prim_path) - current_prim = mesh_prim - current_path_expr = target_prim_path - - prim_view = None - - while prim_view is None: - # TODO: Need to handle the case where API is present but it is disabled - if current_prim.HasAPI(UsdPhysics.ArticulationRootAPI): - prim_view = self._physics_sim_view.create_articulation_view(current_path_expr.replace(".*", "*")) - logger.info(f"Created articulation view for mesh prim at path: {target_prim_path}") - break - - # TODO: Need to handle the case where API is present but it is disabled - if current_prim.HasAPI(UsdPhysics.RigidBodyAPI): - prim_view = self._physics_sim_view.create_rigid_body_view(current_path_expr.replace(".*", "*")) - logger.info(f"Created rigid body view for mesh prim at path: {target_prim_path}") - break - - new_root_prim = current_prim.GetParent() - current_path_expr = current_path_expr.rsplit("/", 1)[0] - if not new_root_prim.IsValid(): - prim_view = XformPrimView(target_prim_path, device=self._device, stage=self.stage) - current_path_expr = target_prim_path - logger.warning( - f"The prim at path {target_prim_path} which is used for raycasting is not a physics prim." - " Defaulting to XFormPrim. \n The pose of the mesh will most likely not" - " be updated correctly when running in headless mode and position lookups will be much slower. \n" - " If possible, ensure that the mesh or its parent is a physics prim (rigid body or articulation)." - ) - break - - # switch the current prim to the parent prim - current_prim = new_root_prim - - # obtain the relative transforms between target prim and the view prims mesh_prims = sim_utils.find_matching_prims(target_prim_path) - view_prims = sim_utils.find_matching_prims(current_path_expr) - if len(mesh_prims) != len(view_prims): - raise RuntimeError( - f"The number of mesh prims ({len(mesh_prims)}) does not match the number of physics prims" - f" ({len(view_prims)})Please specify the correct mesh and physics prim paths more" - " specifically in your target expressions." - ) positions = [] quaternions = [] - for mesh_prim, view_prim in zip(mesh_prims, view_prims): - pos, orientation = sim_utils.resolve_prim_pose(mesh_prim, view_prim) + for mesh_prim in mesh_prims: + pos, orientation = sim_utils.resolve_prim_pose(mesh_prim, mesh_prim) positions.append(torch.tensor(pos, dtype=torch.float32, device=self.device)) quaternions.append(torch.tensor(orientation, dtype=torch.float32, device=self.device)) diff --git a/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster_cfg.py b/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster_cfg.py index 7d91e446adac..66fa97ba330c 100644 --- a/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster_cfg.py +++ b/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster_cfg.py @@ -12,6 +12,7 @@ from isaaclab.markers import VisualizationMarkersCfg from isaaclab.markers.config import RAY_CASTER_MARKER_CFG +from isaaclab.sim.spawners.sensors.sensors_cfg import RayCasterXformCfg from isaaclab.utils import configclass from ..sensor_base_cfg import SensorBaseCfg @@ -36,6 +37,26 @@ class OffsetCfg: class_type: type[RayCaster] | str = "{DIR}.ray_caster:RayCaster" + spawn: RayCasterXformCfg | None = RayCasterXformCfg() + """Spawn configuration for the sensor Xform prim. + + A plain USD Xform is created at :attr:`prim_path` before initialization, matching the + pattern used by :class:`~isaaclab.sensors.camera.camera_cfg.CameraCfg` (which spawns a + Camera prim). The :attr:`prim_path` should therefore be a **new** child path under the + desired parent link (e.g. ``{ENV_REGEX_NS}/Robot/base/raycaster``), **not** the link + itself. + + .. deprecated:: + Passing a :attr:`prim_path` that points at a prim with + ``ArticulationRootAPI`` or ``RigidBodyAPI`` (e.g. + ``{ENV_REGEX_NS}/Robot/base``) is deprecated. The sensor will + automatically append ``/raycaster`` and emit a + :class:`DeprecationWarning`, but users should migrate to the new + child-path convention. + + If ``None``, the prim at :attr:`prim_path` must already exist on the USD stage. + """ + mesh_prim_paths: list[str] = MISSING """The list of mesh primitive paths to ray cast against. @@ -47,22 +68,6 @@ class OffsetCfg: offset: OffsetCfg = OffsetCfg() """The offset pose of the sensor's frame from the sensor's parent frame. Defaults to identity.""" - attach_yaw_only: bool | None = None - """Whether the rays' starting positions and directions only track the yaw orientation. - Defaults to None, which doesn't raise a warning of deprecated usage. - - This is useful for ray-casting height maps, where only yaw rotation is needed. - - .. deprecated:: 2.1.1 - - This attribute is deprecated and will be removed in the future. Please use - :attr:`ray_alignment` instead. - - To get the same behavior as setting this parameter to ``True`` or ``False``, set - :attr:`ray_alignment` to ``"yaw"`` or "base" respectively. - - """ - ray_alignment: Literal["base", "yaw", "world"] = "base" """Specify in what frame the rays are projected onto the ground. Default is "base". diff --git a/source/isaaclab/isaaclab/sim/__init__.pyi b/source/isaaclab/isaaclab/sim/__init__.pyi index ea20e7fcc6cf..054c842bd2b4 100644 --- a/source/isaaclab/isaaclab/sim/__init__.pyi +++ b/source/isaaclab/isaaclab/sim/__init__.pyi @@ -94,8 +94,10 @@ __all__ = [ "MeshCylinderCfg", "MeshSphereCfg", "spawn_camera", + "spawn_ray_caster_xform", "FisheyeCameraCfg", "PinholeCameraCfg", + "RayCasterXformCfg", "spawn_capsule", "spawn_cone", "spawn_cuboid", @@ -262,8 +264,10 @@ from .spawners import ( MeshCylinderCfg, MeshSphereCfg, spawn_camera, + spawn_ray_caster_xform, FisheyeCameraCfg, PinholeCameraCfg, + RayCasterXformCfg, spawn_capsule, spawn_cone, spawn_cuboid, diff --git a/source/isaaclab/isaaclab/sim/spawners/__init__.pyi b/source/isaaclab/isaaclab/sim/spawners/__init__.pyi index 78d2c19021ee..76288197153e 100644 --- a/source/isaaclab/isaaclab/sim/spawners/__init__.pyi +++ b/source/isaaclab/isaaclab/sim/spawners/__init__.pyi @@ -47,8 +47,10 @@ __all__ = [ "MeshCylinderCfg", "MeshSphereCfg", "spawn_camera", + "spawn_ray_caster_xform", "FisheyeCameraCfg", "PinholeCameraCfg", + "RayCasterXformCfg", "spawn_capsule", "spawn_cone", "spawn_cuboid", @@ -114,7 +116,7 @@ from .meshes import ( MeshCylinderCfg, MeshSphereCfg, ) -from .sensors import spawn_camera, FisheyeCameraCfg, PinholeCameraCfg +from .sensors import spawn_camera, spawn_ray_caster_xform, FisheyeCameraCfg, PinholeCameraCfg, RayCasterXformCfg from .shapes import ( spawn_capsule, spawn_cone, diff --git a/source/isaaclab/isaaclab/sim/spawners/sensors/__init__.pyi b/source/isaaclab/isaaclab/sim/spawners/sensors/__init__.pyi index 46d24596932c..ed28f0ee314c 100644 --- a/source/isaaclab/isaaclab/sim/spawners/sensors/__init__.pyi +++ b/source/isaaclab/isaaclab/sim/spawners/sensors/__init__.pyi @@ -5,9 +5,11 @@ __all__ = [ "spawn_camera", + "spawn_ray_caster_xform", "FisheyeCameraCfg", "PinholeCameraCfg", + "RayCasterXformCfg", ] -from .sensors import spawn_camera -from .sensors_cfg import FisheyeCameraCfg, PinholeCameraCfg +from .sensors import spawn_camera, spawn_ray_caster_xform +from .sensors_cfg import FisheyeCameraCfg, PinholeCameraCfg, RayCasterXformCfg diff --git a/source/isaaclab/isaaclab/sim/spawners/sensors/sensors.py b/source/isaaclab/isaaclab/sim/spawners/sensors/sensors.py index 4eb70005e487..df05144455fa 100644 --- a/source/isaaclab/isaaclab/sim/spawners/sensors/sensors.py +++ b/source/isaaclab/isaaclab/sim/spawners/sensors/sensors.py @@ -145,3 +145,53 @@ def spawn_camera( prim.GetAttribute(prim_prop_name).Set(param_value) # return the prim return prim + + +@clone +def spawn_ray_caster_xform( + prim_path: str, + cfg: sensors_cfg.RayCasterXformCfg, + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, + **kwargs, +) -> Usd.Prim: + """Create a USD Xform prim for a ray-cast sensor attachment site. + + The function creates a plain ``Xform`` with the given local pose under its parent. It is + intended for :class:`~isaaclab.sensors.ray_caster.ray_caster.RayCaster` when + :attr:`~isaaclab.sensors.ray_caster.ray_caster_cfg.RayCasterCfg.spawn` is set, mirroring how + :func:`spawn_camera` creates camera prims before :class:`~isaaclab.sim.views.XformPrimView` + is constructed. + + .. note:: + This function is decorated with :func:`clone` that resolves prim path into list of paths + if the input prim path is a regex pattern. Visibility and semantic tags from + :class:`~isaaclab.sim.spawners.spawner_cfg.SpawnerCfg` are applied by the clone wrapper. + + Args: + prim_path: The prim path or pattern to spawn the asset at. + cfg: The configuration instance. + translation: Local translation (x, y, z) [m] w.r.t. the parent prim. Defaults to None + (origin). + orientation: Local orientation as quaternion (x, y, z, w) w.r.t. the parent prim. + Defaults to None (identity). + **kwargs: Additional keyword arguments, like ``clone_in_fabric``. + + Returns: + The created USD prim. + + Raises: + ValueError: If a prim already exists at the given path. + """ + stage = get_current_stage() + if not stage.GetPrimAtPath(prim_path).IsValid(): + prim = create_prim( + prim_path, + "Xform", + translation=translation, + orientation=orientation, + stage=stage, + ) + else: + raise ValueError(f"A prim already exists at path: '{prim_path}'.") + return prim diff --git a/source/isaaclab/isaaclab/sim/spawners/sensors/sensors_cfg.py b/source/isaaclab/isaaclab/sim/spawners/sensors/sensors_cfg.py index 56c9102cf1f1..34c6dad14539 100644 --- a/source/isaaclab/isaaclab/sim/spawners/sensors/sensors_cfg.py +++ b/source/isaaclab/isaaclab/sim/spawners/sensors/sensors_cfg.py @@ -222,3 +222,16 @@ class FisheyeCameraCfg(PinholeCameraCfg): fisheye_polynomial_f: float = 0.0 """Sixth component of fisheye polynomial. Defaults to 0.0.""" + + +@configclass +class RayCasterXformCfg(SpawnerCfg): + """Configuration for spawning a plain USD Xform used as a ray-cast sensor frame. + + The spawned prim has no rigid body or collision API. This matches the pattern used by + :class:`~isaaclab.sensors.camera.camera_cfg.CameraCfg`, where the sensor prim is created at + runtime so :class:`~isaaclab.sim.views.XformPrimView` tracks a non-physics site (required for + backends such as Newton where link prims are physics-owned). + """ + + func: Callable | str = "{DIR}.sensors:spawn_ray_caster_xform" diff --git a/source/isaaclab/test/envs/check_manager_based_env_anymal_locomotion.py b/source/isaaclab/test/envs/check_manager_based_env_anymal_locomotion.py index 07dc3d73bd14..e613c21a261f 100644 --- a/source/isaaclab/test/envs/check_manager_based_env_anymal_locomotion.py +++ b/source/isaaclab/test/envs/check_manager_based_env_anymal_locomotion.py @@ -90,7 +90,8 @@ class MySceneCfg(InteractiveSceneCfg): # sensors height_scanner = RayCasterCfg( - prim_path="{ENV_REGEX_NS}/Robot/base", + prim_path="{ENV_REGEX_NS}/Robot/base/raycaster", + spawn=sim_utils.RayCasterXformCfg(), offset=RayCasterCfg.OffsetCfg(pos=(0.0, 0.0, 20.0)), ray_alignment="yaw", pattern_cfg=patterns.GridPatternCfg(resolution=0.1, size=[1.6, 1.0]), diff --git a/source/isaaclab/test/markers/check_markers_visibility.py b/source/isaaclab/test/markers/check_markers_visibility.py index 98dbee8ddcd7..7e221b76ad21 100644 --- a/source/isaaclab/test/markers/check_markers_visibility.py +++ b/source/isaaclab/test/markers/check_markers_visibility.py @@ -67,7 +67,8 @@ class SensorsSceneCfg(InteractiveSceneCfg): # sensors height_scanner = RayCasterCfg( - prim_path="{ENV_REGEX_NS}/Robot/base", + prim_path="{ENV_REGEX_NS}/Robot/base/raycaster", + spawn=sim_utils.RayCasterXformCfg(), update_period=0.02, offset=RayCasterCfg.OffsetCfg(pos=(0.0, 0.0, 20.0)), ray_alignment="yaw", diff --git a/source/isaaclab/test/scene/check_interactive_scene.py b/source/isaaclab/test/scene/check_interactive_scene.py index 5b2463b315a9..dd42572744d5 100644 --- a/source/isaaclab/test/scene/check_interactive_scene.py +++ b/source/isaaclab/test/scene/check_interactive_scene.py @@ -60,7 +60,8 @@ class MySceneCfg(InteractiveSceneCfg): # sensor - ray caster attached to the base of robot 1 that scans the ground height_scanner = RayCasterCfg( - prim_path="{ENV_REGEX_NS}/Robot_1/base", + prim_path="{ENV_REGEX_NS}/Robot_1/base/raycaster", + spawn=sim_utils.RayCasterXformCfg(), offset=RayCasterCfg.OffsetCfg(pos=(0.0, 0.0, 20.0)), ray_alignment="yaw", pattern_cfg=patterns.GridPatternCfg(resolution=0.1, size=[1.6, 1.0]), diff --git a/source/isaaclab/test/sensors/check_multi_mesh_ray_caster.py b/source/isaaclab/test/sensors/check_multi_mesh_ray_caster.py index 03221e1ce366..4a5a5f8de7de 100644 --- a/source/isaaclab/test/sensors/check_multi_mesh_ray_caster.py +++ b/source/isaaclab/test/sensors/check_multi_mesh_ray_caster.py @@ -139,10 +139,10 @@ def main(): ) # Create a ray-caster sensor ray_caster_cfg = MultiMeshRayCasterCfg( - prim_path="/World/envs/env_.*/ball", + prim_path="/World/envs/env_.*/ball/raycaster", mesh_prim_paths=mesh_targets, pattern_cfg=patterns.GridPatternCfg(resolution=0.1, size=(1.6, 1.0)), - attach_yaw_only=True, + ray_alignment="yaw", debug_vis=not args_cli.headless, ) ray_caster = MultiMeshRayCaster(cfg=ray_caster_cfg) diff --git a/source/isaaclab/test/sensors/check_ray_caster.py b/source/isaaclab/test/sensors/check_ray_caster.py index 5e1e88472ae5..6b9f6931152b 100644 --- a/source/isaaclab/test/sensors/check_ray_caster.py +++ b/source/isaaclab/test/sensors/check_ray_caster.py @@ -108,7 +108,7 @@ def main(): # Create a ray-caster sensor ray_caster_cfg = RayCasterCfg( - prim_path="/World/envs/env_.*/ball", + prim_path="/World/envs/env_.*/ball/raycaster", mesh_prim_paths=["/World/ground"], pattern_cfg=patterns.GridPatternCfg(resolution=0.1, size=(1.6, 1.0)), ray_alignment="yaw", diff --git a/source/isaaclab_newton/isaaclab_newton/sim/views/newton_site_xform_prim_view.py b/source/isaaclab_newton/isaaclab_newton/sim/views/newton_site_xform_prim_view.py index b295fe019edc..af8aa30d8aeb 100644 --- a/source/isaaclab_newton/isaaclab_newton/sim/views/newton_site_xform_prim_view.py +++ b/source/isaaclab_newton/isaaclab_newton/sim/views/newton_site_xform_prim_view.py @@ -14,6 +14,7 @@ from pxr import Gf, Usd, UsdGeom import isaaclab.sim as sim_utils +from isaaclab.physics import PhysicsEvent from isaaclab.sim.views.base_xform_prim_view import BaseXformPrimView from isaaclab_newton.physics.newton_manager import NewtonManager @@ -384,6 +385,19 @@ def __init__(self, prim_path: str, device: str = "cpu", stage: Usd.Stage | None self._prims: list[Usd.Prim] = sim_utils.find_matching_prims(prim_path, stage=stage) model = NewtonManager.get_model() + if model is not None: + self._initialize_impl(model) + else: + self._physics_ready_handle = NewtonManager.register_callback( + self._on_physics_ready, PhysicsEvent.PHYSICS_READY, name=f"site_view_{prim_path}" + ) + + def _on_physics_ready(self, _event) -> None: + """Callback invoked when the Newton model becomes available.""" + self._initialize_impl(NewtonManager.get_model()) + + def _initialize_impl(self, model) -> None: + """Resolve USD prims to Newton body indices and allocate GPU buffers.""" body_labels = list(model.body_label) body_label_set = set(body_labels) body_label_to_idx = {path: idx for idx, path in enumerate(body_labels)} @@ -436,6 +450,7 @@ def __init__(self, prim_path: str, device: str = "cpu", stage: Usd.Stage | None parent_bodies.append(pb_idx) parent_locals.append(pb_local) + device = self._device self._site_body = wp.array(site_bodies, dtype=wp.int32, device=device) self._site_local = wp.array( [wp.transform(*x) for x in site_locals], diff --git a/source/isaaclab_tasks/config/extension.toml b/source/isaaclab_tasks/config/extension.toml index 061921978b4e..1a579ed0ef48 100644 --- a/source/isaaclab_tasks/config/extension.toml +++ b/source/isaaclab_tasks/config/extension.toml @@ -1,7 +1,7 @@ [package] # Note: Semantic Versioning is used: https://semver.org/ -version = "1.5.21" +version = "1.5.22" # Description title = "Isaac Lab Environments" diff --git a/source/isaaclab_tasks/docs/CHANGELOG.rst b/source/isaaclab_tasks/docs/CHANGELOG.rst index 0aa31de28c6a..004efe657353 100644 --- a/source/isaaclab_tasks/docs/CHANGELOG.rst +++ b/source/isaaclab_tasks/docs/CHANGELOG.rst @@ -1,6 +1,18 @@ Changelog --------- +1.5.22 (2026-04-14) +~~~~~~~~~~~~~~~~~~~ + +Changed +^^^^^^^ + +* Updated locomotion :class:`~isaaclab.sensors.ray_caster.ray_caster_cfg.RayCasterCfg` + height-scanner defaults to spawn a ``raycaster`` Xform child under the robot attachment link + (using :class:`~isaaclab.sim.spawners.sensors.sensors_cfg.RayCasterXformCfg`) so the sensor + works with Newton site-based :class:`~isaaclab.sim.views.XformPrimView` tracking. + + 1.5.21 (2026-04-13) ~~~~~~~~~~~~~~~~~~~ diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/anymal_c/anymal_c_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/direct/anymal_c/anymal_c_env_cfg.py index 09e4edcb55a7..99125d824035 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/anymal_c/anymal_c_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/anymal_c/anymal_c_env_cfg.py @@ -139,7 +139,8 @@ class AnymalCRoughEnvCfg(AnymalCFlatEnvCfg): # we add a height scanner for perceptive locomotion height_scanner = RayCasterCfg( - prim_path="/World/envs/env_.*/Robot/base", + prim_path="/World/envs/env_.*/Robot/base/raycaster", + spawn=sim_utils.RayCasterXformCfg(), offset=RayCasterCfg.OffsetCfg(pos=(0.0, 0.0, 20.0)), ray_alignment="yaw", pattern_cfg=patterns.GridPatternCfg(resolution=0.1, size=[1.6, 1.0]), diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/a1/rough_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/a1/rough_env_cfg.py index 6ee9dbe78505..244fd39ceb9b 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/a1/rough_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/a1/rough_env_cfg.py @@ -63,7 +63,7 @@ def __post_init__(self): super().__post_init__() self.scene.robot = UNITREE_A1_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") - self.scene.height_scanner.prim_path = "{ENV_REGEX_NS}/Robot/trunk" + self.scene.height_scanner.prim_path = "{ENV_REGEX_NS}/Robot/trunk/raycaster" # scale down the terrains because the robot is small self.scene.terrain.terrain_generator.sub_terrains["boxes"].grid_height_range = (0.025, 0.1) self.scene.terrain.terrain_generator.sub_terrains["random_rough"].noise_range = (0.01, 0.06) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/cassie/rough_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/cassie/rough_env_cfg.py index 5bd6ba28684f..197ab6f7f22a 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/cassie/rough_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/cassie/rough_env_cfg.py @@ -98,7 +98,7 @@ def __post_init__(self): super().__post_init__() # scene self.scene.robot = CASSIE_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") - self.scene.height_scanner.prim_path = "{ENV_REGEX_NS}/Robot/pelvis" + self.scene.height_scanner.prim_path = "{ENV_REGEX_NS}/Robot/pelvis/raycaster" # actions self.actions.joint_pos.scale = 0.5 diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/digit/rough_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/digit/rough_env_cfg.py index 185e9019a7c1..2298b37b5028 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/digit/rough_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/digit/rough_env_cfg.py @@ -251,7 +251,7 @@ def __post_init__(self): # Scene self.scene.robot = DIGIT_V4_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") - self.scene.height_scanner.prim_path = "{ENV_REGEX_NS}/Robot/torso_base" + self.scene.height_scanner.prim_path = "{ENV_REGEX_NS}/Robot/torso_base/raycaster" self.scene.contact_forces.history_length = self.decimation self.scene.contact_forces.update_period = self.sim.dt self.scene.height_scanner.update_period = self.decimation * self.sim.dt diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/g1/rough_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/g1/rough_env_cfg.py index 6a7035416ab3..263eb606a939 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/g1/rough_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/g1/rough_env_cfg.py @@ -151,7 +151,7 @@ def __post_init__(self): super().__post_init__() # Scene self.scene.robot = G1_MINIMAL_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") - self.scene.height_scanner.prim_path = "{ENV_REGEX_NS}/Robot/torso_link" + self.scene.height_scanner.prim_path = "{ENV_REGEX_NS}/Robot/torso_link/raycaster" # Rewards self.rewards.lin_vel_z_l2.weight = 0.0 diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/go1/rough_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/go1/rough_env_cfg.py index eafd3da45f30..c107585e87a7 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/go1/rough_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/go1/rough_env_cfg.py @@ -63,7 +63,7 @@ def __post_init__(self): super().__post_init__() self.scene.robot = UNITREE_GO1_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") - self.scene.height_scanner.prim_path = "{ENV_REGEX_NS}/Robot/trunk" + self.scene.height_scanner.prim_path = "{ENV_REGEX_NS}/Robot/trunk/raycaster" # scale down the terrains because the robot is small self.scene.terrain.terrain_generator.sub_terrains["boxes"].grid_height_range = (0.025, 0.1) self.scene.terrain.terrain_generator.sub_terrains["random_rough"].noise_range = (0.01, 0.06) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/go2/rough_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/go2/rough_env_cfg.py index 40fbef99e27e..6987eaab9fe4 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/go2/rough_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/go2/rough_env_cfg.py @@ -74,7 +74,7 @@ def __post_init__(self): super().__post_init__() self.scene.robot = UNITREE_GO2_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") - self.scene.height_scanner.prim_path = "{ENV_REGEX_NS}/Robot/base" + self.scene.height_scanner.prim_path = "{ENV_REGEX_NS}/Robot/base/raycaster" # scale down the terrains because the robot is small self.scene.terrain.terrain_generator.sub_terrains["boxes"].grid_height_range = (0.025, 0.1) self.scene.terrain.terrain_generator.sub_terrains["random_rough"].noise_range = (0.01, 0.06) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/h1/rough_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/h1/rough_env_cfg.py index 496aa4007fef..ac369daa9d17 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/h1/rough_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/h1/rough_env_cfg.py @@ -119,7 +119,7 @@ def __post_init__(self): # Scene self.scene.robot = H1_MINIMAL_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") if self.scene.height_scanner: - self.scene.height_scanner.prim_path = "{ENV_REGEX_NS}/Robot/torso_link" + self.scene.height_scanner.prim_path = "{ENV_REGEX_NS}/Robot/torso_link/raycaster" # Rewards self.rewards.undesired_contacts = None diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/velocity_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/velocity_env_cfg.py index 81f38e756975..bd59889ce5ee 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/velocity_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/velocity_env_cfg.py @@ -75,7 +75,8 @@ class MySceneCfg(InteractiveSceneCfg): robot: ArticulationCfg = MISSING # sensors height_scanner = RayCasterCfg( - prim_path="{ENV_REGEX_NS}/Robot/base", + prim_path="{ENV_REGEX_NS}/Robot/base/raycaster", + spawn=sim_utils.RayCasterXformCfg(), offset=RayCasterCfg.OffsetCfg(pos=(0.0, 0.0, 20.0)), ray_alignment="yaw", pattern_cfg=patterns.GridPatternCfg(resolution=0.1, size=[1.6, 1.0]), From b80cb717a178c40caa3db13ce38f71fb9b267ec7 Mon Sep 17 00:00:00 2001 From: Octi Zhang Date: Fri, 10 Apr 2026 03:01:48 -0700 Subject: [PATCH 11/24] add nice deprecation messages --- .../isaaclab/sensors/ray_caster/ray_caster.py | 20 +++++++------------ .../sensors/ray_caster/ray_caster_cfg.py | 5 ++--- 2 files changed, 9 insertions(+), 16 deletions(-) diff --git a/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster.py b/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster.py index 73335523bf01..54a462ecaece 100644 --- a/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster.py +++ b/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster.py @@ -336,36 +336,30 @@ def _resolve_physics_prim_path(cfg: RayCasterCfg) -> None: (e.g. ``{ENV_REGEX_NS}/Robot/base/raycaster``) where a plain Xform is spawned. This helper preserves backward compatibility: when ``spawn`` is set and the resolved - ``prim_path`` points at a prim that already has a physics API, the path is automatically - extended with ``/raycaster`` and a deprecation warning is emitted. + path already exists as a prim with a physics API, the path is automatically extended + with ``/raycaster`` and a deprecation warning is emitted. """ if cfg.spawn is None: return - # Determine which path to inspect. spawn_path is set by InteractiveScene when - # cloning; fall back to prim_path for standalone usage. resolve_path = cfg.spawn.spawn_path if getattr(cfg.spawn, "spawn_path", None) is not None else cfg.prim_path prim = sim_utils.find_first_matching_prim(resolve_path) if prim is None or not prim.IsValid(): return - - is_physics_prim = prim.HasAPI(UsdPhysics.ArticulationRootAPI) or prim.HasAPI(UsdPhysics.RigidBodyAPI) - if not is_physics_prim: + if not (prim.HasAPI(UsdPhysics.ArticulationRootAPI) or prim.HasAPI(UsdPhysics.RigidBodyAPI)): return child_name = "raycaster" - warnings.warn( + msg = ( f"RayCasterCfg.prim_path {cfg.prim_path!r} resolves to a prim with a physics API" f" (ArticulationRootAPI or RigidBodyAPI). This usage is deprecated. Please set" f" prim_path to a non-physics child such as '{cfg.prim_path}/{child_name}'." - " The path has been automatically adjusted for this session.", - DeprecationWarning, - stacklevel=4, + " The path has been automatically adjusted for this session." ) - + logger.warning(msg) + warnings.warn(msg, FutureWarning, stacklevel=4) cfg.prim_path = f"{cfg.prim_path}/{child_name}" - # Also update spawn_path so the Xform is created at the correct location. if getattr(cfg.spawn, "spawn_path", None) is not None: cfg.spawn.spawn_path = f"{cfg.spawn.spawn_path}/{child_name}" diff --git a/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster_cfg.py b/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster_cfg.py index 66fa97ba330c..b8060138e4ce 100644 --- a/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster_cfg.py +++ b/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster_cfg.py @@ -50,9 +50,8 @@ class OffsetCfg: Passing a :attr:`prim_path` that points at a prim with ``ArticulationRootAPI`` or ``RigidBodyAPI`` (e.g. ``{ENV_REGEX_NS}/Robot/base``) is deprecated. The sensor will - automatically append ``/raycaster`` and emit a - :class:`DeprecationWarning`, but users should migrate to the new - child-path convention. + automatically append ``/raycaster`` and emit a warning, but users + should migrate to the new child-path convention. If ``None``, the prim at :attr:`prim_path` must already exist on the USD stage. """ From 215e802f84ff60638acc4cc4bdd3dca7d41b5670 Mon Sep 17 00:00:00 2001 From: Octi Zhang Date: Fri, 10 Apr 2026 23:18:05 -0700 Subject: [PATCH 12/24] fix remaining issues --- docs/source/api/lab/isaaclab.sim.views.rst | 8 +- .../core-concepts/scene_data_providers.rst | 2 +- .../benchmarks/benchmark_view_comparison.py | 38 +++--- .../benchmarks/benchmark_xform_prim_view.py | 34 +++--- source/isaaclab/docs/CHANGELOG.rst | 10 ++ .../isaaclab/scene/interactive_scene.py | 8 +- .../isaaclab/sensors/camera/camera.py | 79 ++++++------ .../isaaclab/sensors/camera/camera_cfg.py | 2 +- .../ray_caster/multi_mesh_ray_caster.py | 22 ++-- .../multi_mesh_ray_caster_camera.py | 8 +- .../sensors/ray_caster/ray_cast_utils.py | 40 ------- .../isaaclab/sensors/ray_caster/ray_caster.py | 112 ++---------------- .../sensors/ray_caster/ray_caster_camera.py | 47 +++++--- .../sensors/ray_caster/ray_caster_cfg.py | 20 ++-- .../isaaclab/isaaclab/sensors/sensor_base.py | 71 +++++++++++ source/isaaclab/isaaclab/sim/__init__.pyi | 9 +- .../isaaclab/sim/spawners/sensors/sensors.py | 4 +- .../sim/spawners/sensors/sensors_cfg.py | 4 +- .../isaaclab/isaaclab/sim/views/__init__.pyi | 12 +- ..._xform_prim_view.py => base_frame_view.py} | 4 +- .../isaaclab/isaaclab/sim/views/frame_view.py | 48 ++++++++ ...d_xform_prim_view.py => usd_frame_view.py} | 7 +- .../isaaclab/sim/views/xform_prim_view.py | 44 +------ .../test/sim/test_views_xform_prim.py | 28 ++--- .../isaaclab/test/sim/xform_contract_tests.py | 8 +- .../test/terrains/check_terrain_importer.py | 4 +- .../test/terrains/test_terrain_importer.py | 4 +- .../locomanipulation_sdg/scene_utils.py | 8 +- source/isaaclab_newton/docs/CHANGELOG.rst | 6 + .../isaaclab_newton/sim/views/__init__.pyi | 4 +- ...prim_view.py => newton_site_frame_view.py} | 14 +-- .../test/sim/test_views_xform_prim_newton.py | 18 +-- source/isaaclab_physx/docs/CHANGELOG.rst | 6 + .../physx_scene_data_provider.py | 10 +- .../isaaclab_physx/sim/views/__init__.pyi | 4 +- ...form_prim_view.py => fabric_frame_view.py} | 14 +-- .../test/sim/test_views_xform_prim_fabric.py | 6 +- source/isaaclab_tasks/docs/CHANGELOG.rst | 2 + .../pick_place/mdp/terminations.py | 2 +- .../isaaclab_teleop/test/test_oxr_device.py | 8 +- 40 files changed, 384 insertions(+), 395 deletions(-) delete mode 100644 source/isaaclab/isaaclab/sensors/ray_caster/ray_cast_utils.py rename source/isaaclab/isaaclab/sim/views/{base_xform_prim_view.py => base_frame_view.py} (97%) create mode 100644 source/isaaclab/isaaclab/sim/views/frame_view.py rename source/isaaclab/isaaclab/sim/views/{usd_xform_prim_view.py => usd_frame_view.py} (98%) rename source/isaaclab_newton/isaaclab_newton/sim/views/{newton_site_xform_prim_view.py => newton_site_frame_view.py} (97%) rename source/isaaclab_physx/isaaclab_physx/sim/views/{fabric_xform_prim_view.py => fabric_frame_view.py} (96%) diff --git a/docs/source/api/lab/isaaclab.sim.views.rst b/docs/source/api/lab/isaaclab.sim.views.rst index 3a5f9bdecfe9..fcb0d281008c 100644 --- a/docs/source/api/lab/isaaclab.sim.views.rst +++ b/docs/source/api/lab/isaaclab.sim.views.rst @@ -7,11 +7,11 @@ .. autosummary:: - XformPrimView + FrameView -XForm Prim View ---------------- +Frame View +---------- -.. autoclass:: XformPrimView +.. autoclass:: FrameView :members: :show-inheritance: diff --git a/docs/source/overview/core-concepts/scene_data_providers.rst b/docs/source/overview/core-concepts/scene_data_providers.rst index 684dfcefcbef..46244317ba2e 100644 --- a/docs/source/overview/core-concepts/scene_data_providers.rst +++ b/docs/source/overview/core-concepts/scene_data_providers.rst @@ -55,7 +55,7 @@ Newton-based visualizers (Newton, Rerun, Viser) require a Newton model/state to The sync pipeline: 1. Reads transforms from PhysX ``RigidBodyView`` (fast tensor API) -2. Falls back to ``XformPrimView`` for bodies not covered by the rigid body view +2. Falls back to :class:`~isaaclab.sim.views.FrameView` for bodies not covered by the rigid body view 3. Converts and writes merged poses into the Newton state via Warp kernels Newton Scene Data Provider diff --git a/scripts/benchmarks/benchmark_view_comparison.py b/scripts/benchmarks/benchmark_view_comparison.py index 346fd4c86af2..a637f687803e 100644 --- a/scripts/benchmarks/benchmark_view_comparison.py +++ b/scripts/benchmarks/benchmark_view_comparison.py @@ -3,13 +3,13 @@ # # SPDX-License-Identifier: BSD-3-Clause -"""Benchmark script comparing XformPrimView backends and PhysX RigidBodyView. +"""Benchmark script comparing FrameView backends and PhysX RigidBodyView. Compares batched transform operation performance across: -- **USD** (baseline): Isaac Lab's XformPrimView via USD XformCache -- **Fabric**: Isaac Lab's XformPrimView via Fabric GPU arrays -- **Newton**: Isaac Lab's Newton XformPrimView via Warp site kernels +- **USD** (baseline): Isaac Lab's FrameView via USD XformCache +- **Fabric**: Isaac Lab's FrameView via Fabric GPU arrays +- **Newton**: Isaac Lab's Newton FrameView via Warp site kernels - **PhysX**: PhysX RigidBodyView via PhysX tensor API (reference) Usage: @@ -29,7 +29,7 @@ from isaaclab.app import AppLauncher -parser = argparse.ArgumentParser(description="Benchmark XformPrimView backends and PhysX RigidBodyView.") +parser = argparse.ArgumentParser(description="Benchmark FrameView backends and PhysX RigidBodyView.") parser.add_argument("--num_envs", type=int, default=1000, help="Number of environments to simulate.") parser.add_argument("--num_iterations", type=int, default=50, help="Number of iterations for each test.") @@ -69,11 +69,11 @@ from pxr import Gf import isaaclab.sim as sim_utils -from isaaclab.sim.views import XformPrimView +from isaaclab.sim.views import FrameView try: from isaaclab_newton.physics import MJWarpSolverCfg, NewtonCfg - from isaaclab_newton.sim.views import NewtonSiteXformPrimView + from isaaclab_newton.sim.views import NewtonSiteFrameView HAS_NEWTON = True except ImportError: @@ -87,7 +87,7 @@ @torch.no_grad() def benchmark_usd_or_fabric(view_type: str, num_iterations: int) -> dict[str, float]: - """Benchmark USD or Fabric XformPrimView.""" + """Benchmark USD or Fabric FrameView.""" timing_results = {} print(" Setting up scene") @@ -121,11 +121,11 @@ def benchmark_usd_or_fabric(view_type: str, num_iterations: int) -> dict[str, fl start_time = time.perf_counter() if view_type == "fabric" and "cuda" not in args_cli.device: raise ValueError("Fabric backend requires CUDA.") - view = XformPrimView(pattern, device=args_cli.device, validate_xform_ops=False) + view = FrameView(pattern, device=args_cli.device, validate_xform_ops=False) num_prims = view.count timing_results["init"] = time.perf_counter() - start_time - print(f" XformPrimView ({view_type.upper()}) managing {num_prims} prims") + print(f" FrameView ({view_type.upper()}) managing {num_prims} prims") positions, orientations = view.get_world_poses() @@ -137,7 +137,7 @@ def benchmark_usd_or_fabric(view_type: str, num_iterations: int) -> dict[str, fl @torch.no_grad() def benchmark_newton(num_iterations: int) -> dict[str, float]: - """Benchmark Newton XformPrimView.""" + """Benchmark Newton FrameView.""" from isaaclab.assets import RigidObjectCfg from isaaclab.scene import InteractiveScene, InteractiveSceneCfg from isaaclab.sim import SimulationCfg, build_simulation_context @@ -177,11 +177,11 @@ class _SceneCfg(InteractiveSceneCfg): print(f" Newton scene setup: {time.perf_counter() - start_time:.4f}s") start_time = time.perf_counter() - view = NewtonSiteXformPrimView("/World/envs/env_.*/Cube/Sensor", device=args_cli.device) + view = NewtonSiteFrameView("/World/envs/env_.*/Cube/Sensor", device=args_cli.device) num_prims = view.count timing_results["init"] = time.perf_counter() - start_time - print(f" Newton XformPrimView managing {num_prims} prims") + print(f" Newton FrameView managing {num_prims} prims") positions, orientations = view.get_world_poses() @@ -271,7 +271,7 @@ def _run_pose_benchmarks( positions: wp.array, orientations: wp.array, ): - """Shared benchmark loop for get/set world poses on any XformPrimView.""" + """Shared benchmark loop for get/set world poses on any FrameView.""" start_time = time.perf_counter() for _ in range(num_iterations): view.get_world_poses() @@ -373,7 +373,7 @@ def print_results(results_dict: dict[str, dict[str, float]], num_prims: int, num print("\nNotes:") print(" - Times are averaged over all iterations") print(" - Speedup > 1.0 means faster than USD baseline") - print(" - PhysX RigidBodyView requires rigid body physics; XformPrimView works with any Xformable prim") + print(" - PhysX RigidBodyView requires rigid body physics; FrameView works with any Xformable prim") print() @@ -384,7 +384,7 @@ def print_results(results_dict: dict[str, dict[str, float]], num_prims: int, num def main(): print("=" * 120) - print("XformPrimView Benchmark: USD vs Fabric vs Newton vs PhysX") + print("FrameView Benchmark: USD vs Fabric vs Newton vs PhysX") print("=" * 120) print(f" Environments: {args_cli.num_envs}") print(f" Iterations: {args_cli.num_iterations}") @@ -401,9 +401,9 @@ def main(): profile_files = {} dispatch = { - "usd": ("usd", "XformPrimView (USD)", lambda n: benchmark_usd_or_fabric("usd", n)), - "fabric": ("fabric", "XformPrimView (Fabric)", lambda n: benchmark_usd_or_fabric("fabric", n)), - "newton": ("newton", "XformPrimView (Newton)", lambda n: benchmark_newton(n)), + "usd": ("usd", "FrameView (USD)", lambda n: benchmark_usd_or_fabric("usd", n)), + "fabric": ("fabric", "FrameView (Fabric)", lambda n: benchmark_usd_or_fabric("fabric", n)), + "newton": ("newton", "FrameView (Newton)", lambda n: benchmark_newton(n)), "physx": ("physx", "PhysX RigidBodyView", lambda n: benchmark_physx(n)), } diff --git a/scripts/benchmarks/benchmark_xform_prim_view.py b/scripts/benchmarks/benchmark_xform_prim_view.py index b4e630bec98f..b682c03f71fc 100644 --- a/scripts/benchmarks/benchmark_xform_prim_view.py +++ b/scripts/benchmarks/benchmark_xform_prim_view.py @@ -3,12 +3,12 @@ # # SPDX-License-Identifier: BSD-3-Clause -"""Benchmark script comparing XformPrimView implementations across backends. +"""Benchmark script comparing FrameView implementations across backends. Compares batched transform operation performance across: -- Isaac Lab XformPrimView (USD backend) -- baseline -- Isaac Lab XformPrimView (Fabric backend) -- Isaac Lab XformPrimView (Newton backend) +- Isaac Lab FrameView (USD backend) -- baseline +- Isaac Lab FrameView (Fabric backend) +- Isaac Lab FrameView (Newton backend) Usage: ./isaaclab.sh -p scripts/benchmarks/benchmark_xform_prim_view.py --num_envs 1024 --device cuda:0 --headless @@ -23,7 +23,7 @@ from isaaclab.app import AppLauncher -parser = argparse.ArgumentParser(description="Benchmark XformPrimView performance across backends.") +parser = argparse.ArgumentParser(description="Benchmark FrameView performance across backends.") parser.add_argument("--num_envs", type=int, default=100, help="Number of environments to simulate.") parser.add_argument("--num_iterations", type=int, default=50, help="Number of iterations for each test.") parser.add_argument("--profile", action="store_true", help="Enable cProfile profiling.") @@ -44,8 +44,8 @@ import torch import warp as wp from isaaclab_newton.physics import MJWarpSolverCfg, NewtonCfg -from isaaclab_newton.sim.views import NewtonSiteXformPrimView -from isaaclab_physx.sim.views import FabricXformPrimView +from isaaclab_newton.sim.views import NewtonSiteFrameView +from isaaclab_physx.sim.views import FabricFrameView from pxr import Gf @@ -53,7 +53,7 @@ from isaaclab.assets import RigidObjectCfg from isaaclab.scene import InteractiveScene, InteractiveSceneCfg from isaaclab.sim import SimulationCfg, build_simulation_context -from isaaclab.sim.views import UsdXformPrimView +from isaaclab.sim.views import UsdFrameView from isaaclab.utils import configclass @@ -77,11 +77,11 @@ class _NewtonSceneCfg(InteractiveSceneCfg): @torch.no_grad() -def benchmark_xform_prim_view( # noqa: C901 +def benchmark_frame_view( # noqa: C901 api: Literal["isaaclab-usd", "isaaclab-fabric", "isaaclab-newton-site"], num_iterations: int, ) -> tuple[dict[str, float], dict[str, torch.Tensor]]: - """Benchmark get/set world/local poses for the given XformPrimView backend.""" + """Benchmark get/set world/local poses for the given FrameView backend.""" timing_results: dict[str, float] = {} computed_results: dict[str, torch.Tensor] = {} device = args_cli.device @@ -109,7 +109,7 @@ def benchmark_xform_prim_view( # noqa: C901 sim.reset() start_time = time.perf_counter() - xform_view = NewtonSiteXformPrimView("/World/envs/env_.*/Object/Sensor", device=device) + xform_view = NewtonSiteFrameView("/World/envs/env_.*/Object/Sensor", device=device) timing_results["init"] = time.perf_counter() - start_time cleanup = lambda: ctx.__exit__(None, None, None) # noqa: E731 @@ -128,7 +128,7 @@ def benchmark_xform_prim_view( # noqa: C901 pattern = "/World/Env_.*/Object" start_time = time.perf_counter() - ViewClass = FabricXformPrimView if use_fabric else UsdXformPrimView + ViewClass = FabricFrameView if use_fabric else UsdFrameView xform_view = ViewClass(pattern, device=device, validate_xform_ops=False) timing_results["init"] = time.perf_counter() - start_time cleanup = lambda: sim.clear_instance() # noqa: E731 @@ -337,7 +337,7 @@ def print_results(results_dict: dict[str, dict[str, float]], num_prims: int, num def main(): print("=" * 120) - print("XformPrimView Benchmark") + print("FrameView Benchmark") print("=" * 120) print(f" Environments: {args_cli.num_envs}") print(f" Iterations: {args_cli.num_iterations}") @@ -354,9 +354,9 @@ def main(): profile_files = {} apis = [ - ("isaaclab-usd", "Isaac Lab XformPrimView (USD)"), - ("isaaclab-fabric", "Isaac Lab XformPrimView (Fabric)"), - ("isaaclab-newton-site", "Isaac Lab XformPrimView (Newton Site)"), + ("isaaclab-usd", "Isaac Lab FrameView (USD)"), + ("isaaclab-fabric", "Isaac Lab FrameView (Fabric)"), + ("isaaclab-newton-site", "Isaac Lab FrameView (Newton Site)"), ] for api_key, api_name in apis: @@ -366,7 +366,7 @@ def main(): profiler = cProfile.Profile() profiler.enable() - timing, computed = benchmark_xform_prim_view(api=api_key, num_iterations=args_cli.num_iterations) + timing, computed = benchmark_frame_view(api=api_key, num_iterations=args_cli.num_iterations) if args_cli.profile: profiler.disable() diff --git a/source/isaaclab/docs/CHANGELOG.rst b/source/isaaclab/docs/CHANGELOG.rst index f381bbc9f492..a3fa93203277 100644 --- a/source/isaaclab/docs/CHANGELOG.rst +++ b/source/isaaclab/docs/CHANGELOG.rst @@ -10,6 +10,16 @@ Changed * Changed :class:`~isaaclab.sensors.RayCaster` to spawn its own non-physics Xform prim via the new :attr:`~isaaclab.sensors.RayCasterCfg.spawn` attribute. ``prim_path`` should now point to a child under the parent link (e.g. ``{ENV_REGEX_NS}/Robot/base/raycaster``). +* Renamed :class:`~isaaclab.sim.views.XformPrimView` to :class:`~isaaclab.sim.views.FrameView`, + ``BaseXformPrimView`` to :class:`~isaaclab.sim.views.BaseFrameView`, + and ``UsdXformPrimView`` to :class:`~isaaclab.sim.views.UsdFrameView`. + ``XformPrimView`` is kept as a deprecated alias. +* Moved :class:`~isaaclab.sensors.RayCasterCfg` offset into the spawned prim's local transform + instead of applying it at runtime. The :class:`~isaaclab.sim.views.FrameView` world pose now + includes the offset directly. +* Unified sensor prim path resolution in :class:`~isaaclab.sensors.SensorBase`. When + ``prim_path`` points at a physics body and a spawner is configured, a child prim is + automatically created underneath. Deprecated ^^^^^^^^^^ diff --git a/source/isaaclab/isaaclab/scene/interactive_scene.py b/source/isaaclab/isaaclab/scene/interactive_scene.py index 7cd0eee7433e..272fad08f61b 100644 --- a/source/isaaclab/isaaclab/scene/interactive_scene.py +++ b/source/isaaclab/isaaclab/scene/interactive_scene.py @@ -32,7 +32,7 @@ from isaaclab.sensors import ContactSensorCfg, FrameTransformerCfg, SensorBase, SensorBaseCfg from isaaclab.sim import SimulationContext from isaaclab.sim.utils.stage import get_current_stage, get_current_stage_id -from isaaclab.sim.views import XformPrimView +from isaaclab.sim.views import FrameView from isaaclab.terrains import TerrainImporter, TerrainImporterCfg # Note: This is a temporary import for the VisuoTactileSensorCfg class. @@ -393,11 +393,11 @@ def surface_grippers(self) -> dict[str, SurfaceGripper]: return self._surface_grippers @property - def extras(self) -> dict[str, XformPrimView]: + def extras(self) -> dict[str, FrameView]: """A dictionary of miscellaneous simulation objects that neither inherit from assets nor sensors. The keys are the names of the miscellaneous objects, and the values are the - :class:`~isaaclab.sim.views.XformPrimView` instances of the corresponding prims. + :class:`~isaaclab.sim.views.FrameView` instances of the corresponding prims. As an example, lights or other props in the scene that do not have any attributes or properties that you want to alter at runtime can be added to this dictionary. @@ -823,7 +823,7 @@ def _add_entities_from_cfg(self): # noqa: C901 ) # store xform prim view corresponding to this asset # all prims in the scene are Xform prims (i.e. have a transform component) - self._extras[asset_name] = XformPrimView(asset_cfg.prim_path, device=self.device, stage=self.stage) + self._extras[asset_name] = FrameView(asset_cfg.prim_path, device=self.device, stage=self.stage) else: raise ValueError(f"Unknown asset config type for {asset_name}: {asset_cfg}") diff --git a/source/isaaclab/isaaclab/sensors/camera/camera.py b/source/isaaclab/isaaclab/sensors/camera/camera.py index 65efe4a375cb..13c940fa9ce4 100644 --- a/source/isaaclab/isaaclab/sensors/camera/camera.py +++ b/source/isaaclab/isaaclab/sensors/camera/camera.py @@ -16,11 +16,10 @@ from pxr import Sdf, UsdGeom -import isaaclab.sim as sim_utils import isaaclab.utils.sensors as sensor_utils from isaaclab.app.settings_manager import get_settings_manager from isaaclab.renderers import BaseRenderer, Renderer -from isaaclab.sim.views import XformPrimView +from isaaclab.sim.views import FrameView from isaaclab.utils import has_kit, to_camel_case from isaaclab.utils.math import ( convert_camera_frame_orientation_convention, @@ -121,36 +120,43 @@ def __init__(self, cfg: CameraCfg): settings = get_settings_manager() settings.set_bool("/isaaclab/render/rtx_sensors", True) - # spawn the asset - if self.cfg.spawn is not None: - # Use spawn_path when set (points to template location for scene-cloned sensors). - # This allows the camera to be spawned inside the asset template (e.g. inside - # proto_asset_0) before clone_environments replicates it to all env paths. - spawn_target = ( - self.cfg.spawn.spawn_path - if getattr(self.cfg.spawn, "spawn_path", None) is not None - else self.cfg.prim_path - ) - # compute the rotation offset - rot = torch.tensor(self.cfg.offset.rot, dtype=torch.float32, device="cpu").unsqueeze(0) - rot_offset = convert_camera_frame_orientation_convention( - rot, origin=self.cfg.offset.convention, target="opengl" - ) - rot_offset = rot_offset.squeeze(0).cpu().numpy() - # ensure vertical aperture is set, otherwise replace with default for squared pixels - if self.cfg.spawn.vertical_aperture is None: - self.cfg.spawn.vertical_aperture = self.cfg.spawn.horizontal_aperture * self.cfg.height / self.cfg.width - self.cfg.spawn.func(spawn_target, self.cfg.spawn, translation=self.cfg.offset.pos, orientation=rot_offset) - # check that spawn was successful; use spawn_path if set (template location) since env - # paths are not yet populated at init time — they are filled in by clone_environments. - check_path = ( - self.cfg.spawn.spawn_path - if self.cfg.spawn is not None and getattr(self.cfg.spawn, "spawn_path", None) is not None - else self.cfg.prim_path + # This is only introduced in isaac sim 6.0 + if has_kit(): + isaac_sim_version = get_isaac_sim_version() + if isaac_sim_version.major >= 6: + # Set RTX flag to enable fast path when no regular RGB/RGBA annotators are requested + needs_color_render = "rgb" in self.cfg.data_types or "rgba" in self.cfg.data_types + if not needs_color_render: + settings.set_bool("/rtx/sdg/force/disableColorRender", True) + + # If we have GUI / viewport enabled, we turn off fast path so that the viewport is not black + if settings.get("/isaaclab/has_gui"): + settings.set_bool("/rtx/sdg/force/disableColorRender", False) + else: + if "albedo" in self.cfg.data_types: + logger.warning( + "Albedo annotator is only supported in Isaac Sim 6.0+. The albedo data type will be ignored." + ) + if any(data_type in self.SIMPLE_SHADING_MODES for data_type in self.cfg.data_types): + logger.warning( + "Simple shading annotators are only supported in Isaac Sim 6.0+. The simple shading data types" + " will be ignored." + ) + + # Set simple shading mode (if requested) before rendering + simple_shading_mode = self._resolve_simple_shading_mode() + if simple_shading_mode is not None: + settings.set_int(self.SIMPLE_SHADING_MODE_SETTING, simple_shading_mode) + + # Compute camera orientation (convention conversion) and spawn + rot = torch.tensor(self.cfg.offset.rot, dtype=torch.float32, device="cpu").unsqueeze(0) + rot_offset = convert_camera_frame_orientation_convention( + rot, origin=self.cfg.offset.convention, target="opengl" ) - matching_prims = sim_utils.find_matching_prims(check_path) - if len(matching_prims) == 0: - raise RuntimeError(f"Could not find prim with path {check_path}.") + rot_offset = rot_offset.squeeze(0).cpu().numpy() + if self.cfg.spawn is not None and self.cfg.spawn.vertical_aperture is None: + self.cfg.spawn.vertical_aperture = self.cfg.spawn.horizontal_aperture * self.cfg.height / self.cfg.width + self._resolve_and_spawn("camera", translation=self.cfg.offset.pos, orientation=rot_offset) # UsdGeom Camera prim for the sensor self._sensor_prims: list[UsdGeom.Camera] = list() @@ -338,7 +344,8 @@ def set_world_poses( # convert torch tensors to warp arrays for the view pos_wp = wp.from_torch(positions.contiguous()) if positions is not None else None ori_wp = wp.from_torch(orientations.contiguous()) if orientations is not None else None - self._view.set_world_poses(pos_wp, ori_wp, env_ids) + idx_wp = wp.from_torch(env_ids.to(dtype=torch.int32), dtype=wp.int32) if env_ids is not None else None + self._view.set_world_poses(pos_wp, ori_wp, idx_wp) def set_world_poses_from_view( self, eyes: torch.Tensor, targets: torch.Tensor, env_ids: Sequence[int] | None = None @@ -361,7 +368,8 @@ def set_world_poses_from_view( up_axis = UsdGeom.GetStageUpAxis(self.stage) # set camera poses using the view orientations = quat_from_matrix(create_rotation_matrix_from_view(eyes, targets, up_axis, device=self._device)) - self._view.set_world_poses(wp.from_torch(eyes.contiguous()), wp.from_torch(orientations.contiguous()), env_ids) + idx_wp = wp.from_torch(env_ids.to(dtype=torch.int32), dtype=wp.int32) if env_ids is not None else None + self._view.set_world_poses(wp.from_torch(eyes.contiguous()), wp.from_torch(orientations.contiguous()), idx_wp) """ Operations @@ -420,7 +428,7 @@ def _initialize_impl(self): # Create a view for the sensor with Fabric enabled for fast pose queries. # TODO: remove sync_usd_on_fabric_write=True once the GPU Fabric sync bug is fixed. - self._view = XformPrimView( + self._view = FrameView( self.cfg.prim_path, device=self._device, stage=self.stage, sync_usd_on_fabric_write=True ) # Check that sizes are correct @@ -615,7 +623,8 @@ def _update_poses(self, env_ids: Sequence[int]): raise RuntimeError("Camera prim is None. Please call 'sim.play()' first.") # get the poses from the view (returns wp.array, convert to torch) - pos_wp, quat_wp = self._view.get_world_poses(env_ids) + indices = wp.from_torch(env_ids.to(dtype=torch.int32), dtype=wp.int32) if env_ids is not None else None + pos_wp, quat_wp = self._view.get_world_poses(indices) self._data.pos_w[env_ids] = wp.to_torch(pos_wp) self._data.quat_w_world[env_ids] = convert_camera_frame_orientation_convention( wp.to_torch(quat_wp), origin="opengl", target="world" diff --git a/source/isaaclab/isaaclab/sensors/camera/camera_cfg.py b/source/isaaclab/isaaclab/sensors/camera/camera_cfg.py index 1b5070cfd214..efd8e1f304c1 100644 --- a/source/isaaclab/isaaclab/sensors/camera/camera_cfg.py +++ b/source/isaaclab/isaaclab/sensors/camera/camera_cfg.py @@ -85,7 +85,7 @@ class OffsetCfg: """Whether to update the latest camera pose when fetching the camera's data. Defaults to False. If True, the latest camera pose is updated in the camera's data which will slow down performance - due to the use of :class:`XformPrimView`. + due to the use of :class:`FrameView`. If False, the pose of the camera during initialization is returned. """ diff --git a/source/isaaclab/isaaclab/sensors/ray_caster/multi_mesh_ray_caster.py b/source/isaaclab/isaaclab/sensors/ray_caster/multi_mesh_ray_caster.py index 9d708a7be1c1..05ea7b687511 100644 --- a/source/isaaclab/isaaclab/sensors/ray_caster/multi_mesh_ray_caster.py +++ b/source/isaaclab/isaaclab/sensors/ray_caster/multi_mesh_ray_caster.py @@ -15,13 +15,12 @@ import warp as wp import isaaclab.sim as sim_utils -from isaaclab.sim.views import BaseXformPrimView -from isaaclab.utils.math import matrix_from_quat, quat_mul +from isaaclab.sim.views import BaseFrameView, FrameView +from isaaclab.utils.math import matrix_from_quat from isaaclab.utils.mesh import PRIMITIVE_MESH_TYPES, create_trimesh_from_geom_mesh, create_trimesh_from_geom_shape from isaaclab.utils.warp import convert_to_warp_mesh, raycast_dynamic_meshes from .multi_mesh_ray_caster_data import MultiMeshRayCasterData -from .ray_cast_utils import obtain_world_pose_from_view from .ray_caster import RayCaster if TYPE_CHECKING: @@ -74,9 +73,7 @@ class MultiMeshRayCaster(RayCaster): cfg: MultiMeshRayCasterCfg """The configuration parameters.""" - mesh_offsets: dict[str, tuple[torch.Tensor, torch.Tensor]] = {} - - mesh_views: ClassVar[dict[str, BaseXformPrimView]] = {} + mesh_views: ClassVar[dict[str, BaseFrameView]] = {} """A dictionary to store mesh views for raycasting, shared across all instances. The keys correspond to the prim path for the mesh views, and values are the corresponding view objects. @@ -290,8 +287,8 @@ def _initialize_warp_meshes(self): mesh_idx += n_meshes_per_env if target_cfg.track_mesh_transforms: - MultiMeshRayCaster.mesh_views[target_prim_path], MultiMeshRayCaster.mesh_offsets[target_prim_path] = ( - self._obtain_trackable_prim_view(target_prim_path) + MultiMeshRayCaster.mesh_views[target_prim_path] = FrameView( + target_prim_path, device=self._device, stage=self.stage ) # throw an error if no meshes are found @@ -361,15 +358,11 @@ def _update_buffers_impl(self, env_mask: wp.array): continue # update position of the target meshes - pos_w, ori_w = obtain_world_pose_from_view(view, None) + pos_wp, quat_wp = view.get_world_poses(None) + pos_w, ori_w = wp.to_torch(pos_wp), wp.to_torch(quat_wp) pos_w = pos_w.squeeze(0) if len(pos_w.shape) == 3 else pos_w ori_w = ori_w.squeeze(0) if len(ori_w.shape) == 3 else ori_w - if target_cfg.prim_expr in MultiMeshRayCaster.mesh_offsets: - pos_offset, ori_offset = MultiMeshRayCaster.mesh_offsets[target_cfg.prim_expr] - pos_w -= pos_offset - ori_w = quat_mul(ori_offset.expand(ori_w.shape[0], -1), ori_w) - count = view.count if count != 1: # Mesh is not global, i.e. we have different meshes for each env count = count // self._num_envs @@ -396,7 +389,6 @@ def _update_buffers_impl(self, env_mask: wp.array): def __del__(self): super().__del__() if RayCaster._instance_count == 0: - MultiMeshRayCaster.mesh_offsets.clear() MultiMeshRayCaster.mesh_views.clear() diff --git a/source/isaaclab/isaaclab/sensors/ray_caster/multi_mesh_ray_caster_camera.py b/source/isaaclab/isaaclab/sensors/ray_caster/multi_mesh_ray_caster_camera.py index a1be3160d99b..d65aebabfe7e 100644 --- a/source/isaaclab/isaaclab/sensors/ray_caster/multi_mesh_ray_caster_camera.py +++ b/source/isaaclab/isaaclab/sensors/ray_caster/multi_mesh_ray_caster_camera.py @@ -16,7 +16,6 @@ from .multi_mesh_ray_caster import MultiMeshRayCaster from .multi_mesh_ray_caster_camera_data import MultiMeshRayCasterCameraData -from .ray_cast_utils import obtain_world_pose_from_view from .ray_caster_camera import RayCasterCamera if TYPE_CHECKING: @@ -117,7 +116,9 @@ def _update_ray_infos(self, env_ids: Sequence[int]): """Updates the ray information buffers.""" # compute poses from current view - pos_w, quat_w = obtain_world_pose_from_view(self._view, env_ids) + indices = wp.from_torch(env_ids.to(dtype=torch.int32), dtype=wp.int32) if env_ids is not None else None + pos_wp, quat_wp = self._view.get_world_poses(indices) + pos_w, quat_w = wp.to_torch(pos_wp), wp.to_torch(quat_wp) pos_w, quat_w = math_utils.combine_frame_transforms( pos_w, quat_w, self._offset_pos[env_ids], self._offset_quat[env_ids] ) @@ -152,7 +153,8 @@ def _update_buffers_impl(self, env_mask: wp.array): continue # update position of the target meshes - pos_w, ori_w = obtain_world_pose_from_view(view, None) + pos_wp, quat_wp = view.get_world_poses(None) + pos_w, ori_w = wp.to_torch(pos_wp), wp.to_torch(quat_wp) pos_w = pos_w.squeeze(0) if len(pos_w.shape) == 3 else pos_w ori_w = ori_w.squeeze(0) if len(ori_w.shape) == 3 else ori_w diff --git a/source/isaaclab/isaaclab/sensors/ray_caster/ray_cast_utils.py b/source/isaaclab/isaaclab/sensors/ray_caster/ray_cast_utils.py deleted file mode 100644 index 2824420c7059..000000000000 --- a/source/isaaclab/isaaclab/sensors/ray_caster/ray_cast_utils.py +++ /dev/null @@ -1,40 +0,0 @@ -# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). -# All rights reserved. -# -# SPDX-License-Identifier: BSD-3-Clause - -"""Utility functions for ray-cast sensors.""" - -from __future__ import annotations - -import torch -import warp as wp - -from isaaclab.sim.views import BaseXformPrimView - - -def obtain_world_pose_from_view( - physx_view: BaseXformPrimView, - env_ids: torch.Tensor, - clone: bool = False, -) -> tuple[torch.Tensor, torch.Tensor]: - """Get the world poses of the prim referenced by the prim view. - - Args: - physx_view: The prim view to get the world poses from. - env_ids: The environment ids of the prims to get the world poses for. - clone: Whether to clone the returned tensors (default: False). - - Returns: - A tuple containing the world positions and orientations of the prims. - Orientation is in (x, y, z, w) format. - """ - indices = wp.from_torch(env_ids.to(dtype=torch.int32), dtype=wp.int32) if env_ids is not None else None - pos_wp, quat_wp = physx_view.get_world_poses(indices) - pos_w = wp.to_torch(pos_wp) - quat_w = wp.to_torch(quat_wp) - - if clone: - return pos_w.clone(), quat_w.clone() - else: - return pos_w, quat_w diff --git a/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster.py b/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster.py index 54a462ecaece..a332f888e81e 100644 --- a/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster.py +++ b/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster.py @@ -6,7 +6,6 @@ from __future__ import annotations import logging -import warnings from collections.abc import Sequence from typing import TYPE_CHECKING, ClassVar @@ -14,18 +13,17 @@ import torch import warp as wp -from pxr import Gf, Usd, UsdGeom, UsdPhysics +from pxr import Gf, Usd, UsdGeom import isaaclab.sim as sim_utils import isaaclab.utils.math as math_utils from isaaclab.markers import VisualizationMarkers -from isaaclab.sim.views import BaseXformPrimView, XformPrimView +from isaaclab.sim.views import FrameView from isaaclab.terrains.trimesh.utils import make_plane from isaaclab.utils.math import quat_apply, quat_apply_yaw from isaaclab.utils.warp import convert_to_warp_mesh, raycast_mesh from ..sensor_base import SensorBase -from .ray_cast_utils import obtain_world_pose_from_view from .ray_caster_data import RayCasterData if TYPE_CHECKING: @@ -69,26 +67,12 @@ def __init__(self, cfg: RayCasterCfg): cfg: The configuration parameters. """ RayCaster._instance_count += 1 - self._resolve_physics_prim_path(cfg) # Initialize base class super().__init__(cfg) - # Spawn the sensor Xform prim (mirrors how Camera spawns a Camera prim). - if self.cfg.spawn is not None: - spawn_target = ( - self.cfg.spawn.spawn_path - if getattr(self.cfg.spawn, "spawn_path", None) is not None - else self.cfg.prim_path - ) - self.cfg.spawn.func(spawn_target, self.cfg.spawn) - # Verify the prim exists (use spawn_path when set because env paths are - # not yet populated — clone_environments runs after this). - check_path = ( - self.cfg.spawn.spawn_path - if self.cfg.spawn is not None and getattr(self.cfg.spawn, "spawn_path", None) is not None - else self.cfg.prim_path - ) - if len(sim_utils.find_matching_prims(check_path)) == 0: - raise RuntimeError(f"Could not find prim with path {check_path!r}.") + # Only position is baked into the Xform — rotation is applied to the + # ray pattern in _initialize_rays_impl so that ray_alignment modes + # (especially "yaw") still operate on the parent body's orientation. + self._resolve_and_spawn("raycaster", translation=self.cfg.offset.pos) # Create empty variables for storing output data self._data = RayCasterData() @@ -151,7 +135,7 @@ def reset(self, env_ids: Sequence[int] | None = None, env_mask: wp.array | None def _initialize_impl(self): super()._initialize_impl() - self._view, self._offset = self._obtain_trackable_prim_view(self.cfg.prim_path) + self._view = FrameView(self.cfg.prim_path, device=self._device, stage=self.stage) # load the meshes by parsing the stage self._initialize_warp_meshes() @@ -216,14 +200,11 @@ def _initialize_warp_meshes(self): ) def _initialize_rays_impl(self): - # compute ray stars and directions self.ray_starts, self.ray_directions = self.cfg.pattern_cfg.func(self.cfg.pattern_cfg, self._device) self.num_rays = len(self.ray_directions) - # apply offset transformation to the rays - offset_pos = torch.tensor(list(self.cfg.offset.pos), device=self._device) + # apply offset rotation to the ray pattern (position offset is baked into the Xform) offset_quat = torch.tensor(list(self.cfg.offset.rot), device=self._device) self.ray_directions = quat_apply(offset_quat.repeat(len(self.ray_directions), 1), self.ray_directions) - self.ray_starts += offset_pos # repeat the rays for each sensor self.ray_starts = self.ray_starts.repeat(self._view.count, 1, 1) self.ray_directions = self.ray_directions.repeat(self._view.count, 1, 1) @@ -239,11 +220,9 @@ def _initialize_rays_impl(self): def _update_ray_infos(self, env_ids: Sequence[int]): """Updates the ray information buffers.""" - - pos_w, quat_w = obtain_world_pose_from_view(self._view, env_ids) - pos_w, quat_w = math_utils.combine_frame_transforms( - pos_w, quat_w, self._offset[0][env_ids], self._offset[1][env_ids] - ) + indices = wp.from_torch(env_ids.to(dtype=torch.int32), dtype=wp.int32) if env_ids is not None else None + pos_wp, quat_wp = self._view.get_world_poses(indices) + pos_w, quat_w = wp.to_torch(pos_wp), wp.to_torch(quat_wp) # apply drift to ray starting position in world frame pos_w += self.drift[env_ids] # store the poses @@ -326,75 +305,6 @@ def _debug_vis_callback(self, event): Internal Helpers. """ - @staticmethod - def _resolve_physics_prim_path(cfg: RayCasterCfg) -> None: - """Auto-detect legacy ``prim_path`` pointing at a physics prim and redirect to a child Xform. - - Before this refactor, ``prim_path`` was expected to point at an existing link prim (e.g. - ``{ENV_REGEX_NS}/Robot/base``) that typically carried an ``ArticulationRootAPI`` or - ``RigidBodyAPI``. The new convention requires ``prim_path`` to be a **non-physics** child - (e.g. ``{ENV_REGEX_NS}/Robot/base/raycaster``) where a plain Xform is spawned. - - This helper preserves backward compatibility: when ``spawn`` is set and the resolved - path already exists as a prim with a physics API, the path is automatically extended - with ``/raycaster`` and a deprecation warning is emitted. - """ - if cfg.spawn is None: - return - - resolve_path = cfg.spawn.spawn_path if getattr(cfg.spawn, "spawn_path", None) is not None else cfg.prim_path - - prim = sim_utils.find_first_matching_prim(resolve_path) - if prim is None or not prim.IsValid(): - return - if not (prim.HasAPI(UsdPhysics.ArticulationRootAPI) or prim.HasAPI(UsdPhysics.RigidBodyAPI)): - return - - child_name = "raycaster" - msg = ( - f"RayCasterCfg.prim_path {cfg.prim_path!r} resolves to a prim with a physics API" - f" (ArticulationRootAPI or RigidBodyAPI). This usage is deprecated. Please set" - f" prim_path to a non-physics child such as '{cfg.prim_path}/{child_name}'." - " The path has been automatically adjusted for this session." - ) - logger.warning(msg) - warnings.warn(msg, FutureWarning, stacklevel=4) - cfg.prim_path = f"{cfg.prim_path}/{child_name}" - if getattr(cfg.spawn, "spawn_path", None) is not None: - cfg.spawn.spawn_path = f"{cfg.spawn.spawn_path}/{child_name}" - - def _obtain_trackable_prim_view( - self, target_prim_path: str - ) -> tuple[BaseXformPrimView, tuple[torch.Tensor, torch.Tensor]]: - """Obtain a prim view that can be used to track the pose of the target prim. - - Creates an :class:`XformPrimView` for the target prim path, which dispatches to the - correct backend implementation automatically (Fabric for PhysX, site-based for Newton). - - The function additionally resolves the relative pose between the mesh and the view prim. - - Args: - target_prim_path: The target prim path to obtain the prim view for. - - Returns: - A tuple containing the :class:`XformPrimView` and the relative offsets - (positions, orientations) of each matched prim. - """ - prim_view = XformPrimView(target_prim_path, device=self._device, stage=self.stage) - - mesh_prims = sim_utils.find_matching_prims(target_prim_path) - positions = [] - quaternions = [] - for mesh_prim in mesh_prims: - pos, orientation = sim_utils.resolve_prim_pose(mesh_prim, mesh_prim) - positions.append(torch.tensor(pos, dtype=torch.float32, device=self.device)) - quaternions.append(torch.tensor(orientation, dtype=torch.float32, device=self.device)) - - positions = torch.stack(positions).to(device=self.device, dtype=torch.float32) - quaternions = torch.stack(quaternions).to(device=self.device, dtype=torch.float32) - - return prim_view, (positions, quaternions) - """ Internal simulation callbacks. """ 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 c27f470dcfc0..e63b8692e2c0 100644 --- a/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster_camera.py +++ b/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster_camera.py @@ -18,7 +18,6 @@ from isaaclab.sensors.camera import CameraData from isaaclab.utils.warp import raycast_mesh -from .ray_cast_utils import obtain_world_pose_from_view from .ray_caster import RayCaster if TYPE_CHECKING: @@ -152,9 +151,13 @@ def reset(self, env_ids: Sequence[int] | None = None, env_mask: wp.array | None env_ids = wp.to_torch(env_mask).nonzero(as_tuple=False).squeeze(-1) elif env_ids is None or isinstance(env_ids, slice): env_ids = self._ALL_INDICES + if not isinstance(env_ids, torch.Tensor): + env_ids = torch.tensor(env_ids, dtype=torch.long, device=self._device) # reset the data # note: this recomputation is useful if one performs events such as randomizations on the camera poses. - pos_w, quat_w = obtain_world_pose_from_view(self._view, env_ids, clone=True) + indices = wp.from_torch(env_ids.to(dtype=torch.int32), dtype=wp.int32) if env_ids is not None else None + pos_wp, quat_wp = self._view.get_world_poses(indices) + pos_w, quat_w = wp.to_torch(pos_wp).clone(), wp.to_torch(quat_wp).clone() pos_w, quat_w = math_utils.combine_frame_transforms( pos_w, quat_w, self._offset_pos[env_ids], self._offset_quat[env_ids] ) @@ -198,7 +201,9 @@ def set_world_poses( env_ids = self._ALL_INDICES # get current positions - pos_w, quat_w = obtain_world_pose_from_view(self._view, env_ids) + indices = wp.from_torch(env_ids.to(dtype=torch.int32), dtype=wp.int32) if env_ids is not None else None + pos_wp, quat_wp = self._view.get_world_poses(indices) + pos_w, quat_w = wp.to_torch(pos_wp), wp.to_torch(quat_wp) if positions is not None: # transform to camera frame pos_offset_world_frame = positions - pos_w @@ -211,7 +216,8 @@ def set_world_poses( self._offset_quat[env_ids] = math_utils.quat_mul(math_utils.quat_inv(quat_w), quat_w_set) # update the data - pos_w, quat_w = obtain_world_pose_from_view(self._view, env_ids, clone=True) + pos_wp2, quat_wp2 = self._view.get_world_poses(indices) + pos_w, quat_w = wp.to_torch(pos_wp2).clone(), wp.to_torch(quat_wp2).clone() pos_w, quat_w = math_utils.combine_frame_transforms( pos_w, quat_w, self._offset_pos[env_ids], self._offset_quat[env_ids] ) @@ -276,7 +282,9 @@ def _update_buffers_impl(self, env_mask: wp.array): self._frame[env_ids] += 1 # compute poses from current view - pos_w, quat_w = obtain_world_pose_from_view(self._view, env_ids, clone=True) + indices = wp.from_torch(env_ids.to(dtype=torch.int32), dtype=wp.int32) if env_ids is not None else None + pos_wp, quat_wp = self._view.get_world_poses(indices) + pos_w, quat_w = wp.to_torch(pos_wp).clone(), wp.to_torch(quat_wp).clone() pos_w, quat_w = math_utils.combine_frame_transforms( pos_w, quat_w, self._offset_pos[env_ids], self._offset_quat[env_ids] ) @@ -415,21 +423,22 @@ def _compute_view_world_poses(self, env_ids: Sequence[int]) -> tuple[torch.Tenso """Obtains the pose of the view the camera is attached to in the world frame. .. deprecated v2.3.1: - This function will be removed in a future release in favor of implementation - :meth:`obtain_world_pose_from_view`. + This function will be removed in a future release. Call + ``self._view.get_world_poses(indices)`` directly instead. Returns: A tuple of the position (in meters) and quaternion (x, y, z, w). """ - # deprecation logger.warning( - "The function '_compute_view_world_poses' will be deprecated in favor of the util method" - " 'obtain_world_pose_from_view'. Please use 'obtain_world_pose_from_view' instead...." + "The function '_compute_view_world_poses' is deprecated." + " Call 'self._view.get_world_poses(indices)' directly instead." ) - return obtain_world_pose_from_view(self._view, env_ids, clone=True) + indices = wp.from_torch(env_ids.to(dtype=torch.int32), dtype=wp.int32) if env_ids is not None else None + pos_wp, quat_wp = self._view.get_world_poses(indices) + return wp.to_torch(pos_wp).clone(), wp.to_torch(quat_wp).clone() def _compute_camera_world_poses(self, env_ids: Sequence[int]) -> tuple[torch.Tensor, torch.Tensor]: """Computes the pose of the camera in the world frame. @@ -441,7 +450,9 @@ def _compute_camera_world_poses(self, env_ids: Sequence[int]) -> tuple[torch.Ten .. code-block:: python - pos_w, quat_w = obtain_world_pose_from_view(self._view, env_ids, clone=True) + indices = wp.from_torch(env_ids.to(dtype=torch.int32), dtype=wp.int32) + pos_wp, quat_wp = self._view.get_world_poses(indices) + pos_w, quat_w = wp.to_torch(pos_wp).clone(), wp.to_torch(quat_wp).clone() pos_w, quat_w = math_utils.combine_frame_transforms( pos_w, quat_w, self._offset_pos[env_ids], self._offset_quat[env_ids] ) @@ -449,14 +460,12 @@ def _compute_camera_world_poses(self, env_ids: Sequence[int]) -> tuple[torch.Ten Returns: A tuple of the position (in meters) and quaternion (x, y, z, w) in "world" convention. """ - - # deprecation logger.warning( - "The function '_compute_camera_world_poses' will be deprecated in favor of the combination of methods" - " 'obtain_world_pose_from_view' and 'math_utils.combine_frame_transforms'. Please use" - " 'obtain_world_pose_from_view' and 'math_utils.combine_frame_transforms' instead...." + "The function '_compute_camera_world_poses' is deprecated." + " Call 'self._view.get_world_poses(indices)' and 'math_utils.combine_frame_transforms' directly instead." ) - # get the pose of the view the camera is attached to - pos_w, quat_w = obtain_world_pose_from_view(self._view, env_ids, clone=True) + indices = wp.from_torch(env_ids.to(dtype=torch.int32), dtype=wp.int32) if env_ids is not None else None + pos_wp, quat_wp = self._view.get_world_poses(indices) + pos_w, quat_w = wp.to_torch(pos_wp).clone(), wp.to_torch(quat_wp).clone() return math_utils.combine_frame_transforms(pos_w, quat_w, self._offset_pos[env_ids], self._offset_quat[env_ids]) diff --git a/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster_cfg.py b/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster_cfg.py index b8060138e4ce..bb28d1321b8e 100644 --- a/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster_cfg.py +++ b/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster_cfg.py @@ -42,18 +42,14 @@ class OffsetCfg: A plain USD Xform is created at :attr:`prim_path` before initialization, matching the pattern used by :class:`~isaaclab.sensors.camera.camera_cfg.CameraCfg` (which spawns a - Camera prim). The :attr:`prim_path` should therefore be a **new** child path under the - desired parent link (e.g. ``{ENV_REGEX_NS}/Robot/base/raycaster``), **not** the link - itself. - - .. deprecated:: - Passing a :attr:`prim_path` that points at a prim with - ``ArticulationRootAPI`` or ``RigidBodyAPI`` (e.g. - ``{ENV_REGEX_NS}/Robot/base``) is deprecated. The sensor will - automatically append ``/raycaster`` and emit a warning, but users - should migrate to the new child-path convention. - - If ``None``, the prim at :attr:`prim_path` must already exist on the USD stage. + Camera prim). The :attr:`prim_path` can be either: + + - A **new** child path under a parent link (e.g. ``{ENV_REGEX_NS}/Robot/base/raycaster``). + - A **physics body** path (e.g. ``{ENV_REGEX_NS}/Robot/base``). In this case, the sensor + will automatically create a child Xform at ``{prim_path}/raycaster``. + + If ``None``, the prim at :attr:`prim_path` must already exist on the USD stage and must + **not** be a physics body. """ mesh_prim_paths: list[str] = MISSING diff --git a/source/isaaclab/isaaclab/sensors/sensor_base.py b/source/isaaclab/isaaclab/sensors/sensor_base.py index 4a9fb91786e5..3b15d8a0171e 100644 --- a/source/isaaclab/isaaclab/sensors/sensor_base.py +++ b/source/isaaclab/isaaclab/sensors/sensor_base.py @@ -12,6 +12,7 @@ from __future__ import annotations import inspect +import logging import re import weakref from abc import ABC, abstractmethod @@ -29,6 +30,8 @@ if TYPE_CHECKING: from .sensor_base_cfg import SensorBaseCfg +logger = logging.getLogger(__name__) + class SensorBase(ABC): """The base class for implementing a sensor. @@ -386,3 +389,71 @@ def _resolve_indices_and_mask( self._reset_mask.zero_() self._reset_mask_torch[env_ids] = True return self._reset_mask + + def _resolve_and_spawn(self, sensor_name: str, **spawn_kwargs) -> None: + """Resolve physics-body prim paths and spawn the sensor prim if needed. + + Behavior matrix (``spawn`` refers to ``cfg.spawn``): + + +----------------+------------------+--------------------------------------------+ + | ``spawn`` | ``prim_path`` | Action | + +================+==================+============================================+ + | not ``None`` | physics body | Append ``/``, spawn child. | + +----------------+------------------+--------------------------------------------+ + | not ``None`` | non-physics prim | Use existing prim, skip spawn. | + | | (already exists) | | + +----------------+------------------+--------------------------------------------+ + | not ``None`` | does not exist | Spawn prim at ``prim_path``. | + +----------------+------------------+--------------------------------------------+ + | ``None`` | physics body | Raise ``ValueError``. | + +----------------+------------------+--------------------------------------------+ + | ``None`` | non-physics prim | Use as-is (no spawn). | + +----------------+------------------+--------------------------------------------+ + + Args: + sensor_name: Short identifier (e.g. ``"raycaster"``, ``"camera"``). + **spawn_kwargs: Extra keyword arguments forwarded to ``cfg.spawn.func`` + (e.g. ``translation``, ``orientation``). + + Raises: + ValueError: If ``spawn`` is ``None`` and ``prim_path`` is a physics body. + RuntimeError: If the prim does not exist after the spawn attempt. + """ + from pxr import UsdPhysics # noqa: PLC0415 + + spawn = getattr(self.cfg, "spawn", None) + has_spawn = spawn is not None + + # Determine the path to probe for physics-body redirect + spawn_path = (getattr(spawn, "spawn_path", None) or self.cfg.prim_path) if has_spawn else None + probe_path = spawn_path if spawn_path is not None else self.cfg.prim_path + + prim = sim_utils.find_first_matching_prim(probe_path) + if prim is not None and prim.IsValid(): + is_physics = prim.HasAPI(UsdPhysics.ArticulationRootAPI) or prim.HasAPI(UsdPhysics.RigidBodyAPI) + if is_physics: + if not has_spawn: + raise ValueError( + f"Sensor prim_path '{self.cfg.prim_path}' resolves to a physics body but" + f" no spawner is configured (spawn=None). Either set spawn or point" + f" prim_path at a non-physics child (e.g. '{self.cfg.prim_path}/{sensor_name}')." + ) + logger.info( + f"Sensor prim_path '{self.cfg.prim_path}' points at a physics body." + f" Redirecting to '{self.cfg.prim_path}/{sensor_name}'." + ) + self.cfg.prim_path = f"{self.cfg.prim_path}/{sensor_name}" + if getattr(spawn, "spawn_path", None) is not None: + spawn.spawn_path = f"{spawn.spawn_path}/{sensor_name}" + + if not has_spawn: + return + + spawn_target = getattr(spawn, "spawn_path", None) or self.cfg.prim_path + prim = sim_utils.find_first_matching_prim(spawn_target) + if prim is None or not prim.IsValid(): + spawn.func(spawn_target, spawn, **spawn_kwargs) + + check_path = getattr(spawn, "spawn_path", None) or self.cfg.prim_path + if len(sim_utils.find_matching_prims(check_path)) == 0: + raise RuntimeError(f"Could not find prim with path {check_path!r}.") diff --git a/source/isaaclab/isaaclab/sim/__init__.pyi b/source/isaaclab/isaaclab/sim/__init__.pyi index 054c842bd2b4..6d535307357e 100644 --- a/source/isaaclab/isaaclab/sim/__init__.pyi +++ b/source/isaaclab/isaaclab/sim/__init__.pyi @@ -166,8 +166,10 @@ __all__ = [ "resolve_prim_pose", "resolve_prim_scale", "convert_world_pose_to_local", - "BaseXformPrimView", - "UsdXformPrimView", + "BaseFrameView", + "UsdFrameView", + "FrameView", + # Deprecated alias "XformPrimView", ] @@ -339,4 +341,5 @@ from .utils import ( resolve_prim_scale, convert_world_pose_to_local, ) -from .views import BaseXformPrimView, UsdXformPrimView, XformPrimView +from .views import BaseFrameView, UsdFrameView, FrameView +from .views import XformPrimView # deprecated alias diff --git a/source/isaaclab/isaaclab/sim/spawners/sensors/sensors.py b/source/isaaclab/isaaclab/sim/spawners/sensors/sensors.py index df05144455fa..468a4d13257d 100644 --- a/source/isaaclab/isaaclab/sim/spawners/sensors/sensors.py +++ b/source/isaaclab/isaaclab/sim/spawners/sensors/sensors.py @@ -159,8 +159,8 @@ def spawn_ray_caster_xform( The function creates a plain ``Xform`` with the given local pose under its parent. It is intended for :class:`~isaaclab.sensors.ray_caster.ray_caster.RayCaster` when - :attr:`~isaaclab.sensors.ray_caster.ray_caster_cfg.RayCasterCfg.spawn` is set, mirroring how - :func:`spawn_camera` creates camera prims before :class:`~isaaclab.sim.views.XformPrimView` + :attr:`~isaaclab.sensors.ray_caster.ray_caster_cfg.RayCasterCfg.spawn` is set, mirroring how + :func:`spawn_camera` creates camera prims before :class:`~isaaclab.sim.views.FrameView` is constructed. .. note:: diff --git a/source/isaaclab/isaaclab/sim/spawners/sensors/sensors_cfg.py b/source/isaaclab/isaaclab/sim/spawners/sensors/sensors_cfg.py index 34c6dad14539..fc5afa5b231d 100644 --- a/source/isaaclab/isaaclab/sim/spawners/sensors/sensors_cfg.py +++ b/source/isaaclab/isaaclab/sim/spawners/sensors/sensors_cfg.py @@ -228,9 +228,9 @@ class FisheyeCameraCfg(PinholeCameraCfg): class RayCasterXformCfg(SpawnerCfg): """Configuration for spawning a plain USD Xform used as a ray-cast sensor frame. - The spawned prim has no rigid body or collision API. This matches the pattern used by + The spawned prim has no rigid body or collision API. This matches the pattern used by :class:`~isaaclab.sensors.camera.camera_cfg.CameraCfg`, where the sensor prim is created at - runtime so :class:`~isaaclab.sim.views.XformPrimView` tracks a non-physics site (required for + runtime so :class:`~isaaclab.sim.views.FrameView` tracks a non-physics site (required for backends such as Newton where link prims are physics-owned). """ diff --git a/source/isaaclab/isaaclab/sim/views/__init__.pyi b/source/isaaclab/isaaclab/sim/views/__init__.pyi index 444f9986b9a5..d578f85d6ada 100644 --- a/source/isaaclab/isaaclab/sim/views/__init__.pyi +++ b/source/isaaclab/isaaclab/sim/views/__init__.pyi @@ -4,11 +4,15 @@ # SPDX-License-Identifier: BSD-3-Clause __all__ = [ - "BaseXformPrimView", - "UsdXformPrimView", + "BaseFrameView", + "UsdFrameView", + "FrameView", + # Deprecated alias "XformPrimView", ] -from .base_xform_prim_view import BaseXformPrimView -from .usd_xform_prim_view import UsdXformPrimView +from .base_frame_view import BaseFrameView +from .usd_frame_view import UsdFrameView +from .frame_view import FrameView +# Deprecated alias from .xform_prim_view import XformPrimView diff --git a/source/isaaclab/isaaclab/sim/views/base_xform_prim_view.py b/source/isaaclab/isaaclab/sim/views/base_frame_view.py similarity index 97% rename from source/isaaclab/isaaclab/sim/views/base_xform_prim_view.py rename to source/isaaclab/isaaclab/sim/views/base_frame_view.py index c54f2a576170..fc59c2ed83ab 100644 --- a/source/isaaclab/isaaclab/sim/views/base_xform_prim_view.py +++ b/source/isaaclab/isaaclab/sim/views/base_frame_view.py @@ -12,12 +12,12 @@ import warp as wp -class BaseXformPrimView(abc.ABC): +class BaseFrameView(abc.ABC): """Abstract interface for reading and writing world-space transforms of multiple prims. Backend-specific implementations (USD/Fabric, Newton GPU state, etc.) subclass this to provide efficient batched pose queries. The factory - :class:`~isaaclab.sim.views.XformPrimView` selects the correct + :class:`~isaaclab.sim.views.FrameView` selects the correct implementation at runtime based on the active physics backend. All getters return ``wp.array``. Setters accept ``wp.array``. diff --git a/source/isaaclab/isaaclab/sim/views/frame_view.py b/source/isaaclab/isaaclab/sim/views/frame_view.py new file mode 100644 index 000000000000..ea9d5bfbeea9 --- /dev/null +++ b/source/isaaclab/isaaclab/sim/views/frame_view.py @@ -0,0 +1,48 @@ +# Copyright (c) 2022-2026, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Backend-dispatching FrameView. + +``FrameView(path, device=...)`` automatically selects the right backend: +- PhysX: :class:`~isaaclab_physx.sim.views.FabricFrameView` +- Newton: :class:`~isaaclab_newton.sim.views.NewtonSiteFrameView` +""" + +from __future__ import annotations + +from isaaclab.utils.backend_utils import FactoryBase + +from .base_frame_view import BaseFrameView + + +class FrameView(FactoryBase, BaseFrameView): + """FrameView that dispatches to the active physics backend. + + Callers use ``FrameView(prim_path, device=device)`` and get the + correct implementation automatically: + + - **PhysX / no backend**: :class:`~isaaclab_physx.sim.views.FabricFrameView` + (Fabric GPU acceleration with USD fallback). + - **Newton**: :class:`~isaaclab_newton.sim.views.NewtonSiteFrameView` + (GPU-resident site-based transforms). + """ + + _backend_class_names = {"physx": "FabricFrameView", "newton": "NewtonSiteFrameView"} + + @classmethod + def _get_backend(cls, *args, **kwargs) -> str: + from isaaclab.sim.simulation_context import SimulationContext # noqa: PLC0415 + + ctx = SimulationContext.instance() + if ctx is None: + return "physx" + manager_name = ctx.physics_manager.__name__.lower() + if "newton" in manager_name: + return "newton" + return "physx" + + def __new__(cls, *args, **kwargs) -> BaseFrameView: + """Create a new FrameView for the active physics backend.""" + return super().__new__(cls, *args, **kwargs) diff --git a/source/isaaclab/isaaclab/sim/views/usd_xform_prim_view.py b/source/isaaclab/isaaclab/sim/views/usd_frame_view.py similarity index 98% rename from source/isaaclab/isaaclab/sim/views/usd_xform_prim_view.py rename to source/isaaclab/isaaclab/sim/views/usd_frame_view.py index db784b27f3ab..4421fa5391ea 100644 --- a/source/isaaclab/isaaclab/sim/views/usd_xform_prim_view.py +++ b/source/isaaclab/isaaclab/sim/views/usd_frame_view.py @@ -15,12 +15,12 @@ import isaaclab.sim as sim_utils -from .base_xform_prim_view import BaseXformPrimView +from .base_frame_view import BaseFrameView logger = logging.getLogger(__name__) -class UsdXformPrimView(BaseXformPrimView): +class UsdFrameView(BaseFrameView): """Batched interface for reading and writing transforms of multiple USD prims. Provides batch operations for getting and setting poses (position and orientation) @@ -32,7 +32,7 @@ class UsdXformPrimView(BaseXformPrimView): - **Local poses**: Positions and orientations relative to each prim's parent For GPU-accelerated Fabric operations, use the PhysX backend variant - obtained via :class:`~isaaclab.sim.views.XformPrimView`. + obtained via :class:`~isaaclab.sim.views.FrameView`. All getters return ``wp.array``. Setters accept ``wp.array``. @@ -160,7 +160,6 @@ def set_world_poses( world_pos = positions_array[idx] if positions_array is not None else None world_quat = orientations_array[idx] if orientations_array is not None else None - # Convert world pose to local pose relative to parent if parent_prim.IsValid() and parent_prim.GetPath() != Sdf.Path.absoluteRootPath: if positions_array is None or orientations_array is None: prim_tf = xform_cache.GetLocalToWorldTransform(prim) diff --git a/source/isaaclab/isaaclab/sim/views/xform_prim_view.py b/source/isaaclab/isaaclab/sim/views/xform_prim_view.py index 05e7a280171e..ce480fa65594 100644 --- a/source/isaaclab/isaaclab/sim/views/xform_prim_view.py +++ b/source/isaaclab/isaaclab/sim/views/xform_prim_view.py @@ -3,46 +3,8 @@ # # SPDX-License-Identifier: BSD-3-Clause -"""Backend-dispatching XformPrimView. +"""Backward-compatibility alias: ``XformPrimView`` -> :class:`FrameView`.""" -``XformPrimView(path, device=...)`` automatically selects the right backend: -- PhysX: :class:`~isaaclab_physx.sim.views.FabricXformPrimView` -- Newton: :class:`~isaaclab_newton.sim.views.NewtonSiteXformPrimView` -""" +from .frame_view import FrameView -from __future__ import annotations - -from isaaclab.utils.backend_utils import FactoryBase - -from .base_xform_prim_view import BaseXformPrimView - - -class XformPrimView(FactoryBase, BaseXformPrimView): - """XformPrimView that dispatches to the active physics backend. - - Callers use ``XformPrimView(prim_path, device=device)`` and get the - correct implementation automatically: - - - **PhysX / no backend**: :class:`~isaaclab_physx.sim.views.FabricXformPrimView` - (Fabric GPU acceleration with USD fallback). - - **Newton**: :class:`~isaaclab_newton.sim.views.NewtonSiteXformPrimView` - (GPU-resident site-based transforms). - """ - - _backend_class_names = {"physx": "FabricXformPrimView", "newton": "NewtonSiteXformPrimView"} - - @classmethod - def _get_backend(cls, *args, **kwargs) -> str: - from isaaclab.sim.simulation_context import SimulationContext # noqa: PLC0415 - - ctx = SimulationContext.instance() - if ctx is None: - return "physx" - manager_name = ctx.physics_manager.__name__.lower() - if "newton" in manager_name: - return "newton" - return "physx" - - def __new__(cls, *args, **kwargs) -> BaseXformPrimView: - """Create a new XformPrimView for the active physics backend.""" - return super().__new__(cls, *args, **kwargs) +XformPrimView = FrameView diff --git a/source/isaaclab/test/sim/test_views_xform_prim.py b/source/isaaclab/test/sim/test_views_xform_prim.py index 75e5d7535c2e..0ae78ddea9bc 100644 --- a/source/isaaclab/test/sim/test_views_xform_prim.py +++ b/source/isaaclab/test/sim/test_views_xform_prim.py @@ -3,7 +3,7 @@ # # SPDX-License-Identifier: BSD-3-Clause -"""USD backend tests for XformPrimView. +"""USD backend tests for FrameView. Imports the shared contract tests and provides the USD-specific ``view_factory`` fixture. Also includes USD-only tests for visibility, @@ -29,7 +29,7 @@ from xform_contract_tests import CHILD_OFFSET, ViewBundle # noqa: E402 import isaaclab.sim as sim_utils # noqa: E402 -from isaaclab.sim.views import UsdXformPrimView as XformPrimView # noqa: E402 +from isaaclab.sim.views import UsdFrameView as FrameView # noqa: E402 from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR # noqa: E402 PARENT_POS = (0.0, 0.0, 1.0) @@ -84,7 +84,7 @@ def factory(num_envs: int, device: str) -> ViewBundle: sim_utils.create_prim(f"/World/Parent_{i}", "Xform", translation=PARENT_POS, stage=stage) sim_utils.create_prim(f"/World/Parent_{i}/Child", "Xform", translation=CHILD_OFFSET, stage=stage) - view = XformPrimView("/World/Parent_.*/Child", device=device) + view = FrameView("/World/Parent_.*/Child", device=device) return ViewBundle( view=view, get_parent_pos=_get_parent_positions, @@ -111,7 +111,7 @@ def test_visibility_toggle(device): for i in range(num_prims): sim_utils.create_prim(f"/World/Object_{i}", "Xform", stage=stage) - view = XformPrimView("/World/Object_.*", device=device) + view = FrameView("/World/Object_.*", device=device) assert torch.all(view.get_visibility()) @@ -139,8 +139,8 @@ def test_visibility_parent_inheritance(device): for i in range(4): sim_utils.create_prim(f"/World/Parent/Child_{i}", "Xform", stage=stage) - parent_view = XformPrimView("/World/Parent", device=device) - children_view = XformPrimView("/World/Parent/Child_.*", device=device) + parent_view = FrameView("/World/Parent", device=device) + children_view = FrameView("/World/Parent/Child_.*", device=device) parent_view.set_visibility(torch.tensor([False], dtype=torch.bool, device=device)) assert not torch.any(children_view.get_visibility()) @@ -167,7 +167,7 @@ def test_prim_ordering_follows_creation_order(device): sim_utils.create_prim(f"/World/Env_{i}/Object_0", "Xform", stage=stage) sim_utils.create_prim(f"/World/Env_{i}/Object_A", "Xform", stage=stage) - view = XformPrimView("/World/Env_.*/Object_.*", device=device) + view = FrameView("/World/Env_.*/Object_.*", device=device) expected = [] for i in range(num_envs): expected += [f"/World/Env_{i}/Object_1", f"/World/Env_{i}/Object_0", f"/World/Env_{i}/Object_A"] @@ -182,7 +182,7 @@ def test_prim_ordering_follows_creation_order(device): @pytest.mark.parametrize("device", ["cpu", "cuda"]) def test_standardize_transform_op(device): - """XformPrimView standardizes a prim with xformOp:transform to translate/orient/scale.""" + """FrameView standardizes a prim with xformOp:transform to translate/orient/scale.""" if device == "cuda" and not torch.cuda.is_available(): pytest.skip("CUDA not available") @@ -194,7 +194,7 @@ def test_standardize_transform_op(device): prim = stage.DefinePrim("/World/TransformPrim", "Xform") UsdGeom.Xformable(prim).AddTransformOp().Set(matrix) - view = XformPrimView("/World/TransformPrim", device=device) + view = FrameView("/World/TransformPrim", device=device) assert sim_utils.validate_standard_xform_ops(view.prims[0]) ordered_ops = UsdGeom.Xformable(view.prims[0]).GetOrderedXformOps() @@ -222,8 +222,8 @@ def test_nested_hierarchy_world_poses(device): sim_utils.create_prim(f"/World/Frame_{i}", "Xform", translation=frame_positions[i], stage=stage) sim_utils.create_prim(f"/World/Frame_{i}/Target", "Xform", translation=target_positions[i], stage=stage) - frames_view = XformPrimView("/World/Frame_.*", device=device) - targets_view = XformPrimView("/World/Frame_.*/Target", device=device) + frames_view = FrameView("/World/Frame_.*", device=device) + targets_view = FrameView("/World/Frame_.*/Target", device=device) frames_view.set_local_poses(translations=torch.tensor(frame_positions, device=device)) targets_view.set_local_poses(translations=torch.tensor(target_positions, device=device)) @@ -254,7 +254,7 @@ def test_compare_get_world_poses_with_isaacsim(): sim_utils.create_prim(f"/World/Env_{i}/Object", "Xform", translation=pos, orientation=quat, stage=stage) pattern = "/World/Env_.*/Object" - isaaclab_view = XformPrimView(pattern, device="cpu") + isaaclab_view = FrameView(pattern, device="cpu") isaacsim_view = _IsaacSimXformPrimView(pattern, reset_xform_properties=False) isaaclab_pos = wp.to_torch(isaaclab_view.get_world_poses()[0]) @@ -272,7 +272,7 @@ def test_compare_get_world_poses_with_isaacsim(): @pytest.mark.parametrize("device", ["cpu", "cuda"]) def test_with_franka_robots(device): - """Verify XformPrimView works with real Franka robot USD assets.""" + """Verify FrameView works with real Franka robot USD assets.""" if device == "cuda" and not torch.cuda.is_available(): pytest.skip("CUDA not available") @@ -282,7 +282,7 @@ def test_with_franka_robots(device): sim_utils.create_prim("/World/Franka_1", "Xform", usd_path=franka_usd_path, stage=stage) sim_utils.create_prim("/World/Franka_2", "Xform", usd_path=franka_usd_path, stage=stage) - view = XformPrimView("/World/Franka_.*", device=device) + view = FrameView("/World/Franka_.*", device=device) assert view.count == 2 positions = wp.to_torch(view.get_world_poses()[0]) diff --git a/source/isaaclab/test/sim/xform_contract_tests.py b/source/isaaclab/test/sim/xform_contract_tests.py index 9e231582a3c4..2266e2bca2f1 100644 --- a/source/isaaclab/test/sim/xform_contract_tests.py +++ b/source/isaaclab/test/sim/xform_contract_tests.py @@ -3,9 +3,9 @@ # # SPDX-License-Identifier: BSD-3-Clause -"""Shared XformPrimView contract tests. +"""Shared FrameView contract tests. -This module defines the invariants that **every** XformPrimView backend +This module defines the invariants that **every** FrameView backend (USD, Fabric, Newton) must satisfy. Backend test files import these tests via ``from isaaclab.test.sim.xform_contract_tests import *`` and provide a ``view_factory`` pytest fixture that builds the backend-specific scene. @@ -17,12 +17,12 @@ def view_factory() -> Callable[[int, str], ViewBundle]: ... Where ``ViewBundle`` is a :class:`NamedTuple`:: class ViewBundle(NamedTuple): - view: BaseXformPrimView + view: BaseFrameView get_parent_pos: Callable[[int, str], torch.Tensor] set_parent_pos: Callable[[torch.Tensor, int], None] teardown: Callable[[], None] -- ``view``: The XformPrimView under test. Must track child prims at +- ``view``: The FrameView under test. Must track child prims at :data:`CHILD_OFFSET` under parent prims/bodies. - ``get_parent_pos(n, device)``: Read the parent prim/body positions. - ``set_parent_pos(positions, n)``: Write the parent prim/body positions. diff --git a/source/isaaclab/test/terrains/check_terrain_importer.py b/source/isaaclab/test/terrains/check_terrain_importer.py index 519f84fc2743..c024d33bb5f1 100644 --- a/source/isaaclab/test/terrains/check_terrain_importer.py +++ b/source/isaaclab/test/terrains/check_terrain_importer.py @@ -153,8 +153,8 @@ def main(): physics_scene_path, "/World/collisions", prim_paths=envs_prim_paths, global_paths=["/World/ground"] ) - # Set ball positions over terrain origins using XformPrimView (before simulation starts) - xform_view = sim_utils.XformPrimView("/World/envs/env_.*/ball") + # Set ball positions over terrain origins using FrameView (before simulation starts) + xform_view = sim_utils.FrameView("/World/envs/env_.*/ball") # cache initial state of the balls ball_initial_positions = terrain_importer.env_origins.clone() ball_initial_positions[:, 2] += 5.0 diff --git a/source/isaaclab/test/terrains/test_terrain_importer.py b/source/isaaclab/test/terrains/test_terrain_importer.py index 3951296e978a..8842c6df673b 100644 --- a/source/isaaclab/test/terrains/test_terrain_importer.py +++ b/source/isaaclab/test/terrains/test_terrain_importer.py @@ -327,8 +327,8 @@ def _populate_scene(sim: SimulationContext, num_balls: int = 2048, geom_sphere: ) # Set ball positions over terrain origins - # Create a view over all the balls using Isaac Lab's XformPrimView - ball_view = sim_utils.XformPrimView("/World/envs/env_.*/ball") + # Create a view over all the balls using Isaac Lab's FrameView + ball_view = sim_utils.FrameView("/World/envs/env_.*/ball") # cache initial state of the balls ball_initial_positions = terrain_importer.env_origins.clone() ball_initial_positions[:, 2] += 5.0 diff --git a/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/scene_utils.py b/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/scene_utils.py index a3b922300468..2a05b5f70096 100644 --- a/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/scene_utils.py +++ b/source/isaaclab_mimic/isaaclab_mimic/locomanipulation_sdg/scene_utils.py @@ -10,7 +10,7 @@ import warp as wp import isaaclab.utils.math as math_utils -from isaaclab.sim.views import XformPrimView +from isaaclab.sim.views import FrameView from .occupancy_map_utils import OccupancyMap, intersect_occupancy_maps from .transform_utils import transform_mul @@ -101,12 +101,12 @@ def __init__(self, scene, entity_name: str): self.scene = scene self.entity_name = entity_name - def _get_xform_view(self) -> XformPrimView: - """Return the XformPrimView for this asset, refreshing it if prims were not yet cloned.""" + def _get_xform_view(self) -> FrameView: + """Return the FrameView for this asset, refreshing it if prims were not yet cloned.""" xform_prim = self.scene[self.entity_name] if xform_prim.count == 0: # The view was created before environment cloning; rebuild it now that prims exist. - xform_prim = XformPrimView(xform_prim._prim_path, device=xform_prim.device) + xform_prim = FrameView(xform_prim._prim_path, device=xform_prim.device) self.scene.extras[self.entity_name] = xform_prim return xform_prim diff --git a/source/isaaclab_newton/docs/CHANGELOG.rst b/source/isaaclab_newton/docs/CHANGELOG.rst index 2ae5c9fe6fec..81a0f0d8111a 100644 --- a/source/isaaclab_newton/docs/CHANGELOG.rst +++ b/source/isaaclab_newton/docs/CHANGELOG.rst @@ -10,6 +10,12 @@ Added * Added :class:`~isaaclab_newton.sim.views.XformPrimView` providing the Newton backend implementation for xform prim views. +Changed +^^^^^^^ + +* Renamed :class:`~isaaclab_newton.sim.views.NewtonSiteXformPrimView` to + :class:`~isaaclab_newton.sim.views.NewtonSiteFrameView`. Old name is kept as a deprecated alias. + 0.5.13 (2026-04-13) ~~~~~~~~~~~~~~~~~~~ diff --git a/source/isaaclab_newton/isaaclab_newton/sim/views/__init__.pyi b/source/isaaclab_newton/isaaclab_newton/sim/views/__init__.pyi index f8a1a09c23f7..433dfc1e8b6a 100644 --- a/source/isaaclab_newton/isaaclab_newton/sim/views/__init__.pyi +++ b/source/isaaclab_newton/isaaclab_newton/sim/views/__init__.pyi @@ -4,7 +4,7 @@ # SPDX-License-Identifier: BSD-3-Clause __all__ = [ - "NewtonSiteXformPrimView", + "NewtonSiteFrameView", ] -from .newton_site_xform_prim_view import NewtonSiteXformPrimView +from .newton_site_frame_view import NewtonSiteFrameView diff --git a/source/isaaclab_newton/isaaclab_newton/sim/views/newton_site_xform_prim_view.py b/source/isaaclab_newton/isaaclab_newton/sim/views/newton_site_frame_view.py similarity index 97% rename from source/isaaclab_newton/isaaclab_newton/sim/views/newton_site_xform_prim_view.py rename to source/isaaclab_newton/isaaclab_newton/sim/views/newton_site_frame_view.py index af8aa30d8aeb..a6b62a597479 100644 --- a/source/isaaclab_newton/isaaclab_newton/sim/views/newton_site_xform_prim_view.py +++ b/source/isaaclab_newton/isaaclab_newton/sim/views/newton_site_frame_view.py @@ -3,7 +3,7 @@ # # SPDX-License-Identifier: BSD-3-Clause -"""Newton-backed XformPrimView — Warp-native, GPU-resident pose queries.""" +"""Newton-backed FrameView — Warp-native, GPU-resident pose queries.""" from __future__ import annotations @@ -15,7 +15,7 @@ import isaaclab.sim as sim_utils from isaaclab.physics import PhysicsEvent -from isaaclab.sim.views.base_xform_prim_view import BaseXformPrimView +from isaaclab.sim.views.base_frame_view import BaseFrameView from isaaclab_newton.physics.newton_manager import NewtonManager @@ -349,7 +349,7 @@ def _write_site_local_from_local_poses_indexed( # ------------------------------------------------------------------ -class NewtonSiteXformPrimView(BaseXformPrimView): +class NewtonSiteFrameView(BaseFrameView): """Batched prim view for non-physics prims tracked as sites on Newton bodies. Each matched USD prim must be a **non-physics** prim (camera, sensor, @@ -417,14 +417,14 @@ def _initialize_impl(self, model) -> None: pp = prim.GetPath().pathString if pp in body_label_set: raise ValueError( - f"XformPrimView prim '{pp}' is a Newton physics body. " - "XformPrimView should only be used for non-physics prims (cameras, sensors, Xform markers). " + f"FrameView prim '{pp}' is a Newton physics body. " + "FrameView should only be used for non-physics prims (cameras, sensors, Xform markers). " "Use Articulation or RigidObject APIs to control physics bodies." ) if pp in shape_label_set: raise ValueError( - f"XformPrimView prim '{pp}' is a Newton collision shape. " - "XformPrimView should only be used for non-physics prims (cameras, sensors, Xform markers). " + f"FrameView prim '{pp}' is a Newton collision shape. " + "FrameView should only be used for non-physics prims (cameras, sensors, Xform markers). " "Use Articulation or RigidObject APIs to control collision shapes." ) diff --git a/source/isaaclab_newton/test/sim/test_views_xform_prim_newton.py b/source/isaaclab_newton/test/sim/test_views_xform_prim_newton.py index cbb50ba1dc3d..cc9567c094cc 100644 --- a/source/isaaclab_newton/test/sim/test_views_xform_prim_newton.py +++ b/source/isaaclab_newton/test/sim/test_views_xform_prim_newton.py @@ -3,7 +3,7 @@ # # SPDX-License-Identifier: BSD-3-Clause -"""Newton backend tests for XformPrimView. +"""Newton backend tests for FrameView. Imports the shared contract tests and provides the Newton-specific ``view_factory`` fixture. Also includes Newton-only guard tests and @@ -21,7 +21,7 @@ import warp as wp from isaaclab_newton.physics import MJWarpSolverCfg, NewtonCfg from isaaclab_newton.physics.newton_manager import NewtonManager -from isaaclab_newton.sim.views import NewtonSiteXformPrimView as XformPrimView +from isaaclab_newton.sim.views import NewtonSiteFrameView as FrameView from xform_contract_tests import * # noqa: F401, F403 — import all contract tests from xform_contract_tests import CHILD_OFFSET, ViewBundle, _wp_vec3f, _wp_vec4f @@ -94,7 +94,7 @@ def factory(num_envs: int, device: str) -> ViewBundle: prim.GetAttribute("xformOp:orient").Set(Gf.Quatd(1.0, 0.0, 0.0, 0.0)) sim.reset() - view = XformPrimView("/World/envs/env_.*/Cube/CameraMount", device=device) + view = FrameView("/World/envs/env_.*/Cube/CameraMount", device=device) return ViewBundle( view=view, @@ -113,7 +113,7 @@ def factory(num_envs: int, device: str) -> ViewBundle: @pytest.mark.parametrize("device", ["cpu", "cuda:0"]) def test_reject_body_path(device): - """XformPrimView rejects prim paths that resolve to a Newton physics body.""" + """FrameView rejects prim paths that resolve to a Newton physics body.""" ctx = _sim_context(device, num_envs=2) sim = ctx.__enter__() sim._app_control_on_stop_handle = None @@ -121,13 +121,13 @@ def test_reject_body_path(device): sim.reset() with pytest.raises(ValueError, match="physics body"): - XformPrimView("/World/envs/env_.*/Cube", device=device) + FrameView("/World/envs/env_.*/Cube", device=device) ctx.__exit__(None, None, None) @pytest.mark.parametrize("device", ["cpu", "cuda:0"]) def test_reject_shape_path(device): - """XformPrimView rejects prim paths that resolve to a Newton collision shape.""" + """FrameView rejects prim paths that resolve to a Newton collision shape.""" ctx = _sim_context(device, num_envs=2) sim = ctx.__enter__() sim._app_control_on_stop_handle = None @@ -139,7 +139,7 @@ def test_reject_shape_path(device): pytest.skip("No shapes in model") with pytest.raises(ValueError, match="collision shape"): - XformPrimView(shape_labels[0], device=device) + FrameView(shape_labels[0], device=device) ctx.__exit__(None, None, None) @@ -163,7 +163,7 @@ def test_world_attached_returns_initial_pose(device): prim.GetAttribute("xformOp:orient").Set(Gf.Quatd(1.0, 0.0, 0.0, 0.0)) sim.reset() - view = XformPrimView("/World/StaticMarker", device=device) + view = FrameView("/World/StaticMarker", device=device) pos = wp.to_torch(view.get_world_poses()[0]) expected = torch.tensor([list(WORLD_MARKER_POS)], device=device) @@ -186,7 +186,7 @@ def test_world_attached_set_world_roundtrip(device): prim.GetAttribute("xformOp:orient").Set(Gf.Quatd(1.0, 0.0, 0.0, 0.0)) sim.reset() - view = XformPrimView("/World/StaticMarker", device=device) + view = FrameView("/World/StaticMarker", device=device) new_pos = _wp_vec3f([[10.0, 20.0, 30.0]], device=device) new_quat = _wp_vec4f([[0.0, 0.0, 0.0, 1.0]], device=device) diff --git a/source/isaaclab_physx/docs/CHANGELOG.rst b/source/isaaclab_physx/docs/CHANGELOG.rst index 0c7fa5f4a47c..3164fab531d0 100644 --- a/source/isaaclab_physx/docs/CHANGELOG.rst +++ b/source/isaaclab_physx/docs/CHANGELOG.rst @@ -10,6 +10,12 @@ Added * Added :class:`~isaaclab_physx.sim.views.XformPrimView` providing the PhysX/Fabric backend implementation for xform prim views. +Changed +^^^^^^^ + +* Renamed :class:`~isaaclab_physx.sim.views.FabricXformPrimView` to + :class:`~isaaclab_physx.sim.views.FabricFrameView`. Old name is kept as a deprecated alias. + 0.5.16 (2026-04-13) ~~~~~~~~~~~~~~~~~~~ diff --git a/source/isaaclab_physx/isaaclab_physx/scene_data_providers/physx_scene_data_provider.py b/source/isaaclab_physx/isaaclab_physx/scene_data_providers/physx_scene_data_provider.py index 995fd0708ff0..3e3778fec166 100644 --- a/source/isaaclab_physx/isaaclab_physx/scene_data_providers/physx_scene_data_provider.py +++ b/source/isaaclab_physx/isaaclab_physx/scene_data_providers/physx_scene_data_provider.py @@ -53,7 +53,7 @@ class PhysxSceneDataProvider(BaseSceneDataProvider): """Scene data provider for Omni PhysX backend. Supports: - - body poses via PhysX tensor views, with XformPrimView fallback + - body poses via PhysX tensor views, with FrameView fallback - camera poses & intrinsics - USD stage handles - Newton model/state handles @@ -547,13 +547,13 @@ def _apply_view_poses(self, view: Any, view_key: str, positions: Any, orientatio return count def _apply_xform_poses(self, positions: Any, orientations: Any, covered: Any, xform_mask: Any) -> int: - """Fill remaining poses using XformPrimView (USD fallback). + """Fill remaining poses using FrameView (USD fallback). This is slower but more robust when PhysX views don't cover all bodies. """ import torch - from isaaclab.sim.views import XformPrimView + from isaaclab.sim.views import FrameView uncovered = torch.where(~covered)[0].cpu().tolist() if not uncovered: @@ -565,7 +565,7 @@ def _apply_xform_poses(self, positions: Any, orientations: Any, covered: Any, xf path = self._rigid_body_paths[idx] try: if path not in self._xform_views: - self._xform_views[path] = XformPrimView( + self._xform_views[path] = FrameView( path, device=self._device, stage=self._stage, validate_xform_ops=False ) @@ -592,7 +592,7 @@ def _apply_xform_poses(self, positions: Any, orientations: Any, covered: Any, xf def _convert_xform_quats(self, orientations: Any, xform_mask: Any) -> Any: """Return quaternions in xyzw convention. - PhysX views, XformPrimView, and resolve_prim_pose() in Isaac Lab all use xyzw. + PhysX views, FrameView, and resolve_prim_pose() in Isaac Lab all use xyzw. Keeping this helper as a no-op preserves a single conversion point if conventions ever diverge again. """ diff --git a/source/isaaclab_physx/isaaclab_physx/sim/views/__init__.pyi b/source/isaaclab_physx/isaaclab_physx/sim/views/__init__.pyi index 6deedc4b8e4d..789d62af9d14 100644 --- a/source/isaaclab_physx/isaaclab_physx/sim/views/__init__.pyi +++ b/source/isaaclab_physx/isaaclab_physx/sim/views/__init__.pyi @@ -4,7 +4,7 @@ # SPDX-License-Identifier: BSD-3-Clause __all__ = [ - "FabricXformPrimView", + "FabricFrameView", ] -from .fabric_xform_prim_view import FabricXformPrimView +from .fabric_frame_view import FabricFrameView diff --git a/source/isaaclab_physx/isaaclab_physx/sim/views/fabric_xform_prim_view.py b/source/isaaclab_physx/isaaclab_physx/sim/views/fabric_frame_view.py similarity index 96% rename from source/isaaclab_physx/isaaclab_physx/sim/views/fabric_xform_prim_view.py rename to source/isaaclab_physx/isaaclab_physx/sim/views/fabric_frame_view.py index 63d3ff968073..87adad2238c4 100644 --- a/source/isaaclab_physx/isaaclab_physx/sim/views/fabric_xform_prim_view.py +++ b/source/isaaclab_physx/isaaclab_physx/sim/views/fabric_frame_view.py @@ -3,7 +3,7 @@ # # SPDX-License-Identifier: BSD-3-Clause -"""PhysX XformPrimView with Fabric GPU acceleration.""" +"""PhysX FrameView with Fabric GPU acceleration.""" from __future__ import annotations @@ -16,8 +16,8 @@ import isaaclab.sim as sim_utils from isaaclab.app.settings_manager import SettingsManager -from isaaclab.sim.views.base_xform_prim_view import BaseXformPrimView -from isaaclab.sim.views.usd_xform_prim_view import UsdXformPrimView +from isaaclab.sim.views.base_frame_view import BaseFrameView +from isaaclab.sim.views.usd_frame_view import UsdFrameView from isaaclab.utils.warp import fabric as fabric_utils logger = logging.getLogger(__name__) @@ -39,10 +39,10 @@ def _to_float32_2d(a: wp.array | torch.Tensor) -> wp.array | torch.Tensor: return a.view(dtype=wp.float32) -class FabricXformPrimView(BaseXformPrimView): - """XformPrimView with Fabric GPU acceleration for the PhysX backend. +class FabricFrameView(BaseFrameView): + """FrameView with Fabric GPU acceleration for the PhysX backend. - Uses composition: holds a :class:`UsdXformPrimView` internally for USD + Uses composition: holds a :class:`UsdFrameView` internally for USD fallback and non-accelerated operations (local poses, visibility, scales when Fabric is disabled). @@ -61,7 +61,7 @@ def __init__( sync_usd_on_fabric_write: bool = False, stage: Usd.Stage | None = None, ): - self._usd_view = UsdXformPrimView(prim_path, device=device, validate_xform_ops=validate_xform_ops, stage=stage) + self._usd_view = UsdFrameView(prim_path, device=device, validate_xform_ops=validate_xform_ops, stage=stage) self._device = device self._sync_usd_on_fabric_write = sync_usd_on_fabric_write diff --git a/source/isaaclab_physx/test/sim/test_views_xform_prim_fabric.py b/source/isaaclab_physx/test/sim/test_views_xform_prim_fabric.py index 0e4275c133a2..66ccd399bd75 100644 --- a/source/isaaclab_physx/test/sim/test_views_xform_prim_fabric.py +++ b/source/isaaclab_physx/test/sim/test_views_xform_prim_fabric.py @@ -3,7 +3,7 @@ # # SPDX-License-Identifier: BSD-3-Clause -"""PhysX Fabric backend tests for XformPrimView. +"""PhysX Fabric backend tests for FrameView. Imports the shared contract tests and provides the Fabric-specific ``view_factory`` fixture (SimulationContext with use_fabric=True, @@ -21,7 +21,7 @@ import pytest # noqa: E402 import torch # noqa: E402 -from isaaclab_physx.sim.views import FabricXformPrimView as XformPrimView # noqa: E402 +from isaaclab_physx.sim.views import FabricFrameView as FrameView # noqa: E402 from xform_contract_tests import * # noqa: F401, F403, E402 from xform_contract_tests import CHILD_OFFSET, ViewBundle # noqa: E402 @@ -94,7 +94,7 @@ def factory(num_envs: int, device: str) -> ViewBundle: sim_utils.create_prim(f"/World/Parent_{i}/Child", "Camera", translation=CHILD_OFFSET, stage=stage) sim_utils.SimulationContext(sim_utils.SimulationCfg(dt=0.01, device=device, use_fabric=True)) - view = XformPrimView("/World/Parent_.*/Child", device=device) + view = FrameView("/World/Parent_.*/Child", device=device, sync_usd_on_fabric_write=True) return ViewBundle( view=view, get_parent_pos=_get_parent_positions, diff --git a/source/isaaclab_tasks/docs/CHANGELOG.rst b/source/isaaclab_tasks/docs/CHANGELOG.rst index 004efe657353..cc2693ac701b 100644 --- a/source/isaaclab_tasks/docs/CHANGELOG.rst +++ b/source/isaaclab_tasks/docs/CHANGELOG.rst @@ -11,6 +11,8 @@ Changed height-scanner defaults to spawn a ``raycaster`` Xform child under the robot attachment link (using :class:`~isaaclab.sim.spawners.sensors.sensors_cfg.RayCasterXformCfg`) so the sensor works with Newton site-based :class:`~isaaclab.sim.views.XformPrimView` tracking. +* Updated all sensor configurations to use :class:`~isaaclab.sim.views.FrameView` instead of + the deprecated ``XformPrimView``. 1.5.21 (2026-04-13) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/mdp/terminations.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/mdp/terminations.py index 2b87dc69df76..8e530a7d71e0 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/mdp/terminations.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomanipulation/pick_place/mdp/terminations.py @@ -51,7 +51,7 @@ def task_done_pick_place_table_frame( env: The RL environment instance. task_link_name: Name of the right wrist link on the robot. object_cfg: Configuration for the object entity. - table_cfg: Configuration for the destination table entity (must be an XformPrimView). + table_cfg: Configuration for the destination table entity (must be a FrameView). right_wrist_max_x: Maximum x position of the right wrist in table frame for task completion. min_x: Minimum x position of the object relative to the table for task completion. max_x: Maximum x position of the object relative to the table for task completion. diff --git a/source/isaaclab_teleop/test/test_oxr_device.py b/source/isaaclab_teleop/test/test_oxr_device.py index 2d2ed8444969..1663a56612d9 100644 --- a/source/isaaclab_teleop/test/test_oxr_device.py +++ b/source/isaaclab_teleop/test/test_oxr_device.py @@ -179,12 +179,12 @@ def test_xr_anchor(empty_env, mock_xrcore): device = OpenXRDevice(OpenXRDeviceCfg(xr_cfg=env_cfg.xr)) # Check that the xr anchor prim is created with the correct pose - xr_anchor_view = sim_utils.XformPrimView("/World/XRAnchor") + xr_anchor_view = sim_utils.FrameView("/World/XRAnchor") assert xr_anchor_view.count == 1 position, orientation = xr_anchor_view.get_world_poses() np.testing.assert_almost_equal(position.numpy(), [[1, 2, 3]]) - # XformPrimView returns quaternion in xyzw format, identity is [0, 0, 0, 1] + # FrameView returns quaternion in xyzw format, identity is [0, 0, 0, 1] np.testing.assert_almost_equal(orientation.numpy(), [[0, 0, 0, 1]]) # Check that xr anchor mode and custom anchor are set correctly @@ -202,7 +202,7 @@ def test_xr_anchor_default(empty_env, mock_xrcore): device = OpenXRDevice(OpenXRDeviceCfg()) # Check that the xr anchor prim is created with the correct default pose - xr_anchor_view = sim_utils.XformPrimView("/World/XRAnchor") + xr_anchor_view = sim_utils.FrameView("/World/XRAnchor") assert xr_anchor_view.count == 1 position, orientation = xr_anchor_view.get_world_poses() @@ -225,7 +225,7 @@ def test_xr_anchor_multiple_devices(empty_env, mock_xrcore): device_2 = OpenXRDevice(OpenXRDeviceCfg()) # Check that the xr anchor prim is created with the correct default pose - xr_anchor_view = sim_utils.XformPrimView("/World/XRAnchor") + xr_anchor_view = sim_utils.FrameView("/World/XRAnchor") assert xr_anchor_view.count == 1 position, orientation = xr_anchor_view.get_world_poses() From 8d4078d2a6658d8d2e67728dc5fde0c4ce6c3d20 Mon Sep 17 00:00:00 2001 From: Octi Zhang Date: Sat, 11 Apr 2026 01:04:01 -0700 Subject: [PATCH 13/24] fix small bug --- .../isaaclab/sensors/ray_caster/ray_caster.py | 9 +-- .../test_multi_mesh_ray_caster_camera.py | 4 +- .../isaaclab/test/sensors/test_ray_caster.py | 77 +++++++++++++++++++ 3 files changed, 84 insertions(+), 6 deletions(-) diff --git a/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster.py b/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster.py index a332f888e81e..889841b030c2 100644 --- a/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster.py +++ b/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster.py @@ -69,10 +69,7 @@ def __init__(self, cfg: RayCasterCfg): RayCaster._instance_count += 1 # Initialize base class super().__init__(cfg) - # Only position is baked into the Xform — rotation is applied to the - # ray pattern in _initialize_rays_impl so that ray_alignment modes - # (especially "yaw") still operate on the parent body's orientation. - self._resolve_and_spawn("raycaster", translation=self.cfg.offset.pos) + self._resolve_and_spawn("raycaster") # Create empty variables for storing output data self._data = RayCasterData() @@ -202,9 +199,11 @@ def _initialize_warp_meshes(self): def _initialize_rays_impl(self): self.ray_starts, self.ray_directions = self.cfg.pattern_cfg.func(self.cfg.pattern_cfg, self._device) self.num_rays = len(self.ray_directions) - # apply offset rotation to the ray pattern (position offset is baked into the Xform) + # apply offset to the ray pattern in local space + offset_pos = torch.tensor(list(self.cfg.offset.pos), device=self._device) offset_quat = torch.tensor(list(self.cfg.offset.rot), device=self._device) self.ray_directions = quat_apply(offset_quat.repeat(len(self.ray_directions), 1), self.ray_directions) + self.ray_starts += offset_pos # repeat the rays for each sensor self.ray_starts = self.ray_starts.repeat(self._view.count, 1, 1) self.ray_directions = self.ray_directions.repeat(self._view.count, 1, 1) diff --git a/source/isaaclab/test/sensors/test_multi_mesh_ray_caster_camera.py b/source/isaaclab/test/sensors/test_multi_mesh_ray_caster_camera.py index 2b079760e16a..019d02cbcc5a 100644 --- a/source/isaaclab/test/sensors/test_multi_mesh_ray_caster_camera.py +++ b/source/isaaclab/test/sensors/test_multi_mesh_ray_caster_camera.py @@ -185,7 +185,9 @@ def test_camera_init_intrinsic_matrix(setup_simulation): prim_path="/World/Camera", mesh_prim_paths=["/World/defaultGroundPlane"], update_period=0, - offset=MultiMeshRayCasterCameraCfg.OffsetCfg(pos=(0.0, 0.0, 0.0), rot=(0.0, 0.0, 0.0, 1.0), convention="world"), + offset=MultiMeshRayCasterCameraCfg.OffsetCfg( + pos=(0.0, 0.0, 0.0), rot=(0.0, 0.0, 0.0, 1.0), convention="world" + ), debug_vis=False, pattern_cfg=patterns.PinholeCameraPatternCfg.from_intrinsic_matrix( intrinsic_matrix=intrinsic_matrix, diff --git a/source/isaaclab/test/sensors/test_ray_caster.py b/source/isaaclab/test/sensors/test_ray_caster.py index 944287c549b4..346cb0acbe96 100644 --- a/source/isaaclab/test/sensors/test_ray_caster.py +++ b/source/isaaclab/test/sensors/test_ray_caster.py @@ -239,3 +239,80 @@ def test_raycast_random_cube(raycast_setup): torch.testing.assert_close(ray_distance, ray_distance_m) torch.testing.assert_close(ray_normal, ray_normal_m) torch.testing.assert_close(ray_face_id, ray_face_id_m) + + +## +# RayCaster sensor-level tests +## + + +def test_raycaster_offset_does_not_affect_pos_w(): + """Verify that cfg.offset.pos shifts ray starts but NOT data.pos_w. + + data.pos_w must reflect the parent body position so that downstream + observations like height_scan (pos_w_z - hit_z - 0.5) produce values + relative to the body, not relative to the offset sensor frame. + + Regression test: previously the offset was baked into the FrameView's + Xform local transform, causing data.pos_w to include the 20m offset + and breaking height-scan observations during training. + """ + import isaaclab.sim as sim_utils + from isaaclab.sensors.ray_caster import RayCaster, RayCasterCfg, patterns + from isaaclab.terrains.trimesh.utils import make_plane + from isaaclab.terrains.utils import create_prim_from_mesh + + sim_utils.create_new_stage() + + # ground plane at z=0 + mesh = make_plane(size=(100, 100), height=0.0, center_zero=True) + create_prim_from_mesh("/World/ground", mesh) + + # parent body at known position + body_pos = (0.0, 0.0, 0.6) + sim_utils.create_prim("/World/Robot", "Xform", translation=body_pos) + + # large z-offset to make the regression obvious + offset_z = 20.0 + cfg = RayCasterCfg( + prim_path="/World/Robot/raycaster", + spawn=sim_utils.RayCasterXformCfg(), + offset=RayCasterCfg.OffsetCfg(pos=(0.0, 0.0, offset_z)), + mesh_prim_paths=["/World/ground"], + pattern_cfg=patterns.GridPatternCfg(resolution=0.5, size=[1.0, 1.0]), + ray_alignment="yaw", + ) + + dt = 0.01 + sim = sim_utils.SimulationContext(sim_utils.SimulationCfg(dt=dt)) + + sensor = RayCaster(cfg) + sim.reset() + sensor.update(dt) + + pos_w = sensor.data.pos_w[0].cpu() + + # pos_w.z should be near the body height, NOT body_height + offset + assert abs(pos_w[2].item() - body_pos[2]) < 1.0, ( + f"data.pos_w.z = {pos_w[2].item():.2f}, expected near body height {body_pos[2]}." + f" If pos_w.z ≈ {body_pos[2] + offset_z}, the offset was incorrectly baked into the FrameView." + ) + + # ray_hits should be near z=0 (ground plane) + hits_z = sensor.data.ray_hits_w[0, :, 2].cpu() + valid = hits_z[~torch.isinf(hits_z)] + if len(valid) > 0: + assert valid.abs().max().item() < 2.0, ( + f"Ray hits z range [{valid.min().item():.2f}, {valid.max().item():.2f}] — expected near ground (z≈0)." + ) + + # height_scan observation: pos_w_z - hit_z - 0.5 should be small, not ~20 + if len(valid) > 0: + height_obs = pos_w[2].item() - valid.mean().item() - 0.5 + assert abs(height_obs) < 5.0, ( + f"height_scan observation = {height_obs:.2f}, expected near 0." + f" If ≈{offset_z}, the offset leaked into data.pos_w." + ) + + sim.stop() + sim.clear_instance() From 7c3e3faf4b895f2307af1040f2a2995c79e44994 Mon Sep 17 00:00:00 2001 From: Octi Zhang Date: Sat, 11 Apr 2026 01:34:05 -0700 Subject: [PATCH 14/24] remove all the "/raycaster" child path --- scripts/demos/sensors/multi_mesh_raycaster.py | 6 +++--- scripts/demos/sensors/raycaster_sensor.py | 2 +- scripts/tutorials/03_envs/create_quadruped_base_env.py | 2 +- scripts/tutorials/04_sensors/add_sensors_on_robot.py | 2 +- scripts/tutorials/04_sensors/run_ray_caster.py | 2 +- source/isaaclab/isaaclab/scene/interactive_scene_cfg.py | 2 +- .../isaaclab/isaaclab/sensors/ray_caster/ray_caster_cfg.py | 4 ++-- .../test/envs/check_manager_based_env_anymal_locomotion.py | 2 +- source/isaaclab/test/markers/check_markers_visibility.py | 2 +- source/isaaclab/test/scene/check_interactive_scene.py | 2 +- source/isaaclab/test/sensors/check_multi_mesh_ray_caster.py | 2 +- source/isaaclab/test/sensors/check_ray_caster.py | 2 +- source/isaaclab/test/sensors/test_ray_caster.py | 2 +- .../isaaclab_tasks/direct/anymal_c/anymal_c_env_cfg.py | 2 +- .../locomotion/velocity/config/a1/rough_env_cfg.py | 2 +- .../locomotion/velocity/config/cassie/rough_env_cfg.py | 2 +- .../locomotion/velocity/config/digit/rough_env_cfg.py | 2 +- .../locomotion/velocity/config/g1/rough_env_cfg.py | 2 +- .../locomotion/velocity/config/go1/rough_env_cfg.py | 2 +- .../locomotion/velocity/config/go2/rough_env_cfg.py | 2 +- .../locomotion/velocity/config/h1/rough_env_cfg.py | 2 +- .../manager_based/locomotion/velocity/velocity_env_cfg.py | 2 +- 22 files changed, 25 insertions(+), 25 deletions(-) diff --git a/scripts/demos/sensors/multi_mesh_raycaster.py b/scripts/demos/sensors/multi_mesh_raycaster.py index e03ff282f2e5..1a586a8aa2d1 100644 --- a/scripts/demos/sensors/multi_mesh_raycaster.py +++ b/scripts/demos/sensors/multi_mesh_raycaster.py @@ -80,7 +80,7 @@ if args_cli.asset_type == "allegro_hand": asset_cfg = ALLEGRO_HAND_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") ray_caster_cfg = MultiMeshRayCasterCfg( - prim_path="{ENV_REGEX_NS}/Robot/raycaster", + prim_path="{ENV_REGEX_NS}/Robot", update_period=1 / 60, offset=MultiMeshRayCasterCfg.OffsetCfg(pos=(0, -0.1, 0.3)), mesh_prim_paths=[ @@ -101,7 +101,7 @@ elif args_cli.asset_type == "anymal_d": asset_cfg = ANYMAL_D_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") ray_caster_cfg = MultiMeshRayCasterCfg( - prim_path="{ENV_REGEX_NS}/Robot/base/raycaster", + prim_path="{ENV_REGEX_NS}/Robot/base", update_period=1 / 60, offset=MultiMeshRayCasterCfg.OffsetCfg(pos=(0, -0.1, 0.3)), mesh_prim_paths=[ @@ -160,7 +160,7 @@ init_state=RigidObjectCfg.InitialStateCfg(pos=(0.0, 0.0, 2.0)), ) ray_caster_cfg = MultiMeshRayCasterCfg( - prim_path="{ENV_REGEX_NS}/Object/raycaster", + prim_path="{ENV_REGEX_NS}/Object", update_period=1 / 60, offset=MultiMeshRayCasterCfg.OffsetCfg(pos=(0, 0.0, 0.6)), mesh_prim_paths=[ diff --git a/scripts/demos/sensors/raycaster_sensor.py b/scripts/demos/sensors/raycaster_sensor.py index a9cfed141f19..d062e8df3dd8 100644 --- a/scripts/demos/sensors/raycaster_sensor.py +++ b/scripts/demos/sensors/raycaster_sensor.py @@ -62,7 +62,7 @@ class RaycasterSensorSceneCfg(InteractiveSceneCfg): robot = ANYMAL_C_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") ray_caster = RayCasterCfg( - prim_path="{ENV_REGEX_NS}/Robot/base/raycaster", + prim_path="{ENV_REGEX_NS}/Robot/base", spawn=sim_utils.RayCasterXformCfg(), update_period=1 / 60, offset=RayCasterCfg.OffsetCfg(pos=(0, 0, 0.5)), diff --git a/scripts/tutorials/03_envs/create_quadruped_base_env.py b/scripts/tutorials/03_envs/create_quadruped_base_env.py index fb51e3991be5..556b01c0bd60 100644 --- a/scripts/tutorials/03_envs/create_quadruped_base_env.py +++ b/scripts/tutorials/03_envs/create_quadruped_base_env.py @@ -103,7 +103,7 @@ class MySceneCfg(InteractiveSceneCfg): # sensors height_scanner = RayCasterCfg( - prim_path="{ENV_REGEX_NS}/Robot/base/raycaster", + prim_path="{ENV_REGEX_NS}/Robot/base", spawn=sim_utils.RayCasterXformCfg(), offset=RayCasterCfg.OffsetCfg(pos=(0.0, 0.0, 20.0)), ray_alignment="yaw", diff --git a/scripts/tutorials/04_sensors/add_sensors_on_robot.py b/scripts/tutorials/04_sensors/add_sensors_on_robot.py index 86c602cd55a1..59f22e0e25dc 100644 --- a/scripts/tutorials/04_sensors/add_sensors_on_robot.py +++ b/scripts/tutorials/04_sensors/add_sensors_on_robot.py @@ -82,7 +82,7 @@ class SensorsSceneCfg(InteractiveSceneCfg): offset=CameraCfg.OffsetCfg(pos=(0.510, 0.0, 0.015), rot=(0.5, -0.5, 0.5, -0.5), convention="ros"), ) height_scanner = RayCasterCfg( - prim_path="{ENV_REGEX_NS}/Robot/base/raycaster", + prim_path="{ENV_REGEX_NS}/Robot/base", spawn=sim_utils.RayCasterXformCfg(), update_period=0.02, offset=RayCasterCfg.OffsetCfg(pos=(0.0, 0.0, 20.0)), diff --git a/scripts/tutorials/04_sensors/run_ray_caster.py b/scripts/tutorials/04_sensors/run_ray_caster.py index a1238a3ef6ea..3e46ef1a08fd 100644 --- a/scripts/tutorials/04_sensors/run_ray_caster.py +++ b/scripts/tutorials/04_sensors/run_ray_caster.py @@ -45,7 +45,7 @@ def define_sensor() -> RayCaster: """Defines the ray-caster sensor to add to the scene.""" # Create a ray-caster sensor ray_caster_cfg = RayCasterCfg( - prim_path="/World/Origin.*/ball/raycaster", + prim_path="/World/Origin.*/ball", mesh_prim_paths=["/World/ground"], pattern_cfg=patterns.GridPatternCfg(resolution=0.1, size=(2.0, 2.0)), ray_alignment="yaw", diff --git a/source/isaaclab/isaaclab/scene/interactive_scene_cfg.py b/source/isaaclab/isaaclab/scene/interactive_scene_cfg.py index 291585de34f8..0b0c542068a0 100644 --- a/source/isaaclab/isaaclab/scene/interactive_scene_cfg.py +++ b/source/isaaclab/isaaclab/scene/interactive_scene_cfg.py @@ -50,7 +50,7 @@ class MySceneCfg(InteractiveSceneCfg): # sensor - ray caster attached to the base of robot 1 that scans the ground height_scanner = RayCasterCfg( - prim_path="{ENV_REGEX_NS}/Robot_1/base/raycaster", + prim_path="{ENV_REGEX_NS}/Robot_1/base", spawn=sim_utils.RayCasterXformCfg(), offset=RayCasterCfg.OffsetCfg(pos=(0.0, 0.0, 20.0)), ray_alignment="yaw", diff --git a/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster_cfg.py b/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster_cfg.py index bb28d1321b8e..d3adec418b80 100644 --- a/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster_cfg.py +++ b/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster_cfg.py @@ -44,9 +44,9 @@ class OffsetCfg: pattern used by :class:`~isaaclab.sensors.camera.camera_cfg.CameraCfg` (which spawns a Camera prim). The :attr:`prim_path` can be either: - - A **new** child path under a parent link (e.g. ``{ENV_REGEX_NS}/Robot/base/raycaster``). + - A **new** child path under a parent link (e.g. ``{ENV_REGEX_NS}/Robot/base``). - A **physics body** path (e.g. ``{ENV_REGEX_NS}/Robot/base``). In this case, the sensor - will automatically create a child Xform at ``{prim_path}/raycaster``. + will automatically create a child Xform at ``{prim_path}``. If ``None``, the prim at :attr:`prim_path` must already exist on the USD stage and must **not** be a physics body. diff --git a/source/isaaclab/test/envs/check_manager_based_env_anymal_locomotion.py b/source/isaaclab/test/envs/check_manager_based_env_anymal_locomotion.py index e613c21a261f..fef0f41f5118 100644 --- a/source/isaaclab/test/envs/check_manager_based_env_anymal_locomotion.py +++ b/source/isaaclab/test/envs/check_manager_based_env_anymal_locomotion.py @@ -90,7 +90,7 @@ class MySceneCfg(InteractiveSceneCfg): # sensors height_scanner = RayCasterCfg( - prim_path="{ENV_REGEX_NS}/Robot/base/raycaster", + prim_path="{ENV_REGEX_NS}/Robot/base", spawn=sim_utils.RayCasterXformCfg(), offset=RayCasterCfg.OffsetCfg(pos=(0.0, 0.0, 20.0)), ray_alignment="yaw", diff --git a/source/isaaclab/test/markers/check_markers_visibility.py b/source/isaaclab/test/markers/check_markers_visibility.py index 7e221b76ad21..be2ae134bb2a 100644 --- a/source/isaaclab/test/markers/check_markers_visibility.py +++ b/source/isaaclab/test/markers/check_markers_visibility.py @@ -67,7 +67,7 @@ class SensorsSceneCfg(InteractiveSceneCfg): # sensors height_scanner = RayCasterCfg( - prim_path="{ENV_REGEX_NS}/Robot/base/raycaster", + prim_path="{ENV_REGEX_NS}/Robot/base", spawn=sim_utils.RayCasterXformCfg(), update_period=0.02, offset=RayCasterCfg.OffsetCfg(pos=(0.0, 0.0, 20.0)), diff --git a/source/isaaclab/test/scene/check_interactive_scene.py b/source/isaaclab/test/scene/check_interactive_scene.py index dd42572744d5..c59c95127919 100644 --- a/source/isaaclab/test/scene/check_interactive_scene.py +++ b/source/isaaclab/test/scene/check_interactive_scene.py @@ -60,7 +60,7 @@ class MySceneCfg(InteractiveSceneCfg): # sensor - ray caster attached to the base of robot 1 that scans the ground height_scanner = RayCasterCfg( - prim_path="{ENV_REGEX_NS}/Robot_1/base/raycaster", + prim_path="{ENV_REGEX_NS}/Robot_1/base", spawn=sim_utils.RayCasterXformCfg(), offset=RayCasterCfg.OffsetCfg(pos=(0.0, 0.0, 20.0)), ray_alignment="yaw", diff --git a/source/isaaclab/test/sensors/check_multi_mesh_ray_caster.py b/source/isaaclab/test/sensors/check_multi_mesh_ray_caster.py index 4a5a5f8de7de..4824d968284f 100644 --- a/source/isaaclab/test/sensors/check_multi_mesh_ray_caster.py +++ b/source/isaaclab/test/sensors/check_multi_mesh_ray_caster.py @@ -139,7 +139,7 @@ def main(): ) # Create a ray-caster sensor ray_caster_cfg = MultiMeshRayCasterCfg( - prim_path="/World/envs/env_.*/ball/raycaster", + prim_path="/World/envs/env_.*/ball", mesh_prim_paths=mesh_targets, pattern_cfg=patterns.GridPatternCfg(resolution=0.1, size=(1.6, 1.0)), ray_alignment="yaw", diff --git a/source/isaaclab/test/sensors/check_ray_caster.py b/source/isaaclab/test/sensors/check_ray_caster.py index 6b9f6931152b..5e1e88472ae5 100644 --- a/source/isaaclab/test/sensors/check_ray_caster.py +++ b/source/isaaclab/test/sensors/check_ray_caster.py @@ -108,7 +108,7 @@ def main(): # Create a ray-caster sensor ray_caster_cfg = RayCasterCfg( - prim_path="/World/envs/env_.*/ball/raycaster", + prim_path="/World/envs/env_.*/ball", mesh_prim_paths=["/World/ground"], pattern_cfg=patterns.GridPatternCfg(resolution=0.1, size=(1.6, 1.0)), ray_alignment="yaw", diff --git a/source/isaaclab/test/sensors/test_ray_caster.py b/source/isaaclab/test/sensors/test_ray_caster.py index 346cb0acbe96..a394b952689c 100644 --- a/source/isaaclab/test/sensors/test_ray_caster.py +++ b/source/isaaclab/test/sensors/test_ray_caster.py @@ -275,7 +275,7 @@ def test_raycaster_offset_does_not_affect_pos_w(): # large z-offset to make the regression obvious offset_z = 20.0 cfg = RayCasterCfg( - prim_path="/World/Robot/raycaster", + prim_path="/World/Robot", spawn=sim_utils.RayCasterXformCfg(), offset=RayCasterCfg.OffsetCfg(pos=(0.0, 0.0, offset_z)), mesh_prim_paths=["/World/ground"], diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/anymal_c/anymal_c_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/direct/anymal_c/anymal_c_env_cfg.py index 99125d824035..92811e688800 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/anymal_c/anymal_c_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/anymal_c/anymal_c_env_cfg.py @@ -139,7 +139,7 @@ class AnymalCRoughEnvCfg(AnymalCFlatEnvCfg): # we add a height scanner for perceptive locomotion height_scanner = RayCasterCfg( - prim_path="/World/envs/env_.*/Robot/base/raycaster", + prim_path="/World/envs/env_.*/Robot/base", spawn=sim_utils.RayCasterXformCfg(), offset=RayCasterCfg.OffsetCfg(pos=(0.0, 0.0, 20.0)), ray_alignment="yaw", diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/a1/rough_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/a1/rough_env_cfg.py index 244fd39ceb9b..6ee9dbe78505 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/a1/rough_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/a1/rough_env_cfg.py @@ -63,7 +63,7 @@ def __post_init__(self): super().__post_init__() self.scene.robot = UNITREE_A1_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") - self.scene.height_scanner.prim_path = "{ENV_REGEX_NS}/Robot/trunk/raycaster" + self.scene.height_scanner.prim_path = "{ENV_REGEX_NS}/Robot/trunk" # scale down the terrains because the robot is small self.scene.terrain.terrain_generator.sub_terrains["boxes"].grid_height_range = (0.025, 0.1) self.scene.terrain.terrain_generator.sub_terrains["random_rough"].noise_range = (0.01, 0.06) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/cassie/rough_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/cassie/rough_env_cfg.py index 197ab6f7f22a..5bd6ba28684f 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/cassie/rough_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/cassie/rough_env_cfg.py @@ -98,7 +98,7 @@ def __post_init__(self): super().__post_init__() # scene self.scene.robot = CASSIE_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") - self.scene.height_scanner.prim_path = "{ENV_REGEX_NS}/Robot/pelvis/raycaster" + self.scene.height_scanner.prim_path = "{ENV_REGEX_NS}/Robot/pelvis" # actions self.actions.joint_pos.scale = 0.5 diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/digit/rough_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/digit/rough_env_cfg.py index 2298b37b5028..185e9019a7c1 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/digit/rough_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/digit/rough_env_cfg.py @@ -251,7 +251,7 @@ def __post_init__(self): # Scene self.scene.robot = DIGIT_V4_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") - self.scene.height_scanner.prim_path = "{ENV_REGEX_NS}/Robot/torso_base/raycaster" + self.scene.height_scanner.prim_path = "{ENV_REGEX_NS}/Robot/torso_base" self.scene.contact_forces.history_length = self.decimation self.scene.contact_forces.update_period = self.sim.dt self.scene.height_scanner.update_period = self.decimation * self.sim.dt diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/g1/rough_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/g1/rough_env_cfg.py index 263eb606a939..6a7035416ab3 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/g1/rough_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/g1/rough_env_cfg.py @@ -151,7 +151,7 @@ def __post_init__(self): super().__post_init__() # Scene self.scene.robot = G1_MINIMAL_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") - self.scene.height_scanner.prim_path = "{ENV_REGEX_NS}/Robot/torso_link/raycaster" + self.scene.height_scanner.prim_path = "{ENV_REGEX_NS}/Robot/torso_link" # Rewards self.rewards.lin_vel_z_l2.weight = 0.0 diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/go1/rough_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/go1/rough_env_cfg.py index c107585e87a7..eafd3da45f30 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/go1/rough_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/go1/rough_env_cfg.py @@ -63,7 +63,7 @@ def __post_init__(self): super().__post_init__() self.scene.robot = UNITREE_GO1_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") - self.scene.height_scanner.prim_path = "{ENV_REGEX_NS}/Robot/trunk/raycaster" + self.scene.height_scanner.prim_path = "{ENV_REGEX_NS}/Robot/trunk" # scale down the terrains because the robot is small self.scene.terrain.terrain_generator.sub_terrains["boxes"].grid_height_range = (0.025, 0.1) self.scene.terrain.terrain_generator.sub_terrains["random_rough"].noise_range = (0.01, 0.06) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/go2/rough_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/go2/rough_env_cfg.py index 6987eaab9fe4..40fbef99e27e 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/go2/rough_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/go2/rough_env_cfg.py @@ -74,7 +74,7 @@ def __post_init__(self): super().__post_init__() self.scene.robot = UNITREE_GO2_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") - self.scene.height_scanner.prim_path = "{ENV_REGEX_NS}/Robot/base/raycaster" + self.scene.height_scanner.prim_path = "{ENV_REGEX_NS}/Robot/base" # scale down the terrains because the robot is small self.scene.terrain.terrain_generator.sub_terrains["boxes"].grid_height_range = (0.025, 0.1) self.scene.terrain.terrain_generator.sub_terrains["random_rough"].noise_range = (0.01, 0.06) diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/h1/rough_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/h1/rough_env_cfg.py index ac369daa9d17..496aa4007fef 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/h1/rough_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/config/h1/rough_env_cfg.py @@ -119,7 +119,7 @@ def __post_init__(self): # Scene self.scene.robot = H1_MINIMAL_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") if self.scene.height_scanner: - self.scene.height_scanner.prim_path = "{ENV_REGEX_NS}/Robot/torso_link/raycaster" + self.scene.height_scanner.prim_path = "{ENV_REGEX_NS}/Robot/torso_link" # Rewards self.rewards.undesired_contacts = None diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/velocity_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/velocity_env_cfg.py index bd59889ce5ee..c5e1fd236b22 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/velocity_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/velocity_env_cfg.py @@ -75,7 +75,7 @@ class MySceneCfg(InteractiveSceneCfg): robot: ArticulationCfg = MISSING # sensors height_scanner = RayCasterCfg( - prim_path="{ENV_REGEX_NS}/Robot/base/raycaster", + prim_path="{ENV_REGEX_NS}/Robot/base", spawn=sim_utils.RayCasterXformCfg(), offset=RayCasterCfg.OffsetCfg(pos=(0.0, 0.0, 20.0)), ray_alignment="yaw", From e0490f8e040f0c92fae436e84742cbbced0826d4 Mon Sep 17 00:00:00 2001 From: Octi Zhang Date: Sat, 11 Apr 2026 01:34:26 -0700 Subject: [PATCH 15/24] fix linter --- .../test/sensors/test_multi_mesh_ray_caster_camera.py | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/source/isaaclab/test/sensors/test_multi_mesh_ray_caster_camera.py b/source/isaaclab/test/sensors/test_multi_mesh_ray_caster_camera.py index 019d02cbcc5a..2b079760e16a 100644 --- a/source/isaaclab/test/sensors/test_multi_mesh_ray_caster_camera.py +++ b/source/isaaclab/test/sensors/test_multi_mesh_ray_caster_camera.py @@ -185,9 +185,7 @@ def test_camera_init_intrinsic_matrix(setup_simulation): prim_path="/World/Camera", mesh_prim_paths=["/World/defaultGroundPlane"], update_period=0, - offset=MultiMeshRayCasterCameraCfg.OffsetCfg( - pos=(0.0, 0.0, 0.0), rot=(0.0, 0.0, 0.0, 1.0), convention="world" - ), + offset=MultiMeshRayCasterCameraCfg.OffsetCfg(pos=(0.0, 0.0, 0.0), rot=(0.0, 0.0, 0.0, 1.0), convention="world"), debug_vis=False, pattern_cfg=patterns.PinholeCameraPatternCfg.from_intrinsic_matrix( intrinsic_matrix=intrinsic_matrix, From 223285dd392165b00992dcd413ad392d34913e5b Mon Sep 17 00:00:00 2001 From: Octi Zhang Date: Sat, 11 Apr 2026 01:45:00 -0700 Subject: [PATCH 16/24] more nice fixes --- scripts/demos/sensors/raycaster_sensor.py | 1 - .../03_envs/create_quadruped_base_env.py | 1 - .../04_sensors/add_sensors_on_robot.py | 1 - .../isaaclab/scene/interactive_scene_cfg.py | 1 - .../sensors/ray_caster/ray_caster_cfg.py | 4 ++-- source/isaaclab/isaaclab/sim/__init__.pyi | 8 ++++---- .../isaaclab/isaaclab/sim/spawners/__init__.pyi | 6 +++--- .../isaaclab/sim/spawners/sensors/__init__.pyi | 8 ++++---- .../isaaclab/sim/spawners/sensors/sensors.py | 17 ++++++----------- .../sim/spawners/sensors/sensors_cfg.py | 15 ++++++++------- ...check_manager_based_env_anymal_locomotion.py | 1 - .../test/markers/check_markers_visibility.py | 1 - .../test/scene/check_interactive_scene.py | 1 - source/isaaclab/test/sensors/test_ray_caster.py | 1 - ...act_tests.py => test_frame_view_contract.py} | 2 +- .../isaaclab/test/sim/test_views_xform_prim.py | 4 ++-- .../test/sim/test_views_xform_prim_newton.py | 4 ++-- .../test/sim/test_views_xform_prim_fabric.py | 4 ++-- .../direct/anymal_c/anymal_c_env_cfg.py | 1 - .../locomotion/velocity/velocity_env_cfg.py | 1 - 20 files changed, 34 insertions(+), 48 deletions(-) rename source/isaaclab/test/sim/{xform_contract_tests.py => test_frame_view_contract.py} (99%) diff --git a/scripts/demos/sensors/raycaster_sensor.py b/scripts/demos/sensors/raycaster_sensor.py index d062e8df3dd8..b05833dca42c 100644 --- a/scripts/demos/sensors/raycaster_sensor.py +++ b/scripts/demos/sensors/raycaster_sensor.py @@ -63,7 +63,6 @@ class RaycasterSensorSceneCfg(InteractiveSceneCfg): ray_caster = RayCasterCfg( prim_path="{ENV_REGEX_NS}/Robot/base", - spawn=sim_utils.RayCasterXformCfg(), update_period=1 / 60, offset=RayCasterCfg.OffsetCfg(pos=(0, 0, 0.5)), mesh_prim_paths=["/World/Ground"], diff --git a/scripts/tutorials/03_envs/create_quadruped_base_env.py b/scripts/tutorials/03_envs/create_quadruped_base_env.py index 556b01c0bd60..78f5b75ec5f8 100644 --- a/scripts/tutorials/03_envs/create_quadruped_base_env.py +++ b/scripts/tutorials/03_envs/create_quadruped_base_env.py @@ -104,7 +104,6 @@ class MySceneCfg(InteractiveSceneCfg): # sensors height_scanner = RayCasterCfg( prim_path="{ENV_REGEX_NS}/Robot/base", - spawn=sim_utils.RayCasterXformCfg(), offset=RayCasterCfg.OffsetCfg(pos=(0.0, 0.0, 20.0)), ray_alignment="yaw", pattern_cfg=patterns.GridPatternCfg(resolution=0.1, size=[1.6, 1.0]), diff --git a/scripts/tutorials/04_sensors/add_sensors_on_robot.py b/scripts/tutorials/04_sensors/add_sensors_on_robot.py index 59f22e0e25dc..31f9a2bcefcb 100644 --- a/scripts/tutorials/04_sensors/add_sensors_on_robot.py +++ b/scripts/tutorials/04_sensors/add_sensors_on_robot.py @@ -83,7 +83,6 @@ class SensorsSceneCfg(InteractiveSceneCfg): ) height_scanner = RayCasterCfg( prim_path="{ENV_REGEX_NS}/Robot/base", - spawn=sim_utils.RayCasterXformCfg(), update_period=0.02, offset=RayCasterCfg.OffsetCfg(pos=(0.0, 0.0, 20.0)), ray_alignment="yaw", diff --git a/source/isaaclab/isaaclab/scene/interactive_scene_cfg.py b/source/isaaclab/isaaclab/scene/interactive_scene_cfg.py index 0b0c542068a0..f4328324152c 100644 --- a/source/isaaclab/isaaclab/scene/interactive_scene_cfg.py +++ b/source/isaaclab/isaaclab/scene/interactive_scene_cfg.py @@ -51,7 +51,6 @@ class MySceneCfg(InteractiveSceneCfg): # sensor - ray caster attached to the base of robot 1 that scans the ground height_scanner = RayCasterCfg( prim_path="{ENV_REGEX_NS}/Robot_1/base", - spawn=sim_utils.RayCasterXformCfg(), offset=RayCasterCfg.OffsetCfg(pos=(0.0, 0.0, 20.0)), ray_alignment="yaw", pattern_cfg=GridPatternCfg(resolution=0.1, size=[1.6, 1.0]), diff --git a/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster_cfg.py b/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster_cfg.py index d3adec418b80..98b356b1bda3 100644 --- a/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster_cfg.py +++ b/source/isaaclab/isaaclab/sensors/ray_caster/ray_caster_cfg.py @@ -12,7 +12,7 @@ from isaaclab.markers import VisualizationMarkersCfg from isaaclab.markers.config import RAY_CASTER_MARKER_CFG -from isaaclab.sim.spawners.sensors.sensors_cfg import RayCasterXformCfg +from isaaclab.sim.spawners.sensors.sensors_cfg import SensorFrameCfg from isaaclab.utils import configclass from ..sensor_base_cfg import SensorBaseCfg @@ -37,7 +37,7 @@ class OffsetCfg: class_type: type[RayCaster] | str = "{DIR}.ray_caster:RayCaster" - spawn: RayCasterXformCfg | None = RayCasterXformCfg() + spawn: SensorFrameCfg | None = SensorFrameCfg() """Spawn configuration for the sensor Xform prim. A plain USD Xform is created at :attr:`prim_path` before initialization, matching the diff --git a/source/isaaclab/isaaclab/sim/__init__.pyi b/source/isaaclab/isaaclab/sim/__init__.pyi index 6d535307357e..d128a4c4eb90 100644 --- a/source/isaaclab/isaaclab/sim/__init__.pyi +++ b/source/isaaclab/isaaclab/sim/__init__.pyi @@ -94,10 +94,10 @@ __all__ = [ "MeshCylinderCfg", "MeshSphereCfg", "spawn_camera", - "spawn_ray_caster_xform", + "spawn_sensor_frame", "FisheyeCameraCfg", "PinholeCameraCfg", - "RayCasterXformCfg", + "SensorFrameCfg", "spawn_capsule", "spawn_cone", "spawn_cuboid", @@ -266,10 +266,10 @@ from .spawners import ( MeshCylinderCfg, MeshSphereCfg, spawn_camera, - spawn_ray_caster_xform, + spawn_sensor_frame, FisheyeCameraCfg, PinholeCameraCfg, - RayCasterXformCfg, + SensorFrameCfg, spawn_capsule, spawn_cone, spawn_cuboid, diff --git a/source/isaaclab/isaaclab/sim/spawners/__init__.pyi b/source/isaaclab/isaaclab/sim/spawners/__init__.pyi index 76288197153e..296527ec3dac 100644 --- a/source/isaaclab/isaaclab/sim/spawners/__init__.pyi +++ b/source/isaaclab/isaaclab/sim/spawners/__init__.pyi @@ -47,10 +47,10 @@ __all__ = [ "MeshCylinderCfg", "MeshSphereCfg", "spawn_camera", - "spawn_ray_caster_xform", + "spawn_sensor_frame", "FisheyeCameraCfg", "PinholeCameraCfg", - "RayCasterXformCfg", + "SensorFrameCfg", "spawn_capsule", "spawn_cone", "spawn_cuboid", @@ -116,7 +116,7 @@ from .meshes import ( MeshCylinderCfg, MeshSphereCfg, ) -from .sensors import spawn_camera, spawn_ray_caster_xform, FisheyeCameraCfg, PinholeCameraCfg, RayCasterXformCfg +from .sensors import spawn_camera, spawn_sensor_frame, FisheyeCameraCfg, PinholeCameraCfg, SensorFrameCfg from .shapes import ( spawn_capsule, spawn_cone, diff --git a/source/isaaclab/isaaclab/sim/spawners/sensors/__init__.pyi b/source/isaaclab/isaaclab/sim/spawners/sensors/__init__.pyi index ed28f0ee314c..ba5b96a44c7d 100644 --- a/source/isaaclab/isaaclab/sim/spawners/sensors/__init__.pyi +++ b/source/isaaclab/isaaclab/sim/spawners/sensors/__init__.pyi @@ -5,11 +5,11 @@ __all__ = [ "spawn_camera", - "spawn_ray_caster_xform", + "spawn_sensor_frame", "FisheyeCameraCfg", "PinholeCameraCfg", - "RayCasterXformCfg", + "SensorFrameCfg", ] -from .sensors import spawn_camera, spawn_ray_caster_xform -from .sensors_cfg import FisheyeCameraCfg, PinholeCameraCfg, RayCasterXformCfg +from .sensors import spawn_camera, spawn_sensor_frame +from .sensors_cfg import FisheyeCameraCfg, PinholeCameraCfg, SensorFrameCfg diff --git a/source/isaaclab/isaaclab/sim/spawners/sensors/sensors.py b/source/isaaclab/isaaclab/sim/spawners/sensors/sensors.py index 468a4d13257d..d0a024f8b66f 100644 --- a/source/isaaclab/isaaclab/sim/spawners/sensors/sensors.py +++ b/source/isaaclab/isaaclab/sim/spawners/sensors/sensors.py @@ -148,25 +148,18 @@ def spawn_camera( @clone -def spawn_ray_caster_xform( +def spawn_sensor_frame( prim_path: str, - cfg: sensors_cfg.RayCasterXformCfg, + cfg: sensors_cfg.SensorFrameCfg, translation: tuple[float, float, float] | None = None, orientation: tuple[float, float, float, float] | None = None, **kwargs, ) -> Usd.Prim: - """Create a USD Xform prim for a ray-cast sensor attachment site. - - The function creates a plain ``Xform`` with the given local pose under its parent. It is - intended for :class:`~isaaclab.sensors.ray_caster.ray_caster.RayCaster` when - :attr:`~isaaclab.sensors.ray_caster.ray_caster_cfg.RayCasterCfg.spawn` is set, mirroring how - :func:`spawn_camera` creates camera prims before :class:`~isaaclab.sim.views.FrameView` - is constructed. + """Create a plain USD Xform prim as a sensor attachment frame. .. note:: This function is decorated with :func:`clone` that resolves prim path into list of paths - if the input prim path is a regex pattern. Visibility and semantic tags from - :class:`~isaaclab.sim.spawners.spawner_cfg.SpawnerCfg` are applied by the clone wrapper. + if the input prim path is a regex pattern. Args: prim_path: The prim path or pattern to spawn the asset at. @@ -195,3 +188,5 @@ def spawn_ray_caster_xform( else: raise ValueError(f"A prim already exists at path: '{prim_path}'.") return prim + + diff --git a/source/isaaclab/isaaclab/sim/spawners/sensors/sensors_cfg.py b/source/isaaclab/isaaclab/sim/spawners/sensors/sensors_cfg.py index fc5afa5b231d..768abdc1cfc7 100644 --- a/source/isaaclab/isaaclab/sim/spawners/sensors/sensors_cfg.py +++ b/source/isaaclab/isaaclab/sim/spawners/sensors/sensors_cfg.py @@ -225,13 +225,14 @@ class FisheyeCameraCfg(PinholeCameraCfg): @configclass -class RayCasterXformCfg(SpawnerCfg): - """Configuration for spawning a plain USD Xform used as a ray-cast sensor frame. +class SensorFrameCfg(SpawnerCfg): + """Spawns a plain USD Xform as a sensor attachment frame. - The spawned prim has no rigid body or collision API. This matches the pattern used by - :class:`~isaaclab.sensors.camera.camera_cfg.CameraCfg`, where the sensor prim is created at - runtime so :class:`~isaaclab.sim.views.FrameView` tracks a non-physics site (required for - backends such as Newton where link prims are physics-owned). + The spawned prim carries no rigid body or collision API. It serves as a + non-physics child under a link so that :class:`~isaaclab.sim.views.FrameView` + can track it on all backends (including Newton, which rejects physics body prims). """ - func: Callable | str = "{DIR}.sensors:spawn_ray_caster_xform" + func: Callable | str = "{DIR}.sensors:spawn_sensor_frame" + + diff --git a/source/isaaclab/test/envs/check_manager_based_env_anymal_locomotion.py b/source/isaaclab/test/envs/check_manager_based_env_anymal_locomotion.py index fef0f41f5118..07dc3d73bd14 100644 --- a/source/isaaclab/test/envs/check_manager_based_env_anymal_locomotion.py +++ b/source/isaaclab/test/envs/check_manager_based_env_anymal_locomotion.py @@ -91,7 +91,6 @@ class MySceneCfg(InteractiveSceneCfg): # sensors height_scanner = RayCasterCfg( prim_path="{ENV_REGEX_NS}/Robot/base", - spawn=sim_utils.RayCasterXformCfg(), offset=RayCasterCfg.OffsetCfg(pos=(0.0, 0.0, 20.0)), ray_alignment="yaw", pattern_cfg=patterns.GridPatternCfg(resolution=0.1, size=[1.6, 1.0]), diff --git a/source/isaaclab/test/markers/check_markers_visibility.py b/source/isaaclab/test/markers/check_markers_visibility.py index be2ae134bb2a..98dbee8ddcd7 100644 --- a/source/isaaclab/test/markers/check_markers_visibility.py +++ b/source/isaaclab/test/markers/check_markers_visibility.py @@ -68,7 +68,6 @@ class SensorsSceneCfg(InteractiveSceneCfg): # sensors height_scanner = RayCasterCfg( prim_path="{ENV_REGEX_NS}/Robot/base", - spawn=sim_utils.RayCasterXformCfg(), update_period=0.02, offset=RayCasterCfg.OffsetCfg(pos=(0.0, 0.0, 20.0)), ray_alignment="yaw", diff --git a/source/isaaclab/test/scene/check_interactive_scene.py b/source/isaaclab/test/scene/check_interactive_scene.py index c59c95127919..5b2463b315a9 100644 --- a/source/isaaclab/test/scene/check_interactive_scene.py +++ b/source/isaaclab/test/scene/check_interactive_scene.py @@ -61,7 +61,6 @@ class MySceneCfg(InteractiveSceneCfg): # sensor - ray caster attached to the base of robot 1 that scans the ground height_scanner = RayCasterCfg( prim_path="{ENV_REGEX_NS}/Robot_1/base", - spawn=sim_utils.RayCasterXformCfg(), offset=RayCasterCfg.OffsetCfg(pos=(0.0, 0.0, 20.0)), ray_alignment="yaw", pattern_cfg=patterns.GridPatternCfg(resolution=0.1, size=[1.6, 1.0]), diff --git a/source/isaaclab/test/sensors/test_ray_caster.py b/source/isaaclab/test/sensors/test_ray_caster.py index a394b952689c..7dcbc3066eb6 100644 --- a/source/isaaclab/test/sensors/test_ray_caster.py +++ b/source/isaaclab/test/sensors/test_ray_caster.py @@ -276,7 +276,6 @@ def test_raycaster_offset_does_not_affect_pos_w(): offset_z = 20.0 cfg = RayCasterCfg( prim_path="/World/Robot", - spawn=sim_utils.RayCasterXformCfg(), offset=RayCasterCfg.OffsetCfg(pos=(0.0, 0.0, offset_z)), mesh_prim_paths=["/World/ground"], pattern_cfg=patterns.GridPatternCfg(resolution=0.5, size=[1.0, 1.0]), diff --git a/source/isaaclab/test/sim/xform_contract_tests.py b/source/isaaclab/test/sim/test_frame_view_contract.py similarity index 99% rename from source/isaaclab/test/sim/xform_contract_tests.py rename to source/isaaclab/test/sim/test_frame_view_contract.py index 2266e2bca2f1..b72b9d78cb4b 100644 --- a/source/isaaclab/test/sim/xform_contract_tests.py +++ b/source/isaaclab/test/sim/test_frame_view_contract.py @@ -7,7 +7,7 @@ This module defines the invariants that **every** FrameView backend (USD, Fabric, Newton) must satisfy. Backend test files import these tests -via ``from isaaclab.test.sim.xform_contract_tests import *`` and provide a +via ``from test_frame_view_contract import *`` and provide a ``view_factory`` pytest fixture that builds the backend-specific scene. The factory signature is:: diff --git a/source/isaaclab/test/sim/test_views_xform_prim.py b/source/isaaclab/test/sim/test_views_xform_prim.py index 0ae78ddea9bc..03b6b4d40d57 100644 --- a/source/isaaclab/test/sim/test_views_xform_prim.py +++ b/source/isaaclab/test/sim/test_views_xform_prim.py @@ -25,8 +25,8 @@ except (ModuleNotFoundError, ImportError): _IsaacSimXformPrimView = None -from xform_contract_tests import * # noqa: F401, F403, E402 -from xform_contract_tests import CHILD_OFFSET, ViewBundle # noqa: E402 +from test_frame_view_contract import * # noqa: F401, F403, E402 +from test_frame_view_contract import CHILD_OFFSET, ViewBundle # noqa: E402 import isaaclab.sim as sim_utils # noqa: E402 from isaaclab.sim.views import UsdFrameView as FrameView # noqa: E402 diff --git a/source/isaaclab_newton/test/sim/test_views_xform_prim_newton.py b/source/isaaclab_newton/test/sim/test_views_xform_prim_newton.py index cc9567c094cc..cab303547cd2 100644 --- a/source/isaaclab_newton/test/sim/test_views_xform_prim_newton.py +++ b/source/isaaclab_newton/test/sim/test_views_xform_prim_newton.py @@ -22,8 +22,8 @@ from isaaclab_newton.physics import MJWarpSolverCfg, NewtonCfg from isaaclab_newton.physics.newton_manager import NewtonManager from isaaclab_newton.sim.views import NewtonSiteFrameView as FrameView -from xform_contract_tests import * # noqa: F401, F403 — import all contract tests -from xform_contract_tests import CHILD_OFFSET, ViewBundle, _wp_vec3f, _wp_vec4f +from test_frame_view_contract import * # noqa: F401, F403 — import all contract tests +from test_frame_view_contract import CHILD_OFFSET, ViewBundle, _wp_vec3f, _wp_vec4f from pxr import Gf diff --git a/source/isaaclab_physx/test/sim/test_views_xform_prim_fabric.py b/source/isaaclab_physx/test/sim/test_views_xform_prim_fabric.py index 66ccd399bd75..2b836a50859a 100644 --- a/source/isaaclab_physx/test/sim/test_views_xform_prim_fabric.py +++ b/source/isaaclab_physx/test/sim/test_views_xform_prim_fabric.py @@ -22,8 +22,8 @@ import pytest # noqa: E402 import torch # noqa: E402 from isaaclab_physx.sim.views import FabricFrameView as FrameView # noqa: E402 -from xform_contract_tests import * # noqa: F401, F403, E402 -from xform_contract_tests import CHILD_OFFSET, ViewBundle # noqa: E402 +from test_frame_view_contract import * # noqa: F401, F403, E402 +from test_frame_view_contract import CHILD_OFFSET, ViewBundle # noqa: E402 from pxr import Gf, UsdGeom # noqa: E402 diff --git a/source/isaaclab_tasks/isaaclab_tasks/direct/anymal_c/anymal_c_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/direct/anymal_c/anymal_c_env_cfg.py index 92811e688800..09e4edcb55a7 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/direct/anymal_c/anymal_c_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/direct/anymal_c/anymal_c_env_cfg.py @@ -140,7 +140,6 @@ class AnymalCRoughEnvCfg(AnymalCFlatEnvCfg): # we add a height scanner for perceptive locomotion height_scanner = RayCasterCfg( prim_path="/World/envs/env_.*/Robot/base", - spawn=sim_utils.RayCasterXformCfg(), offset=RayCasterCfg.OffsetCfg(pos=(0.0, 0.0, 20.0)), ray_alignment="yaw", pattern_cfg=patterns.GridPatternCfg(resolution=0.1, size=[1.6, 1.0]), diff --git a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/velocity_env_cfg.py b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/velocity_env_cfg.py index c5e1fd236b22..81f38e756975 100644 --- a/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/velocity_env_cfg.py +++ b/source/isaaclab_tasks/isaaclab_tasks/manager_based/locomotion/velocity/velocity_env_cfg.py @@ -76,7 +76,6 @@ class MySceneCfg(InteractiveSceneCfg): # sensors height_scanner = RayCasterCfg( prim_path="{ENV_REGEX_NS}/Robot/base", - spawn=sim_utils.RayCasterXformCfg(), offset=RayCasterCfg.OffsetCfg(pos=(0.0, 0.0, 20.0)), ray_alignment="yaw", pattern_cfg=patterns.GridPatternCfg(resolution=0.1, size=[1.6, 1.0]), From e68c5a45b18ad2f0b811f342f5a8c672e261c0bd Mon Sep 17 00:00:00 2001 From: Octi Zhang Date: Sat, 11 Apr 2026 01:45:15 -0700 Subject: [PATCH 17/24] fix linter --- source/isaaclab/isaaclab/sim/spawners/sensors/sensors.py | 2 -- source/isaaclab/isaaclab/sim/spawners/sensors/sensors_cfg.py | 2 -- 2 files changed, 4 deletions(-) diff --git a/source/isaaclab/isaaclab/sim/spawners/sensors/sensors.py b/source/isaaclab/isaaclab/sim/spawners/sensors/sensors.py index d0a024f8b66f..db68d21d8a90 100644 --- a/source/isaaclab/isaaclab/sim/spawners/sensors/sensors.py +++ b/source/isaaclab/isaaclab/sim/spawners/sensors/sensors.py @@ -188,5 +188,3 @@ def spawn_sensor_frame( else: raise ValueError(f"A prim already exists at path: '{prim_path}'.") return prim - - diff --git a/source/isaaclab/isaaclab/sim/spawners/sensors/sensors_cfg.py b/source/isaaclab/isaaclab/sim/spawners/sensors/sensors_cfg.py index 768abdc1cfc7..1ad9dcd73bf2 100644 --- a/source/isaaclab/isaaclab/sim/spawners/sensors/sensors_cfg.py +++ b/source/isaaclab/isaaclab/sim/spawners/sensors/sensors_cfg.py @@ -234,5 +234,3 @@ class SensorFrameCfg(SpawnerCfg): """ func: Callable | str = "{DIR}.sensors:spawn_sensor_frame" - - From b461f403092af35bc1316da4b0a5294e65ef6dc0 Mon Sep 17 00:00:00 2001 From: Antoine Richard Date: Tue, 14 Apr 2026 10:50:45 +0200 Subject: [PATCH 18/24] Add docstrings to Newton FrameView kernels and fix camera rebase MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - Add Google-style docstrings to all 12 Warp kernels in NewtonSiteFrameView with explicit Args sections describing shapes, units, and semantics. - Add missing docstrings to all public methods (__init__, count, get_world_poses, get_scales, set_scales). - Reorder kernel parameters: inputs before outputs. - Update wp.launch calls to use explicit inputs/outputs kwargs. - Treat in-place arrays (site_local, shape_scale) as inputs. - Fix camera.py rebase conflict resolution: keep develop's Renderer abstraction while applying XformPrimView→FrameView rename. --- .../isaaclab/sensors/camera/camera.py | 4 +- .../sim/views/newton_site_frame_view.py | 343 ++++++++++++++---- 2 files changed, 266 insertions(+), 81 deletions(-) diff --git a/source/isaaclab/isaaclab/sensors/camera/camera.py b/source/isaaclab/isaaclab/sensors/camera/camera.py index 13c940fa9ce4..54bf957b2b94 100644 --- a/source/isaaclab/isaaclab/sensors/camera/camera.py +++ b/source/isaaclab/isaaclab/sensors/camera/camera.py @@ -428,9 +428,7 @@ def _initialize_impl(self): # Create a view for the sensor with Fabric enabled for fast pose queries. # TODO: remove sync_usd_on_fabric_write=True once the GPU Fabric sync bug is fixed. - self._view = FrameView( - self.cfg.prim_path, device=self._device, stage=self.stage, sync_usd_on_fabric_write=True - ) + self._view = FrameView(self.cfg.prim_path, device=self._device, stage=self.stage, sync_usd_on_fabric_write=True) # Check that sizes are correct if self._view.count != self._num_envs: raise RuntimeError( diff --git a/source/isaaclab_newton/isaaclab_newton/sim/views/newton_site_frame_view.py b/source/isaaclab_newton/isaaclab_newton/sim/views/newton_site_frame_view.py index a6b62a597479..2300d1494900 100644 --- a/source/isaaclab_newton/isaaclab_newton/sim/views/newton_site_frame_view.py +++ b/source/isaaclab_newton/isaaclab_newton/sim/views/newton_site_frame_view.py @@ -37,10 +37,20 @@ def _compute_site_world_transforms( out_pos: wp.array(dtype=wp.vec3f), out_quat: wp.array(dtype=wp.vec4f), ): - """Compute world transforms for all sites: ``world = body_q[body] * local``. - - When ``site_body[i] == -1`` the site is attached to the world frame and - the world transform equals ``site_local[i]`` directly. + """Compute world-space transforms for every site in the view. + + For each site *i*, computes ``world = body_q[site_body[i]] * site_local[i]`` + and splits the result into position and quaternion outputs. When + ``site_body[i] == -1`` the site is world-attached and ``site_local[i]`` is + returned directly. + + Args: + body_q: Rigid-body world transforms from the Newton state, shape ``[num_bodies]``. + site_body: Per-site body index (flat model-level), shape ``[num_sites]``. + A value of ``-1`` indicates a world-attached site. + site_local: Per-site local offset relative to its parent body, shape ``[num_sites]``. + out_pos: Output world positions [m], shape ``[num_sites]``. + out_quat: Output world orientations as ``(qx, qy, qz, qw)``, shape ``[num_sites]``. """ i = wp.tid() bid = site_body[i] @@ -62,7 +72,20 @@ def _compute_site_world_transforms_indexed( out_pos: wp.array(dtype=wp.vec3f), out_quat: wp.array(dtype=wp.vec4f), ): - """Compute world transforms for a subset of sites selected by ``indices``.""" + """Indexed variant of :func:`_compute_site_world_transforms`. + + Only computes world transforms for the subset of sites selected by + ``indices``. Thread *i* reads ``indices[i]`` to obtain the site index, + then writes the result to ``out_pos[i]`` / ``out_quat[i]``. + + Args: + body_q: Rigid-body world transforms from the Newton state, shape ``[num_bodies]``. + site_body: Per-site body index (flat model-level), shape ``[num_sites]``. + site_local: Per-site local offset relative to its parent body, shape ``[num_sites]``. + indices: Site indices to query, shape ``[M]``. + out_pos: Output world positions [m], shape ``[M]``. + out_quat: Output world orientations as ``(qx, qy, qz, qw)``, shape ``[M]``. + """ i = wp.tid() si = indices[i] bid = site_body[si] @@ -83,7 +106,19 @@ def _gather_scales( num_shapes: wp.int32, out_scales: wp.array(dtype=wp.vec3f), ): - """For each site, find the first shape on the same body and copy its scale.""" + """Gather per-site scales from collision shapes on the same body. + + For each site *i*, linearly scans all shapes to find the first one whose + ``shape_body`` matches ``site_body[i]`` and copies its scale. Falls back + to ``(1, 1, 1)`` if no shape is found on that body. + + Args: + shape_scale: Per-shape scale vectors from the Newton model, shape ``[num_shapes]``. + shape_body: Per-shape parent body index, shape ``[num_shapes]``. + site_body: Per-site body index, shape ``[num_sites]``. + num_shapes: Total number of shapes in the model. + out_scales: Output scale per site, shape ``[num_sites]``. + """ i = wp.tid() bid = site_body[i] found = int(0) @@ -104,7 +139,16 @@ def _gather_scales_indexed( num_shapes: wp.int32, out_scales: wp.array(dtype=wp.vec3f), ): - """Indexed variant of _gather_scales.""" + """Indexed variant of :func:`_gather_scales`. + + Args: + shape_scale: Per-shape scale vectors from the Newton model, shape ``[num_shapes]``. + shape_body: Per-shape parent body index, shape ``[num_shapes]``. + site_body: Per-site body index, shape ``[num_sites]``. + indices: Site indices to query, shape ``[M]``. + num_shapes: Total number of shapes in the model. + out_scales: Output scale per queried site, shape ``[M]``. + """ i = wp.tid() si = indices[i] bid = site_body[si] @@ -119,13 +163,26 @@ def _gather_scales_indexed( @wp.kernel def _scatter_scales( - shape_scale: wp.array(dtype=wp.vec3f), - shape_body: wp.array(dtype=wp.int32), site_body: wp.array(dtype=wp.int32), - num_shapes: wp.int32, new_scales: wp.array(dtype=wp.vec3f), + shape_body: wp.array(dtype=wp.int32), + num_shapes: wp.int32, + shape_scale: wp.array(dtype=wp.vec3f), ): - """For each site, write its scale to all shapes on the same body.""" + """Scatter per-site scales to all collision shapes on the same body. + + For each site *i*, writes ``new_scales[i]`` to every shape whose + ``shape_body`` matches ``site_body[i]``. Multiple shapes on the same + body all receive the same scale. + + Args: + site_body: Per-site body index, shape ``[num_sites]``. + new_scales: New scale to apply per site, shape ``[num_sites]``. + shape_body: Per-shape parent body index, shape ``[num_shapes]``. + num_shapes: Total number of shapes in the model. + shape_scale: Per-shape scale vectors to write into (modified in-place), + shape ``[num_shapes]``. + """ i = wp.tid() bid = site_body[i] for s in range(num_shapes): @@ -135,14 +192,24 @@ def _scatter_scales( @wp.kernel def _scatter_scales_indexed( - shape_scale: wp.array(dtype=wp.vec3f), - shape_body: wp.array(dtype=wp.int32), site_body: wp.array(dtype=wp.int32), indices: wp.array(dtype=wp.int32), - num_shapes: wp.int32, new_scales: wp.array(dtype=wp.vec3f), + shape_body: wp.array(dtype=wp.int32), + num_shapes: wp.int32, + shape_scale: wp.array(dtype=wp.vec3f), ): - """Indexed variant of _scatter_scales.""" + """Indexed variant of :func:`_scatter_scales`. + + Args: + site_body: Per-site body index, shape ``[num_sites]``. + indices: Site indices to update, shape ``[M]``. + new_scales: New scale to apply per selected site, shape ``[M]``. + shape_body: Per-shape parent body index, shape ``[num_shapes]``. + num_shapes: Total number of shapes in the model. + shape_scale: Per-shape scale vectors to write into (modified in-place), + shape ``[num_shapes]``. + """ i = wp.tid() si = indices[i] bid = site_body[si] @@ -160,15 +227,26 @@ def _scatter_scales_indexed( def _write_site_local_from_world_poses( body_q: wp.array(dtype=wp.transformf), site_body: wp.array(dtype=wp.int32), - site_local: wp.array(dtype=wp.transformf), world_pos: wp.array(dtype=wp.vec3f), world_quat: wp.array(dtype=wp.vec4f), + site_local: wp.array(dtype=wp.transformf), ): - """Update ``site_local`` so that ``body_q[bid] * site_local == desired_world``. - - Computes ``site_local[i] = inv(body_q[bid]) * desired_world``. - For world-attached sites (``site_body == -1``) writes the world transform - directly into ``site_local``. + """Update site local offsets so that the sites reach desired world poses. + + For each site *i*, computes + ``site_local[i] = inv(body_q[site_body[i]]) * desired_world`` so that + a subsequent ``body_q[bid] * site_local[i]`` yields the requested world + pose. For world-attached sites (``site_body[i] == -1``) the desired world + transform is written directly into ``site_local[i]``. + + Does **not** modify ``body_q``. + + Args: + body_q: Rigid-body world transforms from the Newton state, shape ``[num_bodies]``. + site_body: Per-site body index (flat model-level), shape ``[num_sites]``. + world_pos: Desired world positions [m], shape ``[num_sites]``. + world_quat: Desired world orientations as ``(qx, qy, qz, qw)``, shape ``[num_sites]``. + site_local: Per-site local offset (modified in-place), shape ``[num_sites]``. """ i = wp.tid() w_pos = world_pos[i] @@ -186,12 +264,21 @@ def _write_site_local_from_world_poses( def _write_site_local_from_world_poses_indexed( body_q: wp.array(dtype=wp.transformf), site_body: wp.array(dtype=wp.int32), - site_local: wp.array(dtype=wp.transformf), indices: wp.array(dtype=wp.int32), world_pos: wp.array(dtype=wp.vec3f), world_quat: wp.array(dtype=wp.vec4f), + site_local: wp.array(dtype=wp.transformf), ): - """Indexed variant of :func:`_write_site_local_from_world_poses`.""" + """Indexed variant of :func:`_write_site_local_from_world_poses`. + + Args: + body_q: Rigid-body world transforms from the Newton state, shape ``[num_bodies]``. + site_body: Per-site body index (flat model-level), shape ``[num_sites]``. + indices: Site indices to update, shape ``[M]``. + world_pos: Desired world positions [m], shape ``[M]``. + world_quat: Desired world orientations as ``(qx, qy, qz, qw)``, shape ``[M]``. + site_local: Per-site local offset (modified in-place), shape ``[num_sites]``. + """ i = wp.tid() si = indices[i] w_pos = world_pos[i] @@ -220,11 +307,23 @@ def _compute_site_local_transforms( out_pos: wp.array(dtype=wp.vec3f), out_quat: wp.array(dtype=wp.vec4f), ): - """Compute parent-relative transforms: ``local = inv(parent_world) * prim_world``. - - When ``site_body[i] == -1`` the prim is attached to the world frame and - ``site_local[i]`` is its world transform. The same convention applies to - ``parent_site_body`` / ``parent_site_local``. + """Compute parent-relative transforms for every site in the view. + + For each site *i*, computes the world pose of both the site and its USD + parent, then returns ``inv(parent_world) * prim_world``. When + ``site_body[i] == -1`` the site is world-attached and ``site_local[i]`` + is used as the world transform directly. The same convention applies to + the parent arrays. + + Args: + body_q: Rigid-body world transforms from the Newton state, shape ``[num_bodies]``. + site_body: Per-site body index (flat model-level), shape ``[num_sites]``. + site_local: Per-site local offset relative to its parent body, shape ``[num_sites]``. + parent_site_body: Per-site USD-parent body index, shape ``[num_sites]``. + parent_site_local: Per-site USD-parent local offset, shape ``[num_sites]``. + out_pos: Output parent-relative positions [m], shape ``[num_sites]``. + out_quat: Output parent-relative orientations as ``(qx, qy, qz, qw)``, + shape ``[num_sites]``. """ i = wp.tid() prim_bid = site_body[i] @@ -256,7 +355,19 @@ def _compute_site_local_transforms_indexed( out_pos: wp.array(dtype=wp.vec3f), out_quat: wp.array(dtype=wp.vec4f), ): - """Compute parent-relative transforms for a subset of sites selected by ``indices``.""" + """Indexed variant of :func:`_compute_site_local_transforms`. + + Args: + body_q: Rigid-body world transforms from the Newton state, shape ``[num_bodies]``. + site_body: Per-site body index (flat model-level), shape ``[num_sites]``. + site_local: Per-site local offset relative to its parent body, shape ``[num_sites]``. + parent_site_body: Per-site USD-parent body index, shape ``[num_sites]``. + parent_site_local: Per-site USD-parent local offset, shape ``[num_sites]``. + indices: Site indices to query, shape ``[M]``. + out_pos: Output parent-relative positions [m], shape ``[M]``. + out_quat: Output parent-relative orientations as ``(qx, qy, qz, qw)``, + shape ``[M]``. + """ i = wp.tid() si = indices[i] prim_bid = site_body[si] @@ -281,17 +392,30 @@ def _compute_site_local_transforms_indexed( def _write_site_local_from_local_poses( body_q: wp.array(dtype=wp.transformf), site_body: wp.array(dtype=wp.int32), - site_local: wp.array(dtype=wp.transformf), parent_site_body: wp.array(dtype=wp.int32), parent_site_local: wp.array(dtype=wp.transformf), local_pos: wp.array(dtype=wp.vec3f), local_quat: wp.array(dtype=wp.vec4f), + site_local: wp.array(dtype=wp.transformf), ): - """Update ``site_local`` so that ``inv(parent_world) * prim_world == desired_local``. - - Computes ``site_local[i] = inv(body_q[bid]) * parent_world * desired_local``. - For world-attached sites (``site_body == -1``) the site local IS the world - transform, so we write ``parent_world * desired_local`` directly. + """Update site local offsets so that sites reach desired parent-relative poses. + + For each site *i*, reconstructs the desired world pose as + ``parent_world * desired_local``, then solves for the body-relative offset: + ``site_local[i] = inv(body_q[bid]) * desired_world``. For world-attached + sites (``site_body[i] == -1``) the world transform is written directly. + + Does **not** modify ``body_q``. + + Args: + body_q: Rigid-body world transforms from the Newton state, shape ``[num_bodies]``. + site_body: Per-site body index (flat model-level), shape ``[num_sites]``. + parent_site_body: Per-site USD-parent body index, shape ``[num_sites]``. + parent_site_local: Per-site USD-parent local offset, shape ``[num_sites]``. + local_pos: Desired parent-relative positions [m], shape ``[num_sites]``. + local_quat: Desired parent-relative orientations as ``(qx, qy, qz, qw)``, + shape ``[num_sites]``. + site_local: Per-site local offset (modified in-place), shape ``[num_sites]``. """ i = wp.tid() parent_bid = parent_site_body[i] @@ -316,14 +440,26 @@ def _write_site_local_from_local_poses( def _write_site_local_from_local_poses_indexed( body_q: wp.array(dtype=wp.transformf), site_body: wp.array(dtype=wp.int32), - site_local: wp.array(dtype=wp.transformf), parent_site_body: wp.array(dtype=wp.int32), parent_site_local: wp.array(dtype=wp.transformf), indices: wp.array(dtype=wp.int32), local_pos: wp.array(dtype=wp.vec3f), local_quat: wp.array(dtype=wp.vec4f), + site_local: wp.array(dtype=wp.transformf), ): - """Indexed variant of :func:`_write_site_local_from_local_poses`.""" + """Indexed variant of :func:`_write_site_local_from_local_poses`. + + Args: + body_q: Rigid-body world transforms from the Newton state, shape ``[num_bodies]``. + site_body: Per-site body index (flat model-level), shape ``[num_sites]``. + parent_site_body: Per-site USD-parent body index, shape ``[num_sites]``. + parent_site_local: Per-site USD-parent local offset, shape ``[num_sites]``. + indices: Site indices to update, shape ``[M]``. + local_pos: Desired parent-relative positions [m], shape ``[M]``. + local_quat: Desired parent-relative orientations as ``(qx, qy, qz, qw)``, + shape ``[M]``. + site_local: Per-site local offset (modified in-place), shape ``[num_sites]``. + """ i = wp.tid() si = indices[i] parent_bid = parent_site_body[si] @@ -378,6 +514,24 @@ class NewtonSiteFrameView(BaseFrameView): """ def __init__(self, prim_path: str, device: str = "cpu", stage: Usd.Stage | None = None, **kwargs): + """Initialize the Newton site-based frame view. + + Resolves all USD prims matching ``prim_path`` and, for each one, walks + the USD ancestor hierarchy to find the nearest Newton rigid body. The + relative transform between the prim and its ancestor body becomes the + site's local offset. + + If the Newton model is already finalized the view initializes + immediately; otherwise initialization is deferred to a + :attr:`PhysicsEvent.PHYSICS_READY` callback. + + Args: + prim_path: USD prim path pattern (may contain regex). + device: Warp device for GPU arrays (e.g. ``"cuda:0"``). + stage: USD stage to search. Defaults to the current stage. + **kwargs: Unused; accepted for interface compatibility with other + :class:`~isaaclab.sim.views.BaseFrameView` backends. + """ self._prim_path = prim_path self._device = device @@ -506,6 +660,7 @@ def _resolve_ancestor_body( @property def count(self) -> int: + """Number of prims in this view.""" return len(self._prims) # ------------------------------------------------------------------ @@ -513,17 +668,25 @@ def count(self) -> int: # ------------------------------------------------------------------ def get_world_poses(self, indices: wp.array | None = None) -> tuple[wp.array, wp.array]: + """Get world-space positions and orientations. + + Args: + indices: Subset of sites to query. ``None`` means all sites. + + Returns: + A tuple ``(positions, orientations)`` as ``wp.array`` of shapes + ``(M, 3)`` and ``(M, 4)`` respectively. + """ state = NewtonManager.get_state_0() if indices is not None: n = len(indices) - idx_wp = indices pos_buf = wp.zeros(n, dtype=wp.vec3f, device=self._device) quat_buf = wp.zeros(n, dtype=wp.vec4f, device=self._device) wp.launch( _compute_site_world_transforms_indexed, dim=n, - inputs=[state.body_q, self._site_body, self._site_local, idx_wp], + inputs=[state.body_q, self._site_body, self._site_local, indices], outputs=[pos_buf, quat_buf], device=self._device, ) @@ -544,11 +707,18 @@ def set_world_poses( orientations: wp.array | None = None, indices: wp.array | None = None, ) -> None: - """Write world poses by updating the site's local offset. + """Set world-space positions and/or orientations. + + Updates the internal ``site_local`` offsets so that + ``body_q[body] * new_site_local`` yields the desired world pose. + Does **not** modify ``body_q``. - Computes the new ``site_local`` such that - ``body_q[body] * new_site_local == desired_world``. - Does not modify ``body_q``. + Args: + positions: Desired world positions ``(M, 3)``. ``None`` leaves + positions unchanged. + orientations: Desired world quaternions ``(M, 4)`` as + ``(qx, qy, qz, qw)``. ``None`` leaves orientations unchanged. + indices: Subset of sites to update. ``None`` means all sites. """ if positions is None and orientations is None: return @@ -562,23 +732,18 @@ def set_world_poses( if orientations is None: orientations = cur_quat - pos_wp = positions - quat_wp = orientations - if indices is not None: - n = len(indices) - idx_wp = indices wp.launch( _write_site_local_from_world_poses_indexed, - dim=n, - inputs=[state.body_q, self._site_body, self._site_local, idx_wp, pos_wp, quat_wp], + dim=len(indices), + inputs=[state.body_q, self._site_body, indices, positions, orientations, self._site_local], device=self._device, ) else: wp.launch( _write_site_local_from_world_poses, dim=self.count, - inputs=[state.body_q, self._site_body, self._site_local, pos_wp, quat_wp], + inputs=[state.body_q, self._site_body, positions, orientations, self._site_local], device=self._device, ) @@ -587,12 +752,21 @@ def set_world_poses( # ------------------------------------------------------------------ def get_local_poses(self, indices: wp.array | None = None) -> tuple[wp.array, wp.array]: - """Get parent-relative poses: ``local = inv(parent_world) * prim_world``.""" + """Get parent-relative positions and orientations. + + Computes ``inv(parent_world) * prim_world`` for each site. + + Args: + indices: Subset of sites to query. ``None`` means all sites. + + Returns: + A tuple ``(translations, orientations)`` as ``wp.array`` of shapes + ``(M, 3)`` and ``(M, 4)`` respectively. + """ state = NewtonManager.get_state_0() if indices is not None: n = len(indices) - idx_wp = indices pos_buf = wp.zeros(n, dtype=wp.vec3f, device=self._device) quat_buf = wp.zeros(n, dtype=wp.vec4f, device=self._device) wp.launch( @@ -604,7 +778,7 @@ def get_local_poses(self, indices: wp.array | None = None) -> tuple[wp.array, wp self._site_local, self._parent_site_body, self._parent_site_local, - idx_wp, + indices, ], outputs=[pos_buf, quat_buf], device=self._device, @@ -632,10 +806,18 @@ def set_local_poses( orientations: wp.array | None = None, indices: wp.array | None = None, ) -> None: - """Write parent-relative poses by updating the site's local offset. + """Set parent-relative translations and/or orientations. + + Updates the internal ``site_local`` offsets so that + ``inv(parent_world) * (body_q[bid] * site_local)`` yields the desired + local pose. Does **not** modify ``body_q``. - Computes the new ``site_local`` such that - ``inv(parent_world) * (body_q[bid] * site_local) == desired_local``. + Args: + translations: Desired parent-relative translations ``(M, 3)``. + ``None`` leaves translations unchanged. + orientations: Desired parent-relative quaternions ``(M, 4)`` as + ``(qx, qy, qz, qw)``. ``None`` leaves orientations unchanged. + indices: Subset of sites to update. ``None`` means all sites. """ if translations is None and orientations is None: return @@ -649,24 +831,19 @@ def set_local_poses( if orientations is None: orientations = cur_quat - pos_wp = translations - quat_wp = orientations - if indices is not None: - n = len(indices) - idx_wp = indices wp.launch( _write_site_local_from_local_poses_indexed, - dim=n, + dim=len(indices), inputs=[ state.body_q, self._site_body, - self._site_local, self._parent_site_body, self._parent_site_local, - idx_wp, - pos_wp, - quat_wp, + indices, + translations, + orientations, + self._site_local, ], device=self._device, ) @@ -677,11 +854,11 @@ def set_local_poses( inputs=[ state.body_q, self._site_body, - self._site_local, self._parent_site_body, self._parent_site_local, - pos_wp, - quat_wp, + translations, + orientations, + self._site_local, ], device=self._device, ) @@ -691,17 +868,24 @@ def set_local_poses( # ------------------------------------------------------------------ def get_scales(self, indices: wp.array | None = None) -> wp.array: + """Get per-site scales by reading from the first collision shape on the same body. + + Args: + indices: Subset of sites to query. ``None`` means all sites. + + Returns: + A ``wp.array`` of shape ``(M, 3)``. + """ model = NewtonManager.get_model() num_shapes = model.shape_count if indices is not None: n = len(indices) - idx_wp = indices out = wp.zeros(n, dtype=wp.vec3f, device=self._device) wp.launch( _gather_scales_indexed, dim=n, - inputs=[model.shape_scale, model.shape_body, self._site_body, idx_wp, num_shapes], + inputs=[model.shape_scale, model.shape_body, self._site_body, indices, num_shapes], outputs=[out], device=self._device, ) @@ -717,24 +901,27 @@ def get_scales(self, indices: wp.array | None = None) -> wp.array: return out def set_scales(self, scales: wp.array, indices: wp.array | None = None) -> None: + """Set per-site scales by writing to all collision shapes on the same body. + + Args: + scales: New scales ``(M, 3)`` as ``wp.array``. + indices: Subset of sites to update. ``None`` means all sites. + """ model = NewtonManager.get_model() num_shapes = model.shape_count - scales_wp = scales if indices is not None: - n = len(indices) - idx_wp = indices wp.launch( _scatter_scales_indexed, - dim=n, - inputs=[model.shape_scale, model.shape_body, self._site_body, idx_wp, num_shapes, scales_wp], + dim=len(indices), + inputs=[self._site_body, indices, scales, model.shape_body, num_shapes, model.shape_scale], device=self._device, ) else: wp.launch( _scatter_scales, dim=self.count, - inputs=[model.shape_scale, model.shape_body, self._site_body, num_shapes, scales_wp], + inputs=[self._site_body, scales, model.shape_body, num_shapes, model.shape_scale], device=self._device, ) From 4e90ad9c743a8552fcbb5ae8136df757acbc9679 Mon Sep 17 00:00:00 2001 From: Kelly Guo Date: Thu, 16 Apr 2026 12:07:36 -0700 Subject: [PATCH 19/24] Apply suggestion from @kellyguo11 Signed-off-by: Kelly Guo --- .../isaaclab_physx/sim/__init__.pyi | 22 +++++++++++++++++++ 1 file changed, 22 insertions(+) diff --git a/source/isaaclab_physx/isaaclab_physx/sim/__init__.pyi b/source/isaaclab_physx/isaaclab_physx/sim/__init__.pyi index 452cda786cd1..cfb557fc1f27 100644 --- a/source/isaaclab_physx/isaaclab_physx/sim/__init__.pyi +++ b/source/isaaclab_physx/isaaclab_physx/sim/__init__.pyi @@ -27,4 +27,26 @@ from .spawners import ( spawn_deformable_body_material, DeformableBodyMaterialCfg, SurfaceDeformableBodyMaterialCfg, +__all__ = [ + "define_deformable_body_properties", + "modify_deformable_body_properties", + "DeformableBodyPropertiesCfg", + "DeformableObjectSpawnerCfg", + "spawn_deformable_body_material", + "DeformableBodyMaterialCfg", + "SurfaceDeformableBodyMaterialCfg", + "views", +] + +from .schemas import ( + define_deformable_body_properties, + modify_deformable_body_properties, + DeformableBodyPropertiesCfg ) +from .spawners import ( + DeformableObjectSpawnerCfg, + spawn_deformable_body_material, + DeformableBodyMaterialCfg, + SurfaceDeformableBodyMaterialCfg, +) +from . import views From 6963b20729089678ddc188064aa65ebe5b6a5dbc Mon Sep 17 00:00:00 2001 From: Kelly Guo Date: Thu, 16 Apr 2026 12:09:01 -0700 Subject: [PATCH 20/24] Update source/isaaclab_physx/isaaclab_physx/sim/__init__.pyi Signed-off-by: Kelly Guo --- .../isaaclab_physx/sim/__init__.pyi | 24 ------------------- 1 file changed, 24 deletions(-) diff --git a/source/isaaclab_physx/isaaclab_physx/sim/__init__.pyi b/source/isaaclab_physx/isaaclab_physx/sim/__init__.pyi index cfb557fc1f27..abc8d0087afd 100644 --- a/source/isaaclab_physx/isaaclab_physx/sim/__init__.pyi +++ b/source/isaaclab_physx/isaaclab_physx/sim/__init__.pyi @@ -3,30 +3,6 @@ # # SPDX-License-Identifier: BSD-3-Clause -__all__ = [ - "views", -] - -from . import views - "define_deformable_body_properties", - "modify_deformable_body_properties", - "DeformableBodyPropertiesCfg", - "DeformableObjectSpawnerCfg", - "spawn_deformable_body_material", - "DeformableBodyMaterialCfg", - "SurfaceDeformableBodyMaterialCfg", -] - -from .schemas import ( - define_deformable_body_properties, - modify_deformable_body_properties, - DeformableBodyPropertiesCfg -) -from .spawners import ( - DeformableObjectSpawnerCfg, - spawn_deformable_body_material, - DeformableBodyMaterialCfg, - SurfaceDeformableBodyMaterialCfg, __all__ = [ "define_deformable_body_properties", "modify_deformable_body_properties", From b7d1bddc7e050b9b9748c36222b3cfec5c57c894 Mon Sep 17 00:00:00 2001 From: Antoine Richard Date: Fri, 17 Apr 2026 09:12:30 +0200 Subject: [PATCH 21/24] Fix CI test failures in XformPrimView refactor PR Rename test_frame_view_contract.py to frame_view_contract_utils.py so pytest no longer collects it as a standalone test file. The module is a shared contract library imported by backend-specific test files that provide the required view_factory fixture. Remove accidental renderer code from Camera.__init__ that was introduced during a bad rebase conflict resolution. The Isaac Sim 6.0 fast-path settings and _resolve_simple_shading_mode() call belong in the renderer, not in Camera, and referenced attributes that don't exist on the class. --- .../isaaclab/sensors/camera/camera.py | 28 ------------------- ...ntract.py => frame_view_contract_utils.py} | 2 +- .../test/sim/test_views_xform_prim.py | 4 +-- .../test/sim/test_views_xform_prim_newton.py | 4 +-- .../test/sim/test_views_xform_prim_fabric.py | 4 +-- 5 files changed, 7 insertions(+), 35 deletions(-) rename source/isaaclab/test/sim/{test_frame_view_contract.py => frame_view_contract_utils.py} (99%) diff --git a/source/isaaclab/isaaclab/sensors/camera/camera.py b/source/isaaclab/isaaclab/sensors/camera/camera.py index 54bf957b2b94..096fd31d9eb8 100644 --- a/source/isaaclab/isaaclab/sensors/camera/camera.py +++ b/source/isaaclab/isaaclab/sensors/camera/camera.py @@ -120,34 +120,6 @@ def __init__(self, cfg: CameraCfg): settings = get_settings_manager() settings.set_bool("/isaaclab/render/rtx_sensors", True) - # This is only introduced in isaac sim 6.0 - if has_kit(): - isaac_sim_version = get_isaac_sim_version() - if isaac_sim_version.major >= 6: - # Set RTX flag to enable fast path when no regular RGB/RGBA annotators are requested - needs_color_render = "rgb" in self.cfg.data_types or "rgba" in self.cfg.data_types - if not needs_color_render: - settings.set_bool("/rtx/sdg/force/disableColorRender", True) - - # If we have GUI / viewport enabled, we turn off fast path so that the viewport is not black - if settings.get("/isaaclab/has_gui"): - settings.set_bool("/rtx/sdg/force/disableColorRender", False) - else: - if "albedo" in self.cfg.data_types: - logger.warning( - "Albedo annotator is only supported in Isaac Sim 6.0+. The albedo data type will be ignored." - ) - if any(data_type in self.SIMPLE_SHADING_MODES for data_type in self.cfg.data_types): - logger.warning( - "Simple shading annotators are only supported in Isaac Sim 6.0+. The simple shading data types" - " will be ignored." - ) - - # Set simple shading mode (if requested) before rendering - simple_shading_mode = self._resolve_simple_shading_mode() - if simple_shading_mode is not None: - settings.set_int(self.SIMPLE_SHADING_MODE_SETTING, simple_shading_mode) - # Compute camera orientation (convention conversion) and spawn rot = torch.tensor(self.cfg.offset.rot, dtype=torch.float32, device="cpu").unsqueeze(0) rot_offset = convert_camera_frame_orientation_convention( diff --git a/source/isaaclab/test/sim/test_frame_view_contract.py b/source/isaaclab/test/sim/frame_view_contract_utils.py similarity index 99% rename from source/isaaclab/test/sim/test_frame_view_contract.py rename to source/isaaclab/test/sim/frame_view_contract_utils.py index b72b9d78cb4b..37734fee4322 100644 --- a/source/isaaclab/test/sim/test_frame_view_contract.py +++ b/source/isaaclab/test/sim/frame_view_contract_utils.py @@ -7,7 +7,7 @@ This module defines the invariants that **every** FrameView backend (USD, Fabric, Newton) must satisfy. Backend test files import these tests -via ``from test_frame_view_contract import *`` and provide a +via ``from frame_view_contract_utils import *`` and provide a ``view_factory`` pytest fixture that builds the backend-specific scene. The factory signature is:: diff --git a/source/isaaclab/test/sim/test_views_xform_prim.py b/source/isaaclab/test/sim/test_views_xform_prim.py index 03b6b4d40d57..2b40705f732a 100644 --- a/source/isaaclab/test/sim/test_views_xform_prim.py +++ b/source/isaaclab/test/sim/test_views_xform_prim.py @@ -25,8 +25,8 @@ except (ModuleNotFoundError, ImportError): _IsaacSimXformPrimView = None -from test_frame_view_contract import * # noqa: F401, F403, E402 -from test_frame_view_contract import CHILD_OFFSET, ViewBundle # noqa: E402 +from frame_view_contract_utils import * # noqa: F401, F403, E402 +from frame_view_contract_utils import CHILD_OFFSET, ViewBundle # noqa: E402 import isaaclab.sim as sim_utils # noqa: E402 from isaaclab.sim.views import UsdFrameView as FrameView # noqa: E402 diff --git a/source/isaaclab_newton/test/sim/test_views_xform_prim_newton.py b/source/isaaclab_newton/test/sim/test_views_xform_prim_newton.py index cab303547cd2..9785b6d62e2e 100644 --- a/source/isaaclab_newton/test/sim/test_views_xform_prim_newton.py +++ b/source/isaaclab_newton/test/sim/test_views_xform_prim_newton.py @@ -19,11 +19,11 @@ import pytest import torch import warp as wp +from frame_view_contract_utils import * # noqa: F401, F403 — import all contract tests +from frame_view_contract_utils import CHILD_OFFSET, ViewBundle, _wp_vec3f, _wp_vec4f from isaaclab_newton.physics import MJWarpSolverCfg, NewtonCfg from isaaclab_newton.physics.newton_manager import NewtonManager from isaaclab_newton.sim.views import NewtonSiteFrameView as FrameView -from test_frame_view_contract import * # noqa: F401, F403 — import all contract tests -from test_frame_view_contract import CHILD_OFFSET, ViewBundle, _wp_vec3f, _wp_vec4f from pxr import Gf diff --git a/source/isaaclab_physx/test/sim/test_views_xform_prim_fabric.py b/source/isaaclab_physx/test/sim/test_views_xform_prim_fabric.py index 2b836a50859a..0bc77ccf7223 100644 --- a/source/isaaclab_physx/test/sim/test_views_xform_prim_fabric.py +++ b/source/isaaclab_physx/test/sim/test_views_xform_prim_fabric.py @@ -21,9 +21,9 @@ import pytest # noqa: E402 import torch # noqa: E402 +from frame_view_contract_utils import * # noqa: F401, F403, E402 +from frame_view_contract_utils import CHILD_OFFSET, ViewBundle # noqa: E402 from isaaclab_physx.sim.views import FabricFrameView as FrameView # noqa: E402 -from test_frame_view_contract import * # noqa: F401, F403, E402 -from test_frame_view_contract import CHILD_OFFSET, ViewBundle # noqa: E402 from pxr import Gf, UsdGeom # noqa: E402 From 709ba255c072e7481df4b829b9b8a73b6dccf0a2 Mon Sep 17 00:00:00 2001 From: Antoine Richard Date: Fri, 17 Apr 2026 13:33:14 +0200 Subject: [PATCH 22/24] Add missing prims property to NewtonSiteFrameView Camera._initialize_impl accesses self._view.prims to iterate USD prims. UsdFrameView and FabricFrameView expose this property but NewtonSiteFrameView only had the private _prims attribute, causing AttributeError when Camera runs on the Newton backend. --- .../isaaclab_newton/sim/views/newton_site_frame_view.py | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/source/isaaclab_newton/isaaclab_newton/sim/views/newton_site_frame_view.py b/source/isaaclab_newton/isaaclab_newton/sim/views/newton_site_frame_view.py index 2300d1494900..e4f2285cb528 100644 --- a/source/isaaclab_newton/isaaclab_newton/sim/views/newton_site_frame_view.py +++ b/source/isaaclab_newton/isaaclab_newton/sim/views/newton_site_frame_view.py @@ -658,6 +658,11 @@ def _resolve_ancestor_body( return WORLD_BODY_INDEX, _gf_matrix_to_xform7(prim_world_tf) + @property + def prims(self) -> list: + """List of USD prims being managed by this view.""" + return self._prims + @property def count(self) -> int: """Number of prims in this view.""" From 09a99973723e3707b440d2e32d708d4dbe03eff6 Mon Sep 17 00:00:00 2001 From: Antoine Richard Date: Fri, 17 Apr 2026 14:37:47 +0200 Subject: [PATCH 23/24] Fix Camera env_ids type handling for FrameView warp conversion Camera methods assumed env_ids is always a torch.Tensor when converting to warp arrays, but callers (e.g. visuotactile sensor) can pass plain lists. Convert to tensor before calling .to(). Also update visuotactile test assertion to accept the new error message from FrameView when a prim path matches zero prims. --- source/isaaclab/isaaclab/sensors/camera/camera.py | 13 +++++++++++-- .../test/sensors/test_visuotactile_sensor.py | 3 ++- 2 files changed, 13 insertions(+), 3 deletions(-) diff --git a/source/isaaclab/isaaclab/sensors/camera/camera.py b/source/isaaclab/isaaclab/sensors/camera/camera.py index 096fd31d9eb8..22c96af1779e 100644 --- a/source/isaaclab/isaaclab/sensors/camera/camera.py +++ b/source/isaaclab/isaaclab/sensors/camera/camera.py @@ -316,7 +316,12 @@ def set_world_poses( # convert torch tensors to warp arrays for the view pos_wp = wp.from_torch(positions.contiguous()) if positions is not None else None ori_wp = wp.from_torch(orientations.contiguous()) if orientations is not None else None - idx_wp = wp.from_torch(env_ids.to(dtype=torch.int32), dtype=wp.int32) if env_ids is not None else None + if env_ids is not None: + if not isinstance(env_ids, torch.Tensor): + env_ids = torch.tensor(env_ids, dtype=torch.int32, device=self._device) + idx_wp = wp.from_torch(env_ids.to(dtype=torch.int32), dtype=wp.int32) + else: + idx_wp = None self._view.set_world_poses(pos_wp, ori_wp, idx_wp) def set_world_poses_from_view( @@ -340,7 +345,9 @@ def set_world_poses_from_view( up_axis = UsdGeom.GetStageUpAxis(self.stage) # set camera poses using the view orientations = quat_from_matrix(create_rotation_matrix_from_view(eyes, targets, up_axis, device=self._device)) - idx_wp = wp.from_torch(env_ids.to(dtype=torch.int32), dtype=wp.int32) if env_ids is not None else None + if not isinstance(env_ids, torch.Tensor): + env_ids = torch.tensor(env_ids, dtype=torch.int32, device=self._device) + idx_wp = wp.from_torch(env_ids.to(dtype=torch.int32), dtype=wp.int32) self._view.set_world_poses(wp.from_torch(eyes.contiguous()), wp.from_torch(orientations.contiguous()), idx_wp) """ @@ -593,6 +600,8 @@ def _update_poses(self, env_ids: Sequence[int]): raise RuntimeError("Camera prim is None. Please call 'sim.play()' first.") # get the poses from the view (returns wp.array, convert to torch) + if env_ids is not None and not isinstance(env_ids, torch.Tensor): + env_ids = torch.tensor(env_ids, dtype=torch.int32, device=self._device) indices = wp.from_torch(env_ids.to(dtype=torch.int32), dtype=wp.int32) if env_ids is not None else None pos_wp, quat_wp = self._view.get_world_poses(indices) self._data.pos_w[env_ids] = wp.to_torch(pos_wp) diff --git a/source/isaaclab_contrib/test/sensors/test_visuotactile_sensor.py b/source/isaaclab_contrib/test/sensors/test_visuotactile_sensor.py index 88a2249bafa0..5ea9a373fb3b 100644 --- a/source/isaaclab_contrib/test/sensors/test_visuotactile_sensor.py +++ b/source/isaaclab_contrib/test/sensors/test_visuotactile_sensor.py @@ -307,7 +307,8 @@ def test_sensor_cam_set_wrong_prim(setup_tactile_cam): sim.reset() robot.update(dt) sensor.update(dt) - assert "Could not find prim with path" in str(excinfo.value) + err_msg = str(excinfo.value) + assert "Could not find prim with path" in err_msg or "does not match the number of environments" in err_msg @pytest.mark.isaacsim_ci From ee3d9fdb14e267321919d6f85478903a4a960fc4 Mon Sep 17 00:00:00 2001 From: Antoine Richard Date: Mon, 20 Apr 2026 09:46:49 +0200 Subject: [PATCH 24/24] Fix malformed RST table and add migration guide for FrameView rename MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Fix the malformed RST grid table in the sensors __init__.py docstring where the Ray Caster row had misaligned column widths, causing the Sphinx doc build to fail. Add a migration guide section documenting the XformPrimView → FrameView rename across all backends (BaseFrameView, UsdFrameView, FrameView, FabricFrameView, NewtonSiteFrameView). Also add BaseFrameView and UsdFrameView to the isaaclab.sim.views API docs. --- docs/source/api/lab/isaaclab.sim.views.rst | 16 +++++++ .../migration/migrating_to_isaaclab_3-0.rst | 43 +++++++++++++++++++ source/isaaclab/isaaclab/sensors/__init__.py | 2 +- 3 files changed, 60 insertions(+), 1 deletion(-) diff --git a/docs/source/api/lab/isaaclab.sim.views.rst b/docs/source/api/lab/isaaclab.sim.views.rst index fcb0d281008c..e06c4e54a246 100644 --- a/docs/source/api/lab/isaaclab.sim.views.rst +++ b/docs/source/api/lab/isaaclab.sim.views.rst @@ -7,8 +7,24 @@ .. autosummary:: + BaseFrameView + UsdFrameView FrameView +Base Frame View +--------------- + +.. autoclass:: BaseFrameView + :members: + :show-inheritance: + +USD Frame View +-------------- + +.. autoclass:: UsdFrameView + :members: + :show-inheritance: + Frame View ---------- diff --git a/docs/source/migration/migrating_to_isaaclab_3-0.rst b/docs/source/migration/migrating_to_isaaclab_3-0.rst index 12e0501c9028..cdf6ada9b2c6 100644 --- a/docs/source/migration/migrating_to_isaaclab_3-0.rst +++ b/docs/source/migration/migrating_to_isaaclab_3-0.rst @@ -98,6 +98,49 @@ The following classes have been moved to ``isaaclab_physx``: installation steps are required. +Renaming of ``XformPrimView`` to ``FrameView`` +----------------------------------------------- + +Isaac Lab's ``XformPrimView`` and related classes have been renamed to ``FrameView`` to +better reflect their purpose and avoid confusion with Isaac Sim's ``XFormPrim`` class +hierarchy. The old ``XformPrimView`` name is kept as a deprecated alias. + +The rename applies across all backends: + +.. list-table:: + :header-rows: 1 + :widths: 50 50 + + * - Isaac Lab 2.x + - Isaac Lab 3.0 + * - ``BaseXformPrimView`` + - :class:`~isaaclab.sim.views.BaseFrameView` + * - ``UsdXformPrimView`` + - :class:`~isaaclab.sim.views.UsdFrameView` + * - ``XformPrimView`` + - :class:`~isaaclab.sim.views.FrameView` + * - ``FabricXformPrimView`` + - :class:`~isaaclab_physx.sim.views.FabricFrameView` + * - ``NewtonSiteXformPrimView`` + - :class:`~isaaclab_newton.sim.views.NewtonSiteFrameView` + +For most users, the only change needed is updating imports: + +.. code-block:: python + + # Before + from isaaclab.sim.views import XformPrimView + + # After + from isaaclab.sim.views import FrameView + +The :class:`~isaaclab.sim.views.FrameView` factory automatically dispatches to the correct +backend (:class:`~isaaclab_physx.sim.views.FabricFrameView` for PhysX, +:class:`~isaaclab_newton.sim.views.NewtonSiteFrameView` for Newton) based on the active +physics backend. The deprecated ``XformPrimView`` alias continues to work but will be +removed in a future release. + + Unchanged Imports ----------------- diff --git a/source/isaaclab/isaaclab/sensors/__init__.py b/source/isaaclab/isaaclab/sensors/__init__.py index faefd42c743b..717fc4a7163c 100644 --- a/source/isaaclab/isaaclab/sensors/__init__.py +++ b/source/isaaclab/isaaclab/sensors/__init__.py @@ -26,7 +26,7 @@ +---------------------+---------------------------+---------------------------------------------------------------+ | Contact Sensor | /World/robot/feet_* | Leaf is available and checks if the schema exists | +---------------------+---------------------------+---------------------------------------------------------------+ -| Ray Caster | /World/robot/base/raycast | ``spawn`` creates an Xform leaf; else the leaf must exist | +| Ray Caster | /World/robot/base/raycast | ``spawn`` creates an Xform leaf; else the leaf must exist | +---------------------+---------------------------+---------------------------------------------------------------+ | Frame Transformer | /World/robot/base | Leaf exists and is a physics body (Articulation / Rigid Body) | +---------------------+---------------------------+---------------------------------------------------------------+