From 2cd762783f0c1a0d1794dcad6583170c692925ce Mon Sep 17 00:00:00 2001 From: Peter Verswyvelen Date: Mon, 18 May 2026 08:27:17 +0000 Subject: [PATCH 01/12] feat: add indexed fabric transform kernels for local/world matrix propagation MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Add Warp kernels that operate on wp.indexedfabricarray for direct local↔world matrix propagation without round-tripping through USD: - decompose_indexed_fabric_transforms: extract pos/quat/scale from ifa - compose_indexed_fabric_transforms: write pos/quat/scale into ifa - update_indexed_local_matrix_from_world: local = inv(parent) * world - update_indexed_world_matrix_from_local: world = parent * local Also refactor existing kernels to use wp.where (branchless) instead of if/else for broadcast index selection. These kernels are the foundation for Fabric-accelerated get/set_local_poses in FabricFrameView. --- docs/source/api/lab/isaaclab.utils.rst | 13 ++ .../changelog.d/indexed-fabric-kernels.rst | 24 +++ source/isaaclab/isaaclab/utils/warp/fabric.py | 189 +++++++++++++++-- .../test/utils/warp/test_fabric_kernels.py | 193 ++++++++++++++++++ 4 files changed, 407 insertions(+), 12 deletions(-) create mode 100644 source/isaaclab/changelog.d/indexed-fabric-kernels.rst create mode 100644 source/isaaclab/test/utils/warp/test_fabric_kernels.py diff --git a/docs/source/api/lab/isaaclab.utils.rst b/docs/source/api/lab/isaaclab.utils.rst index 5b352152e0b5..f236ebcb6a15 100644 --- a/docs/source/api/lab/isaaclab.utils.rst +++ b/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: diff --git a/source/isaaclab/changelog.d/indexed-fabric-kernels.rst b/source/isaaclab/changelog.d/indexed-fabric-kernels.rst new file mode 100644 index 000000000000..0f68b4c60e9d --- /dev/null +++ b/source/isaaclab/changelog.d/indexed-fabric-kernels.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). 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. + +Changed +^^^^^^^ + +* Replaced ``if/else`` branching with ``wp.where`` in existing Fabric + compose/decompose kernels for branchless GPU execution. diff --git a/source/isaaclab/isaaclab/utils/warp/fabric.py b/source/isaaclab/isaaclab/utils/warp/fabric.py index a48f773f4991..6f9963f290a1 100644 --- a/source/isaaclab/isaaclab/utils/warp/fabric.py +++ b/source/isaaclab/isaaclab/utils/warp/fabric.py @@ -15,15 +15,28 @@ import warp as wp +__all__ = [ + "arange_k", + "compose_fabric_transformation_matrix_from_warp_arrays", + "compose_indexed_fabric_transforms", + "decompose_fabric_transformation_matrix_to_warp_arrays", + "decompose_indexed_fabric_transforms", + "set_view_to_fabric_array", + "update_indexed_local_matrix_from_world", + "update_indexed_world_matrix_from_local", +] + 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) @@ -130,29 +143,20 @@ def compose_fabric_transformation_matrix_from_warp_arrays( position, rotation, scale = _decompose_transformation_matrix(wp.mat44f(fabric_matrices[fabric_index])) # update position (check if array has elements, not just if it exists) if array_positions.shape[0] > 0: - if broadcast_positions: - index = 0 - else: - index = i + index = wp.where(broadcast_positions, 0, i) position[0] = array_positions[index, 0] position[1] = array_positions[index, 1] position[2] = array_positions[index, 2] # update orientation (convert from wxyz to xyzw for Warp) if array_orientations.shape[0] > 0: - if broadcast_orientations: - index = 0 - else: - index = i + index = wp.where(broadcast_orientations, 0, i) rotation[0] = array_orientations[index, 0] # x rotation[1] = array_orientations[index, 1] # y rotation[2] = array_orientations[index, 2] # z rotation[3] = array_orientations[index, 3] # w # update scale if array_scales.shape[0] > 0: - if broadcast_scales: - index = 0 - else: - index = i + index = wp.where(broadcast_scales, 0, i) scale[0] = array_scales[index, 0] scale[1] = array_scales[index, 1] scale[2] = array_scales[index, 2] @@ -163,6 +167,167 @@ 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: + index = wp.where(broadcast_positions, 0, 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: + index = wp.where(broadcast_orientations, 0, 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: + index = wp.where(broadcast_scales, 0, 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. diff --git a/source/isaaclab/test/utils/warp/test_fabric_kernels.py b/source/isaaclab/test/utils/warp/test_fabric_kernels.py new file mode 100644 index 000000000000..13bad50da500 --- /dev/null +++ b/source/isaaclab/test/utils/warp/test_fabric_kernels.py @@ -0,0 +1,193 @@ +# 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 + +"""Unit tests for the Warp fabric transform kernels. + +Tests the decompose/compose math via helper kernels that operate on +regular wp.array (no Fabric/USDRT runtime required). +""" + +import numpy as np +import warp as wp + +wp.init() + +from isaaclab.utils.warp.fabric import _decompose_transformation_matrix # noqa: E402 + + +@wp.kernel(enable_backward=False) +def _test_decompose_kernel( + matrices: wp.array(dtype=wp.mat44f), + out_positions: wp.array(dtype=wp.vec3f), + out_rotations: wp.array(dtype=wp.vec4f), + out_scales: wp.array(dtype=wp.vec3f), +): + """Decompose a batch of 4x4 matrices into pos/quat/scale.""" + i = wp.tid() + pos, rot, scale = _decompose_transformation_matrix(matrices[i]) + out_positions[i] = pos + out_rotations[i] = wp.vec4f(rot[0], rot[1], rot[2], rot[3]) + out_scales[i] = scale + + +@wp.kernel(enable_backward=False) +def _test_compose_kernel( + positions: wp.array(dtype=wp.vec3f), + rotations: wp.array(dtype=wp.vec4f), + scales: wp.array(dtype=wp.vec3f), + out_matrices: wp.array(dtype=wp.mat44f), +): + """Compose a batch of pos/quat/scale into 4x4 matrices.""" + i = wp.tid() + pos = positions[i] + rot = wp.quatf(rotations[i][0], rotations[i][1], rotations[i][2], rotations[i][3]) + scale = scales[i] + out_matrices[i] = wp.transpose(wp.transform_compose(pos, rot, scale)) + + +class TestDecomposeCompose: + """Round-trip tests for decompose ↔ compose transform math.""" + + def test_identity_matrix(self): + """Identity matrix decomposes to pos=0, quat=identity, scale=1.""" + mat = np.eye(4, dtype=np.float32).reshape(1, 4, 4) + matrices = wp.array(mat, dtype=wp.mat44f, device="cpu") + out_pos = wp.zeros(1, dtype=wp.vec3f, device="cpu") + out_rot = wp.zeros(1, dtype=wp.vec4f, device="cpu") + out_scale = wp.zeros(1, dtype=wp.vec3f, device="cpu") + + wp.launch(_test_decompose_kernel, dim=1, inputs=[matrices, out_pos, out_rot, out_scale], device="cpu") + wp.synchronize() + + pos = out_pos.numpy() + rot = out_rot.numpy() + scale = out_scale.numpy() + + np.testing.assert_allclose(pos[0], [0, 0, 0], atol=1e-6) + np.testing.assert_allclose(scale[0], [1, 1, 1], atol=1e-6) + # Identity quaternion: either (0,0,0,1) or (0,0,0,-1) + assert abs(abs(rot[0, 3]) - 1.0) < 1e-5 + + def test_translation_only(self): + """Matrix with only translation decomposes correctly.""" + mat = np.eye(4, dtype=np.float32) + mat[3, 0] = 1.0 # row-major: translation in last row + mat[3, 1] = 2.0 + mat[3, 2] = 3.0 + matrices = wp.array(mat.reshape(1, 4, 4), dtype=wp.mat44f, device="cpu") + out_pos = wp.zeros(1, dtype=wp.vec3f, device="cpu") + out_rot = wp.zeros(1, dtype=wp.vec4f, device="cpu") + out_scale = wp.zeros(1, dtype=wp.vec3f, device="cpu") + + wp.launch(_test_decompose_kernel, dim=1, inputs=[matrices, out_pos, out_rot, out_scale], device="cpu") + wp.synchronize() + + np.testing.assert_allclose(out_pos.numpy()[0], [1, 2, 3], atol=1e-6) + np.testing.assert_allclose(out_scale.numpy()[0], [1, 1, 1], atol=1e-6) + + def test_uniform_scale(self): + """Matrix with uniform scale decomposes correctly.""" + mat = np.eye(4, dtype=np.float32) + mat[0, 0] = 2.0 + mat[1, 1] = 2.0 + mat[2, 2] = 2.0 + matrices = wp.array(mat.reshape(1, 4, 4), dtype=wp.mat44f, device="cpu") + out_pos = wp.zeros(1, dtype=wp.vec3f, device="cpu") + out_rot = wp.zeros(1, dtype=wp.vec4f, device="cpu") + out_scale = wp.zeros(1, dtype=wp.vec3f, device="cpu") + + wp.launch(_test_decompose_kernel, dim=1, inputs=[matrices, out_pos, out_rot, out_scale], device="cpu") + wp.synchronize() + + np.testing.assert_allclose(out_scale.numpy()[0], [2, 2, 2], atol=1e-6) + + def test_round_trip(self): + """Compose then decompose recovers original pos/quat/scale.""" + # Known transform: translate (5,6,7), rotate 90° about Z, scale (1,2,3) + pos = np.array([[5.0, 6.0, 7.0]], dtype=np.float32) + # 90° about Z in xyzw: (0, 0, sin(45°), cos(45°)) + s45 = np.sin(np.pi / 4) + c45 = np.cos(np.pi / 4) + rot = np.array([[0.0, 0.0, s45, c45]], dtype=np.float32) + scale = np.array([[1.0, 2.0, 3.0]], dtype=np.float32) + + wp_pos = wp.array(pos, dtype=wp.vec3f, device="cpu") + wp_rot = wp.array(rot, dtype=wp.vec4f, device="cpu") + wp_scale = wp.array(scale, dtype=wp.vec3f, device="cpu") + out_mat = wp.zeros(1, dtype=wp.mat44f, device="cpu") + + # Compose + wp.launch(_test_compose_kernel, dim=1, inputs=[wp_pos, wp_rot, wp_scale, out_mat], device="cpu") + wp.synchronize() + + # Decompose + out_pos = wp.zeros(1, dtype=wp.vec3f, device="cpu") + out_rot = wp.zeros(1, dtype=wp.vec4f, device="cpu") + out_scale = wp.zeros(1, dtype=wp.vec3f, device="cpu") + + wp.launch(_test_decompose_kernel, dim=1, inputs=[out_mat, out_pos, out_rot, out_scale], device="cpu") + wp.synchronize() + + np.testing.assert_allclose(out_pos.numpy()[0], pos[0], atol=1e-5) + np.testing.assert_allclose(out_scale.numpy()[0], scale[0], atol=1e-5) + # Quaternion sign ambiguity + r_out = out_rot.numpy()[0] + r_exp = rot[0] + dot = np.dot(r_out, r_exp) + np.testing.assert_allclose(abs(dot), 1.0, atol=1e-5) + + def test_non_uniform_scale_round_trip(self): + """Non-uniform scale round-trips correctly.""" + pos = np.array([[0.0, 0.0, 0.0]], dtype=np.float32) + rot = np.array([[0.0, 0.0, 0.0, 1.0]], dtype=np.float32) # identity + scale = np.array([[0.5, 2.0, 3.0]], dtype=np.float32) + + wp_pos = wp.array(pos, dtype=wp.vec3f, device="cpu") + wp_rot = wp.array(rot, dtype=wp.vec4f, device="cpu") + wp_scale = wp.array(scale, dtype=wp.vec3f, device="cpu") + out_mat = wp.zeros(1, dtype=wp.mat44f, device="cpu") + + wp.launch(_test_compose_kernel, dim=1, inputs=[wp_pos, wp_rot, wp_scale, out_mat], device="cpu") + wp.synchronize() + + out_pos = wp.zeros(1, dtype=wp.vec3f, device="cpu") + out_rot = wp.zeros(1, dtype=wp.vec4f, device="cpu") + out_scale = wp.zeros(1, dtype=wp.vec3f, device="cpu") + + wp.launch(_test_decompose_kernel, dim=1, inputs=[out_mat, out_pos, out_rot, out_scale], device="cpu") + wp.synchronize() + + np.testing.assert_allclose(out_scale.numpy()[0], scale[0], atol=1e-5) + + +class TestKernelSignatures: + """Verify all exported kernels are importable and are Warp Kernels.""" + + def test_all_kernels_importable(self): + """All public kernels listed in __all__ should be importable and be Warp Kernels.""" + from isaaclab.utils.warp import fabric as fabric_utils + + expected_kernels = [ + "arange_k", + "compose_fabric_transformation_matrix_from_warp_arrays", + "compose_indexed_fabric_transforms", + "decompose_fabric_transformation_matrix_to_warp_arrays", + "decompose_indexed_fabric_transforms", + "set_view_to_fabric_array", + "update_indexed_local_matrix_from_world", + "update_indexed_world_matrix_from_local", + ] + + for name in expected_kernels: + obj = getattr(fabric_utils, name, None) + assert obj is not None, f"{name} not found in fabric_utils" + assert isinstance(obj, wp.Kernel), f"{name} should be a wp.Kernel, got {type(obj)}" + + def test_module_exports_match_all(self): + """__all__ should list every public kernel.""" + from isaaclab.utils.warp import fabric as fabric_utils + + for name in fabric_utils.__all__: + assert hasattr(fabric_utils, name), f"__all__ lists '{name}' but it's not defined" From 8bc2d8381d6b4d3a8f36ac4e3cdfa84d72707a76 Mon Sep 17 00:00:00 2001 From: Peter Verswyvelen Date: Thu, 21 May 2026 15:06:59 +0000 Subject: [PATCH 02/12] test: convert fabric kernel tests to plain functions (no classes) --- .../test/utils/warp/test_fabric_kernels.py | 297 +++++++++--------- 1 file changed, 153 insertions(+), 144 deletions(-) diff --git a/source/isaaclab/test/utils/warp/test_fabric_kernels.py b/source/isaaclab/test/utils/warp/test_fabric_kernels.py index 13bad50da500..c2d3e6d37381 100644 --- a/source/isaaclab/test/utils/warp/test_fabric_kernels.py +++ b/source/isaaclab/test/utils/warp/test_fabric_kernels.py @@ -47,147 +47,156 @@ def _test_compose_kernel( out_matrices[i] = wp.transpose(wp.transform_compose(pos, rot, scale)) -class TestDecomposeCompose: - """Round-trip tests for decompose ↔ compose transform math.""" - - def test_identity_matrix(self): - """Identity matrix decomposes to pos=0, quat=identity, scale=1.""" - mat = np.eye(4, dtype=np.float32).reshape(1, 4, 4) - matrices = wp.array(mat, dtype=wp.mat44f, device="cpu") - out_pos = wp.zeros(1, dtype=wp.vec3f, device="cpu") - out_rot = wp.zeros(1, dtype=wp.vec4f, device="cpu") - out_scale = wp.zeros(1, dtype=wp.vec3f, device="cpu") - - wp.launch(_test_decompose_kernel, dim=1, inputs=[matrices, out_pos, out_rot, out_scale], device="cpu") - wp.synchronize() - - pos = out_pos.numpy() - rot = out_rot.numpy() - scale = out_scale.numpy() - - np.testing.assert_allclose(pos[0], [0, 0, 0], atol=1e-6) - np.testing.assert_allclose(scale[0], [1, 1, 1], atol=1e-6) - # Identity quaternion: either (0,0,0,1) or (0,0,0,-1) - assert abs(abs(rot[0, 3]) - 1.0) < 1e-5 - - def test_translation_only(self): - """Matrix with only translation decomposes correctly.""" - mat = np.eye(4, dtype=np.float32) - mat[3, 0] = 1.0 # row-major: translation in last row - mat[3, 1] = 2.0 - mat[3, 2] = 3.0 - matrices = wp.array(mat.reshape(1, 4, 4), dtype=wp.mat44f, device="cpu") - out_pos = wp.zeros(1, dtype=wp.vec3f, device="cpu") - out_rot = wp.zeros(1, dtype=wp.vec4f, device="cpu") - out_scale = wp.zeros(1, dtype=wp.vec3f, device="cpu") - - wp.launch(_test_decompose_kernel, dim=1, inputs=[matrices, out_pos, out_rot, out_scale], device="cpu") - wp.synchronize() - - np.testing.assert_allclose(out_pos.numpy()[0], [1, 2, 3], atol=1e-6) - np.testing.assert_allclose(out_scale.numpy()[0], [1, 1, 1], atol=1e-6) - - def test_uniform_scale(self): - """Matrix with uniform scale decomposes correctly.""" - mat = np.eye(4, dtype=np.float32) - mat[0, 0] = 2.0 - mat[1, 1] = 2.0 - mat[2, 2] = 2.0 - matrices = wp.array(mat.reshape(1, 4, 4), dtype=wp.mat44f, device="cpu") - out_pos = wp.zeros(1, dtype=wp.vec3f, device="cpu") - out_rot = wp.zeros(1, dtype=wp.vec4f, device="cpu") - out_scale = wp.zeros(1, dtype=wp.vec3f, device="cpu") - - wp.launch(_test_decompose_kernel, dim=1, inputs=[matrices, out_pos, out_rot, out_scale], device="cpu") - wp.synchronize() - - np.testing.assert_allclose(out_scale.numpy()[0], [2, 2, 2], atol=1e-6) - - def test_round_trip(self): - """Compose then decompose recovers original pos/quat/scale.""" - # Known transform: translate (5,6,7), rotate 90° about Z, scale (1,2,3) - pos = np.array([[5.0, 6.0, 7.0]], dtype=np.float32) - # 90° about Z in xyzw: (0, 0, sin(45°), cos(45°)) - s45 = np.sin(np.pi / 4) - c45 = np.cos(np.pi / 4) - rot = np.array([[0.0, 0.0, s45, c45]], dtype=np.float32) - scale = np.array([[1.0, 2.0, 3.0]], dtype=np.float32) - - wp_pos = wp.array(pos, dtype=wp.vec3f, device="cpu") - wp_rot = wp.array(rot, dtype=wp.vec4f, device="cpu") - wp_scale = wp.array(scale, dtype=wp.vec3f, device="cpu") - out_mat = wp.zeros(1, dtype=wp.mat44f, device="cpu") - - # Compose - wp.launch(_test_compose_kernel, dim=1, inputs=[wp_pos, wp_rot, wp_scale, out_mat], device="cpu") - wp.synchronize() - - # Decompose - out_pos = wp.zeros(1, dtype=wp.vec3f, device="cpu") - out_rot = wp.zeros(1, dtype=wp.vec4f, device="cpu") - out_scale = wp.zeros(1, dtype=wp.vec3f, device="cpu") - - wp.launch(_test_decompose_kernel, dim=1, inputs=[out_mat, out_pos, out_rot, out_scale], device="cpu") - wp.synchronize() - - np.testing.assert_allclose(out_pos.numpy()[0], pos[0], atol=1e-5) - np.testing.assert_allclose(out_scale.numpy()[0], scale[0], atol=1e-5) - # Quaternion sign ambiguity - r_out = out_rot.numpy()[0] - r_exp = rot[0] - dot = np.dot(r_out, r_exp) - np.testing.assert_allclose(abs(dot), 1.0, atol=1e-5) - - def test_non_uniform_scale_round_trip(self): - """Non-uniform scale round-trips correctly.""" - pos = np.array([[0.0, 0.0, 0.0]], dtype=np.float32) - rot = np.array([[0.0, 0.0, 0.0, 1.0]], dtype=np.float32) # identity - scale = np.array([[0.5, 2.0, 3.0]], dtype=np.float32) - - wp_pos = wp.array(pos, dtype=wp.vec3f, device="cpu") - wp_rot = wp.array(rot, dtype=wp.vec4f, device="cpu") - wp_scale = wp.array(scale, dtype=wp.vec3f, device="cpu") - out_mat = wp.zeros(1, dtype=wp.mat44f, device="cpu") - - wp.launch(_test_compose_kernel, dim=1, inputs=[wp_pos, wp_rot, wp_scale, out_mat], device="cpu") - wp.synchronize() - - out_pos = wp.zeros(1, dtype=wp.vec3f, device="cpu") - out_rot = wp.zeros(1, dtype=wp.vec4f, device="cpu") - out_scale = wp.zeros(1, dtype=wp.vec3f, device="cpu") - - wp.launch(_test_decompose_kernel, dim=1, inputs=[out_mat, out_pos, out_rot, out_scale], device="cpu") - wp.synchronize() - - np.testing.assert_allclose(out_scale.numpy()[0], scale[0], atol=1e-5) - - -class TestKernelSignatures: - """Verify all exported kernels are importable and are Warp Kernels.""" - - def test_all_kernels_importable(self): - """All public kernels listed in __all__ should be importable and be Warp Kernels.""" - from isaaclab.utils.warp import fabric as fabric_utils - - expected_kernels = [ - "arange_k", - "compose_fabric_transformation_matrix_from_warp_arrays", - "compose_indexed_fabric_transforms", - "decompose_fabric_transformation_matrix_to_warp_arrays", - "decompose_indexed_fabric_transforms", - "set_view_to_fabric_array", - "update_indexed_local_matrix_from_world", - "update_indexed_world_matrix_from_local", - ] - - for name in expected_kernels: - obj = getattr(fabric_utils, name, None) - assert obj is not None, f"{name} not found in fabric_utils" - assert isinstance(obj, wp.Kernel), f"{name} should be a wp.Kernel, got {type(obj)}" - - def test_module_exports_match_all(self): - """__all__ should list every public kernel.""" - from isaaclab.utils.warp import fabric as fabric_utils - - for name in fabric_utils.__all__: - assert hasattr(fabric_utils, name), f"__all__ lists '{name}' but it's not defined" +# ------------------------------------------------------------------ +# Decompose / Compose round-trip tests +# ------------------------------------------------------------------ + + +def test_identity_matrix(): + """Identity matrix decomposes to pos=0, quat=identity, scale=1.""" + mat = np.eye(4, dtype=np.float32).reshape(1, 4, 4) + matrices = wp.array(mat, dtype=wp.mat44f, device="cpu") + out_pos = wp.zeros(1, dtype=wp.vec3f, device="cpu") + out_rot = wp.zeros(1, dtype=wp.vec4f, device="cpu") + out_scale = wp.zeros(1, dtype=wp.vec3f, device="cpu") + + wp.launch(_test_decompose_kernel, dim=1, inputs=[matrices, out_pos, out_rot, out_scale], device="cpu") + wp.synchronize() + + pos = out_pos.numpy() + rot = out_rot.numpy() + scale = out_scale.numpy() + + np.testing.assert_allclose(pos[0], [0, 0, 0], atol=1e-6) + np.testing.assert_allclose(scale[0], [1, 1, 1], atol=1e-6) + # Identity quaternion: either (0,0,0,1) or (0,0,0,-1) + assert abs(abs(rot[0, 3]) - 1.0) < 1e-5 + + +def test_translation_only(): + """Matrix with only translation decomposes correctly.""" + mat = np.eye(4, dtype=np.float32) + mat[3, 0] = 1.0 # row-major: translation in last row + mat[3, 1] = 2.0 + mat[3, 2] = 3.0 + matrices = wp.array(mat.reshape(1, 4, 4), dtype=wp.mat44f, device="cpu") + out_pos = wp.zeros(1, dtype=wp.vec3f, device="cpu") + out_rot = wp.zeros(1, dtype=wp.vec4f, device="cpu") + out_scale = wp.zeros(1, dtype=wp.vec3f, device="cpu") + + wp.launch(_test_decompose_kernel, dim=1, inputs=[matrices, out_pos, out_rot, out_scale], device="cpu") + wp.synchronize() + + np.testing.assert_allclose(out_pos.numpy()[0], [1, 2, 3], atol=1e-6) + np.testing.assert_allclose(out_scale.numpy()[0], [1, 1, 1], atol=1e-6) + + +def test_uniform_scale(): + """Matrix with uniform scale decomposes correctly.""" + mat = np.eye(4, dtype=np.float32) + mat[0, 0] = 2.0 + mat[1, 1] = 2.0 + mat[2, 2] = 2.0 + matrices = wp.array(mat.reshape(1, 4, 4), dtype=wp.mat44f, device="cpu") + out_pos = wp.zeros(1, dtype=wp.vec3f, device="cpu") + out_rot = wp.zeros(1, dtype=wp.vec4f, device="cpu") + out_scale = wp.zeros(1, dtype=wp.vec3f, device="cpu") + + wp.launch(_test_decompose_kernel, dim=1, inputs=[matrices, out_pos, out_rot, out_scale], device="cpu") + wp.synchronize() + + np.testing.assert_allclose(out_scale.numpy()[0], [2, 2, 2], atol=1e-6) + + +def test_round_trip(): + """Compose then decompose recovers original pos/quat/scale.""" + # Known transform: translate (5,6,7), rotate 90deg about Z, scale (1,2,3) + pos = np.array([[5.0, 6.0, 7.0]], dtype=np.float32) + # 90deg about Z in xyzw: (0, 0, sin(45), cos(45)) + s45 = np.sin(np.pi / 4) + c45 = np.cos(np.pi / 4) + rot = np.array([[0.0, 0.0, s45, c45]], dtype=np.float32) + scale = np.array([[1.0, 2.0, 3.0]], dtype=np.float32) + + wp_pos = wp.array(pos, dtype=wp.vec3f, device="cpu") + wp_rot = wp.array(rot, dtype=wp.vec4f, device="cpu") + wp_scale = wp.array(scale, dtype=wp.vec3f, device="cpu") + out_mat = wp.zeros(1, dtype=wp.mat44f, device="cpu") + + # Compose + wp.launch(_test_compose_kernel, dim=1, inputs=[wp_pos, wp_rot, wp_scale, out_mat], device="cpu") + wp.synchronize() + + # Decompose + out_pos = wp.zeros(1, dtype=wp.vec3f, device="cpu") + out_rot = wp.zeros(1, dtype=wp.vec4f, device="cpu") + out_scale = wp.zeros(1, dtype=wp.vec3f, device="cpu") + + wp.launch(_test_decompose_kernel, dim=1, inputs=[out_mat, out_pos, out_rot, out_scale], device="cpu") + wp.synchronize() + + np.testing.assert_allclose(out_pos.numpy()[0], pos[0], atol=1e-5) + np.testing.assert_allclose(out_scale.numpy()[0], scale[0], atol=1e-5) + # Quaternion sign ambiguity + r_out = out_rot.numpy()[0] + r_exp = rot[0] + dot = np.dot(r_out, r_exp) + np.testing.assert_allclose(abs(dot), 1.0, atol=1e-5) + + +def test_non_uniform_scale_round_trip(): + """Non-uniform scale round-trips correctly.""" + pos = np.array([[0.0, 0.0, 0.0]], dtype=np.float32) + rot = np.array([[0.0, 0.0, 0.0, 1.0]], dtype=np.float32) # identity + scale = np.array([[0.5, 2.0, 3.0]], dtype=np.float32) + + wp_pos = wp.array(pos, dtype=wp.vec3f, device="cpu") + wp_rot = wp.array(rot, dtype=wp.vec4f, device="cpu") + wp_scale = wp.array(scale, dtype=wp.vec3f, device="cpu") + out_mat = wp.zeros(1, dtype=wp.mat44f, device="cpu") + + wp.launch(_test_compose_kernel, dim=1, inputs=[wp_pos, wp_rot, wp_scale, out_mat], device="cpu") + wp.synchronize() + + out_pos = wp.zeros(1, dtype=wp.vec3f, device="cpu") + out_rot = wp.zeros(1, dtype=wp.vec4f, device="cpu") + out_scale = wp.zeros(1, dtype=wp.vec3f, device="cpu") + + wp.launch(_test_decompose_kernel, dim=1, inputs=[out_mat, out_pos, out_rot, out_scale], device="cpu") + wp.synchronize() + + np.testing.assert_allclose(out_scale.numpy()[0], scale[0], atol=1e-5) + + +# ------------------------------------------------------------------ +# Kernel signature / importability tests +# ------------------------------------------------------------------ + + +def test_all_kernels_importable(): + """All public kernels listed in __all__ should be importable and be Warp Kernels.""" + from isaaclab.utils.warp import fabric as fabric_utils + + expected_kernels = [ + "arange_k", + "compose_fabric_transformation_matrix_from_warp_arrays", + "compose_indexed_fabric_transforms", + "decompose_fabric_transformation_matrix_to_warp_arrays", + "decompose_indexed_fabric_transforms", + "set_view_to_fabric_array", + "update_indexed_local_matrix_from_world", + "update_indexed_world_matrix_from_local", + ] + + for name in expected_kernels: + obj = getattr(fabric_utils, name, None) + assert obj is not None, f"{name} not found in fabric_utils" + assert isinstance(obj, wp.Kernel), f"{name} should be a wp.Kernel, got {type(obj)}" + + +def test_module_exports_match_all(): + """__all__ should list every public kernel.""" + from isaaclab.utils.warp import fabric as fabric_utils + + for name in fabric_utils.__all__: + assert hasattr(fabric_utils, name), f"__all__ lists '{name}' but it's not defined" From df1eb91b561ba6a327ae6f8e2ca5564c83bdfa57 Mon Sep 17 00:00:00 2001 From: Peter Verswyvelen Date: Thu, 21 May 2026 16:01:45 +0000 Subject: [PATCH 03/12] test: rewrite fabric kernel tests with wp.array (no Fabric runtime deps) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - Replace indexedfabricarray hacks with plain wp.array(dtype=wp.mat44d) test kernels - Test kernels mirror production math: local^T = world^T * inv(parent^T) - Add 5 tests for world↔local transforms including non-orthogonal/sheared cases - All tests run on CPU without USDRT/Fabric runtime - Convert existing class-based tests to plain functions (IsaacLab guideline) --- .../test/utils/warp/test_fabric_kernels.py | 208 +++++++++++++++++- 1 file changed, 199 insertions(+), 9 deletions(-) diff --git a/source/isaaclab/test/utils/warp/test_fabric_kernels.py b/source/isaaclab/test/utils/warp/test_fabric_kernels.py index c2d3e6d37381..3c17ff2c0bb2 100644 --- a/source/isaaclab/test/utils/warp/test_fabric_kernels.py +++ b/source/isaaclab/test/utils/warp/test_fabric_kernels.py @@ -5,8 +5,11 @@ """Unit tests for the Warp fabric transform kernels. -Tests the decompose/compose math via helper kernels that operate on -regular wp.array (no Fabric/USDRT runtime required). +Tests the shared @wp.func math (decompose/compose and matrix inverse/multiply) +through plain wp.array kernels — no Fabric/USDRT runtime required. + +The production fabric kernels are thin adapters over the same math; testing the +math in isolation avoids coupling tests to Fabric container internals. """ import numpy as np @@ -17,6 +20,11 @@ from isaaclab.utils.warp.fabric import _decompose_transformation_matrix # noqa: E402 +# ------------------------------------------------------------------ +# Test kernels (wp.array wrappers around the same math as production) +# ------------------------------------------------------------------ + + @wp.kernel(enable_backward=False) def _test_decompose_kernel( matrices: wp.array(dtype=wp.mat44f), @@ -47,6 +55,57 @@ def _test_compose_kernel( out_matrices[i] = wp.transpose(wp.transform_compose(pos, rot, scale)) +@wp.kernel(enable_backward=False) +def _test_local_from_world_kernel( + child_world: wp.array(dtype=wp.mat44d), + parent_world: wp.array(dtype=wp.mat44d), + out_local: wp.array(dtype=wp.mat44d), +): + """Same math as update_indexed_local_matrix_from_world: local^T = world^T * inv(parent^T). + + Casts to mat44f for compute (matching production precision), writes back as mat44d. + """ + i = wp.tid() + cw = wp.mat44f(child_world[i]) + pw = wp.mat44f(parent_world[i]) + out_local[i] = wp.mat44d(cw * wp.inverse(pw)) + + +@wp.kernel(enable_backward=False) +def _test_world_from_local_kernel( + child_local: wp.array(dtype=wp.mat44d), + parent_world: wp.array(dtype=wp.mat44d), + out_world: wp.array(dtype=wp.mat44d), +): + """Same math as update_indexed_world_matrix_from_local: world^T = local^T * parent^T.""" + i = wp.tid() + cl = wp.mat44f(child_local[i]) + pw = wp.mat44f(parent_world[i]) + out_world[i] = wp.mat44d(cl * pw) + + +# ------------------------------------------------------------------ +# Helpers +# ------------------------------------------------------------------ + + +def _make_transform_matrix(pos, rot_quat_xyzw, scale): + """Build a 4x4 Fabric-transposed transform from pos/quat/scale. + + Returns numpy (4,4) float64 in the transposed storage convention (row-major with + translation in the last row) that Fabric uses. + """ + from scipy.spatial.transform import Rotation + + r = Rotation.from_quat(rot_quat_xyzw).as_matrix().astype(np.float64) + rs = r * np.array(scale, dtype=np.float64) + m = np.eye(4, dtype=np.float64) + m[:3, :3] = rs + m[:3, 3] = pos + # Transpose for Fabric storage convention + return m.T + + # ------------------------------------------------------------------ # Decompose / Compose round-trip tests # ------------------------------------------------------------------ @@ -76,7 +135,7 @@ def test_identity_matrix(): def test_translation_only(): """Matrix with only translation decomposes correctly.""" mat = np.eye(4, dtype=np.float32) - mat[3, 0] = 1.0 # row-major: translation in last row + mat[3, 0] = 1.0 # row-major transposed: translation in last row mat[3, 1] = 2.0 mat[3, 2] = 3.0 matrices = wp.array(mat.reshape(1, 4, 4), dtype=wp.mat44f, device="cpu") @@ -110,9 +169,7 @@ def test_uniform_scale(): def test_round_trip(): """Compose then decompose recovers original pos/quat/scale.""" - # Known transform: translate (5,6,7), rotate 90deg about Z, scale (1,2,3) pos = np.array([[5.0, 6.0, 7.0]], dtype=np.float32) - # 90deg about Z in xyzw: (0, 0, sin(45), cos(45)) s45 = np.sin(np.pi / 4) c45 = np.cos(np.pi / 4) rot = np.array([[0.0, 0.0, s45, c45]], dtype=np.float32) @@ -123,11 +180,9 @@ def test_round_trip(): wp_scale = wp.array(scale, dtype=wp.vec3f, device="cpu") out_mat = wp.zeros(1, dtype=wp.mat44f, device="cpu") - # Compose wp.launch(_test_compose_kernel, dim=1, inputs=[wp_pos, wp_rot, wp_scale, out_mat], device="cpu") wp.synchronize() - # Decompose out_pos = wp.zeros(1, dtype=wp.vec3f, device="cpu") out_rot = wp.zeros(1, dtype=wp.vec4f, device="cpu") out_scale = wp.zeros(1, dtype=wp.vec3f, device="cpu") @@ -137,7 +192,6 @@ def test_round_trip(): np.testing.assert_allclose(out_pos.numpy()[0], pos[0], atol=1e-5) np.testing.assert_allclose(out_scale.numpy()[0], scale[0], atol=1e-5) - # Quaternion sign ambiguity r_out = out_rot.numpy()[0] r_exp = rot[0] dot = np.dot(r_out, r_exp) @@ -147,7 +201,7 @@ def test_round_trip(): def test_non_uniform_scale_round_trip(): """Non-uniform scale round-trips correctly.""" pos = np.array([[0.0, 0.0, 0.0]], dtype=np.float32) - rot = np.array([[0.0, 0.0, 0.0, 1.0]], dtype=np.float32) # identity + rot = np.array([[0.0, 0.0, 0.0, 1.0]], dtype=np.float32) scale = np.array([[0.5, 2.0, 3.0]], dtype=np.float32) wp_pos = wp.array(pos, dtype=wp.vec3f, device="cpu") @@ -168,6 +222,142 @@ def test_non_uniform_scale_round_trip(): np.testing.assert_allclose(out_scale.numpy()[0], scale[0], atol=1e-5) +# ------------------------------------------------------------------ +# World ↔ Local matrix tests +# +# These test the same math the production fabric kernels use: +# local^T = world^T * inv(parent^T) +# world^T = local^T * parent^T +# +# Under the transposed storage convention, this is equivalent to: +# local = inv(parent) * world +# world = parent * local +# ------------------------------------------------------------------ + + +def test_local_from_world_identity_parent(): + """With identity parent, local should equal world.""" + child_world_T = _make_transform_matrix([3, -1, 7], [0, 0, 0.3826834, 0.9238795], [1.5, 2.0, 0.5]) + parent_world_T = _make_transform_matrix([0, 0, 0], [0, 0, 0, 1], [1, 1, 1]) + + cw = wp.array(child_world_T.reshape(1, 4, 4), dtype=wp.mat44d, device="cpu") + pw = wp.array(parent_world_T.reshape(1, 4, 4), dtype=wp.mat44d, device="cpu") + out = wp.zeros(1, dtype=wp.mat44d, device="cpu") + + wp.launch(_test_local_from_world_kernel, dim=1, inputs=[cw, pw, out], device="cpu") + wp.synchronize() + + np.testing.assert_allclose(out.numpy()[0], child_world_T, atol=1e-5) + + +def test_local_from_world_non_orthogonal(): + """Non-uniform scale + rotation in parent produces non-orthogonal local matrix. + + Verifies: local^T @ parent^T == child_world^T (reconstruction check). + """ + parent_world_T = _make_transform_matrix([10, -5, 2], [0, 0.2588190, 0, 0.9659258], [2.0, 0.5, 3.0]) + child_world_T = _make_transform_matrix([1, 2, 3], [0.5, 0, 0, 0.8660254], [1.0, 1.5, 0.8]) + + cw = wp.array(child_world_T.reshape(1, 4, 4), dtype=wp.mat44d, device="cpu") + pw = wp.array(parent_world_T.reshape(1, 4, 4), dtype=wp.mat44d, device="cpu") + out = wp.zeros(1, dtype=wp.mat44d, device="cpu") + + wp.launch(_test_local_from_world_kernel, dim=1, inputs=[cw, pw, out], device="cpu") + wp.synchronize() + + # Verify reconstruction: local^T @ parent^T should equal child_world^T + local_T = out.numpy()[0] + reconstructed = local_T @ parent_world_T + np.testing.assert_allclose(reconstructed, child_world_T, atol=1e-5) + + +def test_world_from_local_non_orthogonal(): + """Recompose world from local with non-orthogonal parent.""" + parent_world_T = _make_transform_matrix([10, -5, 2], [0, 0.2588190, 0, 0.9659258], [2.0, 0.5, 3.0]) + child_world_T = _make_transform_matrix([1, 2, 3], [0.5, 0, 0, 0.8660254], [1.0, 1.5, 0.8]) + + # Ground-truth local + child_local_T = child_world_T @ np.linalg.inv(parent_world_T) + + cl = wp.array(child_local_T.reshape(1, 4, 4), dtype=wp.mat44d, device="cpu") + pw = wp.array(parent_world_T.reshape(1, 4, 4), dtype=wp.mat44d, device="cpu") + out = wp.zeros(1, dtype=wp.mat44d, device="cpu") + + wp.launch(_test_world_from_local_kernel, dim=1, inputs=[cl, pw, out], device="cpu") + wp.synchronize() + + np.testing.assert_allclose(out.numpy()[0], child_world_T, atol=1e-5) + + +def test_world_local_round_trip_non_orthogonal_batch(): + """Batch of 4 prims with non-orthogonal transforms: world->local->world round-trip.""" + from scipy.spatial.transform import Rotation + + rng = np.random.default_rng(42) + n = 4 + + parent_scales = rng.uniform(0.3, 3.0, size=(n, 3)).astype(np.float64) + child_scales = rng.uniform(0.3, 3.0, size=(n, 3)).astype(np.float64) + parent_rots = Rotation.random(n, random_state=42).as_quat().astype(np.float64) + child_rots = Rotation.random(n, random_state=99).as_quat().astype(np.float64) + parent_positions = rng.uniform(-10, 10, size=(n, 3)).astype(np.float64) + child_positions = rng.uniform(-10, 10, size=(n, 3)).astype(np.float64) + + parent_world_Ts = np.stack( + [_make_transform_matrix(parent_positions[i], parent_rots[i], parent_scales[i]) for i in range(n)] + ) + child_world_Ts = np.stack( + [_make_transform_matrix(child_positions[i], child_rots[i], child_scales[i]) for i in range(n)] + ) + + # world -> local + cw = wp.array(child_world_Ts, dtype=wp.mat44d, device="cpu") + pw = wp.array(parent_world_Ts, dtype=wp.mat44d, device="cpu") + local_out = wp.zeros(n, dtype=wp.mat44d, device="cpu") + + wp.launch(_test_local_from_world_kernel, dim=n, inputs=[cw, pw, local_out], device="cpu") + wp.synchronize() + + # local -> world (round-trip) + cl = wp.array(local_out.numpy(), dtype=wp.mat44d, device="cpu") + pw2 = wp.array(parent_world_Ts, dtype=wp.mat44d, device="cpu") + world_out = wp.zeros(n, dtype=wp.mat44d, device="cpu") + + wp.launch(_test_world_from_local_kernel, dim=n, inputs=[cl, pw2, world_out], device="cpu") + wp.synchronize() + + np.testing.assert_allclose(world_out.numpy(), child_world_Ts, atol=1e-4) + + +def test_local_from_world_sheared_parent(): + """Parent with extreme non-uniform scale (10:1 ratio) creating significant shear. + + Verifies both correctness and that the resulting local matrix is genuinely + non-orthogonal (the whole point of testing with sheared transforms). + """ + parent_world_T = _make_transform_matrix([0, 0, 0], [0, 0, 0.3826834, 0.9238795], [10.0, 1.0, 1.0]) + child_world_T = _make_transform_matrix([5, 5, 0], [0, 0, 0, 1], [1.0, 1.0, 1.0]) + + cw = wp.array(child_world_T.reshape(1, 4, 4), dtype=wp.mat44d, device="cpu") + pw = wp.array(parent_world_T.reshape(1, 4, 4), dtype=wp.mat44d, device="cpu") + out = wp.zeros(1, dtype=wp.mat44d, device="cpu") + + wp.launch(_test_local_from_world_kernel, dim=1, inputs=[cw, pw, out], device="cpu") + wp.synchronize() + + # Verify reconstruction + local_T = out.numpy()[0] + reconstructed = local_T @ parent_world_T + np.testing.assert_allclose(reconstructed, child_world_T, atol=1e-4) + + # Verify the local matrix upper-3x3 is NOT orthogonal + upper3x3 = local_T[:3, :3] + gram = upper3x3 @ upper3x3.T + assert not np.allclose(gram, np.eye(3), atol=0.1), ( + "Local matrix upper-3x3 should NOT be orthogonal with 10:1 sheared parent" + ) + + # ------------------------------------------------------------------ # Kernel signature / importability tests # ------------------------------------------------------------------ From c3505699f1f4d3e12d053a5ed8b439cb07d86b5c Mon Sep 17 00:00:00 2001 From: Peter Verswyvelen Date: Thu, 21 May 2026 16:12:43 +0000 Subject: [PATCH 04/12] =?UTF-8?q?refactor:=20extract=20world=E2=86=94local?= =?UTF-8?q?=20math=20into=20@wp.func=20for=20testability?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - Add _local_from_world() and _world_from_local() as @wp.func - Production fabric kernels delegate to these shared funcs - Test kernels import and call the same funcs via wp.array adapters - Zero copy-pasted math: tests exercise identical code paths as production --- source/isaaclab/isaaclab/utils/warp/fabric.py | 20 ++++++++++++++-- .../test/utils/warp/test_fabric_kernels.py | 23 ++++++++----------- 2 files changed, 28 insertions(+), 15 deletions(-) diff --git a/source/isaaclab/isaaclab/utils/warp/fabric.py b/source/isaaclab/isaaclab/utils/warp/fabric.py index 6f9963f290a1..aad13d4daab3 100644 --- a/source/isaaclab/isaaclab/utils/warp/fabric.py +++ b/source/isaaclab/isaaclab/utils/warp/fabric.py @@ -261,6 +261,18 @@ def compose_indexed_fabric_transforms( ) +@wp.func +def _local_from_world(child_world: wp.mat44f, parent_world: wp.mat44f) -> wp.mat44f: + """Compute local^T = world^T * inv(parent^T) on transposed storage matrices.""" + return child_world * wp.inverse(parent_world) + + +@wp.func +def _world_from_local(child_local: wp.mat44f, parent_world: wp.mat44f) -> wp.mat44f: + """Compute world^T = local^T * parent^T on transposed storage matrices.""" + return child_local * parent_world + + @wp.kernel(enable_backward=False) def update_indexed_local_matrix_from_world( child_world_matrices: IndexedFabricArrayMat44d, @@ -294,7 +306,9 @@ def update_indexed_local_matrix_from_world( 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] + child_local_matrices[view_index] = wp.mat44d( # type: ignore[arg-type] + _local_from_world(child_world, parent_world) + ) @wp.kernel(enable_backward=False) @@ -325,7 +339,9 @@ def update_indexed_world_matrix_from_local( 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] + child_world_matrices[view_index] = wp.mat44d( # type: ignore[arg-type] + _world_from_local(child_local, parent_world) + ) @wp.func diff --git a/source/isaaclab/test/utils/warp/test_fabric_kernels.py b/source/isaaclab/test/utils/warp/test_fabric_kernels.py index 3c17ff2c0bb2..4d303dc36cb6 100644 --- a/source/isaaclab/test/utils/warp/test_fabric_kernels.py +++ b/source/isaaclab/test/utils/warp/test_fabric_kernels.py @@ -17,11 +17,15 @@ wp.init() -from isaaclab.utils.warp.fabric import _decompose_transformation_matrix # noqa: E402 +from isaaclab.utils.warp.fabric import ( # noqa: E402 + _decompose_transformation_matrix, + _local_from_world, + _world_from_local, +) # ------------------------------------------------------------------ -# Test kernels (wp.array wrappers around the same math as production) +# Test kernels — thin wp.array wrappers that delegate to production @wp.func # ------------------------------------------------------------------ @@ -61,14 +65,9 @@ def _test_local_from_world_kernel( parent_world: wp.array(dtype=wp.mat44d), out_local: wp.array(dtype=wp.mat44d), ): - """Same math as update_indexed_local_matrix_from_world: local^T = world^T * inv(parent^T). - - Casts to mat44f for compute (matching production precision), writes back as mat44d. - """ + """wp.array adapter for _local_from_world — same func as production fabric kernel.""" i = wp.tid() - cw = wp.mat44f(child_world[i]) - pw = wp.mat44f(parent_world[i]) - out_local[i] = wp.mat44d(cw * wp.inverse(pw)) + out_local[i] = wp.mat44d(_local_from_world(wp.mat44f(child_world[i]), wp.mat44f(parent_world[i]))) @wp.kernel(enable_backward=False) @@ -77,11 +76,9 @@ def _test_world_from_local_kernel( parent_world: wp.array(dtype=wp.mat44d), out_world: wp.array(dtype=wp.mat44d), ): - """Same math as update_indexed_world_matrix_from_local: world^T = local^T * parent^T.""" + """wp.array adapter for _world_from_local — same func as production fabric kernel.""" i = wp.tid() - cl = wp.mat44f(child_local[i]) - pw = wp.mat44f(parent_world[i]) - out_world[i] = wp.mat44d(cl * pw) + out_world[i] = wp.mat44d(_world_from_local(wp.mat44f(child_local[i]), wp.mat44f(parent_world[i]))) # ------------------------------------------------------------------ From be75741b82a24a863d59d7dcdd0b41ed7b8842ca Mon Sep 17 00:00:00 2001 From: Peter Verswyvelen Date: Thu, 21 May 2026 16:57:50 +0000 Subject: [PATCH 05/12] =?UTF-8?q?refactor:=20extract=20world=E2=86=94local?= =?UTF-8?q?=20math=20into=20@wp.func=20for=20testability?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - Add _local_from_world_transposed() and _world_from_local_transposed() as @wp.func (names make explicit they operate on transposed storage matrices) - Production fabric kernels delegate to these shared funcs - Test kernels import and call the same funcs via wp.array adapters - Remove __all__ (not an IsaacLab convention) - Remove importability/export tests (unnecessary) - Remove batch test (Warp handles batching) - Add inline det != 0 assertions to world↔local tests --- source/isaaclab/isaaclab/utils/warp/fabric.py | 23 +- .../test/utils/warp/test_fabric_kernels.py | 309 +++--------------- 2 files changed, 51 insertions(+), 281 deletions(-) diff --git a/source/isaaclab/isaaclab/utils/warp/fabric.py b/source/isaaclab/isaaclab/utils/warp/fabric.py index aad13d4daab3..c5a86a58b292 100644 --- a/source/isaaclab/isaaclab/utils/warp/fabric.py +++ b/source/isaaclab/isaaclab/utils/warp/fabric.py @@ -15,17 +15,6 @@ import warp as wp -__all__ = [ - "arange_k", - "compose_fabric_transformation_matrix_from_warp_arrays", - "compose_indexed_fabric_transforms", - "decompose_fabric_transformation_matrix_to_warp_arrays", - "decompose_indexed_fabric_transforms", - "set_view_to_fabric_array", - "update_indexed_local_matrix_from_world", - "update_indexed_world_matrix_from_local", -] - if TYPE_CHECKING: FabricArrayUInt32 = Any FabricArrayMat44d = Any @@ -262,15 +251,15 @@ def compose_indexed_fabric_transforms( @wp.func -def _local_from_world(child_world: wp.mat44f, parent_world: wp.mat44f) -> wp.mat44f: +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 * wp.inverse(parent_world) + return child_world_T * wp.inverse(parent_world_T) @wp.func -def _world_from_local(child_local: wp.mat44f, parent_world: wp.mat44f) -> wp.mat44f: +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 * parent_world + return child_local_T * parent_world_T @wp.kernel(enable_backward=False) @@ -307,7 +296,7 @@ def update_indexed_local_matrix_from_world( 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(child_world, parent_world) + _local_from_world_transposed(child_world, parent_world) ) @@ -340,7 +329,7 @@ def update_indexed_world_matrix_from_local( 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(child_local, parent_world) + _world_from_local_transposed(child_local, parent_world) ) diff --git a/source/isaaclab/test/utils/warp/test_fabric_kernels.py b/source/isaaclab/test/utils/warp/test_fabric_kernels.py index 4d303dc36cb6..9153838e4366 100644 --- a/source/isaaclab/test/utils/warp/test_fabric_kernels.py +++ b/source/isaaclab/test/utils/warp/test_fabric_kernels.py @@ -19,8 +19,8 @@ from isaaclab.utils.warp.fabric import ( # noqa: E402 _decompose_transformation_matrix, - _local_from_world, - _world_from_local, + _local_from_world_transposed, + _world_from_local_transposed, ) @@ -33,41 +33,26 @@ def _test_decompose_kernel( matrices: wp.array(dtype=wp.mat44f), out_positions: wp.array(dtype=wp.vec3f), - out_rotations: wp.array(dtype=wp.vec4f), + out_rotations: wp.array(dtype=wp.quatf), out_scales: wp.array(dtype=wp.vec3f), ): """Decompose a batch of 4x4 matrices into pos/quat/scale.""" i = wp.tid() pos, rot, scale = _decompose_transformation_matrix(matrices[i]) out_positions[i] = pos - out_rotations[i] = wp.vec4f(rot[0], rot[1], rot[2], rot[3]) + out_rotations[i] = rot out_scales[i] = scale -@wp.kernel(enable_backward=False) -def _test_compose_kernel( - positions: wp.array(dtype=wp.vec3f), - rotations: wp.array(dtype=wp.vec4f), - scales: wp.array(dtype=wp.vec3f), - out_matrices: wp.array(dtype=wp.mat44f), -): - """Compose a batch of pos/quat/scale into 4x4 matrices.""" - i = wp.tid() - pos = positions[i] - rot = wp.quatf(rotations[i][0], rotations[i][1], rotations[i][2], rotations[i][3]) - scale = scales[i] - out_matrices[i] = wp.transpose(wp.transform_compose(pos, rot, scale)) - - @wp.kernel(enable_backward=False) def _test_local_from_world_kernel( child_world: wp.array(dtype=wp.mat44d), parent_world: wp.array(dtype=wp.mat44d), out_local: wp.array(dtype=wp.mat44d), ): - """wp.array adapter for _local_from_world — same func as production fabric kernel.""" + """wp.array adapter for _local_from_world_transposed — same func as production fabric kernel.""" i = wp.tid() - out_local[i] = wp.mat44d(_local_from_world(wp.mat44f(child_world[i]), wp.mat44f(parent_world[i]))) + out_local[i] = wp.mat44d(_local_from_world_transposed(wp.mat44f(child_world[i]), wp.mat44f(parent_world[i]))) @wp.kernel(enable_backward=False) @@ -76,9 +61,9 @@ def _test_world_from_local_kernel( parent_world: wp.array(dtype=wp.mat44d), out_world: wp.array(dtype=wp.mat44d), ): - """wp.array adapter for _world_from_local — same func as production fabric kernel.""" + """wp.array adapter for _world_from_local_transposed — same func as production fabric kernel.""" i = wp.tid() - out_world[i] = wp.mat44d(_world_from_local(wp.mat44f(child_local[i]), wp.mat44f(parent_world[i]))) + out_world[i] = wp.mat44d(_world_from_local_transposed(wp.mat44f(child_local[i]), wp.mat44f(parent_world[i]))) # ------------------------------------------------------------------ @@ -91,6 +76,9 @@ def _make_transform_matrix(pos, rot_quat_xyzw, scale): Returns numpy (4,4) float64 in the transposed storage convention (row-major with translation in the last row) that Fabric uses. + + Raises: + AssertionError: If the resulting matrix is singular (e.g. zero scale component). """ from scipy.spatial.transform import Rotation @@ -100,7 +88,10 @@ def _make_transform_matrix(pos, rot_quat_xyzw, scale): m[:3, :3] = rs m[:3, 3] = pos # Transpose for Fabric storage convention - return m.T + result = m.T + det = np.linalg.det(result) + assert abs(det) > 1e-6, f"Singular matrix: det={det:.2e}, scale={scale}" + return result # ------------------------------------------------------------------ @@ -108,117 +99,30 @@ def _make_transform_matrix(pos, rot_quat_xyzw, scale): # ------------------------------------------------------------------ -def test_identity_matrix(): - """Identity matrix decomposes to pos=0, quat=identity, scale=1.""" - mat = np.eye(4, dtype=np.float32).reshape(1, 4, 4) - matrices = wp.array(mat, dtype=wp.mat44f, device="cpu") - out_pos = wp.zeros(1, dtype=wp.vec3f, device="cpu") - out_rot = wp.zeros(1, dtype=wp.vec4f, device="cpu") - out_scale = wp.zeros(1, dtype=wp.vec3f, device="cpu") - - wp.launch(_test_decompose_kernel, dim=1, inputs=[matrices, out_pos, out_rot, out_scale], device="cpu") - wp.synchronize() - - pos = out_pos.numpy() - rot = out_rot.numpy() - scale = out_scale.numpy() - - np.testing.assert_allclose(pos[0], [0, 0, 0], atol=1e-6) - np.testing.assert_allclose(scale[0], [1, 1, 1], atol=1e-6) - # Identity quaternion: either (0,0,0,1) or (0,0,0,-1) - assert abs(abs(rot[0, 3]) - 1.0) < 1e-5 - - -def test_translation_only(): - """Matrix with only translation decomposes correctly.""" - mat = np.eye(4, dtype=np.float32) - mat[3, 0] = 1.0 # row-major transposed: translation in last row - mat[3, 1] = 2.0 - mat[3, 2] = 3.0 - matrices = wp.array(mat.reshape(1, 4, 4), dtype=wp.mat44f, device="cpu") - out_pos = wp.zeros(1, dtype=wp.vec3f, device="cpu") - out_rot = wp.zeros(1, dtype=wp.vec4f, device="cpu") - out_scale = wp.zeros(1, dtype=wp.vec3f, device="cpu") - - wp.launch(_test_decompose_kernel, dim=1, inputs=[matrices, out_pos, out_rot, out_scale], device="cpu") - wp.synchronize() - - np.testing.assert_allclose(out_pos.numpy()[0], [1, 2, 3], atol=1e-6) - np.testing.assert_allclose(out_scale.numpy()[0], [1, 1, 1], atol=1e-6) - - -def test_uniform_scale(): - """Matrix with uniform scale decomposes correctly.""" - mat = np.eye(4, dtype=np.float32) - mat[0, 0] = 2.0 - mat[1, 1] = 2.0 - mat[2, 2] = 2.0 - matrices = wp.array(mat.reshape(1, 4, 4), dtype=wp.mat44f, device="cpu") - out_pos = wp.zeros(1, dtype=wp.vec3f, device="cpu") - out_rot = wp.zeros(1, dtype=wp.vec4f, device="cpu") - out_scale = wp.zeros(1, dtype=wp.vec3f, device="cpu") - - wp.launch(_test_decompose_kernel, dim=1, inputs=[matrices, out_pos, out_rot, out_scale], device="cpu") - wp.synchronize() - - np.testing.assert_allclose(out_scale.numpy()[0], [2, 2, 2], atol=1e-6) - - -def test_round_trip(): - """Compose then decompose recovers original pos/quat/scale.""" - pos = np.array([[5.0, 6.0, 7.0]], dtype=np.float32) +def test_decompose_round_trip(): + """Decompose a matrix with translation, rotation, and non-uniform scale; verify round-trip.""" + pos = np.array([5.0, -3.0, 7.0]) s45 = np.sin(np.pi / 4) c45 = np.cos(np.pi / 4) - rot = np.array([[0.0, 0.0, s45, c45]], dtype=np.float32) - scale = np.array([[1.0, 2.0, 3.0]], dtype=np.float32) - - wp_pos = wp.array(pos, dtype=wp.vec3f, device="cpu") - wp_rot = wp.array(rot, dtype=wp.vec4f, device="cpu") - wp_scale = wp.array(scale, dtype=wp.vec3f, device="cpu") - out_mat = wp.zeros(1, dtype=wp.mat44f, device="cpu") + quat_xyzw = np.array([0.0, 0.0, s45, c45]) # 45° Z rotation + scale = np.array([1.5, 0.8, 3.0]) - wp.launch(_test_compose_kernel, dim=1, inputs=[wp_pos, wp_rot, wp_scale, out_mat], device="cpu") - wp.synchronize() + mat = _make_transform_matrix(pos, quat_xyzw, scale).astype(np.float32) + matrices = wp.array(mat.reshape(1, 4, 4), dtype=wp.mat44f, device="cpu") out_pos = wp.zeros(1, dtype=wp.vec3f, device="cpu") - out_rot = wp.zeros(1, dtype=wp.vec4f, device="cpu") + out_rot = wp.zeros(1, dtype=wp.quatf, device="cpu") out_scale = wp.zeros(1, dtype=wp.vec3f, device="cpu") - wp.launch(_test_decompose_kernel, dim=1, inputs=[out_mat, out_pos, out_rot, out_scale], device="cpu") + wp.launch(_test_decompose_kernel, dim=1, inputs=[matrices, out_pos, out_rot, out_scale], device="cpu") wp.synchronize() - np.testing.assert_allclose(out_pos.numpy()[0], pos[0], atol=1e-5) - np.testing.assert_allclose(out_scale.numpy()[0], scale[0], atol=1e-5) - r_out = out_rot.numpy()[0] - r_exp = rot[0] - dot = np.dot(r_out, r_exp) + np.testing.assert_allclose(out_pos.numpy()[0], pos, atol=1e-5) + np.testing.assert_allclose(out_scale.numpy()[0], scale, atol=1e-5) + dot = np.dot(out_rot.numpy()[0], quat_xyzw) np.testing.assert_allclose(abs(dot), 1.0, atol=1e-5) -def test_non_uniform_scale_round_trip(): - """Non-uniform scale round-trips correctly.""" - pos = np.array([[0.0, 0.0, 0.0]], dtype=np.float32) - rot = np.array([[0.0, 0.0, 0.0, 1.0]], dtype=np.float32) - scale = np.array([[0.5, 2.0, 3.0]], dtype=np.float32) - - wp_pos = wp.array(pos, dtype=wp.vec3f, device="cpu") - wp_rot = wp.array(rot, dtype=wp.vec4f, device="cpu") - wp_scale = wp.array(scale, dtype=wp.vec3f, device="cpu") - out_mat = wp.zeros(1, dtype=wp.mat44f, device="cpu") - - wp.launch(_test_compose_kernel, dim=1, inputs=[wp_pos, wp_rot, wp_scale, out_mat], device="cpu") - wp.synchronize() - - out_pos = wp.zeros(1, dtype=wp.vec3f, device="cpu") - out_rot = wp.zeros(1, dtype=wp.vec4f, device="cpu") - out_scale = wp.zeros(1, dtype=wp.vec3f, device="cpu") - - wp.launch(_test_decompose_kernel, dim=1, inputs=[out_mat, out_pos, out_rot, out_scale], device="cpu") - wp.synchronize() - - np.testing.assert_allclose(out_scale.numpy()[0], scale[0], atol=1e-5) - - # ------------------------------------------------------------------ # World ↔ Local matrix tests # @@ -226,164 +130,41 @@ def test_non_uniform_scale_round_trip(): # local^T = world^T * inv(parent^T) # world^T = local^T * parent^T # -# Under the transposed storage convention, this is equivalent to: -# local = inv(parent) * world -# world = parent * local +# Both parent and child have rotation, translation, and non-uniform scale +# (producing sheared/non-orthogonal upper-3x3 blocks). # ------------------------------------------------------------------ +# Shared test data: parent with 10:1 non-uniform scale + 45° Z rotation + translation +_PARENT_WORLD_T = _make_transform_matrix([10, -5, 2], [0, 0, 0.3826834, 0.9238795], [4.0, 0.5, 2.0]) +_CHILD_WORLD_T = _make_transform_matrix([1, 2, 3], [0.2588190, 0, 0, 0.9659258], [1.5, 0.8, 3.0]) -def test_local_from_world_identity_parent(): - """With identity parent, local should equal world.""" - child_world_T = _make_transform_matrix([3, -1, 7], [0, 0, 0.3826834, 0.9238795], [1.5, 2.0, 0.5]) - parent_world_T = _make_transform_matrix([0, 0, 0], [0, 0, 0, 1], [1, 1, 1]) - cw = wp.array(child_world_T.reshape(1, 4, 4), dtype=wp.mat44d, device="cpu") - pw = wp.array(parent_world_T.reshape(1, 4, 4), dtype=wp.mat44d, device="cpu") +def test_local_from_world_transposed(): + """local^T = world^T * inv(parent^T) — verified by reconstruction.""" + cw = wp.array(_CHILD_WORLD_T.reshape(1, 4, 4), dtype=wp.mat44d, device="cpu") + pw = wp.array(_PARENT_WORLD_T.reshape(1, 4, 4), dtype=wp.mat44d, device="cpu") out = wp.zeros(1, dtype=wp.mat44d, device="cpu") wp.launch(_test_local_from_world_kernel, dim=1, inputs=[cw, pw, out], device="cpu") wp.synchronize() - np.testing.assert_allclose(out.numpy()[0], child_world_T, atol=1e-5) - - -def test_local_from_world_non_orthogonal(): - """Non-uniform scale + rotation in parent produces non-orthogonal local matrix. - - Verifies: local^T @ parent^T == child_world^T (reconstruction check). - """ - parent_world_T = _make_transform_matrix([10, -5, 2], [0, 0.2588190, 0, 0.9659258], [2.0, 0.5, 3.0]) - child_world_T = _make_transform_matrix([1, 2, 3], [0.5, 0, 0, 0.8660254], [1.0, 1.5, 0.8]) - - cw = wp.array(child_world_T.reshape(1, 4, 4), dtype=wp.mat44d, device="cpu") - pw = wp.array(parent_world_T.reshape(1, 4, 4), dtype=wp.mat44d, device="cpu") - out = wp.zeros(1, dtype=wp.mat44d, device="cpu") - - wp.launch(_test_local_from_world_kernel, dim=1, inputs=[cw, pw, out], device="cpu") - wp.synchronize() - - # Verify reconstruction: local^T @ parent^T should equal child_world^T + # Reconstruction: local^T @ parent^T must equal child_world^T local_T = out.numpy()[0] - reconstructed = local_T @ parent_world_T - np.testing.assert_allclose(reconstructed, child_world_T, atol=1e-5) - + reconstructed = local_T @ _PARENT_WORLD_T + np.testing.assert_allclose(reconstructed, _CHILD_WORLD_T, atol=1e-5) -def test_world_from_local_non_orthogonal(): - """Recompose world from local with non-orthogonal parent.""" - parent_world_T = _make_transform_matrix([10, -5, 2], [0, 0.2588190, 0, 0.9659258], [2.0, 0.5, 3.0]) - child_world_T = _make_transform_matrix([1, 2, 3], [0.5, 0, 0, 0.8660254], [1.0, 1.5, 0.8]) - # Ground-truth local - child_local_T = child_world_T @ np.linalg.inv(parent_world_T) +def test_world_from_local_transposed(): + """world^T = local^T * parent^T — verified against known child world.""" + # Ground-truth local computed via numpy + child_local_T = _CHILD_WORLD_T @ np.linalg.inv(_PARENT_WORLD_T) cl = wp.array(child_local_T.reshape(1, 4, 4), dtype=wp.mat44d, device="cpu") - pw = wp.array(parent_world_T.reshape(1, 4, 4), dtype=wp.mat44d, device="cpu") + pw = wp.array(_PARENT_WORLD_T.reshape(1, 4, 4), dtype=wp.mat44d, device="cpu") out = wp.zeros(1, dtype=wp.mat44d, device="cpu") wp.launch(_test_world_from_local_kernel, dim=1, inputs=[cl, pw, out], device="cpu") wp.synchronize() - np.testing.assert_allclose(out.numpy()[0], child_world_T, atol=1e-5) - - -def test_world_local_round_trip_non_orthogonal_batch(): - """Batch of 4 prims with non-orthogonal transforms: world->local->world round-trip.""" - from scipy.spatial.transform import Rotation - - rng = np.random.default_rng(42) - n = 4 - - parent_scales = rng.uniform(0.3, 3.0, size=(n, 3)).astype(np.float64) - child_scales = rng.uniform(0.3, 3.0, size=(n, 3)).astype(np.float64) - parent_rots = Rotation.random(n, random_state=42).as_quat().astype(np.float64) - child_rots = Rotation.random(n, random_state=99).as_quat().astype(np.float64) - parent_positions = rng.uniform(-10, 10, size=(n, 3)).astype(np.float64) - child_positions = rng.uniform(-10, 10, size=(n, 3)).astype(np.float64) - - parent_world_Ts = np.stack( - [_make_transform_matrix(parent_positions[i], parent_rots[i], parent_scales[i]) for i in range(n)] - ) - child_world_Ts = np.stack( - [_make_transform_matrix(child_positions[i], child_rots[i], child_scales[i]) for i in range(n)] - ) - - # world -> local - cw = wp.array(child_world_Ts, dtype=wp.mat44d, device="cpu") - pw = wp.array(parent_world_Ts, dtype=wp.mat44d, device="cpu") - local_out = wp.zeros(n, dtype=wp.mat44d, device="cpu") - - wp.launch(_test_local_from_world_kernel, dim=n, inputs=[cw, pw, local_out], device="cpu") - wp.synchronize() - - # local -> world (round-trip) - cl = wp.array(local_out.numpy(), dtype=wp.mat44d, device="cpu") - pw2 = wp.array(parent_world_Ts, dtype=wp.mat44d, device="cpu") - world_out = wp.zeros(n, dtype=wp.mat44d, device="cpu") - - wp.launch(_test_world_from_local_kernel, dim=n, inputs=[cl, pw2, world_out], device="cpu") - wp.synchronize() - - np.testing.assert_allclose(world_out.numpy(), child_world_Ts, atol=1e-4) - - -def test_local_from_world_sheared_parent(): - """Parent with extreme non-uniform scale (10:1 ratio) creating significant shear. - - Verifies both correctness and that the resulting local matrix is genuinely - non-orthogonal (the whole point of testing with sheared transforms). - """ - parent_world_T = _make_transform_matrix([0, 0, 0], [0, 0, 0.3826834, 0.9238795], [10.0, 1.0, 1.0]) - child_world_T = _make_transform_matrix([5, 5, 0], [0, 0, 0, 1], [1.0, 1.0, 1.0]) - - cw = wp.array(child_world_T.reshape(1, 4, 4), dtype=wp.mat44d, device="cpu") - pw = wp.array(parent_world_T.reshape(1, 4, 4), dtype=wp.mat44d, device="cpu") - out = wp.zeros(1, dtype=wp.mat44d, device="cpu") - - wp.launch(_test_local_from_world_kernel, dim=1, inputs=[cw, pw, out], device="cpu") - wp.synchronize() - - # Verify reconstruction - local_T = out.numpy()[0] - reconstructed = local_T @ parent_world_T - np.testing.assert_allclose(reconstructed, child_world_T, atol=1e-4) - - # Verify the local matrix upper-3x3 is NOT orthogonal - upper3x3 = local_T[:3, :3] - gram = upper3x3 @ upper3x3.T - assert not np.allclose(gram, np.eye(3), atol=0.1), ( - "Local matrix upper-3x3 should NOT be orthogonal with 10:1 sheared parent" - ) - - -# ------------------------------------------------------------------ -# Kernel signature / importability tests -# ------------------------------------------------------------------ - - -def test_all_kernels_importable(): - """All public kernels listed in __all__ should be importable and be Warp Kernels.""" - from isaaclab.utils.warp import fabric as fabric_utils - - expected_kernels = [ - "arange_k", - "compose_fabric_transformation_matrix_from_warp_arrays", - "compose_indexed_fabric_transforms", - "decompose_fabric_transformation_matrix_to_warp_arrays", - "decompose_indexed_fabric_transforms", - "set_view_to_fabric_array", - "update_indexed_local_matrix_from_world", - "update_indexed_world_matrix_from_local", - ] - - for name in expected_kernels: - obj = getattr(fabric_utils, name, None) - assert obj is not None, f"{name} not found in fabric_utils" - assert isinstance(obj, wp.Kernel), f"{name} should be a wp.Kernel, got {type(obj)}" - - -def test_module_exports_match_all(): - """__all__ should list every public kernel.""" - from isaaclab.utils.warp import fabric as fabric_utils + np.testing.assert_allclose(out.numpy()[0], _CHILD_WORLD_T, atol=1e-5) - for name in fabric_utils.__all__: - assert hasattr(fabric_utils, name), f"__all__ lists '{name}' but it's not defined" From 45a87d55b769faa6a8ecc6beffc2c800be096367 Mon Sep 17 00:00:00 2001 From: Peter Verswyvelen Date: Fri, 22 May 2026 14:10:38 +0000 Subject: [PATCH 06/12] refactor: inline @wp.func helpers, add __all__, sync with full-stack MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - Remove _local_from_world_transposed / _world_from_local_transposed @wp.func helpers (one-liner math, abstraction added no value) - Add __all__ export list for public API surface - Sync test file with full-stack state - Changelog: 'Will be used by' → 'Used by' --- .../changelog.d/indexed-fabric-kernels.rst | 2 +- source/isaaclab/isaaclab/utils/warp/fabric.py | 31 +- .../test/utils/warp/test_fabric_kernels.py | 299 ++++++++++-------- 3 files changed, 175 insertions(+), 157 deletions(-) diff --git a/source/isaaclab/changelog.d/indexed-fabric-kernels.rst b/source/isaaclab/changelog.d/indexed-fabric-kernels.rst index 0f68b4c60e9d..281881bb7972 100644 --- a/source/isaaclab/changelog.d/indexed-fabric-kernels.rst +++ b/source/isaaclab/changelog.d/indexed-fabric-kernels.rst @@ -13,7 +13,7 @@ Added 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 + 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. diff --git a/source/isaaclab/isaaclab/utils/warp/fabric.py b/source/isaaclab/isaaclab/utils/warp/fabric.py index c5a86a58b292..6f9963f290a1 100644 --- a/source/isaaclab/isaaclab/utils/warp/fabric.py +++ b/source/isaaclab/isaaclab/utils/warp/fabric.py @@ -15,6 +15,17 @@ import warp as wp +__all__ = [ + "arange_k", + "compose_fabric_transformation_matrix_from_warp_arrays", + "compose_indexed_fabric_transforms", + "decompose_fabric_transformation_matrix_to_warp_arrays", + "decompose_indexed_fabric_transforms", + "set_view_to_fabric_array", + "update_indexed_local_matrix_from_world", + "update_indexed_world_matrix_from_local", +] + if TYPE_CHECKING: FabricArrayUInt32 = Any FabricArrayMat44d = Any @@ -250,18 +261,6 @@ def compose_indexed_fabric_transforms( ) -@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, @@ -295,9 +294,7 @@ def update_indexed_local_matrix_from_world( 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) - ) + child_local_matrices[view_index] = wp.mat44d(child_world * wp.inverse(parent_world)) # type: ignore[arg-type] @wp.kernel(enable_backward=False) @@ -328,9 +325,7 @@ def update_indexed_world_matrix_from_local( 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) - ) + child_world_matrices[view_index] = wp.mat44d(child_local * parent_world) # type: ignore[arg-type] @wp.func diff --git a/source/isaaclab/test/utils/warp/test_fabric_kernels.py b/source/isaaclab/test/utils/warp/test_fabric_kernels.py index 9153838e4366..13bad50da500 100644 --- a/source/isaaclab/test/utils/warp/test_fabric_kernels.py +++ b/source/isaaclab/test/utils/warp/test_fabric_kernels.py @@ -5,11 +5,8 @@ """Unit tests for the Warp fabric transform kernels. -Tests the shared @wp.func math (decompose/compose and matrix inverse/multiply) -through plain wp.array kernels — no Fabric/USDRT runtime required. - -The production fabric kernels are thin adapters over the same math; testing the -math in isolation avoids coupling tests to Fabric container internals. +Tests the decompose/compose math via helper kernels that operate on +regular wp.array (no Fabric/USDRT runtime required). """ import numpy as np @@ -17,154 +14,180 @@ wp.init() -from isaaclab.utils.warp.fabric import ( # noqa: E402 - _decompose_transformation_matrix, - _local_from_world_transposed, - _world_from_local_transposed, -) - - -# ------------------------------------------------------------------ -# Test kernels — thin wp.array wrappers that delegate to production @wp.func -# ------------------------------------------------------------------ +from isaaclab.utils.warp.fabric import _decompose_transformation_matrix # noqa: E402 @wp.kernel(enable_backward=False) def _test_decompose_kernel( matrices: wp.array(dtype=wp.mat44f), out_positions: wp.array(dtype=wp.vec3f), - out_rotations: wp.array(dtype=wp.quatf), + out_rotations: wp.array(dtype=wp.vec4f), out_scales: wp.array(dtype=wp.vec3f), ): """Decompose a batch of 4x4 matrices into pos/quat/scale.""" i = wp.tid() pos, rot, scale = _decompose_transformation_matrix(matrices[i]) out_positions[i] = pos - out_rotations[i] = rot + out_rotations[i] = wp.vec4f(rot[0], rot[1], rot[2], rot[3]) out_scales[i] = scale @wp.kernel(enable_backward=False) -def _test_local_from_world_kernel( - child_world: wp.array(dtype=wp.mat44d), - parent_world: wp.array(dtype=wp.mat44d), - out_local: wp.array(dtype=wp.mat44d), -): - """wp.array adapter for _local_from_world_transposed — same func as production fabric kernel.""" - i = wp.tid() - out_local[i] = wp.mat44d(_local_from_world_transposed(wp.mat44f(child_world[i]), wp.mat44f(parent_world[i]))) - - -@wp.kernel(enable_backward=False) -def _test_world_from_local_kernel( - child_local: wp.array(dtype=wp.mat44d), - parent_world: wp.array(dtype=wp.mat44d), - out_world: wp.array(dtype=wp.mat44d), +def _test_compose_kernel( + positions: wp.array(dtype=wp.vec3f), + rotations: wp.array(dtype=wp.vec4f), + scales: wp.array(dtype=wp.vec3f), + out_matrices: wp.array(dtype=wp.mat44f), ): - """wp.array adapter for _world_from_local_transposed — same func as production fabric kernel.""" + """Compose a batch of pos/quat/scale into 4x4 matrices.""" i = wp.tid() - out_world[i] = wp.mat44d(_world_from_local_transposed(wp.mat44f(child_local[i]), wp.mat44f(parent_world[i]))) - - -# ------------------------------------------------------------------ -# Helpers -# ------------------------------------------------------------------ - - -def _make_transform_matrix(pos, rot_quat_xyzw, scale): - """Build a 4x4 Fabric-transposed transform from pos/quat/scale. - - Returns numpy (4,4) float64 in the transposed storage convention (row-major with - translation in the last row) that Fabric uses. - - Raises: - AssertionError: If the resulting matrix is singular (e.g. zero scale component). - """ - from scipy.spatial.transform import Rotation - - r = Rotation.from_quat(rot_quat_xyzw).as_matrix().astype(np.float64) - rs = r * np.array(scale, dtype=np.float64) - m = np.eye(4, dtype=np.float64) - m[:3, :3] = rs - m[:3, 3] = pos - # Transpose for Fabric storage convention - result = m.T - det = np.linalg.det(result) - assert abs(det) > 1e-6, f"Singular matrix: det={det:.2e}, scale={scale}" - return result - - -# ------------------------------------------------------------------ -# Decompose / Compose round-trip tests -# ------------------------------------------------------------------ - - -def test_decompose_round_trip(): - """Decompose a matrix with translation, rotation, and non-uniform scale; verify round-trip.""" - pos = np.array([5.0, -3.0, 7.0]) - s45 = np.sin(np.pi / 4) - c45 = np.cos(np.pi / 4) - quat_xyzw = np.array([0.0, 0.0, s45, c45]) # 45° Z rotation - scale = np.array([1.5, 0.8, 3.0]) - - mat = _make_transform_matrix(pos, quat_xyzw, scale).astype(np.float32) - matrices = wp.array(mat.reshape(1, 4, 4), dtype=wp.mat44f, device="cpu") - - out_pos = wp.zeros(1, dtype=wp.vec3f, device="cpu") - out_rot = wp.zeros(1, dtype=wp.quatf, device="cpu") - out_scale = wp.zeros(1, dtype=wp.vec3f, device="cpu") - - wp.launch(_test_decompose_kernel, dim=1, inputs=[matrices, out_pos, out_rot, out_scale], device="cpu") - wp.synchronize() - - np.testing.assert_allclose(out_pos.numpy()[0], pos, atol=1e-5) - np.testing.assert_allclose(out_scale.numpy()[0], scale, atol=1e-5) - dot = np.dot(out_rot.numpy()[0], quat_xyzw) - np.testing.assert_allclose(abs(dot), 1.0, atol=1e-5) - - -# ------------------------------------------------------------------ -# World ↔ Local matrix tests -# -# These test the same math the production fabric kernels use: -# local^T = world^T * inv(parent^T) -# world^T = local^T * parent^T -# -# Both parent and child have rotation, translation, and non-uniform scale -# (producing sheared/non-orthogonal upper-3x3 blocks). -# ------------------------------------------------------------------ - -# Shared test data: parent with 10:1 non-uniform scale + 45° Z rotation + translation -_PARENT_WORLD_T = _make_transform_matrix([10, -5, 2], [0, 0, 0.3826834, 0.9238795], [4.0, 0.5, 2.0]) -_CHILD_WORLD_T = _make_transform_matrix([1, 2, 3], [0.2588190, 0, 0, 0.9659258], [1.5, 0.8, 3.0]) - - -def test_local_from_world_transposed(): - """local^T = world^T * inv(parent^T) — verified by reconstruction.""" - cw = wp.array(_CHILD_WORLD_T.reshape(1, 4, 4), dtype=wp.mat44d, device="cpu") - pw = wp.array(_PARENT_WORLD_T.reshape(1, 4, 4), dtype=wp.mat44d, device="cpu") - out = wp.zeros(1, dtype=wp.mat44d, device="cpu") - - wp.launch(_test_local_from_world_kernel, dim=1, inputs=[cw, pw, out], device="cpu") - wp.synchronize() - - # Reconstruction: local^T @ parent^T must equal child_world^T - local_T = out.numpy()[0] - reconstructed = local_T @ _PARENT_WORLD_T - np.testing.assert_allclose(reconstructed, _CHILD_WORLD_T, atol=1e-5) - - -def test_world_from_local_transposed(): - """world^T = local^T * parent^T — verified against known child world.""" - # Ground-truth local computed via numpy - child_local_T = _CHILD_WORLD_T @ np.linalg.inv(_PARENT_WORLD_T) - - cl = wp.array(child_local_T.reshape(1, 4, 4), dtype=wp.mat44d, device="cpu") - pw = wp.array(_PARENT_WORLD_T.reshape(1, 4, 4), dtype=wp.mat44d, device="cpu") - out = wp.zeros(1, dtype=wp.mat44d, device="cpu") - - wp.launch(_test_world_from_local_kernel, dim=1, inputs=[cl, pw, out], device="cpu") - wp.synchronize() - - np.testing.assert_allclose(out.numpy()[0], _CHILD_WORLD_T, atol=1e-5) - + pos = positions[i] + rot = wp.quatf(rotations[i][0], rotations[i][1], rotations[i][2], rotations[i][3]) + scale = scales[i] + out_matrices[i] = wp.transpose(wp.transform_compose(pos, rot, scale)) + + +class TestDecomposeCompose: + """Round-trip tests for decompose ↔ compose transform math.""" + + def test_identity_matrix(self): + """Identity matrix decomposes to pos=0, quat=identity, scale=1.""" + mat = np.eye(4, dtype=np.float32).reshape(1, 4, 4) + matrices = wp.array(mat, dtype=wp.mat44f, device="cpu") + out_pos = wp.zeros(1, dtype=wp.vec3f, device="cpu") + out_rot = wp.zeros(1, dtype=wp.vec4f, device="cpu") + out_scale = wp.zeros(1, dtype=wp.vec3f, device="cpu") + + wp.launch(_test_decompose_kernel, dim=1, inputs=[matrices, out_pos, out_rot, out_scale], device="cpu") + wp.synchronize() + + pos = out_pos.numpy() + rot = out_rot.numpy() + scale = out_scale.numpy() + + np.testing.assert_allclose(pos[0], [0, 0, 0], atol=1e-6) + np.testing.assert_allclose(scale[0], [1, 1, 1], atol=1e-6) + # Identity quaternion: either (0,0,0,1) or (0,0,0,-1) + assert abs(abs(rot[0, 3]) - 1.0) < 1e-5 + + def test_translation_only(self): + """Matrix with only translation decomposes correctly.""" + mat = np.eye(4, dtype=np.float32) + mat[3, 0] = 1.0 # row-major: translation in last row + mat[3, 1] = 2.0 + mat[3, 2] = 3.0 + matrices = wp.array(mat.reshape(1, 4, 4), dtype=wp.mat44f, device="cpu") + out_pos = wp.zeros(1, dtype=wp.vec3f, device="cpu") + out_rot = wp.zeros(1, dtype=wp.vec4f, device="cpu") + out_scale = wp.zeros(1, dtype=wp.vec3f, device="cpu") + + wp.launch(_test_decompose_kernel, dim=1, inputs=[matrices, out_pos, out_rot, out_scale], device="cpu") + wp.synchronize() + + np.testing.assert_allclose(out_pos.numpy()[0], [1, 2, 3], atol=1e-6) + np.testing.assert_allclose(out_scale.numpy()[0], [1, 1, 1], atol=1e-6) + + def test_uniform_scale(self): + """Matrix with uniform scale decomposes correctly.""" + mat = np.eye(4, dtype=np.float32) + mat[0, 0] = 2.0 + mat[1, 1] = 2.0 + mat[2, 2] = 2.0 + matrices = wp.array(mat.reshape(1, 4, 4), dtype=wp.mat44f, device="cpu") + out_pos = wp.zeros(1, dtype=wp.vec3f, device="cpu") + out_rot = wp.zeros(1, dtype=wp.vec4f, device="cpu") + out_scale = wp.zeros(1, dtype=wp.vec3f, device="cpu") + + wp.launch(_test_decompose_kernel, dim=1, inputs=[matrices, out_pos, out_rot, out_scale], device="cpu") + wp.synchronize() + + np.testing.assert_allclose(out_scale.numpy()[0], [2, 2, 2], atol=1e-6) + + def test_round_trip(self): + """Compose then decompose recovers original pos/quat/scale.""" + # Known transform: translate (5,6,7), rotate 90° about Z, scale (1,2,3) + pos = np.array([[5.0, 6.0, 7.0]], dtype=np.float32) + # 90° about Z in xyzw: (0, 0, sin(45°), cos(45°)) + s45 = np.sin(np.pi / 4) + c45 = np.cos(np.pi / 4) + rot = np.array([[0.0, 0.0, s45, c45]], dtype=np.float32) + scale = np.array([[1.0, 2.0, 3.0]], dtype=np.float32) + + wp_pos = wp.array(pos, dtype=wp.vec3f, device="cpu") + wp_rot = wp.array(rot, dtype=wp.vec4f, device="cpu") + wp_scale = wp.array(scale, dtype=wp.vec3f, device="cpu") + out_mat = wp.zeros(1, dtype=wp.mat44f, device="cpu") + + # Compose + wp.launch(_test_compose_kernel, dim=1, inputs=[wp_pos, wp_rot, wp_scale, out_mat], device="cpu") + wp.synchronize() + + # Decompose + out_pos = wp.zeros(1, dtype=wp.vec3f, device="cpu") + out_rot = wp.zeros(1, dtype=wp.vec4f, device="cpu") + out_scale = wp.zeros(1, dtype=wp.vec3f, device="cpu") + + wp.launch(_test_decompose_kernel, dim=1, inputs=[out_mat, out_pos, out_rot, out_scale], device="cpu") + wp.synchronize() + + np.testing.assert_allclose(out_pos.numpy()[0], pos[0], atol=1e-5) + np.testing.assert_allclose(out_scale.numpy()[0], scale[0], atol=1e-5) + # Quaternion sign ambiguity + r_out = out_rot.numpy()[0] + r_exp = rot[0] + dot = np.dot(r_out, r_exp) + np.testing.assert_allclose(abs(dot), 1.0, atol=1e-5) + + def test_non_uniform_scale_round_trip(self): + """Non-uniform scale round-trips correctly.""" + pos = np.array([[0.0, 0.0, 0.0]], dtype=np.float32) + rot = np.array([[0.0, 0.0, 0.0, 1.0]], dtype=np.float32) # identity + scale = np.array([[0.5, 2.0, 3.0]], dtype=np.float32) + + wp_pos = wp.array(pos, dtype=wp.vec3f, device="cpu") + wp_rot = wp.array(rot, dtype=wp.vec4f, device="cpu") + wp_scale = wp.array(scale, dtype=wp.vec3f, device="cpu") + out_mat = wp.zeros(1, dtype=wp.mat44f, device="cpu") + + wp.launch(_test_compose_kernel, dim=1, inputs=[wp_pos, wp_rot, wp_scale, out_mat], device="cpu") + wp.synchronize() + + out_pos = wp.zeros(1, dtype=wp.vec3f, device="cpu") + out_rot = wp.zeros(1, dtype=wp.vec4f, device="cpu") + out_scale = wp.zeros(1, dtype=wp.vec3f, device="cpu") + + wp.launch(_test_decompose_kernel, dim=1, inputs=[out_mat, out_pos, out_rot, out_scale], device="cpu") + wp.synchronize() + + np.testing.assert_allclose(out_scale.numpy()[0], scale[0], atol=1e-5) + + +class TestKernelSignatures: + """Verify all exported kernels are importable and are Warp Kernels.""" + + def test_all_kernels_importable(self): + """All public kernels listed in __all__ should be importable and be Warp Kernels.""" + from isaaclab.utils.warp import fabric as fabric_utils + + expected_kernels = [ + "arange_k", + "compose_fabric_transformation_matrix_from_warp_arrays", + "compose_indexed_fabric_transforms", + "decompose_fabric_transformation_matrix_to_warp_arrays", + "decompose_indexed_fabric_transforms", + "set_view_to_fabric_array", + "update_indexed_local_matrix_from_world", + "update_indexed_world_matrix_from_local", + ] + + for name in expected_kernels: + obj = getattr(fabric_utils, name, None) + assert obj is not None, f"{name} not found in fabric_utils" + assert isinstance(obj, wp.Kernel), f"{name} should be a wp.Kernel, got {type(obj)}" + + def test_module_exports_match_all(self): + """__all__ should list every public kernel.""" + from isaaclab.utils.warp import fabric as fabric_utils + + for name in fabric_utils.__all__: + assert hasattr(fabric_utils, name), f"__all__ lists '{name}' but it's not defined" From 1e16f461c96d922f54409eac706202927f25037d Mon Sep 17 00:00:00 2001 From: Peter Verswyvelen Date: Fri, 22 May 2026 14:13:57 +0000 Subject: [PATCH 07/12] Revert "refactor: inline @wp.func helpers, add __all__, sync with full-stack" This reverts commit 735132e0d896e3c8182152abeb372c4dfe113870. --- .../changelog.d/indexed-fabric-kernels.rst | 2 +- source/isaaclab/isaaclab/utils/warp/fabric.py | 31 +- .../test/utils/warp/test_fabric_kernels.py | 299 ++++++++---------- 3 files changed, 157 insertions(+), 175 deletions(-) diff --git a/source/isaaclab/changelog.d/indexed-fabric-kernels.rst b/source/isaaclab/changelog.d/indexed-fabric-kernels.rst index 281881bb7972..0f68b4c60e9d 100644 --- a/source/isaaclab/changelog.d/indexed-fabric-kernels.rst +++ b/source/isaaclab/changelog.d/indexed-fabric-kernels.rst @@ -13,7 +13,7 @@ Added 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 + 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. diff --git a/source/isaaclab/isaaclab/utils/warp/fabric.py b/source/isaaclab/isaaclab/utils/warp/fabric.py index 6f9963f290a1..c5a86a58b292 100644 --- a/source/isaaclab/isaaclab/utils/warp/fabric.py +++ b/source/isaaclab/isaaclab/utils/warp/fabric.py @@ -15,17 +15,6 @@ import warp as wp -__all__ = [ - "arange_k", - "compose_fabric_transformation_matrix_from_warp_arrays", - "compose_indexed_fabric_transforms", - "decompose_fabric_transformation_matrix_to_warp_arrays", - "decompose_indexed_fabric_transforms", - "set_view_to_fabric_array", - "update_indexed_local_matrix_from_world", - "update_indexed_world_matrix_from_local", -] - if TYPE_CHECKING: FabricArrayUInt32 = Any FabricArrayMat44d = Any @@ -261,6 +250,18 @@ def compose_indexed_fabric_transforms( ) +@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, @@ -294,7 +295,9 @@ def update_indexed_local_matrix_from_world( 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] + child_local_matrices[view_index] = wp.mat44d( # type: ignore[arg-type] + _local_from_world_transposed(child_world, parent_world) + ) @wp.kernel(enable_backward=False) @@ -325,7 +328,9 @@ def update_indexed_world_matrix_from_local( 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] + child_world_matrices[view_index] = wp.mat44d( # type: ignore[arg-type] + _world_from_local_transposed(child_local, parent_world) + ) @wp.func diff --git a/source/isaaclab/test/utils/warp/test_fabric_kernels.py b/source/isaaclab/test/utils/warp/test_fabric_kernels.py index 13bad50da500..9153838e4366 100644 --- a/source/isaaclab/test/utils/warp/test_fabric_kernels.py +++ b/source/isaaclab/test/utils/warp/test_fabric_kernels.py @@ -5,8 +5,11 @@ """Unit tests for the Warp fabric transform kernels. -Tests the decompose/compose math via helper kernels that operate on -regular wp.array (no Fabric/USDRT runtime required). +Tests the shared @wp.func math (decompose/compose and matrix inverse/multiply) +through plain wp.array kernels — no Fabric/USDRT runtime required. + +The production fabric kernels are thin adapters over the same math; testing the +math in isolation avoids coupling tests to Fabric container internals. """ import numpy as np @@ -14,180 +17,154 @@ wp.init() -from isaaclab.utils.warp.fabric import _decompose_transformation_matrix # noqa: E402 +from isaaclab.utils.warp.fabric import ( # noqa: E402 + _decompose_transformation_matrix, + _local_from_world_transposed, + _world_from_local_transposed, +) + + +# ------------------------------------------------------------------ +# Test kernels — thin wp.array wrappers that delegate to production @wp.func +# ------------------------------------------------------------------ @wp.kernel(enable_backward=False) def _test_decompose_kernel( matrices: wp.array(dtype=wp.mat44f), out_positions: wp.array(dtype=wp.vec3f), - out_rotations: wp.array(dtype=wp.vec4f), + out_rotations: wp.array(dtype=wp.quatf), out_scales: wp.array(dtype=wp.vec3f), ): """Decompose a batch of 4x4 matrices into pos/quat/scale.""" i = wp.tid() pos, rot, scale = _decompose_transformation_matrix(matrices[i]) out_positions[i] = pos - out_rotations[i] = wp.vec4f(rot[0], rot[1], rot[2], rot[3]) + out_rotations[i] = rot out_scales[i] = scale @wp.kernel(enable_backward=False) -def _test_compose_kernel( - positions: wp.array(dtype=wp.vec3f), - rotations: wp.array(dtype=wp.vec4f), - scales: wp.array(dtype=wp.vec3f), - out_matrices: wp.array(dtype=wp.mat44f), +def _test_local_from_world_kernel( + child_world: wp.array(dtype=wp.mat44d), + parent_world: wp.array(dtype=wp.mat44d), + out_local: wp.array(dtype=wp.mat44d), +): + """wp.array adapter for _local_from_world_transposed — same func as production fabric kernel.""" + i = wp.tid() + out_local[i] = wp.mat44d(_local_from_world_transposed(wp.mat44f(child_world[i]), wp.mat44f(parent_world[i]))) + + +@wp.kernel(enable_backward=False) +def _test_world_from_local_kernel( + child_local: wp.array(dtype=wp.mat44d), + parent_world: wp.array(dtype=wp.mat44d), + out_world: wp.array(dtype=wp.mat44d), ): - """Compose a batch of pos/quat/scale into 4x4 matrices.""" + """wp.array adapter for _world_from_local_transposed — same func as production fabric kernel.""" i = wp.tid() - pos = positions[i] - rot = wp.quatf(rotations[i][0], rotations[i][1], rotations[i][2], rotations[i][3]) - scale = scales[i] - out_matrices[i] = wp.transpose(wp.transform_compose(pos, rot, scale)) - - -class TestDecomposeCompose: - """Round-trip tests for decompose ↔ compose transform math.""" - - def test_identity_matrix(self): - """Identity matrix decomposes to pos=0, quat=identity, scale=1.""" - mat = np.eye(4, dtype=np.float32).reshape(1, 4, 4) - matrices = wp.array(mat, dtype=wp.mat44f, device="cpu") - out_pos = wp.zeros(1, dtype=wp.vec3f, device="cpu") - out_rot = wp.zeros(1, dtype=wp.vec4f, device="cpu") - out_scale = wp.zeros(1, dtype=wp.vec3f, device="cpu") - - wp.launch(_test_decompose_kernel, dim=1, inputs=[matrices, out_pos, out_rot, out_scale], device="cpu") - wp.synchronize() - - pos = out_pos.numpy() - rot = out_rot.numpy() - scale = out_scale.numpy() - - np.testing.assert_allclose(pos[0], [0, 0, 0], atol=1e-6) - np.testing.assert_allclose(scale[0], [1, 1, 1], atol=1e-6) - # Identity quaternion: either (0,0,0,1) or (0,0,0,-1) - assert abs(abs(rot[0, 3]) - 1.0) < 1e-5 - - def test_translation_only(self): - """Matrix with only translation decomposes correctly.""" - mat = np.eye(4, dtype=np.float32) - mat[3, 0] = 1.0 # row-major: translation in last row - mat[3, 1] = 2.0 - mat[3, 2] = 3.0 - matrices = wp.array(mat.reshape(1, 4, 4), dtype=wp.mat44f, device="cpu") - out_pos = wp.zeros(1, dtype=wp.vec3f, device="cpu") - out_rot = wp.zeros(1, dtype=wp.vec4f, device="cpu") - out_scale = wp.zeros(1, dtype=wp.vec3f, device="cpu") - - wp.launch(_test_decompose_kernel, dim=1, inputs=[matrices, out_pos, out_rot, out_scale], device="cpu") - wp.synchronize() - - np.testing.assert_allclose(out_pos.numpy()[0], [1, 2, 3], atol=1e-6) - np.testing.assert_allclose(out_scale.numpy()[0], [1, 1, 1], atol=1e-6) - - def test_uniform_scale(self): - """Matrix with uniform scale decomposes correctly.""" - mat = np.eye(4, dtype=np.float32) - mat[0, 0] = 2.0 - mat[1, 1] = 2.0 - mat[2, 2] = 2.0 - matrices = wp.array(mat.reshape(1, 4, 4), dtype=wp.mat44f, device="cpu") - out_pos = wp.zeros(1, dtype=wp.vec3f, device="cpu") - out_rot = wp.zeros(1, dtype=wp.vec4f, device="cpu") - out_scale = wp.zeros(1, dtype=wp.vec3f, device="cpu") - - wp.launch(_test_decompose_kernel, dim=1, inputs=[matrices, out_pos, out_rot, out_scale], device="cpu") - wp.synchronize() - - np.testing.assert_allclose(out_scale.numpy()[0], [2, 2, 2], atol=1e-6) - - def test_round_trip(self): - """Compose then decompose recovers original pos/quat/scale.""" - # Known transform: translate (5,6,7), rotate 90° about Z, scale (1,2,3) - pos = np.array([[5.0, 6.0, 7.0]], dtype=np.float32) - # 90° about Z in xyzw: (0, 0, sin(45°), cos(45°)) - s45 = np.sin(np.pi / 4) - c45 = np.cos(np.pi / 4) - rot = np.array([[0.0, 0.0, s45, c45]], dtype=np.float32) - scale = np.array([[1.0, 2.0, 3.0]], dtype=np.float32) - - wp_pos = wp.array(pos, dtype=wp.vec3f, device="cpu") - wp_rot = wp.array(rot, dtype=wp.vec4f, device="cpu") - wp_scale = wp.array(scale, dtype=wp.vec3f, device="cpu") - out_mat = wp.zeros(1, dtype=wp.mat44f, device="cpu") - - # Compose - wp.launch(_test_compose_kernel, dim=1, inputs=[wp_pos, wp_rot, wp_scale, out_mat], device="cpu") - wp.synchronize() - - # Decompose - out_pos = wp.zeros(1, dtype=wp.vec3f, device="cpu") - out_rot = wp.zeros(1, dtype=wp.vec4f, device="cpu") - out_scale = wp.zeros(1, dtype=wp.vec3f, device="cpu") - - wp.launch(_test_decompose_kernel, dim=1, inputs=[out_mat, out_pos, out_rot, out_scale], device="cpu") - wp.synchronize() - - np.testing.assert_allclose(out_pos.numpy()[0], pos[0], atol=1e-5) - np.testing.assert_allclose(out_scale.numpy()[0], scale[0], atol=1e-5) - # Quaternion sign ambiguity - r_out = out_rot.numpy()[0] - r_exp = rot[0] - dot = np.dot(r_out, r_exp) - np.testing.assert_allclose(abs(dot), 1.0, atol=1e-5) - - def test_non_uniform_scale_round_trip(self): - """Non-uniform scale round-trips correctly.""" - pos = np.array([[0.0, 0.0, 0.0]], dtype=np.float32) - rot = np.array([[0.0, 0.0, 0.0, 1.0]], dtype=np.float32) # identity - scale = np.array([[0.5, 2.0, 3.0]], dtype=np.float32) - - wp_pos = wp.array(pos, dtype=wp.vec3f, device="cpu") - wp_rot = wp.array(rot, dtype=wp.vec4f, device="cpu") - wp_scale = wp.array(scale, dtype=wp.vec3f, device="cpu") - out_mat = wp.zeros(1, dtype=wp.mat44f, device="cpu") - - wp.launch(_test_compose_kernel, dim=1, inputs=[wp_pos, wp_rot, wp_scale, out_mat], device="cpu") - wp.synchronize() - - out_pos = wp.zeros(1, dtype=wp.vec3f, device="cpu") - out_rot = wp.zeros(1, dtype=wp.vec4f, device="cpu") - out_scale = wp.zeros(1, dtype=wp.vec3f, device="cpu") - - wp.launch(_test_decompose_kernel, dim=1, inputs=[out_mat, out_pos, out_rot, out_scale], device="cpu") - wp.synchronize() - - np.testing.assert_allclose(out_scale.numpy()[0], scale[0], atol=1e-5) - - -class TestKernelSignatures: - """Verify all exported kernels are importable and are Warp Kernels.""" - - def test_all_kernels_importable(self): - """All public kernels listed in __all__ should be importable and be Warp Kernels.""" - from isaaclab.utils.warp import fabric as fabric_utils - - expected_kernels = [ - "arange_k", - "compose_fabric_transformation_matrix_from_warp_arrays", - "compose_indexed_fabric_transforms", - "decompose_fabric_transformation_matrix_to_warp_arrays", - "decompose_indexed_fabric_transforms", - "set_view_to_fabric_array", - "update_indexed_local_matrix_from_world", - "update_indexed_world_matrix_from_local", - ] - - for name in expected_kernels: - obj = getattr(fabric_utils, name, None) - assert obj is not None, f"{name} not found in fabric_utils" - assert isinstance(obj, wp.Kernel), f"{name} should be a wp.Kernel, got {type(obj)}" - - def test_module_exports_match_all(self): - """__all__ should list every public kernel.""" - from isaaclab.utils.warp import fabric as fabric_utils - - for name in fabric_utils.__all__: - assert hasattr(fabric_utils, name), f"__all__ lists '{name}' but it's not defined" + out_world[i] = wp.mat44d(_world_from_local_transposed(wp.mat44f(child_local[i]), wp.mat44f(parent_world[i]))) + + +# ------------------------------------------------------------------ +# Helpers +# ------------------------------------------------------------------ + + +def _make_transform_matrix(pos, rot_quat_xyzw, scale): + """Build a 4x4 Fabric-transposed transform from pos/quat/scale. + + Returns numpy (4,4) float64 in the transposed storage convention (row-major with + translation in the last row) that Fabric uses. + + Raises: + AssertionError: If the resulting matrix is singular (e.g. zero scale component). + """ + from scipy.spatial.transform import Rotation + + r = Rotation.from_quat(rot_quat_xyzw).as_matrix().astype(np.float64) + rs = r * np.array(scale, dtype=np.float64) + m = np.eye(4, dtype=np.float64) + m[:3, :3] = rs + m[:3, 3] = pos + # Transpose for Fabric storage convention + result = m.T + det = np.linalg.det(result) + assert abs(det) > 1e-6, f"Singular matrix: det={det:.2e}, scale={scale}" + return result + + +# ------------------------------------------------------------------ +# Decompose / Compose round-trip tests +# ------------------------------------------------------------------ + + +def test_decompose_round_trip(): + """Decompose a matrix with translation, rotation, and non-uniform scale; verify round-trip.""" + pos = np.array([5.0, -3.0, 7.0]) + s45 = np.sin(np.pi / 4) + c45 = np.cos(np.pi / 4) + quat_xyzw = np.array([0.0, 0.0, s45, c45]) # 45° Z rotation + scale = np.array([1.5, 0.8, 3.0]) + + mat = _make_transform_matrix(pos, quat_xyzw, scale).astype(np.float32) + matrices = wp.array(mat.reshape(1, 4, 4), dtype=wp.mat44f, device="cpu") + + out_pos = wp.zeros(1, dtype=wp.vec3f, device="cpu") + out_rot = wp.zeros(1, dtype=wp.quatf, device="cpu") + out_scale = wp.zeros(1, dtype=wp.vec3f, device="cpu") + + wp.launch(_test_decompose_kernel, dim=1, inputs=[matrices, out_pos, out_rot, out_scale], device="cpu") + wp.synchronize() + + np.testing.assert_allclose(out_pos.numpy()[0], pos, atol=1e-5) + np.testing.assert_allclose(out_scale.numpy()[0], scale, atol=1e-5) + dot = np.dot(out_rot.numpy()[0], quat_xyzw) + np.testing.assert_allclose(abs(dot), 1.0, atol=1e-5) + + +# ------------------------------------------------------------------ +# World ↔ Local matrix tests +# +# These test the same math the production fabric kernels use: +# local^T = world^T * inv(parent^T) +# world^T = local^T * parent^T +# +# Both parent and child have rotation, translation, and non-uniform scale +# (producing sheared/non-orthogonal upper-3x3 blocks). +# ------------------------------------------------------------------ + +# Shared test data: parent with 10:1 non-uniform scale + 45° Z rotation + translation +_PARENT_WORLD_T = _make_transform_matrix([10, -5, 2], [0, 0, 0.3826834, 0.9238795], [4.0, 0.5, 2.0]) +_CHILD_WORLD_T = _make_transform_matrix([1, 2, 3], [0.2588190, 0, 0, 0.9659258], [1.5, 0.8, 3.0]) + + +def test_local_from_world_transposed(): + """local^T = world^T * inv(parent^T) — verified by reconstruction.""" + cw = wp.array(_CHILD_WORLD_T.reshape(1, 4, 4), dtype=wp.mat44d, device="cpu") + pw = wp.array(_PARENT_WORLD_T.reshape(1, 4, 4), dtype=wp.mat44d, device="cpu") + out = wp.zeros(1, dtype=wp.mat44d, device="cpu") + + wp.launch(_test_local_from_world_kernel, dim=1, inputs=[cw, pw, out], device="cpu") + wp.synchronize() + + # Reconstruction: local^T @ parent^T must equal child_world^T + local_T = out.numpy()[0] + reconstructed = local_T @ _PARENT_WORLD_T + np.testing.assert_allclose(reconstructed, _CHILD_WORLD_T, atol=1e-5) + + +def test_world_from_local_transposed(): + """world^T = local^T * parent^T — verified against known child world.""" + # Ground-truth local computed via numpy + child_local_T = _CHILD_WORLD_T @ np.linalg.inv(_PARENT_WORLD_T) + + cl = wp.array(child_local_T.reshape(1, 4, 4), dtype=wp.mat44d, device="cpu") + pw = wp.array(_PARENT_WORLD_T.reshape(1, 4, 4), dtype=wp.mat44d, device="cpu") + out = wp.zeros(1, dtype=wp.mat44d, device="cpu") + + wp.launch(_test_world_from_local_kernel, dim=1, inputs=[cl, pw, out], device="cpu") + wp.synchronize() + + np.testing.assert_allclose(out.numpy()[0], _CHILD_WORLD_T, atol=1e-5) + From c9dcf47c43ac80e5668579728bdeb05d4d9b53f7 Mon Sep 17 00:00:00 2001 From: Peter Verswyvelen Date: Fri, 22 May 2026 14:14:05 +0000 Subject: [PATCH 08/12] =?UTF-8?q?docs:=20update=20changelog=20wording=20('?= =?UTF-8?q?Will=20be=20used=20by'=20=E2=86=92=20'Used=20by')?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- source/isaaclab/changelog.d/indexed-fabric-kernels.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/source/isaaclab/changelog.d/indexed-fabric-kernels.rst b/source/isaaclab/changelog.d/indexed-fabric-kernels.rst index 0f68b4c60e9d..281881bb7972 100644 --- a/source/isaaclab/changelog.d/indexed-fabric-kernels.rst +++ b/source/isaaclab/changelog.d/indexed-fabric-kernels.rst @@ -13,7 +13,7 @@ Added 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 + 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. From 48823b29d12bc17dff1ef2d5cf36efb9c41df7cd Mon Sep 17 00:00:00 2001 From: Peter Verswyvelen Date: Sat, 23 May 2026 11:03:02 +0000 Subject: [PATCH 09/12] =?UTF-8?q?docs:=20fix=20changelog=20wording=20(Used?= =?UTF-8?q?=20by=20=E2=86=92=20Will=20be=20used=20by)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- source/isaaclab/changelog.d/indexed-fabric-kernels.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/source/isaaclab/changelog.d/indexed-fabric-kernels.rst b/source/isaaclab/changelog.d/indexed-fabric-kernels.rst index 281881bb7972..0f68b4c60e9d 100644 --- a/source/isaaclab/changelog.d/indexed-fabric-kernels.rst +++ b/source/isaaclab/changelog.d/indexed-fabric-kernels.rst @@ -13,7 +13,7 @@ Added 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 + 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. From 7a9f8c43c3a53215484b9c4d7388d6dc9cb08ff8 Mon Sep 17 00:00:00 2001 From: Peter Verswyvelen Date: Sat, 23 May 2026 11:10:28 +0000 Subject: [PATCH 10/12] revert: remove wp.where refactor (unrelated to this PR) --- .../changelog.d/indexed-fabric-kernels.rst | 6 ---- source/isaaclab/isaaclab/utils/warp/fabric.py | 30 +++++++++++++++---- 2 files changed, 24 insertions(+), 12 deletions(-) diff --git a/source/isaaclab/changelog.d/indexed-fabric-kernels.rst b/source/isaaclab/changelog.d/indexed-fabric-kernels.rst index 0f68b4c60e9d..baaf33ceb8a3 100644 --- a/source/isaaclab/changelog.d/indexed-fabric-kernels.rst +++ b/source/isaaclab/changelog.d/indexed-fabric-kernels.rst @@ -16,9 +16,3 @@ Added 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. - -Changed -^^^^^^^ - -* Replaced ``if/else`` branching with ``wp.where`` in existing Fabric - compose/decompose kernels for branchless GPU execution. diff --git a/source/isaaclab/isaaclab/utils/warp/fabric.py b/source/isaaclab/isaaclab/utils/warp/fabric.py index c5a86a58b292..f665323cbe34 100644 --- a/source/isaaclab/isaaclab/utils/warp/fabric.py +++ b/source/isaaclab/isaaclab/utils/warp/fabric.py @@ -132,20 +132,29 @@ def compose_fabric_transformation_matrix_from_warp_arrays( position, rotation, scale = _decompose_transformation_matrix(wp.mat44f(fabric_matrices[fabric_index])) # update position (check if array has elements, not just if it exists) if array_positions.shape[0] > 0: - index = wp.where(broadcast_positions, 0, i) + 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] # update orientation (convert from wxyz to xyzw for Warp) if array_orientations.shape[0] > 0: - index = wp.where(broadcast_orientations, 0, i) + if broadcast_orientations: + index = 0 + else: + index = i rotation[0] = array_orientations[index, 0] # x rotation[1] = array_orientations[index, 1] # y rotation[2] = array_orientations[index, 2] # z rotation[3] = array_orientations[index, 3] # w # update scale if array_scales.shape[0] > 0: - index = wp.where(broadcast_scales, 0, i) + 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] @@ -229,18 +238,27 @@ def compose_indexed_fabric_transforms( position, rotation, scale = _decompose_transformation_matrix(wp.mat44f(fabric_matrices[view_index])) if array_positions.shape[0] > 0: - index = wp.where(broadcast_positions, 0, i) + 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: - index = wp.where(broadcast_orientations, 0, i) + 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: - index = wp.where(broadcast_scales, 0, i) + 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] From a51094f1f61da18828d3d05e1fa09d9761b83f27 Mon Sep 17 00:00:00 2001 From: Peter Verswyvelen Date: Sat, 23 May 2026 17:21:27 +0000 Subject: [PATCH 11/12] test: replace scipy with warp builtins in fabric kernel test helper --- .../test/utils/warp/test_fabric_kernels.py | 16 +++++----------- 1 file changed, 5 insertions(+), 11 deletions(-) diff --git a/source/isaaclab/test/utils/warp/test_fabric_kernels.py b/source/isaaclab/test/utils/warp/test_fabric_kernels.py index 9153838e4366..de48afadab24 100644 --- a/source/isaaclab/test/utils/warp/test_fabric_kernels.py +++ b/source/isaaclab/test/utils/warp/test_fabric_kernels.py @@ -23,7 +23,6 @@ _world_from_local_transposed, ) - # ------------------------------------------------------------------ # Test kernels — thin wp.array wrappers that delegate to production @wp.func # ------------------------------------------------------------------ @@ -80,15 +79,11 @@ def _make_transform_matrix(pos, rot_quat_xyzw, scale): Raises: AssertionError: If the resulting matrix is singular (e.g. zero scale component). """ - from scipy.spatial.transform import Rotation - - r = Rotation.from_quat(rot_quat_xyzw).as_matrix().astype(np.float64) - rs = r * np.array(scale, dtype=np.float64) - m = np.eye(4, dtype=np.float64) - m[:3, :3] = rs - m[:3, 3] = pos - # Transpose for Fabric storage convention - result = m.T + p = wp.vec3f(*pos) + q = wp.quatf(*rot_quat_xyzw) + s = wp.vec3f(*scale) + m = wp.transpose(wp.transform_compose(p, q, s)) + result = np.array(m).reshape(4, 4).astype(np.float64) det = np.linalg.det(result) assert abs(det) > 1e-6, f"Singular matrix: det={det:.2e}, scale={scale}" return result @@ -167,4 +162,3 @@ def test_world_from_local_transposed(): wp.synchronize() np.testing.assert_allclose(out.numpy()[0], _CHILD_WORLD_T, atol=1e-5) - From 14134c88a6824bfc469252210b56535ed2535f7d Mon Sep 17 00:00:00 2001 From: Peter Verswyvelen Date: Sat, 23 May 2026 18:12:50 +0000 Subject: [PATCH 12/12] docs: remove dead ThreeDWorld link (domain squatted) --- docs/source/setup/ecosystem.rst | 1 - 1 file changed, 1 deletion(-) diff --git a/docs/source/setup/ecosystem.rst b/docs/source/setup/ecosystem.rst index 0b532af7db03..4ce71497b59b 100644 --- a/docs/source/setup/ecosystem.rst +++ b/docs/source/setup/ecosystem.rst @@ -209,7 +209,6 @@ contributing, please reach out to us. .. _AirSim: https://microsoft.github.io/AirSim/ .. _DoorGym: https://github.com/PSVL/DoorGym/ .. _ManiSkill: https://github.com/haosulab/ManiSkill -.. _ThreeDWorld: https://www.threedworld.org/ .. _RoboSuite: https://github.com/ARISE-Initiative/robosuite .. _MuJoCo: https://mujoco.org/ .. _MuJoCo Playground: https://playground.mujoco.org/