perf: Implement Fabric-aware get/set_local_poses on FabricFrameView

docs/source/api/index.rst

@@ -131,6 +131,7 @@ The following modules are available in the ``isaaclab_physx`` extension:
    sensors
    sim.schemas
    sim.spawners
+   sim.views
 
 .. toctree::
    :hidden:
@@ -142,6 +143,7 @@ The following modules are available in the ``isaaclab_physx`` extension:
    lab_physx/isaaclab_physx.sensors
    lab_physx/isaaclab_physx.sim.schemas
    lab_physx/isaaclab_physx.sim.spawners
+   lab_physx/isaaclab_physx.sim.views
 
 isaaclab_newton extension
 -------------------------

docs/source/api/lab/isaaclab.utils.rst

@@ -188,3 +188,16 @@ Warp operations
    :members:
    :imported-members:
    :show-inheritance:
+
+Warp Fabric kernels
+^^^^^^^^^^^^^^^^^^^
+
+Warp kernels for reading and writing Fabric ``Matrix4d`` attributes
+(``omni:fabric:worldMatrix`` / ``omni:fabric:localMatrix``) via
+:class:`wp.fabricarray` and :class:`wp.indexedfabricarray`.  Used by
+:class:`~isaaclab_physx.sim.views.FabricFrameView` to keep child world and
+local matrices consistent without round-tripping through USD.
+
+.. automodule:: isaaclab.utils.warp.fabric
+   :members:
+   :show-inheritance:

docs/source/api/lab_physx/isaaclab_physx.sim.views.rst

@@ -0,0 +1,17 @@
+isaaclab\_physx.sim.views
+=========================
+
+.. automodule:: isaaclab_physx.sim.views
+
+   .. rubric:: Classes
+
+   .. autosummary::
+
+      FabricFrameView
+
+Fabric Frame View
+-----------------
+
+.. autoclass:: FabricFrameView
+   :members:
+   :show-inheritance:

scripts/benchmarks/benchmark_view_comparison.py

@@ -271,26 +271,71 @@ def _run_pose_benchmarks(
     positions: wp.array,
     orientations: wp.array,
 ):
-    """Shared benchmark loop for get/set world poses on any FrameView."""
+    """Shared benchmark loop for get/set {world,local} poses on any FrameView."""
+
+    # FrameView getters now return ProxyArray; older callers worked with wp.array
+    # directly. Support both transparently.
+    def _as_wp(a):
+        return a.warp if hasattr(a, "warp") else a
+
+    positions_wp = _as_wp(positions)
+    orientations_wp = _as_wp(orientations)
+
     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
 
-    new_positions = wp.clone(positions)
+    new_positions = wp.clone(positions_wp)
     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_positions, orientations)
+        view.set_world_poses(new_positions, orientations_wp)
     timing_results["set_world_poses"] = (time.perf_counter() - start_time) / num_iterations
 
+    # Interleaved set→get on world poses — the realistic write/read pattern for
+    # downstream consumers (e.g. cameras updating their pose then immediately
+    # querying it).
+    start_time = time.perf_counter()
+    for _ in range(num_iterations):
+        view.set_world_poses(new_positions, orientations_wp)
+        view.get_world_poses()
+    timing_results["interleaved_world"] = (time.perf_counter() - start_time) / num_iterations
+
+    # Local poses — Fabric-aware path on FabricFrameView, USD path otherwise.
+    if hasattr(view, "get_local_poses"):
+        start_time = time.perf_counter()
+        for _ in range(num_iterations):
+            view.get_local_poses()
+        timing_results["get_local_poses"] = (time.perf_counter() - start_time) / num_iterations
+
+    if hasattr(view, "set_local_poses"):
+        local_pos, local_ori = view.get_local_poses()
+        local_pos_t = (
+            local_pos.torch
+            if hasattr(local_pos, "torch")
+            else (wp.to_torch(local_pos) if isinstance(local_pos, wp.array) else local_pos)
+        )
+        local_ori_t = (
+            local_ori.torch
+            if hasattr(local_ori, "torch")
+            else (wp.to_torch(local_ori) if isinstance(local_ori, wp.array) else local_ori)
+        )
+        new_local_pos = wp.from_torch(local_pos_t.clone().contiguous())
+        new_local_ori = wp.from_torch(local_ori_t.clone().contiguous())
+
+        start_time = time.perf_counter()
+        for _ in range(num_iterations):
+            view.set_local_poses(translations=new_local_pos, orientations=new_local_ori)
+        timing_results["set_local_poses"] = (time.perf_counter() - start_time) / num_iterations
+
     ret_pos, ret_quat = view.get_world_poses()
-    ret_pos_t = wp.to_torch(ret_pos)
-    ret_quat_t = wp.to_torch(ret_quat)
-    ori_t = wp.to_torch(orientations)
+    ret_pos_t = ret_pos.torch if hasattr(ret_pos, "torch") else wp.to_torch(ret_pos)
+    ret_quat_t = ret_quat.torch if hasattr(ret_quat, "torch") else wp.to_torch(ret_quat)
+    ori_t = wp.to_torch(orientations_wp)
 
     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)
