feat: add indexed fabric transform kernels for local/world matrix propagation

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`. Will be 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:

source/isaaclab/changelog.d/indexed-fabric-kernels.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).  Will be 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,192 @@ 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.func
+def _local_from_world_transposed(child_world_T: wp.mat44f, parent_world_T: wp.mat44f) -> wp.mat44f:
+    """Compute local^T = world^T * inv(parent^T) on transposed storage matrices."""
+    return child_world_T * wp.inverse(parent_world_T)
+
+
+@wp.func
+def _world_from_local_transposed(child_local_T: wp.mat44f, parent_world_T: wp.mat44f) -> wp.mat44f:
+    """Compute world^T = local^T * parent^T on transposed storage matrices."""
+    return child_local_T * parent_world_T
+
+
+@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(  # type: ignore[arg-type]
+        _local_from_world_transposed(child_world, parent_world)
+    )
+
+
+@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(  # type: ignore[arg-type]
+        _world_from_local_transposed(child_local, parent_world)
+    )
+
+
 @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.