Implement Fabric-aware get/set_local_poses via indexedfabricarray

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/pv-5381-rebased.rst

@@ -0,0 +1,18 @@
+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.

source/isaaclab/isaaclab/utils/warp/fabric.py

@@ -18,12 +18,14 @@
 if TYPE_CHECKING:
     FabricArrayUInt32 = Any
     FabricArrayMat44d = Any
+    IndexedFabricArrayMat44d = Any
     ArrayUInt32 = Any
     ArrayUInt32_1d = Any
     ArrayFloat32_2d = Any
 else:
     FabricArrayUInt32 = wp.fabricarray(dtype=wp.uint32)
     FabricArrayMat44d = wp.fabricarray(dtype=wp.mat44d)
+    IndexedFabricArrayMat44d = wp.indexedfabricarray(dtype=wp.mat44d)
     ArrayUInt32 = wp.array(ndim=1, dtype=wp.uint32)
     ArrayUInt32_1d = wp.array(dtype=wp.uint32)
     ArrayFloat32_2d = wp.array(ndim=2, dtype=wp.float32)
@@ -163,6 +165,176 @@ def compose_fabric_transformation_matrix_from_warp_arrays(
     )
 
 
+@wp.kernel(enable_backward=False)
+def decompose_indexed_fabric_transforms(
+    fabric_matrices: IndexedFabricArrayMat44d,
+    array_positions: ArrayFloat32_2d,
+    array_orientations: ArrayFloat32_2d,
+    array_scales: ArrayFloat32_2d,
+    indices: ArrayUInt32,
+):
+    """Decompose indexed Fabric transformation matrices into position, orientation, and scale.
+
+    Like :func:`decompose_fabric_transformation_matrix_to_warp_arrays` but operates on a
+    :class:`wp.indexedfabricarray` that already encodes the view-to-fabric mapping, removing
+    the need for a separate ``mapping`` array.
+
+    Args:
+        fabric_matrices: Indexed fabric array containing 4x4 transformation matrices.
+        array_positions: Output array for positions [m], shape (N, 3).
+        array_orientations: Output array for quaternions in xyzw format, shape (N, 4).
+        array_scales: Output array for scales, shape (N, 3).
+        indices: View indices to process (subset selection).
+    """
+    output_index = wp.tid()
+    view_index = indices[output_index]
+
+    position, rotation, scale = _decompose_transformation_matrix(wp.mat44f(fabric_matrices[view_index]))
+
+    if array_positions.shape[0] > 0:
+        array_positions[output_index, 0] = position[0]
+        array_positions[output_index, 1] = position[1]
+        array_positions[output_index, 2] = position[2]
+    if array_orientations.shape[0] > 0:
+        array_orientations[output_index, 0] = rotation[0]
+        array_orientations[output_index, 1] = rotation[1]
+        array_orientations[output_index, 2] = rotation[2]
+        array_orientations[output_index, 3] = rotation[3]
+    if array_scales.shape[0] > 0:
+        array_scales[output_index, 0] = scale[0]
+        array_scales[output_index, 1] = scale[1]
+        array_scales[output_index, 2] = scale[2]
+
+
+@wp.kernel(enable_backward=False)
+def compose_indexed_fabric_transforms(
+    fabric_matrices: IndexedFabricArrayMat44d,
+    array_positions: ArrayFloat32_2d,
+    array_orientations: ArrayFloat32_2d,
+    array_scales: ArrayFloat32_2d,
+    broadcast_positions: bool,
+    broadcast_orientations: bool,
+    broadcast_scales: bool,
+    indices: ArrayUInt32,
+):
+    """Compose indexed Fabric transformation matrices from position, orientation, and scale.
+
+    Like :func:`compose_fabric_transformation_matrix_from_warp_arrays` but operates on a
+    :class:`wp.indexedfabricarray` that already encodes the view-to-fabric mapping, removing
+    the need for a separate ``mapping`` array.
+
+    Args:
+        fabric_matrices: Indexed fabric array containing 4x4 transformation matrices to update.
+        array_positions: Input array for positions [m], shape (N, 3).
+        array_orientations: Input array for quaternions in xyzw format, shape (N, 4).
+        array_scales: Input array for scales, shape (N, 3).
+        broadcast_positions: If True, use first position for all prims.
+        broadcast_orientations: If True, use first orientation for all prims.
+        broadcast_scales: If True, use first scale for all prims.
+        indices: View indices to process (subset selection).
+    """
+    i = wp.tid()
+    view_index = indices[i]
+    position, rotation, scale = _decompose_transformation_matrix(wp.mat44f(fabric_matrices[view_index]))
+
+    if array_positions.shape[0] > 0:
+        if broadcast_positions:
+            index = 0
+        else:
+            index = i
+        position[0] = array_positions[index, 0]
+        position[1] = array_positions[index, 1]
+        position[2] = array_positions[index, 2]
+    if array_orientations.shape[0] > 0:
+        if broadcast_orientations:
+            index = 0
+        else:
+            index = i
+        rotation[0] = array_orientations[index, 0]
+        rotation[1] = array_orientations[index, 1]
+        rotation[2] = array_orientations[index, 2]
+        rotation[3] = array_orientations[index, 3]
+    if array_scales.shape[0] > 0:
+        if broadcast_scales:
+            index = 0
+        else:
+            index = i
+        scale[0] = array_scales[index, 0]
+        scale[1] = array_scales[index, 1]
+        scale[2] = array_scales[index, 2]
+
+    fabric_matrices[view_index] = wp.mat44d(  # type: ignore[arg-type]
+        wp.transpose(wp.transform_compose(position, rotation, scale))  # type: ignore[arg-type]
+    )
+
+
+@wp.kernel(enable_backward=False)
+def update_indexed_local_matrix_from_world(
+    child_world_matrices: IndexedFabricArrayMat44d,
+    parent_world_matrices: IndexedFabricArrayMat44d,
+    child_local_matrices: IndexedFabricArrayMat44d,
+    indices: ArrayUInt32,
+):
+    """Recompute child localMatrix from (parent worldMatrix, child worldMatrix).
+
+    Computes ``child_local = inv(parent_world) * child_world`` per prim and writes the
+    result back to the child's :data:`omni:fabric:localMatrix` so that subsequent
+    ``get_local_poses`` calls see consistent values after a world-pose write.
+
+    All three indexed arrays are expected to be indexed by the same per-view indices
+    (i.e. ``view_to_child_fabric``, ``view_to_parent_fabric``, ``view_to_child_fabric``)
+    so the kernel only needs the view-side indices.
+
+    Storage convention: Fabric matrices are stored as the transpose of the standard
+    column-major math convention.  Math is ``local = inv(parent) * world``; under
+    the transpose identity ``(A * B)^T = B^T * A^T`` (and ``inv(A^T) = inv(A)^T``)
+    that is equivalent to storage-side ``local^T = world^T * inv(parent^T)``, so we
+    can compute it directly on the stored matrices without explicit transposes.
+
+    Args:
+        child_world_matrices: Indexed fabric array of child world matrices (read).
+        parent_world_matrices: Indexed fabric array of parent world matrices (read).
+        child_local_matrices: Indexed fabric array of child local matrices (written).
+        indices: View indices to process.
+    """
+    i = wp.tid()
+    view_index = indices[i]
+    child_world = wp.mat44f(child_world_matrices[view_index])
+    parent_world = wp.mat44f(parent_world_matrices[view_index])
+    child_local_matrices[view_index] = wp.mat44d(child_world * wp.inverse(parent_world))  # type: ignore[arg-type]
+
+
+@wp.kernel(enable_backward=False)
+def update_indexed_world_matrix_from_local(
+    child_local_matrices: IndexedFabricArrayMat44d,
+    parent_world_matrices: IndexedFabricArrayMat44d,
+    child_world_matrices: IndexedFabricArrayMat44d,
+    indices: ArrayUInt32,
+):
+    """Recompute child worldMatrix from (parent worldMatrix, child localMatrix).
+
+    Computes ``child_world = parent_world * child_local`` per prim and writes the
+    result back to the child's :data:`omni:fabric:worldMatrix`.  Used after a
+    ``set_local_poses`` write so that subsequent ``get_world_poses`` calls see
+    consistent values.  Mirror of :func:`update_indexed_local_matrix_from_world`.
+
+    Args:
+        child_local_matrices: Indexed fabric array of child local matrices (read).
+        parent_world_matrices: Indexed fabric array of parent world matrices (read).
+        child_world_matrices: Indexed fabric array of child world matrices (written).
+        indices: View indices to process.
+
+    Storage convention: same as :func:`update_indexed_local_matrix_from_world`.
+    Math is ``world = parent * local``; under the transpose identity that becomes
+    storage-side ``world^T = local^T * parent^T``, no explicit transposes needed.
+    """
+    i = wp.tid()
+    view_index = indices[i]
+    child_local = wp.mat44f(child_local_matrices[view_index])
+    parent_world = wp.mat44f(parent_world_matrices[view_index])
+    child_world_matrices[view_index] = wp.mat44d(child_local * parent_world)  # type: ignore[arg-type]
+
+
 @wp.func
 def _decompose_transformation_matrix(m: Any):  # -> tuple[wp.vec3f, wp.quatf, wp.vec3f]
     """Decompose a 4x4 transformation matrix into position, orientation, and scale.

source/isaaclab_physx/changelog.d/fabric-frame-view-local-poses.rst

@@ -0,0 +1,25 @@
+Fixed
+^^^^^
+
+* Fixed :meth:`~isaaclab_physx.sim.views.FabricFrameView.get_local_poses`
+  returning stale USD values after Fabric world-pose writes.  Local poses
+  are now read directly from Fabric's ``omni:fabric:localMatrix`` via
+  :class:`wp.indexedfabricarray`, and are kept consistent with worldMatrix
+  through Warp kernels that propagate either direction on writes.
+
+Changed
+^^^^^^^
+
+* Reworked :class:`~isaaclab_physx.sim.views.FabricFrameView` to follow
+  the prototype design from ``bareya/pbarejko/camera-update``: three
+  persistent ``PrimSelection`` instances (one per access mode), path-based
+  view → fabric index mapping (no custom prim attributes), and Warp kernels
+  that operate on :class:`wp.indexedfabricarray` so the kernels just index
+  ``ifa[view_index]`` instead of taking a separate mapping array.
+* :meth:`~isaaclab_physx.sim.views.FabricFrameView.set_local_poses` now
+  writes ``omni:fabric:localMatrix`` directly through Fabric.  The next
+  ``get_world_poses`` runs a Warp kernel that recomputes
+  ``child_world = parent_world * child_local``.  Symmetrically,
+  ``set_world_poses`` runs a kernel that recomputes
+  ``child_local = inv(parent_world) * child_world`` so subsequent
+  ``get_local_poses`` calls return consistent values.