@@ -327,6 +372,9 @@ def print_results(results_dict: dict[str, dict[str, float]], num_prims: int, num
         ("Initialization", "init"),
         ("Get World Poses", "get_world_poses"),
         ("Set World Poses", "set_world_poses"),
+        ("Interleaved Set->Get", "interleaved_world"),
+        ("Get Local Poses", "get_local_poses"),
+        ("Set Local Poses", "set_local_poses"),
     ]
 
     for op_name, op_key in operations:

source/isaaclab/changelog.d/fix-fabric-local-matrix.rst

@@ -0,0 +1,24 @@
+Added
+^^^^^
+
+* Added :func:`~isaaclab.utils.warp.fabric.decompose_indexed_fabric_transforms`
+  and :func:`~isaaclab.utils.warp.fabric.compose_indexed_fabric_transforms`
+  Warp kernels.  They mirror the existing
+  ``decompose_fabric_transformation_matrix_to_warp_arrays`` /
+  ``compose_fabric_transformation_matrix_from_warp_arrays`` kernels but
+  operate on :class:`wp.indexedfabricarray`, so the view-to-fabric mapping
+  is baked into the array and the kernel just dereferences
+  ``ifa[view_index]`` instead of taking a separate ``mapping`` argument.
+* Added :func:`~isaaclab.utils.warp.fabric.update_indexed_local_matrix_from_world`
+  and :func:`~isaaclab.utils.warp.fabric.update_indexed_world_matrix_from_local`
+  Warp kernels that propagate ``local = world * inv(parent)`` and
+  ``world = local * parent`` directly on Fabric storage matrices (no
+  explicit transposes).  Used by
+  :class:`~isaaclab_physx.sim.views.FabricFrameView` to keep child world and
+  local matrices consistent across writes without round-tripping through USD.
+* Added :meth:`~isaaclab.sim.SimulationContext.get_service` and
+  :meth:`~isaaclab.sim.SimulationContext.set_service` — a typed singleton
+  service locator on :class:`~isaaclab.sim.SimulationContext`.  Backend-specific
+  caches (e.g. Fabric hierarchy handles) register themselves here instead of
+  living as class-level globals.  Services are automatically cleared on
+  :meth:`~isaaclab.sim.SimulationContext.clear_instance`.

source/isaaclab/isaaclab/sim/simulation_context.py

@@ -11,7 +11,7 @@
 import traceback
 from collections.abc import Iterator
 from contextlib import contextmanager
-from typing import TYPE_CHECKING, Any
+from typing import TYPE_CHECKING, Any, TypeVar
 
 import toml
 import torch
@@ -38,6 +38,8 @@
 if TYPE_CHECKING:
     from isaaclab.cloner.clone_plan import ClonePlan
 
+_T = TypeVar("_T")
+
 from .simulation_cfg import SimulationCfg
 from .spawners import DomeLightCfg, GroundPlaneCfg
 
@@ -214,6 +216,11 @@ def __init__(self, cfg: SimulationCfg | None = None):
             order=5,
         )
 
+        # Singleton service registry — backend-specific caches (e.g. Fabric hierarchy)
+        # register themselves here, keyed by their class.  All services are cleared when
+        # the SimulationContext is torn down via clear_instance().
+        self._services: dict[type, object] = {}
+
         type(self)._instance = self  # Mark as valid singleton only after successful init
 
     def _apply_render_cfg_settings(self) -> None:
@@ -852,6 +859,32 @@ def get_setting(self, name: str) -> Any:
         """Get a setting value."""
         return self._settings_helper.get(name)
 
+    def get_service(self, cls: type[_T]) -> _T | None:
+        """Retrieve a registered singleton service by its class.
+
+        Args:
+            cls: The service class used as key.
+
+        Returns:
+            The registered instance, or ``None`` if not registered.
+        """
+        return self._services.get(cls)  # type: ignore[return-value]
+
+    def set_service(self, cls: type[_T], instance: _T) -> None:
+        """Register a singleton service, keyed by its class.
+
+        Overwrites any previously registered instance for the same class.
+        The service is automatically cleared when :meth:`clear_instance` is called.
+
+        Args:
+            cls: The service class used as key.
+            instance: The service instance to register.
+        """
+        old = self._services.get(cls)
+        if old is not None and old is not instance and hasattr(old, "close"):
+            old.close()
+        self._services[cls] = instance
+
     @classmethod
     def clear_instance(cls) -> None:
         """Clean up resources and clear the singleton instance."""
@@ -865,6 +898,12 @@ def clear_instance(cls) -> None:
                 viz.close()
             cls._instance._visualizers.clear()
 
+            # Close and drop all registered singleton services
+            for service in cls._instance._services.values():
+                if hasattr(service, "close"):
+                    service.close()
+            cls._instance._services.clear()
+
             # Tear down the stage. We skip clear_stage() (prim-by-prim deletion) since
             # close_stage() + app shutdown destroy the entire stage at once.
             stage_utils.close_stage()